From 89bcebd631024a1a91babb7b68f197dc3b6e5ca8 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 30 Jan 2020 19:45:01 +1100 Subject: [PATCH] EKF: Prevent covariance initialisation overwriting yaw variance Also typo fix --- EKF/ekf.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 8fc20e77e1..9cea396cea 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -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; } }