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

EKF: update default tuning parameters

Browse Source 
Parameters needed to be changed when covariance prediction error was corrected.
The magnetic earth and body field process noise now use separate parameters to help with tuning
This commit is contained in:
Paul Riseborough
2016-04-05 22:23:37 +02:00
committed by Roman
parent eaf94935f0
commit 470098e182
2 changed files with 20 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/covariance.cpp
+5 -5
View File
@@ -153,17 +153,17 @@ void Ekf::predictCovariance()
float d_ang_scale_sig = dt * math::constrain(_params.gyro_scale_p_noise, 0.0f, 1e-2f);
float mag_I_sig, mag_B_sig;
// Don't continue to grow the body 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
// 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
if (_control_status.flags.mag_3D && (P[16][16] + P[17][17] + P[18][8]) < 0.1f) {
mag_I_sig = dt * math::constrain(_params.mag_p_noise, 0.0f, 1e-1f);
mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.0f, 1e-1f);
} else {
mag_I_sig = 0.0f;
}
// Don't continue to grow the earth 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
// 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
if (_control_status.flags.mag_3D && (P[19][19] + P[20][20] + P[21][21]) < 0.1f) {
mag_B_sig = dt * math::constrain(_params.mag_p_noise, 0.0f, 1e-1f);
mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.0f, 1e-1f);
} else {
mag_B_sig = 0.0f;
@@ -210,7 +210,7 @@ void Ekf::predictCovariance()
// inputs to the system are 3 delta angles and 3 delta velocities
float daxNoise, dayNoise, dazNoise;
float dvxNoise, dvyNoise, dvzNoise;
float gyro_noise = math::constrain(_params.gyro_noise, 1e-4f, 1e-2f);
float gyro_noise = math::constrain(_params.gyro_noise, 1e-4f, 1e-1f);
daxNoise = dayNoise = dazNoise = dt * gyro_noise;
float accel_noise = math::constrain(_params.accel_noise, 1e-2f, 1.0f);
dvxNoise = dvyNoise = dvzNoise = dt * accel_noise;