Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: fix wind speed covariance initialisation bug

This commit is contained in:
Paul Riseborough 2021-09-20 21:20:20 +10:00 committed by Andrew Tridgell
parent ea15834401
commit bc5efb6cd2
1 changed files with 3 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -88,11 +88,9 @@ void NavEKF3_core::setWindMagStateLearningMode()
}
// set the wind state variances to the measurement uncertainty
for (uint8_t index=22; index<=23; index++) {
zeroCols(P, 22, 23);
zeroRows(P, 22, 23);
P[index][index] = trueAirspeedVariance;
}
zeroCols(P, 22, 23);
zeroRows(P, 22, 23);
P[22][22] = P[23][23] = trueAirspeedVariance;
windStatesAligned = true;