Mirrors_Framework/ardupilot
1
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot.git synced 2026-10-02 10:23:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: use FinishFusion in FuseOptFlow

Browse Source
This commit is contained in:
Thomas Watson
2026-05-05 10:06:37 +10:00
committed by Andrew Tridgell
parent 5495683d10
commit 5964a0d113
1 changed files with 3 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp
+3 -27
View File
@@ -692,33 +692,9 @@ void NavEKF3_core::FuseOptFlow(const of_elements &ofDataDelayed, bool really_fus
}
}
// Check that we are not going to drive any variances negative and skip the update if so
bool healthyFusion = true;
for (uint8_t i= 0; i<=stateIndexLim; i++) {
if (KHP[i][i] > P[i][i]) {
healthyFusion = false;
}
}
if (healthyFusion) {
// update the covariance matrix
for (uint8_t i= 0; i<=stateIndexLim; i++) {
for (uint8_t j= 0; j<=stateIndexLim; j++) {
P[i][j] = P[i][j] - KHP[i][j];
}
}
// correct the state vector
for (uint8_t j= 0; j<=stateIndexLim; j++) {
statesArray[j] = statesArray[j] - Kfusion[j] * flowInnov[obsIndex];
}
stateStruct.quat.normalize();
// force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning.
ForceSymmetry();
ConstrainVariances();
} else {
// record bad axis
// finish fusion from KHP and Kfusion
if (FinishFusion(flowInnov[obsIndex])) {
// fault, record bad axis
if (obsIndex == 0) {
faultStatus.bad_xflow = true;
} else if (obsIndex == 1) {