fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-02 05:04:08 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Prevent covariance initialisation overwriting yaw variance

Browse Source 
Also typo fix
This commit is contained in:
Paul Riseborough 2020-01-30 19:45:01 +11:00 committed by Lorenz Meier
parent a3347b7e06
commit 89bcebd631
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
EKF/ekf.cpp
View File

@ -193,6 +193,9 @@ bool Ekf::initialiseFilter()
// calculate the initial magnetic field and yaw alignment
_control_status.flags.yaw_align = resetMagHeading(_mag_lpf.getState(), false, false);
// initialise the state covariance matrix now we have starting values for all the states
initialiseCovariance();
// update the yaw angle variance using the variance of the measurement
if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
// using magnetic heading tuning parameter
@ -213,9 +216,6 @@ bool Ekf::initialiseFilter()
// reset the output predictor state history to match the EKF initial values
alignOutputFilter();
// initialise the state covariance matrix now we have starting values for all lthe states
initialiseCovariance();
return true;
}
}