fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 02:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Use intuitive units for gyro and accel bias process noise

Browse Source 
This also makes the tuning less sensitive to changes in the EKF update rate.
This commit is contained in:
Paul Riseborough
2016-05-05 21:23:03 +10:00
parent 799865c934
commit 29a361d3a3
2 changed files with 11 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/common.h
+4 -4
View File
@@ -170,8 +170,8 @@ struct parameters {
float accel_noise; // IMU acceleration noise use for covariance prediction (m/sec/sec)
// process noise
float gyro_bias_p_noise; // process noise for IMU delta angle bias prediction (rad/sec)
float accel_bias_p_noise; // process noise for IMU delta velocity bias prediction (m/sec/sec)
float gyro_bias_p_noise; // process noise for IMU rate gyro bias prediction (rad/sec**2)
float accel_bias_p_noise; // process noise for IMU accelerometer bias prediction (m/sec**3)
float mage_p_noise; // process noise for earth magnetic field prediction (Guass/sec)
float magb_p_noise; // process noise for body magnetic field prediction (Guass/sec)
float wind_vel_p_noise; // process noise for wind velocity prediction (m/sec/sec)
@@ -250,8 +250,8 @@ struct parameters {
accel_noise = 2.5e-1f;
// process noise
gyro_bias_p_noise = 5.0e-6f;
accel_bias_p_noise = 1.0e-7f;
gyro_bias_p_noise = 1.0e-3f;
accel_bias_p_noise = 1.0e-5f;
mage_p_noise = 1.0e-3f;
magb_p_noise = 1.0e-4f;
wind_vel_p_noise = 1.0e-1f;
EKF/covariance.cpp
+7 -3
View File
@@ -143,11 +143,14 @@ void Ekf::predictCovariance()
// compute noise variance for stationary processes
float process_noise[_k_num_states] = {};
float d_ang_bias_sig = dt * math::constrain(_params.gyro_bias_p_noise, 0.0f, 1.0f);
float d_vel_bias_sig = dt * math::constrain(_params.accel_bias_p_noise, 0.0f, 1.0f);
float mag_I_sig, mag_B_sig;
// convert rate of change of rate gyro bias (rad/s**2) as specified by the parameter to an expected change in delta angle (rad) since the last update
float d_ang_bias_sig = dt * dt * math::constrain(_params.gyro_bias_p_noise, 0.0f, 1.0f);
// convert rate of change of acceerometer bias (m/s**3) as specified by the parameter to an expected change in delta velocity (m/s) since the last update
float d_vel_bias_sig = dt * dt * math::constrain(_params.accel_bias_p_noise, 0.0f, 1.0f);
// Don't continue to grow the earth field variances if they are becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
float mag_I_sig;
if (_control_status.flags.mag_3D && (P[16][16] + P[17][17] + P[18][8]) < 0.1f) {
mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.0f, 1.0f);
@@ -156,6 +159,7 @@ void Ekf::predictCovariance()
}
// Don't continue to grow the body field variances if they is becoming too large or we are not doing 3-axis fusion as this can make the covariance matrix badly conditioned
float mag_B_sig;
if (_control_status.flags.mag_3D && (P[19][19] + P[20][20] + P[21][21]) < 0.1f) {
mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.0f, 1.0f);