diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 9a093d250c..fb5e574a14 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -59,8 +59,10 @@ void NavEKF3_core::setWindMagStateLearningMode() bool setWindInhibit = (!useAirspeed() && !assume_zero_sideslip()) || onGround || (PV_AidingMode == AID_NONE); if (!inhibitWindStates && setWindInhibit) { inhibitWindStates = true; + updateStateIndexLim(); } else if (inhibitWindStates && !setWindInhibit) { inhibitWindStates = false; + updateStateIndexLim(); // set states and variances if (yawAlignComplete && useAirspeed()) { // if we have airspeed and a valid heading, set the wind states to the reciprocal of the vehicle heading @@ -106,8 +108,10 @@ void NavEKF3_core::setWindMagStateLearningMode() bool setMagInhibit = !magCalRequested || magCalDenied; if (!inhibitMagStates && setMagInhibit) { inhibitMagStates = true; + updateStateIndexLim(); } else if (inhibitMagStates && !setMagInhibit) { inhibitMagStates = false; + updateStateIndexLim(); if (magFieldLearned) { // if we have already learned the field states, then retain the learned variances P[16][16] = earthMagFieldVar.x; @@ -137,6 +141,8 @@ void NavEKF3_core::setWindMagStateLearningMode() if (tiltAlignComplete && inhibitDelVelBiasStates) { // activate the states inhibitDelVelBiasStates = false; + updateStateIndexLim(); + // set the initial covariance values P[13][13] = sq(ACCEL_BIAS_LIM_SCALER * frontend->_accBiasLim * dtEkfAvg); P[14][14] = P[13][13]; @@ -146,6 +152,8 @@ void NavEKF3_core::setWindMagStateLearningMode() if (tiltAlignComplete && inhibitDelAngBiasStates) { // activate the states inhibitDelAngBiasStates = false; + updateStateIndexLim(); + // set the initial covariance values P[10][10] = sq(radians(InitialGyroBiasUncertainty() * dtEkfAvg)); P[11][11] = P[10][10]; @@ -159,8 +167,13 @@ void NavEKF3_core::setWindMagStateLearningMode() finalInflightMagInit = false; } - // Adjust the indexing limits used to address the covariance, states and other EKF arrays to avoid unnecessary operations - // if we are not using those states + updateStateIndexLim(); +} + +// Adjust the indexing limits used to address the covariance, states and other EKF arrays to avoid unnecessary operations +// if we are not using those states +void NavEKF3_core::updateStateIndexLim() +{ if (inhibitWindStates) { if (inhibitMagStates) { if (inhibitDelVelBiasStates) { diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_core.h b/libraries/AP_NavEKF3/AP_NavEKF3_core.h index ba88a3788d..f172bc1312 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_core.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3_core.h @@ -766,6 +766,9 @@ private: // update timing statistics structure void updateTimingStatistics(void); + + // Update the state index limit based on which states are active + void updateStateIndexLim(void); // Variables bool statesInitialised; // boolean true when filter states have been initialised @@ -853,8 +856,8 @@ private: float tasTestRatio; // sum of squares of true airspeed innovation divided by fail threshold bool inhibitWindStates; // true when wind states and covariances are to remain constant bool inhibitMagStates; // true when magnetic field states are inactive - bool inhibitDelVelBiasStates; // true when delta velocity bias states are inactive - bool inhibitDelAngBiasStates; + bool inhibitDelVelBiasStates; // true when IMU delta velocity bias states are inactive + bool inhibitDelAngBiasStates; // true when IMU delta angle bias states are inactive bool gpsNotAvailable; // bool true when valid GPS data is not available struct Location EKF_origin; // LLH origin of the NED axis system bool validOrigin; // true when the EKF origin is valid