zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

ekf2: incorporate fixes to covariance prediction and initialisation 

Update ecl library reference.
Update default parameters
sbg
Paul Riseborough 9 years ago committed by Lorenz Meier
parent
222566de6e
commit
e66a3bd99f
3 changed files with 28 additions and 16 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      src/lib/ecl
  2. 6
      src/modules/ekf2/ekf2_main.cpp
  3. 36
      src/modules/ekf2/ekf2_params.c

2
src/lib/ecl

@ -1 +1 @@ @@ -1 +1 @@
Subproject commit 8678a939e29379ba5288f75a1d31a2b08d5ddc6b
Subproject commit 430d4b1cf8c00efd06097ca9a834a6629025e847

6
src/modules/ekf2/ekf2_main.cpp
Unescape View File

@ -172,7 +172,8 @@ private: @@ -172,7 +172,8 @@ private:
control::BlockParamFloat *_gyro_bias_p_noise;
control::BlockParamFloat *_accel_bias_p_noise;
control::BlockParamFloat *_gyro_scale_p_noise;
control::BlockParamFloat *_mag_p_noise;
control::BlockParamFloat *_mage_p_noise;
control::BlockParamFloat *_magb_p_noise;
control::BlockParamFloat *_wind_vel_p_noise;
control::BlockParamFloat *_terrain_p_noise; // terrain offset state random walk (m/s)
control::BlockParamFloat *_terrain_gradient; // magnitude of terrain gradient (m/m)
@ -254,7 +255,8 @@ Ekf2::Ekf2(): @@ -254,7 +255,8 @@ Ekf2::Ekf2():
_gyro_bias_p_noise(new control::BlockParamFloat(this, "EKF2_GYR_B_NOISE", false, &_params->gyro_bias_p_noise)),
_accel_bias_p_noise(new control::BlockParamFloat(this, "EKF2_ACC_B_NOISE", false, &_params->accel_bias_p_noise)),
_gyro_scale_p_noise(new control::BlockParamFloat(this, "EKF2_GYR_S_NOISE", false, &_params->gyro_scale_p_noise)),
_mag_p_noise(new control::BlockParamFloat(this, "EKF2_MAG_B_NOISE", false, &_params->mag_p_noise)),
_mage_p_noise(new control::BlockParamFloat(this, "EKF2_MAG_E_NOISE", false, &_params->mage_p_noise)),
_magb_p_noise(new control::BlockParamFloat(this, "EKF2_MAG_B_NOISE", false, &_params->magb_p_noise)),
_wind_vel_p_noise(new control::BlockParamFloat(this, "EKF2_WIND_NOISE", false, &_params->wind_vel_p_noise)),
_terrain_p_noise(new control::BlockParamFloat(this, "EKF2_TERR_NOISE", false, &_params->terrain_p_noise)),
_terrain_gradient(new control::BlockParamFloat(this, "EKF2_TERR_GRAD", false, &_params->terrain_gradient)),

36
src/modules/ekf2/ekf2_params.c
Unescape View File

