aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_NavEKF3: do not store prediction-enabled as state

Browse Source 
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
This commit is contained in:
Peter Barker 2024-07-04 13:42:49 +10:00 committed by Andrew Tridgell
parent a110c9c39f
commit c8a20726ff
3 changed files with 4 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -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();

7
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -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();
@ -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;
@ -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) {

3
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -766,7 +766,7 @@ private:
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();
@ -1193,7 +1193,6 @@ private:
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)