From c4c0f819b51bab7089e511930541f6cb5d92924a Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 20 Apr 2015 21:34:25 +1000 Subject: [PATCH] AP_NavEKF: Ensure Covariance initialisation uses correct IMU time step --- libraries/AP_NavEKF/AP_NavEKF.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index aaf4556cfc..b627fd2707 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -575,15 +575,15 @@ bool NavEKF::InitialiseFilterDynamic(void) // set to true now that states have be initialised statesInitialised = true; - // initialise the covariance matrix - CovarianceInit(); - // define Earth rotation vector in the NED navigation frame calcEarthRateNED(earthRateNED, _ahrs->get_home().lat); // initialise IMU pre-processing states readIMUData(); + // initialise the covariance matrix + CovarianceInit(); + return true; } @@ -662,15 +662,15 @@ bool NavEKF::InitialiseFilterBootstrap(void) // set to true now we have intialised the states statesInitialised = true; - // initialise the covariance matrix - CovarianceInit(); - // define Earth rotation vector in the NED navigation frame calcEarthRateNED(earthRateNED, _ahrs->get_home().lat); // initialise IMU pre-processing states readIMUData(); + // initialise the covariance matrix + CovarianceInit(); + return true; }