Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: Don't use fake measurements to learn dvel bias

This commit is contained in:
Paul Riseborough 2021-03-14 08:02:13 +11:00 committed by Andrew Tridgell
parent c709ad9c80
commit 918606e71d
1 changed files with 5 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp
View File

@ -881,8 +881,11 @@ void NavEKF3_core::FuseVelPosNED()
memset(&Kfusion[10], 0, 12); memset(&Kfusion[10], 0, 12);
} }
// inhibit delta velocity bias state estimation by setting Kalman gains to zero // Inhibit delta velocity bias state estimation by setting Kalman gains to zero
if (!inhibitDelVelBiasStates) { // Don't use 'fake' horizontal measurements used to constrain attitude drift during
// periods of non-aiding to learn bias as these can give incorrect esitmates.
const bool horizInhibit = PV_AidingMode == AID_NONE && obsIndex != 2 && obsIndex != 5;
if (!horizInhibit && !inhibitDelVelBiasStates) {
for (uint8_t i = 13; i<=15; i++) { for (uint8_t i = 13; i<=15; i++) {
if (!dvelBiasAxisInhibit[i-13]) { if (!dvelBiasAxisInhibit[i-13]) {
Kfusion[i] = P[i][stateIndex]*SK; Kfusion[i] = P[i][stateIndex]*SK;