Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: Add tuning parameters for magentic yaw fusion

Browse Source 
Enable simple magnetic yaw fusion observation noise and innovation consistency check gate width to be tuned independantly.
This commit is contained in:
Paul Riseborough 2016-06-13 20:51:45 +10:00 committed by Andrew Tridgell
parent dc6836988c
commit 1400dc9d02
3 changed files with 24 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -434,6 +434,26 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
// @Values: 0:Disabled,1:FirstIMU,3:FirstAndSecondIMU,7:AllIMUs
// @User: Advanced
AP_GROUPINFO("LOG_MASK", 36, NavEKF2, _logging_mask, 1),
// control of magentic yaw angle fusion
// @Param: YAW_M_NSE
// @DisplayName: Yaw measurement noise (rad)
// @Description: This is the RMS value of noise in yaw measurements from the magnetometer. Increasing it reduces the weighting on these measurements.
// @Range: 0.05 1.0
// @Increment: 0.05
// @User: Advanced
// @Units: gauss
AP_GROUPINFO("YAW_M_NSE", 37, NavEKF2, _yawNoise, 0.5f),
// @Param: YAW_I_GATE
// @DisplayName: Yaw measurement gate size
// @Description: This sets the percentage number of standard deviations applied to the magnetometer yaw measurement innovation consistency check. Decreasing it makes it more likely that good measurements will be rejected. Increasing it makes it more likely that bad measurements will be accepted.
// @Range: 100 1000
// @Increment: 25
// @User: Advanced
AP_GROUPINFO("YAW_I_GATE", 38, NavEKF2, _yawInnovGate, 300),
AP_GROUPEND
};

2
libraries/AP_NavEKF2/AP_NavEKF2.h
View File

@ -317,6 +317,8 @@ private:
AP_Int16 _gpsCheckScaler; // Percentage increase to be applied to GPS pre-flight accuracy and drift thresholds
AP_Float _noaidHorizNoise; // horizontal position measurement noise assumed when synthesised zero position measurements are used to constrain attitude drift : m
AP_Int8 _logging_mask; // mask of IMUs to log
AP_Float _yawNoise; // magnetic yaw measurement noise : rad
AP_Int16 _yawInnovGate; // Percentage number of standard deviations applied to magnetic yaw innovation consistency check
// Tuning parameters
const float gpsNEVelVarAccScale; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration

4
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -717,7 +717,7 @@ void NavEKF2_core::fuseEulerYaw()
float q3 = stateStruct.quat[3];
// compass measurement error variance (rad^2)
const float R_YAW = 0.25f;
const float R_YAW = sq(frontend->_yawNoise);
// calculate observation jacobian, predicted yaw and zero yaw body to earth rotation matrix
// determine if a 321 or 312 Euler sequence is best
@ -853,7 +853,7 @@ void NavEKF2_core::fuseEulerYaw()
}
// calculate the innovation test ratio
yawTestRatio = sq(innovation) / (sq(MAX(0.01f * (float)frontend->_magInnovGate, 1.0f)) * varInnov);
yawTestRatio = sq(innovation) / (sq(MAX(0.01f * (float)frontend->_yawInnovGate, 1.0f)) * varInnov);
// Declare the magnetometer unhealthy if the innovation test fails
if (yawTestRatio > 1.0f) {