Browse Source

AP_NavEKF3: Add missing axis specific dvel bias update inhibit

c415-sdk
Paul Riseborough 4 years ago committed by Andrew Tridgell
parent
commit
c709ad9c80
  1. 6
      libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

6
libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

@ -884,7 +884,11 @@ void NavEKF3_core::FuseVelPosNED() @@ -884,7 +884,11 @@ void NavEKF3_core::FuseVelPosNED()
// inhibit delta velocity bias state estimation by setting Kalman gains to zero
if (!inhibitDelVelBiasStates) {
for (uint8_t i = 13; i<=15; i++) {
Kfusion[i] = P[i][stateIndex]*SK;
if (!dvelBiasAxisInhibit[i-13]) {
Kfusion[i] = P[i][stateIndex]*SK;
} else {
Kfusion[i] = 0.0f;
}
}
} else {
// zero indexes 13 to 15 = 3*4 bytes

Loading…
Cancel
Save