aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: Reduce yaw drift when operating without external aiding

Browse Source 
Also fixes bug in the determination of the total angular variance threshold for when to fuse a zero innovation yaw measurement.
This commit is contained in:
Paul Riseborough 2021-03-14 12:12:11 +11:00 committed by Andrew Tridgell
parent 53c4b163ce
commit 44be7161c0
1 changed files with 3 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp
View File

@ -244,10 +244,9 @@ void NavEKF3_core::SelectMagFusion()
if (onGroundNotMoving) {
// fuse last known good yaw angle before we stopped moving to allow yaw bias learning when on ground before flight
fuseEulerYaw(yawFusionMethod::STATIC);
} else if (onGround || (sq(P[0][0])+sq(P[1][1])+sq(P[2][2])+sq(P[3][3]) > 0.01f)) {
// prevent uncontrolled yaw variance growth by fusing a zero innovation
// when not on ground allow more variance growth so yaw can be corrected
// by manoeuvring
} else if (onGround || PV_AidingMode == AID_NONE || (P[0][0]+P[1][1]+P[2][2]+P[3][3] > 0.01f)) {
// prevent uncontrolled yaw variance growth that can destabilise the covariance matrix
// by fusing a zero innovation
fuseEulerYaw(yawFusionMethod::PREDICTED);
}
}