fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 07:13:33 +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-02-05 15:20:34 +01:00
committed by Lorenz Meier
parent a3347b7e06
commit 89bcebd631
1 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.cpp
+3 -3
View File
@@ -193,6 +193,9 @@ bool Ekf::initialiseFilter()
// calculate the initial magnetic field and yaw alignment // calculate the initial magnetic field and yaw alignment
_control_status.flags.yaw_align = resetMagHeading(_mag_lpf.getState(), false, false); _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 // update the yaw angle variance using the variance of the measurement
if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) { if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
// using magnetic heading tuning parameter // using magnetic heading tuning parameter
@@ -213,9 +216,6 @@ bool Ekf::initialiseFilter()
// reset the output predictor state history to match the EKF initial values // reset the output predictor state history to match the EKF initial values
alignOutputFilter(); alignOutputFilter();
// initialise the state covariance matrix now we have starting values for all lthe states
initialiseCovariance();
return true; return true;
} }
} }