Browse Source

AP_NavEKF : Increase the tuning range for magnetometer measurement noise

mission-4.1.18
Paul Riseborough 11 years ago committed by Andrew Tridgell
parent
commit
62eff63267
  1. 6
      libraries/AP_NavEKF/AP_NavEKF.cpp

6
libraries/AP_NavEKF/AP_NavEKF.cpp

@ -60,8 +60,8 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = { @@ -60,8 +60,8 @@ const AP_Param::GroupInfo NavEKF::var_info[] PROGMEM = {
// @Param: MAG_NOISE
// @DisplayName: Magntometer measurement noise (Gauss)
// @Description: This is the RMS value of noise in magnetometer measurements. Increasing it reduces the weighting on these measurements.
// @Range: 0.05 - 0.5
// @Increment: 0.05
// @Range: 0.01 - 0.5
// @Increment: 0.01
// @User: advanced
AP_GROUPINFO("MAG_NOISE", 4, NavEKF, _magNoise, 0.05f),
@ -1762,7 +1762,7 @@ void NavEKF::FuseMagnetometer() @@ -1762,7 +1762,7 @@ void NavEKF::FuseMagnetometer()
MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE + DCM[2][2]*magD + magZbias;
// scale magnetometer observation error with total angular rate
R_MAG = sq(constrain_float(_magNoise, 0.05f, 0.5f)) + sq(_magVarRateScale*dAngIMU.length() / dtIMU);
R_MAG = sq(constrain_float(_magNoise, 0.01f, 0.5f)) + sq(_magVarRateScale*dAngIMU.length() / dtIMU);
// Calculate observation jacobians
SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1;

Loading…
Cancel
Save