fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: incorporate fixes to covariance prediction and initialisation

Browse Source 
Update ecl library reference.
Update default parameters
This commit is contained in:
Paul Riseborough 2016-04-01 17:57:57 -07:00 committed by Lorenz Meier
parent 222566de6e
commit e66a3bd99f
3 changed files with 28 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/lib/ecl

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

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

@ -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():
_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
View File

@ -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);
* @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);
* @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);
* @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 sensor bias and earth magnetic field prediction.
* 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, 2.5e-2f);
PARAM_DEFINE_FLOAT(EKF2_MAG_B_NOISE, 5.0e-4f);
/**
* Process noise for earth magnetic field prediction.
*
* @group EKF2
* @min 0.0
* @max 0.1
* @unit Gauss/s
*/
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);
* @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);
* @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);
* @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);
* @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);
* @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);
* @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