From d25883f7129eb5de6d84fb8980e0273bf64daf00 Mon Sep 17 00:00:00 2001 From: priseborough Date: Wed, 2 Apr 2014 17:52:13 +1100 Subject: [PATCH] AP_NavEKF : Limit rate of Z accel bias adaptation Aliasing can causes the bias estimate to fluctuate very rapidly as it tries to keep up, which degrades the benefit of switching between accelerometers to avoid aliasing. This patch give a much more stable bias estimate during aliasing, and allows the bias to adapt at a maximum rate of 1.0 m/s2 in 50 seconds --- libraries/AP_NavEKF/AP_NavEKF.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 55fd81f72a..82ee594a14 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -1825,9 +1825,10 @@ void NavEKF::FuseVelPosNED() // Calculate height measurement innovations using single IMU states float hgtInnov1 = statesAtHgtTime[26] - observation[obsIndex]; float hgtInnov2 = statesAtHgtTime[30] - observation[obsIndex]; - // Correct single IMU prediction states using height measurement - states[13] = states[13] - Kfusion[13] * hgtInnov1; // IMU1 Z accel bias - states[22] = states[22] - Kfusion[22] * hgtInnov2; // IMU2 Z accel bias + // Correct single IMU prediction states using height measurement, limiting rate of change of bias to 0.02 m/s3 + float correctionLimit = 0.02f * dtIMU *dtVelPos; + states[13] = states[13] - constrain_float(Kfusion[13] * hgtInnov1, -correctionLimit, correctionLimit); // IMU1 Z accel bias + states[22] = states[22] - constrain_float(Kfusion[22] * hgtInnov2, -correctionLimit, correctionLimit); // IMU2 Z accel bias for (uint8_t i = 23; i<=26; i++) { states[i] = states[i] - Kfusion[i] * hgtInnov1; // IMU1 velNED,posD