aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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 // Calculate height measurement innovations using single IMU states
float hgtInnov1 = statesAtHgtTime.posD1 - observation[obsIndex]; float hgtInnov1 = statesAtHgtTime.posD1 - observation[obsIndex];
float hgtInnov2 = statesAtHgtTime.posD2 - 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 if (vehicleArmed) {
float correctionLimit = 0.005f * dtIMU * dtVelPos; // Correct single IMU prediction states using height measurement, limiting rate of change of bias to 0.005 m/s3
if (!vehicleArmed) { float correctionLimit = 0.005f * dtIMU * dtVelPos;
correctionLimit *= accelBiasNoiseScaler; 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++) { for (uint8_t i = 23; i<=26; i++) {
states[i] = states[i] - Kfusion[i] * hgtInnov1; // IMU1 velNED,posD states[i] = states[i] - Kfusion[i] * hgtInnov1; // IMU1 velNED,posD
} }