From 5058405f8c02985bc87a995640093ed0437cd359 Mon Sep 17 00:00:00 2001
From: priseborough <p_riseborough@live.com.au>
Date: Wed, 10 May 2017 08:07:21 +1000
Subject: [PATCH] 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.
---
 libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp | 20 ++++++++++++++------
 1 file changed, 14 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
index 662675caa1..9a093d250c 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp
@@ -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;
     }