zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_NavEKF2: Improve ability to tune magnetic field learning 

Allow different process noise to be set for body (sensor bias) and earth field states.
This allows a stable magnetometer bias estimate to be available at end of flight whilst still allowing for external magnetic anomalies during landing.
Adjust default values to give stable mag bias learning and fast learning of external anomalies.
master
Paul Riseborough 9 years ago committed by Andrew Tridgell
parent
6523481c76
commit
165335b9e3
3 changed files with 29 additions and 14 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. 36
      libraries/AP_NavEKF2/AP_NavEKF2.cpp
  2. 3
      libraries/AP_NavEKF2/AP_NavEKF2.h
  3. 4
      libraries/AP_NavEKF2/AP_NavEKF2_core.cpp

36
libraries/AP_NavEKF2/AP_NavEKF2.cpp
Unescape View File

@ -25,7 +25,8 @@ @@ -25,7 +25,8 @@
#define GBIAS_P_NSE_DEFAULT 1.0E-04f
#define GSCALE_P_NSE_DEFAULT 5.0E-04f
#define ABIAS_P_NSE_DEFAULT 1.0E-03f
#define MAG_P_NSE_DEFAULT 2.5E-02f
#define MAGB_P_NSE_DEFAULT 5.0E-04f
#define MAGE_P_NSE_DEFAULT 5.0E-03f
#define VEL_I_GATE_DEFAULT 500
#define POS_I_GATE_DEFAULT 500
#define HGT_I_GATE_DEFAULT 500
@ -49,7 +50,8 @@ @@ -49,7 +50,8 @@
#define GBIAS_P_NSE_DEFAULT 1.0E-04f
#define GSCALE_P_NSE_DEFAULT 5.0E-04f
#define ABIAS_P_NSE_DEFAULT 1.0E-03f
#define MAG_P_NSE_DEFAULT 2.5E-02f
#define MAGB_P_NSE_DEFAULT 5.0E-04f
#define MAGE_P_NSE_DEFAULT 5.0E-03f
#define VEL_I_GATE_DEFAULT 500
#define POS_I_GATE_DEFAULT 500
#define HGT_I_GATE_DEFAULT 500
@ -73,7 +75,8 @@ @@ -73,7 +75,8 @@
#define GBIAS_P_NSE_DEFAULT 1.0E-04f
#define GSCALE_P_NSE_DEFAULT 5.0E-04f
#define ABIAS_P_NSE_DEFAULT 1.0E-03f
#define MAG_P_NSE_DEFAULT 2.5E-02f
#define MAGB_P_NSE_DEFAULT 5.0E-04f
#define MAGE_P_NSE_DEFAULT 5.0E-03f
#define VEL_I_GATE_DEFAULT 500
#define POS_I_GATE_DEFAULT 500
#define HGT_I_GATE_DEFAULT 500
@ -97,7 +100,8 @@ @@ -97,7 +100,8 @@
#define GBIAS_P_NSE_DEFAULT 1.0E-04f
#define GSCALE_P_NSE_DEFAULT 5.0E-04f
#define ABIAS_P_NSE_DEFAULT 1.0E-03f
#define MAG_P_NSE_DEFAULT 2.5E-02f
#define MAGB_P_NSE_DEFAULT 5.0E-04f
#define MAGE_P_NSE_DEFAULT 5.0E-03f
#define VEL_I_GATE_DEFAULT 500
#define POS_I_GATE_DEFAULT 500
#define HGT_I_GATE_DEFAULT 500
@ -373,13 +377,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = { @@ -373,13 +377,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
// @Units: m/s/s/s
AP_GROUPINFO("ABIAS_P_NSE", 28, NavEKF2, _accelBiasProcessNoise, ABIAS_P_NSE_DEFAULT),
// @Param: MAG_P_NSE
// @DisplayName: Magnetic field process noise (gauss/s)
// @Description: This state process noise controls the growth of magnetic field state error estimates. Increasing it makes magnetic field bias estimation faster and noisier.
// @Range: 0.0001 0.01
// @User: Advanced
// @Units: gauss/s
AP_GROUPINFO("MAG_P_NSE", 29, NavEKF2, _magProcessNoise, MAG_P_NSE_DEFAULT),
// 29 previously used for EK2_MAG_P_NSE parameter that has been replaced with EK2_MAGE_P_NSE and EK2_MAGB_P_NSE
// @Param: WIND_P_NSE
// @DisplayName: Wind velocity process noise (m/s^2)
@ -462,6 +460,22 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = { @@ -462,6 +460,22 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
// @User: Advanced
AP_GROUPINFO("TAU_OUTPUT", 39, NavEKF2, _tauVelPosOutput, -1),
// @Param: MAGE_P_NSE
// @DisplayName: Earth magnetic field process noise (gauss/s)
// @Description: This state process noise controls the growth of earth magnetic field state error estimates. Increasing it makes earth magnetic field estimation faster and noisier.
// @Range: 0.00001 0.01
// @User: Advanced
// @Units: gauss/s
AP_GROUPINFO("MAGE_P_NSE", 40, NavEKF2, _magEarthProcessNoise, MAGE_P_NSE_DEFAULT),
// @Param: MAGB_P_NSE
// @DisplayName: Body magnetic field process noise (gauss/s)
// @Description: This state process noise controls the growth of body magnetic field state error estimates. Increasing it makes magnetometer bias error estimation faster and noisier.
// @Range: 0.00001 0.01
// @User: Advanced
// @Units: gauss/s
AP_GROUPINFO("MAGB_P_NSE", 41, NavEKF2, _magBodyProcessNoise, MAGB_P_NSE_DEFAULT),
AP_GROUPEND
};

3
libraries/AP_NavEKF2/AP_NavEKF2.h
Unescape View File

@ -289,7 +289,8 @@ private: @@ -289,7 +289,8 @@ private:
AP_Float _easNoise; // equivalent airspeed measurement noise : m/s
AP_Float _windVelProcessNoise; // wind velocity state process noise : m/s^2
AP_Float _wndVarHgtRateScale; // scale factor applied to wind process noise due to height rate
AP_Float _magProcessNoise; // magnetic field process noise : gauss/sec
AP_Float _magEarthProcessNoise; // Earth magnetic field process noise : gauss/sec
AP_Float _magBodyProcessNoise; // Body magnetic field process noise : gauss/sec
AP_Float _gyrNoise; // gyro process noise : rad/s
AP_Float _accNoise; // accelerometer process noise : m/s^2
AP_Float _gyroBiasProcessNoise; // gyro bias state process noise : rad/s

4
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
Unescape View File

@ -741,8 +741,8 @@ void NavEKF2_core::CovariancePrediction() @@ -741,8 +741,8 @@ void NavEKF2_core::CovariancePrediction()
dAngBiasSigma = sq(dt) * constrain_float(frontend->_gyroBiasProcessNoise, 0.0f, 1.0f);
dVelBiasSigma = sq(dt) * constrain_float(frontend->_accelBiasProcessNoise, 0.0f, 1.0f);
dAngScaleSigma = dt * constrain_float(frontend->_gyroScaleProcessNoise, 0.0f, 1.0f);
magEarthSigma = dt * constrain_float(frontend->_magProcessNoise, 0.0f, 1.0f);
magBodySigma = dt * constrain_float(frontend->_magProcessNoise, 0.0f, 1.0f);
magEarthSigma = dt * constrain_float(frontend->_magEarthProcessNoise, 0.0f, 1.0f);
magBodySigma = dt * constrain_float(frontend->_magBodyProcessNoise, 0.0f, 1.0f);
for (uint8_t i= 0; i<=8; i++) processNoise[i] = 0.0f;
for (uint8_t i=9; i<=11; i++) processNoise[i] = dAngBiasSigma;
for (uint8_t i=12; i<=14; i++) processNoise[i] = dAngScaleSigma;

Write Preview
Loading…
Cancel
Save