Browse Source

AP_NavEKF3: add gyro bias state inhibit and rework index limit calculation

Inhibiting gyro bias estimation during the initial tilt alignment speeds alignment.
The calculation of the maxmum state index required has been modified so that it can handle all combinations of inhibited states.
Limiting the maximum state index accessed by all EKF operations result in significant processing reductions when higher index states are not being used.
master
priseborough 8 years ago committed by Francisco Ferreira
parent
commit
5058405f8c
  1. 14
      libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

14
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@ -161,12 +161,20 @@ void NavEKF3_core::setWindMagStateLearningMode() @@ -161,12 +161,20 @@ void NavEKF3_core::setWindMagStateLearningMode()
// 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
if (inhibitMagStates && inhibitWindStates && inhibitDelVelBiasStates) {
if (inhibitWindStates) {
if (inhibitMagStates) {
if (inhibitDelVelBiasStates) {
if (inhibitDelAngBiasStates) {
stateIndexLim = 9;
} else {
stateIndexLim = 12;
} else if (inhibitMagStates && !inhibitWindStates) {
}
} else {
stateIndexLim = 15;
} else if (inhibitWindStates) {
}
} else {
stateIndexLim = 21;
}
} else {
stateIndexLim = 23;
}

Loading…
Cancel
Save