Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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.
This commit is contained in:
priseborough 2017-05-10 08:07:21 +10:00 committed by Francisco Ferreira
parent 0cba133a1e
commit 5058405f8c
1 changed files with 14 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
View File

@ -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) {
stateIndexLim = 12;
} else if (inhibitMagStates && !inhibitWindStates) {
stateIndexLim = 15;
} else if (inhibitWindStates) {
stateIndexLim = 21;
if (inhibitWindStates) {
if (inhibitMagStates) {
if (inhibitDelVelBiasStates) {
if (inhibitDelAngBiasStates) {
stateIndexLim = 9;
} else {
stateIndexLim = 12;
}
} else {
stateIndexLim = 15;
}
} else {
stateIndexLim = 21;
}
} else {
stateIndexLim = 23;
}