mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-17 02:52:12 +01:00
Put all EKF initialisation values in a separate UAVObject EKFConfiguration,
adapted insgps13state.c/.h, modules/Attitude/revolution, configrevowidget.cpp, accordingly.
This commit is contained in:
Corvus Corax
parent
48288c740e
commit
25c58ff074
@ -61,7 +61,7 @@ void INSCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt,
|
||||
void INSResetP(float PDiag[13]);
|
||||
void INSGetP(float PDiag[13]);
|
||||
void INSSetState(float pos[3], float vel[3], float q[4], float gyro_bias[3], float accel_bias[3]);
|
||||
void INSSetPosVelVar(float PosVar, float VelVar);
|
||||
void INSSetPosVelVar(float PosVar[3], float VelVar[3]);
|
||||
void INSSetGyroBias(float gyro_bias[3]);
|
||||
void INSSetAccelVar(float accel_var[3]);
|
||||
void INSSetGyroVar(float gyro_var[3]);
|
||||
|
||||
@ -191,14 +191,14 @@ void INSPosVelReset(float pos[3], float vel[3])
|
||||
X[5] = vel[2];
|
||||
}
|
||||
|
||||
void INSSetPosVelVar(float PosVar, float VelVar)
|
||||
void INSSetPosVelVar(float PosVar[3], float VelVar[3])
|
||||
{
|
||||
R[0] = PosVar;
|
||||
R[1] = PosVar;
|
||||
R[2] = PosVar;
|
||||
R[3] = VelVar;
|
||||
R[4] = VelVar;
|
||||
R[5] = VelVar;
|
||||
R[0] = PosVar[0];
|
||||
R[1] = PosVar[1];
|
||||
R[2] = PosVar[2];
|
||||
R[3] = VelVar[0];
|
||||
R[4] = VelVar[1];
|
||||
R[5] = VelVar[2];
|
||||
}
|
||||
|
||||
void INSSetGyroBias(float gyro_bias[3])
|
||||
|
||||
@ -66,7 +66,7 @@
|
||||
#include "homelocation.h"
|
||||
#include "magnetometer.h"
|
||||
#include "positionactual.h"
|
||||
#include "ekfinitialvariance.h"
|
||||
#include "ekfconfiguration.h"
|
||||
#include "ekfstatevariance.h"
|
||||
#include "revocalibration.h"
|
||||
#include "revosettings.h"
|
||||
@ -104,7 +104,7 @@ static xQueueHandle gpsVelQueue;
|
||||
static AttitudeSettingsData attitudeSettings;
|
||||
static HomeLocationData homeLocation;
|
||||
static RevoCalibrationData revoCalibration;
|
||||
static EKFInitialVarianceData ekfInitialVariance;
|
||||
static EKFConfigurationData ekfConfiguration;
|
||||
static RevoSettingsData revoSettings;
|
||||
static FlightStatusData flightStatus;
|
||||
const uint32_t SENSOR_QUEUE_SIZE = 10;
|
||||
@ -164,7 +164,7 @@ int32_t AttitudeInitialize(void)
|
||||
VelocityActualInitialize();
|
||||
RevoSettingsInitialize();
|
||||
RevoCalibrationInitialize();
|
||||
EKFInitialVarianceInitialize();
|
||||
EKFConfigurationInitialize();
|
||||
EKFStateVarianceInitialize();
|
||||
FlightStatusInitialize();
|
||||
|
||||
@ -192,7 +192,7 @@ int32_t AttitudeInitialize(void)
|
||||
RevoSettingsConnectCallback(&settingsUpdatedCb);
|
||||
RevoCalibrationConnectCallback(&settingsUpdatedCb);
|
||||
HomeLocationConnectCallback(&settingsUpdatedCb);
|
||||
EKFInitialVarianceConnectCallback(&settingsUpdatedCb);
|
||||
EKFConfigurationConnectCallback(&settingsUpdatedCb);
|
||||
FlightStatusConnectCallback(&settingsUpdatedCb);
|
||||
|
||||
return 0;
|
||||
@ -712,8 +712,12 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
|
||||
value_error = true;
|
||||
}
|
||||
|
||||
if ( invalid_var(revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_POS]) ||
|
||||
invalid_var(revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_VEL]) ) {
|
||||
if ( invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH]) ||
|
||||
invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSEAST]) ||
|
||||
invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSDOWN]) ||
|
||||
invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH]) ||
|
||||
invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELEAST]) ||
|
||||
invalid_var(ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELDOWN]) ) {
|
||||
gps_updated = false;
|
||||
value_error = true;
|
||||
}
|
||||
@ -768,11 +772,11 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
|
||||
|
||||
// Reset the INS algorithm
|
||||
INSGPSInit();
|
||||
INSSetMagVar(revoCalibration.mag_var);
|
||||
INSSetAccelVar(revoCalibration.accel_var);
|
||||
INSSetGyroVar(revoCalibration.gyro_var);
|
||||
INSSetGyroBiasVar(revoCalibration.gyro_bias_var);
|
||||
INSSetBaroVar(revoCalibration.baro_var);
|
||||
INSSetMagVar(&ekfConfiguration.R[EKFCONFIGURATION_R_MAGX]);
|
||||
INSSetAccelVar(&ekfConfiguration.Q[EKFCONFIGURATION_Q_ACCELX]);
|
||||
INSSetGyroVar(&ekfConfiguration.Q[EKFCONFIGURATION_Q_GYROX]);
|
||||
INSSetGyroBiasVar(&ekfConfiguration.Q[EKFCONFIGURATION_Q_GYRODRIFTX]);
|
||||
INSSetBaroVar(ekfConfiguration.R[EKFCONFIGURATION_R_BAROZ]);
|
||||
|
||||
// Initialize the gyro bias
|
||||
float gyro_bias[3] = {0.0f, 0.0f, 0.0f};
|
||||
@ -834,7 +838,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
|
||||
float q[4] = { attitudeActual.q1, attitudeActual.q2, attitudeActual.q3, attitudeActual.q4 };
|
||||
INSSetState(pos, zeros, q, zeros, zeros);
|
||||
|
||||
INSResetP(ekfInitialVariance.P);
|
||||
INSResetP(ekfConfiguration.P);
|
||||
|
||||
} else {
|
||||
// Run prediction a bit before any corrections
|
||||
@ -904,7 +908,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
|
||||
|
||||
if (gps_updated && outdoor_mode)
|
||||
{
|
||||
INSSetPosVelVar(revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_POS], revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_VEL]);
|
||||
INSSetPosVelVar(&ekfConfiguration.R[EKFCONFIGURATION_R_GPSPOSNORTH], &ekfConfiguration.R[EKFCONFIGURATION_R_GPSVELNORTH]);
|
||||
sensors |= POS_SENSORS;
|
||||
|
||||
if (0) { // Old code to take horizontal velocity from GPS Position update
|
||||
@ -922,7 +926,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
|
||||
* ( -NED[2] - baroData.Altitude );
|
||||
|
||||
} else if (!outdoor_mode) {
|
||||
INSSetPosVelVar(1e6f, 1e5f);
|
||||
INSSetPosVelVar((float[3]){1e6f,1e6f,1e6f}, (float[3]){1e5f,1e5f,1e5f});
|
||||
vel[0] = vel[1] = vel[2] = 0.0f;
|
||||
NED[0] = NED[1] = 0.0f;
|
||||
NED[2] = -(baroData.Altitude + baroOffset);
|
||||
@ -949,7 +953,7 @@ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
|
||||
if ( !gps_vel_updated && !gps_updated) {
|
||||
// feed airspeed into EKF, treat wind as 1e2 variance
|
||||
sensors |= HORIZ_SENSORS | VERT_SENSORS;
|
||||
INSSetPosVelVar(1e6f, 1e2f);
|
||||
INSSetPosVelVar((float[3]){1e6f,1e6f,1e6f}, (float[3]){1e2f,1e2f,1e2f});
|
||||
// rotate airspeed vector into NED frame - airspeed is measured in X axis only
|
||||
float R[3][3];
|
||||
Quaternion2R(Nav.q,R);
|
||||
@ -1021,38 +1025,34 @@ static void settingsUpdatedCb(UAVObjEvent * ev)
|
||||
if (ev == NULL || ev->obj == FlightStatusHandle()) {
|
||||
FlightStatusGet(&flightStatus);
|
||||
}
|
||||
if (ev == NULL || ev->obj == RevoCalibrationHandle()) {
|
||||
RevoCalibrationGet(&revoCalibration);
|
||||
}
|
||||
// change of these settings require reinitialization of the EKF
|
||||
// when an error flag has been risen, we also listen to flightStatus updates,
|
||||
// since we are waiting for the system to get disarmed so we can reinitialize safely.
|
||||
if (ev == NULL || ev->obj == RevoCalibrationHandle() ||
|
||||
ev->obj == EKFInitialVarianceHandle() ||
|
||||
ev->obj == EKFConfigurationHandle() ||
|
||||
ev->obj == RevoSettingsHandle() ||
|
||||
( variance_error==true && ev->obj == FlightStatusHandle() )
|
||||
) {
|
||||
|
||||
bool error = false;
|
||||
RevoCalibrationGet(&revoCalibration);
|
||||
|
||||
if ( invalid_var( revoCalibration.mag_var[0] ) ||
|
||||
invalid_var( revoCalibration.mag_var[1] ) ||
|
||||
invalid_var( revoCalibration.mag_var[2] ) ||
|
||||
invalid_var( revoCalibration.accel_var[0] ) ||
|
||||
invalid_var( revoCalibration.accel_var[1] ) ||
|
||||
invalid_var( revoCalibration.accel_var[2] ) ||
|
||||
invalid_var( revoCalibration.gyro_var[0] ) ||
|
||||
invalid_var( revoCalibration.gyro_var[1] ) ||
|
||||
invalid_var( revoCalibration.gyro_var[2] ) ||
|
||||
invalid_var( revoCalibration.gyro_bias_var[0] ) ||
|
||||
invalid_var( revoCalibration.gyro_bias_var[1] ) ||
|
||||
invalid_var( revoCalibration.gyro_bias_var[2] ) ||
|
||||
invalid_var( revoCalibration.baro_var ) ) {
|
||||
error = true;
|
||||
}
|
||||
|
||||
EKFInitialVarianceGet(&ekfInitialVariance);
|
||||
EKFConfigurationGet(&ekfConfiguration);
|
||||
int t;
|
||||
for (t=0; t < EKFINITIALVARIANCE_P_NUMELEM; t++) {
|
||||
if (invalid_var(ekfInitialVariance.P[t])) {
|
||||
for (t=0; t < EKFCONFIGURATION_P_NUMELEM; t++) {
|
||||
if (invalid_var(ekfConfiguration.P[t])) {
|
||||
error = true;
|
||||
}
|
||||
}
|
||||
for (t=0; t < EKFCONFIGURATION_Q_NUMELEM; t++) {
|
||||
if (invalid_var(ekfConfiguration.Q[t])) {
|
||||
error = true;
|
||||
}
|
||||
}
|
||||
for (t=0; t < EKFCONFIGURATION_R_NUMELEM; t++) {
|
||||
if (invalid_var(ekfConfiguration.R[t])) {
|
||||
error = true;
|
||||
}
|
||||
}
|
||||
|
||||
@ -68,7 +68,7 @@ UAVOBJSRCFILENAMES += positionactual
|
||||
UAVOBJSRCFILENAMES += ratedesired
|
||||
UAVOBJSRCFILENAMES += relaytuning
|
||||
UAVOBJSRCFILENAMES += relaytuningsettings
|
||||
UAVOBJSRCFILENAMES += ekfinitialvariance
|
||||
UAVOBJSRCFILENAMES += ekfconfiguration
|
||||
UAVOBJSRCFILENAMES += ekfstatevariance
|
||||
UAVOBJSRCFILENAMES += revocalibration
|
||||
UAVOBJSRCFILENAMES += revosettings
|
||||
|
||||
@ -73,7 +73,7 @@ UAVOBJSRCFILENAMES += positionactual
|
||||
UAVOBJSRCFILENAMES += ratedesired
|
||||
UAVOBJSRCFILENAMES += relaytuning
|
||||
UAVOBJSRCFILENAMES += relaytuningsettings
|
||||
UAVOBJSRCFILENAMES += ekfinitialvariance
|
||||
UAVOBJSRCFILENAMES += ekfconfiguration
|
||||
UAVOBJSRCFILENAMES += ekfstatevariance
|
||||
UAVOBJSRCFILENAMES += revocalibration
|
||||
UAVOBJSRCFILENAMES += revosettings
|
||||
|
||||
@ -41,6 +41,7 @@
|
||||
#include <QDesktopServices>
|
||||
#include <QUrl>
|
||||
#include <attitudesettings.h>
|
||||
#include <ekfconfiguration.h>
|
||||
#include <revocalibration.h>
|
||||
#include <homelocation.h>
|
||||
#include <accels.h>
|
||||
@ -212,6 +213,7 @@ ConfigRevoWidget::ConfigRevoWidget(QWidget *parent) :
|
||||
// Must set up the UI (above) before setting up the UAVO mappings or refreshWidgetValues
|
||||
// will be dealing with some null pointers
|
||||
addUAVObject("RevoCalibration");
|
||||
addUAVObject("EKFConfiguration");
|
||||
autoLoadWidgets();
|
||||
|
||||
// Connect the signals
|
||||
@ -965,20 +967,20 @@ void ConfigRevoWidget::doGetNoiseSample(UAVObject * obj)
|
||||
disconnect(gyros, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doGetNoiseSample(UAVObject*)));
|
||||
disconnect(mags, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(doGetNoiseSample(UAVObject*)));
|
||||
|
||||
RevoCalibration *revoCalibration = RevoCalibration::GetInstance(getObjectManager());
|
||||
Q_ASSERT(revoCalibration);
|
||||
if(revoCalibration) {
|
||||
RevoCalibration::DataFields revoCalData = revoCalibration->getData();
|
||||
revoCalData.accel_var[RevoCalibration::ACCEL_VAR_X] = listVar(accel_accum_x);
|
||||
revoCalData.accel_var[RevoCalibration::ACCEL_VAR_Y] = listVar(accel_accum_y);
|
||||
revoCalData.accel_var[RevoCalibration::ACCEL_VAR_Z] = listVar(accel_accum_z);
|
||||
revoCalData.gyro_var[RevoCalibration::GYRO_VAR_X] = listVar(gyro_accum_x);
|
||||
revoCalData.gyro_var[RevoCalibration::GYRO_VAR_Y] = listVar(gyro_accum_y);
|
||||
revoCalData.gyro_var[RevoCalibration::GYRO_VAR_Z] = listVar(gyro_accum_z);
|
||||
revoCalData.mag_var[RevoCalibration::MAG_VAR_X] = listVar(mag_accum_x);
|
||||
revoCalData.mag_var[RevoCalibration::MAG_VAR_Y] = listVar(mag_accum_y);
|
||||
revoCalData.mag_var[RevoCalibration::MAG_VAR_Z] = listVar(mag_accum_z);
|
||||
revoCalibration->setData(revoCalData);
|
||||
EKFConfiguration *ekfConfiguration = EKFConfiguration::GetInstance(getObjectManager());
|
||||
Q_ASSERT(ekfConfiguration);
|
||||
if(ekfConfiguration) {
|
||||
EKFConfiguration::DataFields revoCalData = ekfConfiguration->getData();
|
||||
revoCalData.Q[EKFConfiguration::Q_ACCELX] = listVar(accel_accum_x);
|
||||
revoCalData.Q[EKFConfiguration::Q_ACCELY] = listVar(accel_accum_y);
|
||||
revoCalData.Q[EKFConfiguration::Q_ACCELZ] = listVar(accel_accum_z);
|
||||
revoCalData.Q[EKFConfiguration::Q_GYROX] = listVar(gyro_accum_x);
|
||||
revoCalData.Q[EKFConfiguration::Q_GYROY] = listVar(gyro_accum_y);
|
||||
revoCalData.Q[EKFConfiguration::Q_GYROZ] = listVar(gyro_accum_z);
|
||||
revoCalData.R[EKFConfiguration::R_MAGX] = listVar(mag_accum_x);
|
||||
revoCalData.R[EKFConfiguration::R_MAGY] = listVar(mag_accum_y);
|
||||
revoCalData.R[EKFConfiguration::R_MAGZ] = listVar(mag_accum_z);
|
||||
ekfConfiguration->setData(revoCalData);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -989,42 +991,42 @@ void ConfigRevoWidget::doGetNoiseSample(UAVObject * obj)
|
||||
*/
|
||||
void ConfigRevoWidget::drawVariancesGraph()
|
||||
{
|
||||
RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager());
|
||||
Q_ASSERT(revoCalibration);
|
||||
if(!revoCalibration)
|
||||
EKFConfiguration * ekfConfiguration = EKFConfiguration::GetInstance(getObjectManager());
|
||||
Q_ASSERT(ekfConfiguration);
|
||||
if(!ekfConfiguration)
|
||||
return;
|
||||
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
|
||||
EKFConfiguration::DataFields ekfConfigurationData = ekfConfiguration->getData();
|
||||
|
||||
// The expected range is from 1E-6 to 1E-1
|
||||
double steps = 6; // 6 bars on the graph
|
||||
float accel_x_var = -1/steps*(1+steps+log10(revoCalibrationData.accel_var[RevoCalibration::ACCEL_VAR_X]));
|
||||
float accel_x_var = -1/steps*(1+steps+log10(ekfConfigurationData.Q[EKFConfiguration::Q_ACCELX]));
|
||||
if(accel_x)
|
||||
accel_x->setTransform(QTransform::fromScale(1,accel_x_var),false);
|
||||
float accel_y_var = -1/steps*(1+steps+log10(revoCalibrationData.accel_var[RevoCalibration::ACCEL_VAR_Y]));
|
||||
float accel_y_var = -1/steps*(1+steps+log10(ekfConfigurationData.Q[EKFConfiguration::Q_ACCELY]));
|
||||
if(accel_y)
|
||||
accel_y->setTransform(QTransform::fromScale(1,accel_y_var),false);
|
||||
float accel_z_var = -1/steps*(1+steps+log10(revoCalibrationData.accel_var[RevoCalibration::ACCEL_VAR_Z]));
|
||||
float accel_z_var = -1/steps*(1+steps+log10(ekfConfigurationData.Q[EKFConfiguration::Q_ACCELZ]));
|
||||
if(accel_z)
|
||||
accel_z->setTransform(QTransform::fromScale(1,accel_z_var),false);
|
||||
|
||||
float gyro_x_var = -1/steps*(1+steps+log10(revoCalibrationData.gyro_var[RevoCalibration::GYRO_VAR_X]));
|
||||
float gyro_x_var = -1/steps*(1+steps+log10(ekfConfigurationData.Q[EKFConfiguration::Q_GYROX]));
|
||||
if(gyro_x)
|
||||
gyro_x->setTransform(QTransform::fromScale(1,gyro_x_var),false);
|
||||
float gyro_y_var = -1/steps*(1+steps+log10(revoCalibrationData.gyro_var[RevoCalibration::GYRO_VAR_Y]));
|
||||
float gyro_y_var = -1/steps*(1+steps+log10(ekfConfigurationData.Q[EKFConfiguration::Q_GYROY]));
|
||||
if(gyro_y)
|
||||
gyro_y->setTransform(QTransform::fromScale(1,gyro_y_var),false);
|
||||
float gyro_z_var = -1/steps*(1+steps+log10(revoCalibrationData.gyro_var[RevoCalibration::GYRO_VAR_Z]));
|
||||
float gyro_z_var = -1/steps*(1+steps+log10(ekfConfigurationData.Q[EKFConfiguration::Q_GYROZ]));
|
||||
if(gyro_z)
|
||||
gyro_z->setTransform(QTransform::fromScale(1,gyro_z_var),false);
|
||||
|
||||
// Scale by 1e-3 because mag vars are much higher.
|
||||
float mag_x_var = -1/steps*(1+steps+log10(1e-3*revoCalibrationData.mag_var[RevoCalibration::MAG_VAR_X]));
|
||||
float mag_x_var = -1/steps*(1+steps+log10(1e-3*ekfConfigurationData.R[EKFConfiguration::R_MAGX]));
|
||||
if(mag_x)
|
||||
mag_x->setTransform(QTransform::fromScale(1,mag_x_var),false);
|
||||
float mag_y_var = -1/steps*(1+steps+log10(1e-3*revoCalibrationData.mag_var[RevoCalibration::MAG_VAR_Y]));
|
||||
float mag_y_var = -1/steps*(1+steps+log10(1e-3*ekfConfigurationData.R[EKFConfiguration::R_MAGY]));
|
||||
if(mag_y)
|
||||
mag_y->setTransform(QTransform::fromScale(1,mag_y_var),false);
|
||||
float mag_z_var = -1/steps*(1+steps+log10(1e-3*revoCalibrationData.mag_var[RevoCalibration::MAG_VAR_Z]));
|
||||
float mag_z_var = -1/steps*(1+steps+log10(1e-3*ekfConfigurationData.R[EKFConfiguration::R_MAGZ]));
|
||||
if(mag_z)
|
||||
mag_z->setTransform(QTransform::fromScale(1,mag_z_var),false);
|
||||
}
|
||||
|
||||
@ -33,7 +33,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \
|
||||
$$UAVOBJECT_SYNTHETICS/altholdsmoothed.h \
|
||||
$$UAVOBJECT_SYNTHETICS/altitudeholddesired.h \
|
||||
$$UAVOBJECT_SYNTHETICS/altitudeholdsettings.h \
|
||||
$$UAVOBJECT_SYNTHETICS/ekfinitialvariance.h \
|
||||
$$UAVOBJECT_SYNTHETICS/ekfconfiguration.h \
|
||||
$$UAVOBJECT_SYNTHETICS/ekfstatevariance.h \
|
||||
$$UAVOBJECT_SYNTHETICS/revocalibration.h \
|
||||
$$UAVOBJECT_SYNTHETICS/revosettings.h \
|
||||
@ -114,7 +114,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \
|
||||
$$UAVOBJECT_SYNTHETICS/altholdsmoothed.cpp \
|
||||
$$UAVOBJECT_SYNTHETICS/altitudeholddesired.cpp \
|
||||
$$UAVOBJECT_SYNTHETICS/altitudeholdsettings.cpp \
|
||||
$$UAVOBJECT_SYNTHETICS/ekfinitialvariance.cpp \
|
||||
$$UAVOBJECT_SYNTHETICS/ekfconfiguration.cpp \
|
||||
$$UAVOBJECT_SYNTHETICS/ekfstatevariance.cpp \
|
||||
$$UAVOBJECT_SYNTHETICS/revocalibration.cpp \
|
||||
$$UAVOBJECT_SYNTHETICS/revosettings.cpp \
|
||||
|
||||
64
shared/uavobjectdefinition/ekfconfiguration.xml
Normal file
64
shared/uavobjectdefinition/ekfconfiguration.xml
Normal file
@ -0,0 +1,64 @@
|
||||
<xml>
|
||||
<object name="EKFConfiguration" singleinstance="true" settings="true">
|
||||
<description>Extended Kalman Filter initialisation</description>
|
||||
<field name="P" units="1^2" type="float" defaultvalue="
|
||||
25.0, 25.0, 25.0,
|
||||
5.0, 5.0, 5.0,
|
||||
0.007, 0.007, 0.007, 0.007,
|
||||
0.0001, 0.0001, 0.0001">
|
||||
<elementnames>
|
||||
<elementname>PositionNorth</elementname>
|
||||
<elementname>PositionEast</elementname>
|
||||
<elementname>PositionDown</elementname>
|
||||
<elementname>VelocityNorth</elementname>
|
||||
<elementname>VelocityEast</elementname>
|
||||
<elementname>VelocityDown</elementname>
|
||||
<elementname>AttitudeQ1</elementname>
|
||||
<elementname>AttitudeQ2</elementname>
|
||||
<elementname>AttitudeQ3</elementname>
|
||||
<elementname>AttitudeQ4</elementname>
|
||||
<elementname>GyroDriftX</elementname>
|
||||
<elementname>GyroDriftY</elementname>
|
||||
<elementname>GyroDriftZ</elementname>
|
||||
</elementnames>
|
||||
</field>
|
||||
<field name="Q" units="1^2" type="float" defaultvalue="
|
||||
0.1, 0.1, 0.1,
|
||||
10.0, 10.0, 10.0,
|
||||
0.00000002, 0.00000002, 0.00000002">
|
||||
<elementnames>
|
||||
<elementname>GyroX</elementname>
|
||||
<elementname>GyroY</elementname>
|
||||
<elementname>GyroZ</elementname>
|
||||
<elementname>AccelX</elementname>
|
||||
<elementname>AccelY</elementname>
|
||||
<elementname>AccelZ</elementname>
|
||||
<elementname>GyroDriftX</elementname>
|
||||
<elementname>GyroDriftY</elementname>
|
||||
<elementname>GyroDriftZ</elementname>
|
||||
</elementnames>
|
||||
</field>
|
||||
<field name="R" units="1^2" type="float" defaultvalue="
|
||||
1.0, 1.0, 1.0,
|
||||
0.1, 0.1, 0.1,
|
||||
1.0, 1.0, 1.0,
|
||||
0.1">
|
||||
<elementnames>
|
||||
<elementname>GPSPosNorth</elementname>
|
||||
<elementname>GPSPosEast</elementname>
|
||||
<elementname>GPSPosDown</elementname>
|
||||
<elementname>GPSVelNorth</elementname>
|
||||
<elementname>GPSVelEast</elementname>
|
||||
<elementname>GPSVelDown</elementname>
|
||||
<elementname>MagX</elementname>
|
||||
<elementname>MagY</elementname>
|
||||
<elementname>MagZ</elementname>
|
||||
<elementname>BaroZ</elementname>
|
||||
</elementnames>
|
||||
</field>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
|
||||
<telemetryflight acked="true" updatemode="onchange" period="0"/>
|
||||
<logging updatemode="manual" period="0"/>
|
||||
</object>
|
||||
</xml>
|
||||
@ -1,30 +0,0 @@
|
||||
<xml>
|
||||
<object name="EKFInitialVariance" singleinstance="true" settings="true">
|
||||
<description>Extended Kalman Filter covariance initialisation</description>
|
||||
<field name="P" units="1^2" type="float" defaultvalue="
|
||||
25.0, 25.0, 25.0,
|
||||
5.0, 5.0, 5.0,
|
||||
0.007, 0.007, 0.007, 0.007,
|
||||
0.0001, 0.0001, 0.0001">
|
||||
<elementnames>
|
||||
<elementname>PositionNorth</elementname>
|
||||
<elementname>PositionEast</elementname>
|
||||
<elementname>PositionDown</elementname>
|
||||
<elementname>VelocityNorth</elementname>
|
||||
<elementname>VelocityEast</elementname>
|
||||
<elementname>VelocityDown</elementname>
|
||||
<elementname>AttitudeQ1</elementname>
|
||||
<elementname>AttitudeQ2</elementname>
|
||||
<elementname>AttitudeQ3</elementname>
|
||||
<elementname>AttitudeQ4</elementname>
|
||||
<elementname>GyroDriftX</elementname>
|
||||
<elementname>GyroDriftY</elementname>
|
||||
<elementname>GyroDriftZ</elementname>
|
||||
</elementnames>
|
||||
</field>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
|
||||
<telemetryflight acked="true" updatemode="onchange" period="0"/>
|
||||
<logging updatemode="manual" period="0"/>
|
||||
</object>
|
||||
</xml>
|
||||
@ -3,22 +3,16 @@
|
||||
<description>Settings for the INS to control the algorithm and what is updated</description>
|
||||
|
||||
|
||||
<!-- Sensor noises -->
|
||||
<!-- Sensor calibration -->
|
||||
<field name="accel_bias" units="m/s" type="float" elementnames="X,Y,Z" defaultvalue="0,0,0"/>
|
||||
<field name="accel_scale" units="gain" type="float" elementnames="X,Y,Z" defaultvalue="1,1,1"/>
|
||||
<field name="accel_var" units="(m/s)^2" type="float" elementnames="X,Y,Z" defaultvalue="10,10,10"/>
|
||||
<field name="gyro_bias" units="deg/s" type="float" elementnames="X,Y,Z" defaultvalue="0,0,0"/>
|
||||
<field name="gyro_scale" units="gain" type="float" elementnames="X,Y,Z" defaultvalue="1,1,1"/>
|
||||
<field name="gyro_var" units="(deg/s)^2" type="float" elementnames="X,Y,Z" defaultvalue="0.1,0.1,0.1"/>
|
||||
<field name="gyro_bias_var" units="(deg/s)^2" type="float" elementnames="X,Y,Z" defaultvalue="0.00000002,0.00000002,0.00000002"/>
|
||||
<field name="mag_bias" units="mGau" type="float" elementnames="X,Y,Z" defaultvalue="0,0,0"/>
|
||||
<field name="mag_scale" units="gain" type="float" elementnames="X,Y,Z" defaultvalue="1"/>
|
||||
<field name="mag_var" units="mGau^2" type="float" elementnames="X,Y,Z" defaultvalue="1,1,1"/>
|
||||
|
||||
<field name="gps_var" units="m^2" type="float" elementnames="Pos,Vel" defaultvalue="1,0.1"/>
|
||||
<field name="baro_var" units="m^2" type="float" elements="1" defaultvalue="0.1"/>
|
||||
|
||||
<!-- These settings are related to how the sensors are post processed -->
|
||||
<!-- TODO: reimplement, put elsewhere (later) -->
|
||||
<field name="BiasCorrectedRaw" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>
|
||||
<field name="MagBiasNullingRate" units="" type="float" elements="1" defaultvalue="0"/>
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user