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

AP_NavEKF: Speed up flow sensor gyro bias correction

Browse Source
This commit is contained in:
priseborough 2014-11-10 18:40:33 +11:00 committed by Andrew Tridgell
parent d7ad45ebda
commit 6f8971d80a
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -2779,7 +2779,7 @@ void NavEKF::RunAuxiliaryEKF()
// calculate the innovation consistency test ratio // calculate the innovation consistency test ratio
auxFlowTestRatio[obsIndex] = sq(auxFlowObsInnov[obsIndex]) / (sq(_flowInnovGate) * auxFlowObsInnovVar[obsIndex]); auxFlowTestRatio[obsIndex] = sq(auxFlowObsInnov[obsIndex]) / (sq(_flowInnovGate) * auxFlowObsInnovVar[obsIndex]);
if (auxFlowTestRatio[obsIndex] < 1.0f) { if ((auxFlowTestRatio[obsIndex] < 1.0f) && (flowRadXY[obsIndex] < 2.0f)) {
// correct the state // correct the state
for (uint8_t i = 0; i < 2 ; i++) { for (uint8_t i = 0; i < 2 ; i++) {
flowStates[i] -= K_OPT[i][obsIndex] * auxFlowObsInnov[obsIndex]; flowStates[i] -= K_OPT[i][obsIndex] * auxFlowObsInnov[obsIndex];
@ -4027,8 +4027,8 @@ void NavEKF::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, V
// recall angular rates averaged across flow observation period allowing for processing, transmission and intersample delays // recall angular rates averaged across flow observation period allowing for processing, transmission and intersample delays
RecallOmega(omegaAcrossFlowTime, imuSampleTime_ms - flowTimeDeltaAvg_ms - _msecFLowDelay, flowMeaTime_ms - _msecFLowDelay); RecallOmega(omegaAcrossFlowTime, imuSampleTime_ms - flowTimeDeltaAvg_ms - _msecFLowDelay, flowMeaTime_ms - _msecFLowDelay);
// calculate bias errors on flow sensor gyro rates // calculate bias errors on flow sensor gyro rates
flowGyroBias.x = 0.999f * flowGyroBias.x + 0.001f * (rawGyroRates.x - omegaAcrossFlowTime.x); flowGyroBias.x = 0.99f * flowGyroBias.x + 0.01f * (rawGyroRates.x - omegaAcrossFlowTime.x);
flowGyroBias.y = 0.999f * flowGyroBias.y + 0.001f * (rawGyroRates.y - omegaAcrossFlowTime.y); flowGyroBias.y = 0.99f * flowGyroBias.y + 0.01f * (rawGyroRates.y - omegaAcrossFlowTime.y);
// correct flow sensor rates for bias // correct flow sensor rates for bias
omegaAcrossFlowTime.x = rawGyroRates.x - flowGyroBias.x; omegaAcrossFlowTime.x = rawGyroRates.x - flowGyroBias.x;
omegaAcrossFlowTime.y = rawGyroRates.y - flowGyroBias.y; omegaAcrossFlowTime.y = rawGyroRates.y - flowGyroBias.y;