From b6c4a4ec484c6bc83f316b9d9ab75673daac1439 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 4 Jul 2024 13:42:49 +1000 Subject: [PATCH] AP_NavEKF3: do not store prediction-enabled as state this is only used in one place, and that place is called from the same routine setting the persistent state. The only other place which calls readIMUData shouldn't be running the prediction step, but mmay, depending on the previous setting of the prediction step. We are not initialising this state on filter reset, so it's possible that the state will be set when we do an InitialiseFilterBootstrap, which is probably not desired --- libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp | 2 +- libraries/AP_NavEKF3/AP_NavEKF3_core.cpp | 7 ++----- libraries/AP_NavEKF3/AP_NavEKF3_core.h | 3 +-- 3 files changed, 4 insertions(+), 8 deletions(-) diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp index 89e810cae6d96..f635c185af7cb 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp @@ -374,7 +374,7 @@ void NavEKF3_core::readMagData() * Downsampling is done using a method that does not introduce coning or sculling * errors. */ -void NavEKF3_core::readIMUData() +void NavEKF3_core::readIMUData(bool startPredictEnabled) { const auto &ins = dal.ins(); diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp index 67298272ffe85..1928c73c169b6 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.cpp @@ -475,7 +475,7 @@ bool NavEKF3_core::InitialiseFilterBootstrap(void) } // read all the sensors required to start the EKF the states - readIMUData(); + readIMUData(false); // don't allow prediction readMagData(); readGpsData(); readGpsYawData(); @@ -617,9 +617,6 @@ void NavEKF3_core::CovarianceInit() // Update Filter States - this should be called whenever new IMU data is available void NavEKF3_core::UpdateFilter(bool predict) { - // Set the flag to indicate to the filter that the front-end has given permission for a new state prediction cycle to be started - startPredictEnabled = predict; - // don't run filter updates if states have not been initialised if (!statesInitialised) { return; @@ -636,7 +633,7 @@ void NavEKF3_core::UpdateFilter(bool predict) controlFilterModes(); // read IMU data as delta angles and velocities - readIMUData(); + readIMUData(predict); // Run the EKF equations to estimate at the fusion time horizon if new IMU data is available in the buffer if (runUpdates) { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index 322ebc6907ab2..94749ded525c3 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -766,7 +766,7 @@ class NavEKF3_core : public NavEKF_core_common void correctDeltaVelocity(Vector3F &delVel, ftype delVelDT, uint8_t accel_index); // update IMU delta angle and delta velocity measurements - void readIMUData(); + void readIMUData(bool startPredictEnabled); // update estimate of inactive bias states void learnInactiveBiases(); @@ -1191,7 +1191,6 @@ class NavEKF3_core : public NavEKF_core_common uint8_t magSelectIndex; // Index of the magnetometer that is being used by the EKF bool runUpdates; // boolean true when the EKF updates can be run uint32_t framesSincePredict; // number of frames lapsed since EKF instance did a state prediction - bool startPredictEnabled; // boolean true when the frontend has given permission to start a new state prediciton cycle uint8_t localFilterTimeStep_ms; // average number of msec between filter updates ftype posDownObsNoise; // observation noise variance on the vertical position used by the state and covariance update step (m^2) Vector3F delAngCorrected; // corrected IMU delta angle vector at the EKF time horizon (rad)