Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-21 16:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF: remove accel bias rate limit when disarmed

Browse Source
This commit is contained in:
Paul Riseborough 2015-03-21 14:51:13 -07:00 committed by Andrew Tridgell
parent fe76cb4c0b
commit 98c32012fa
1 changed files with 11 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -2163,14 +2163,18 @@ void NavEKF::FuseVelPosNED()
// Calculate height measurement innovations using single IMU states
float hgtInnov1 = statesAtHgtTime.posD1 - observation[obsIndex];
float hgtInnov2 = statesAtHgtTime.posD2 - observation[obsIndex];
// Correct single IMU prediction states using height measurement, limiting rate of change of bias to 0.005 m/s3
// Increase allowed rate of change when disarmed as estimates will be less noisy
float correctionLimit = 0.005f * dtIMU * dtVelPos;
if (!vehicleArmed) {
correctionLimit *= accelBiasNoiseScaler;
if (vehicleArmed) {
// Correct single IMU prediction states using height measurement, limiting rate of change of bias to 0.005 m/s3
float correctionLimit = 0.005f * dtIMU * dtVelPos;
state.accel_zbias1 -= constrain_float(Kfusion[13] * hgtInnov1, -correctionLimit, correctionLimit); // IMU1 Z accel bias
state.accel_zbias2 -= constrain_float(Kfusion[22] * hgtInnov2, -correctionLimit, correctionLimit); // IMU2 Z accel bias
} else {
// When disarmed, do not rate limit accel bias learning
state.accel_zbias1 -= Kfusion[13] * hgtInnov1; // IMU1 Z accel bias
state.accel_zbias2 -= Kfusion[22] * hgtInnov2; // IMU2 Z accel bias
}
state.accel_zbias1 -= constrain_float(Kfusion[13] * hgtInnov1, -correctionLimit, correctionLimit); // IMU1 Z accel bias
state.accel_zbias2 -= 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
}