aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF: Remove duplicate flow measurement state correction

Browse Source
This commit is contained in:
priseborough 2015-01-07 12:35:05 +11:00 committed by Andrew Tridgell
parent 92bb75a635
commit 300ba65f64
1 changed files with 0 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AP_NavEKF/AP_NavEKF.cpp
View File

@ -2961,11 +2961,6 @@ void NavEKF::FuseOptFlow()
} else if (!flowXfailed) {
prevFlowFuseTime_ms = imuSampleTime_ms;
}
// correct the state vector
for (uint8_t j = 0; j <= 21; j++)
{
states[j] = states[j] - Kfusion[j] * innovOptFlow[obsIndex];
}
// Attitude, velocity and position corrections are averaged across multiple prediction cycles between now and the anticipated time for the next measurement.
// Don't do averaging of quaternion state corrections if total angle change across predicted interval is going to exceed 0.1 rad
bool highRates = ((flowUpdateCountMax * correctedDelAng.length()) > 0.1f);