@ -174,10 +174,10 @@ PARAM_DEFINE_FLOAT(EKF2_REQ_VDRIFT, 0.5f); @@ -174,10 +174,10 @@ PARAM_DEFINE_FLOAT(EKF2_REQ_VDRIFT, 0.5f);
*
* @group EKF2
* @min 0.0001
* @max 0.01
* @max 0.1
* @unit rad/s
*/
PARAM_DEFINE_FLOAT(EKF2_GYR_NOISE, 1.0e-3f);
PARAM_DEFINE_FLOAT(EKF2_GYR_NOISE, 6.0e-2f);
/**
* Accelerometer noise for covariance prediction.
@ -197,7 +197,7 @@ PARAM_DEFINE_FLOAT(EKF2_ACC_NOISE, 0.25f); @@ -197,7 +197,7 @@ PARAM_DEFINE_FLOAT(EKF2_ACC_NOISE, 0.25f);
* @max 0.0001
* @unit rad/s
*/
PARAM_DEFINE_FLOAT(EKF2_GYR_B_NOISE, 7.0e-5f);
PARAM_DEFINE_FLOAT(EKF2_GYR_B_NOISE, 2.5e-6f);
/**
* Process noise for delta velocity z bias prediction.
@ -207,7 +207,7 @@ PARAM_DEFINE_FLOAT(EKF2_GYR_B_NOISE, 7.0e-5f); @@ -207,7 +207,7 @@ PARAM_DEFINE_FLOAT(EKF2_GYR_B_NOISE, 7.0e-5f);
* @max 0.01
* @unit m/s/s
*/
PARAM_DEFINE_FLOAT(EKF2_ACC_B_NOISE, 1.0e-4f);
PARAM_DEFINE_FLOAT(EKF2_ACC_B_NOISE, 3.0e-5f);
/**
* Process noise for delta angle scale factor prediction.
@ -216,17 +216,27 @@ PARAM_DEFINE_FLOAT(EKF2_ACC_B_NOISE, 1.0e-4f); @@ -216,17 +216,27 @@ PARAM_DEFINE_FLOAT(EKF2_ACC_B_NOISE, 1.0e-4f);
* @min 0.0
* @max 0.01
*/
PARAM_DEFINE_FLOAT(EKF2_GYR_S_NOISE, 3.0e-3f);
PARAM_DEFINE_FLOAT(EKF2_GYR_S_NOISE, 3.0e-4f);
/**
* Process noise for body magnetic field prediction.
*
* @group EKF2
* @min 0.0
* @max 0.1
* @unit Gauss/s
*/
PARAM_DEFINE_FLOAT(EKF2_MAG_B_NOISE, 5.0e-4f);
/**
* Process noise for sensor bias and earth magnetic field prediction.
* Process noise for earth magnetic field prediction.
*
* @group EKF2
* @min 0.0
* @max 0.1
* @unit Gauss/s
*/
PARAM_DEFINE_FLOAT(EKF2_MAG_B_NOISE, 2.5e-2f);
PARAM_DEFINE_FLOAT(EKF2_MAG_E_NOISE, 2.5e-3f);
/**
* Process noise for wind velocity prediction.
@ -256,7 +266,7 @@ PARAM_DEFINE_FLOAT(EKF2_GPS_V_NOISE, 0.5f); @@ -256,7 +266,7 @@ PARAM_DEFINE_FLOAT(EKF2_GPS_V_NOISE, 0.5f);
* @max 10.0
* @unit m
*/
PARAM_DEFINE_FLOAT(EKF2_GPS_P_NOISE, 1.0f);
PARAM_DEFINE_FLOAT(EKF2_GPS_P_NOISE, 0.5f);
/**
* Measurement noise for non-aiding position hold.
@ -286,7 +296,7 @@ PARAM_DEFINE_FLOAT(EKF2_BARO_NOISE, 3.0f); @@ -286,7 +296,7 @@ PARAM_DEFINE_FLOAT(EKF2_BARO_NOISE, 3.0f);
* @max 1.0
* @unit rad
*/
PARAM_DEFINE_FLOAT(EKF2_HEAD_NOISE, 0.17f);
PARAM_DEFINE_FLOAT(EKF2_HEAD_NOISE, 0.3f);
/**
* Measurement noise for magnetometer 3-axis fusion.
@ -323,7 +333,7 @@ PARAM_DEFINE_FLOAT(EKF2_MAG_DECL, 0); @@ -323,7 +333,7 @@ PARAM_DEFINE_FLOAT(EKF2_MAG_DECL, 0);
* @min 1.0
* @unit SD
*/
PARAM_DEFINE_FLOAT(EKF2_HDG_GATE, 3.0f);
PARAM_DEFINE_FLOAT(EKF2_HDG_GATE, 2.6f);
/**
* Gate size for magnetometer XYZ component fusion
@ -370,7 +380,7 @@ PARAM_DEFINE_INT32(EKF2_MAG_TYPE, 0); @@ -370,7 +380,7 @@ PARAM_DEFINE_INT32(EKF2_MAG_TYPE, 0);
* @min 1.0
* @unit SD
*/
PARAM_DEFINE_FLOAT(EKF2_BARO_GATE, 3.0f);
PARAM_DEFINE_FLOAT(EKF2_BARO_GATE, 5.0f);
/**
* Gate size for GPS horizontal position fusion
@ -379,7 +389,7 @@ PARAM_DEFINE_FLOAT(EKF2_BARO_GATE, 3.0f); @@ -379,7 +389,7 @@ PARAM_DEFINE_FLOAT(EKF2_BARO_GATE, 3.0f);
* @min 1.0
* @unit SD
*/
PARAM_DEFINE_FLOAT(EKF2_GPS_P_GATE, 3.0f);
PARAM_DEFINE_FLOAT(EKF2_GPS_P_GATE, 5.0f);
/**
* Gate size for GPS velocity fusion
@ -388,7 +398,7 @@ PARAM_DEFINE_FLOAT(EKF2_GPS_P_GATE, 3.0f); @@ -388,7 +398,7 @@ PARAM_DEFINE_FLOAT(EKF2_GPS_P_GATE, 3.0f);
* @min 1.0
* @unit SD
*/
PARAM_DEFINE_FLOAT(EKF2_GPS_V_GATE, 3.0f);
PARAM_DEFINE_FLOAT(EKF2_GPS_V_GATE, 5.0f);
/**
* Replay mode

Write Preview
Loading…
Cancel
Save