Skip to content

Commit

Permalink
AP_NavEKF3: do not store prediction-enabled as state
Browse files Browse the repository at this point in the history
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
  • Loading branch information
peterbarker authored and tridge committed Jul 9, 2024
1 parent a110c9c commit c8a2072
Show file tree
Hide file tree
Showing 3 changed files with 4 additions and 8 deletions.
2 changes: 1 addition & 1 deletion libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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();

Expand Down
7 changes: 2 additions & 5 deletions libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -477,7 +477,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();
Expand Down Expand Up @@ -619,9 +619,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;
Expand All @@ -638,7 +635,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) {
Expand Down
3 changes: 1 addition & 2 deletions libraries/AP_NavEKF3/AP_NavEKF3_core.h
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand Down Expand Up @@ -1193,7 +1193,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)
Expand Down

0 comments on commit c8a2072

Please sign in to comment.