AP_NavEKF3: add fusion method for body frame odometry data

libraries/AP_NavEKF3/AP_NavEKF3.cpp

@@ -1108,6 +1108,40 @@ void NavEKF3::getFlowDebug(int8_t instance, float &varFlow, float &gndOffset, fl
     }
 }
 
+/*
+ * Write body frame linear and angular displacement measurements from a visual odometry sensor
+ *
+ * quality is a normalised confidence value from 0 to 100
+ * delPos is the XYZ change in linear position measured in body frame and relative to the inertial reference at timeStamp_ms (m)
+ * delAng is the XYZ angular rotation measured in body frame and relative to the inertial reference at timeStamp_ms (rad)
+ * delTime is the time interval for the measurement of delPos and delAng (sec)
+ * timeStamp_ms is the timestamp of the last image used to calculate delPos and delAng (msec)
+ * posOffset is the XYZ body frame position of the camera focal point (m)
+*/
+void NavEKF3::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset)
+{
+    if (core) {
+        for (uint8_t i=0; i<num_cores; i++) {
+            core[i].writeBodyFrameOdom(quality, delPos, delAng, delTime, timeStamp_ms, posOffset);
+        }
+    }
+}
+
+// return data for debugging body frame odometry fusion
+uint32_t NavEKF3::getBodyFrameOdomDebug(int8_t instance, Vector3f &velInnov, Vector3f &velInnovVar)
+{
+    uint32_t ret = 0;
+    if (instance < 0 || instance >= num_cores) {
+        instance = primary;
+    }
+
+    if (core) {
+        ret = core[instance].getBodyFrameOdomDebug(velInnov, velInnovVar);
+    }
+
+    return ret;
+}
+
 // return data for debugging range beacon fusion
 bool NavEKF3::getRangeBeaconDebug(int8_t instance, uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED,
                                   float &offsetHigh, float &offsetLow, Vector3f &posNED)

libraries/AP_NavEKF3/AP_NavEKF3.h

@@ -196,6 +196,28 @@ public:
     // posOffset is the XYZ flow sensor position in the body frame in m
     void writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, const Vector3f &posOffset);
 
+    /*
+     * Write body frame linear and angular displacement measurements from a visual odometry sensor
+     *
+     * quality is a normalised confidence value from 0 to 100
+     * delPos is the XYZ change in linear position measured in body frame and relative to the inertial reference at timeStamp_ms (m)
+     * delAng is the XYZ angular rotation measured in body frame and relative to the inertial reference at timeStamp_ms (rad)
+     * delTime is the time interval for the measurement of delPos and delAng (sec)
+     * timeStamp_ms is the timestamp of the last image used to calculate delPos and delAng (msec)
+     * posOffset is the XYZ body frame position of the camera focal point (m)
+    */
+    void writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset);
+
+    /*
+     * Return data for debugging body frame odometry fusion:
+     *
+     * velInnov are the XYZ body frame velocity innovations (m/s)
+     * velInnovVar are the XYZ body frame velocity innovation variances (m/s)**2
+     *
+     * Return the system time stamp of the last update (msec)
+     */
+    uint32_t getBodyFrameOdomDebug(int8_t instance, Vector3f &velInnov, Vector3f &velInnovVar);
+
     // return data for debugging optical flow fusion for the specified instance
     // An out of range instance (eg -1) returns data for the the primary instance
     void getFlowDebug(int8_t instance, float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr);

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@@ -191,7 +191,7 @@ void NavEKF3_core::setAidingMode()
         // GPS aiding is the preferred option unless excluded by the user
         if(readyToUseGPS() || readyToUseRangeBeacon()) {
             PV_AidingMode = AID_ABSOLUTE;
-        } else if (readyToUseOptFlow()) {
+        } else if (readyToUseOptFlow() || readyToUseBodyOdm()) {
             PV_AidingMode = AID_RELATIVE;
         }
     } else if (PV_AidingMode == AID_RELATIVE) {
@@ -199,11 +199,15 @@ void NavEKF3_core::setAidingMode()
          bool flowSensorTimeout = ((imuSampleTime_ms - flowValidMeaTime_ms) > 5000);
          // Check if the fusion has timed out (flow measurements have been rejected for too long)
          bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000);
+         // Check if the body odometry flow sensor has timed out
+         bool bodyOdmSensorTimeout = ((imuSampleTime_ms - bodyOdmMeasTime_ms) > 5000);
+         // Check if the fusion has timed out (body odometry measurements have been rejected for too long)
+         bool bodyOdmFusionTimeout = ((imuSampleTime_ms - prevBodyVelFuseTime_ms) > 5000);
          // Enable switch to absolute position mode if GPS or range beacon data is available
          // If GPS or range beacons data is not available and flow fusion has timed out, then fall-back to no-aiding
          if(readyToUseGPS() || readyToUseRangeBeacon()) {
              PV_AidingMode = AID_ABSOLUTE;
-         } else if (flowSensorTimeout || flowFusionTimeout) {
+         } else if ((flowSensorTimeout || flowFusionTimeout) && (bodyOdmSensorTimeout || bodyOdmFusionTimeout)) {
              PV_AidingMode = AID_NONE;
          }
      } else if (PV_AidingMode == AID_ABSOLUTE) {
@@ -213,6 +217,9 @@ void NavEKF3_core::setAidingMode()
          // Check if optical flow data is being used
          bool optFlowUsed = (imuSampleTime_ms - prevFlowFuseTime_ms <= minTestTime_ms);
 
+         // Check if body odometry data is being used
+         bool bodyOdmUsed = (imuSampleTime_ms - prevBodyVelFuseTime_ms <= minTestTime_ms);
+
          // Check if airspeed data is being used
          bool airSpdUsed = (imuSampleTime_ms - lastTasPassTime_ms <= minTestTime_ms);
 
@@ -224,10 +231,10 @@ void NavEKF3_core::setAidingMode()
          bool gpsVelUsed = (imuSampleTime_ms - lastVelPassTime_ms <= minTestTime_ms);
 
          // Check if attitude drift has been constrained by a measurement source
-         bool attAiding = gpsPosUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed;
+         bool attAiding = gpsPosUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed || bodyOdmUsed;
 
          // check if velocity drift has been constrained by a measurement source
-         bool velAiding = gpsVelUsed || airSpdUsed || optFlowUsed;
+         bool velAiding = gpsVelUsed || airSpdUsed || optFlowUsed || bodyOdmUsed;
 
          // check if position drift has been constrained by a measurement source
          bool posAiding = gpsPosUsed || rngBcnUsed;
@@ -278,7 +285,7 @@ void NavEKF3_core::setAidingMode()
         // set various  usage modes based on the condition when we start aiding. These are then held until aiding is stopped.
         if (PV_AidingMode == AID_NONE) {
             // We have ceased aiding
-            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF3 IMU%u has stopped aiding",(unsigned)imu_index);
+            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF3 IMU%u stopped aiding",(unsigned)imu_index);
             // When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors
             posTimeout = true;
             velTimeout = true;
@@ -293,15 +300,23 @@ void NavEKF3_core::setAidingMode()
             meaHgtAtTakeOff = baroDataDelayed.hgt;
             // reset the vertical position state to faster recover from baro errors experienced during touchdown
             stateStruct.position.z = -meaHgtAtTakeOff;
+            // reset relative aiding sensor fusion activity status
+            flowFusionActive = false;
+            bodyVelFusionActive = false;
         } else if (PV_AidingMode == AID_RELATIVE) {
-            // We have commenced aiding, but GPS usage has been prohibited so use optical flow only
-            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u is using optical flow",(unsigned)imu_index);
+            // We are doing relative position navigation where velocity errors are constrained, but position drift will occur
+            GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u started relative aiding",(unsigned)imu_index);
+            if (readyToUseOptFlow()) {
+                // Reset time stamps
+                flowValidMeaTime_ms = imuSampleTime_ms;
+                prevFlowFuseTime_ms = imuSampleTime_ms;
+            } else if (readyToUseBodyOdm()) {
+                 // Reset time stamps
+                lastbodyVelPassTime_ms = imuSampleTime_ms;
+                prevBodyVelFuseTime_ms = imuSampleTime_ms;
+            }
             posTimeout = true;
             velTimeout = true;
-            // Reset the last valid flow measurement time
-            flowValidMeaTime_ms = imuSampleTime_ms;
-            // Reset the last valid flow fusion time
-            prevFlowFuseTime_ms = imuSampleTime_ms;
         } else if (PV_AidingMode == AID_ABSOLUTE) {
             if (readyToUseGPS()) {
                 // We are commencing aiding using GPS - this is the preferred method
@@ -376,6 +391,16 @@ bool NavEKF3_core::readyToUseOptFlow(void) const
     return (imuSampleTime_ms - flowMeaTime_ms < 200) && tiltAlignComplete && delAngBiasLearned;
 }
 
+// return true if the filter is ready to start using body frame odometry measurements
+bool NavEKF3_core::readyToUseBodyOdm(void) const
+{
+    // We need stable roll/pitch angles and gyro bias estimates but do not need the yaw angle aligned to use these measurements
+    return (imuSampleTime_ms - bodyOdmMeasTime_ms < 200)
+            && bodyOdmDataNew.velErr < 1.0f
+            && tiltAlignComplete
+            && delAngBiasLearned;
+}
+
 // return true if the filter to be ready to use gps
 bool NavEKF3_core::readyToUseGPS(void) const
 {
@@ -467,13 +492,12 @@ void  NavEKF3_core::updateFilterStatus(void)
 {
     // init return value
     filterStatus.value = 0;
-    bool doingFlowNav = (PV_AidingMode == AID_RELATIVE) && flowDataValid;
+    bool doingBodyVelNav = (PV_AidingMode != AID_NONE) && (imuSampleTime_ms - prevBodyVelFuseTime_ms < 5000);
+    bool doingFlowNav = (PV_AidingMode != AID_NONE) && flowDataValid;
     bool doingWindRelNav = !tasTimeout && assume_zero_sideslip();
     bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == AID_ABSOLUTE);
     bool someVertRefData = (!velTimeout && useGpsVertVel) || !hgtTimeout;
-    bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav;
-    bool optFlowNavPossible = flowDataValid && delAngBiasLearned;
-    bool gpsNavPossible = !gpsNotAvailable && gpsGoodToAlign && delAngBiasLearned;
+    bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav || doingBodyVelNav;
     bool filterHealthy = healthy() && tiltAlignComplete && (yawAlignComplete || (!use_compass() && (PV_AidingMode != AID_ABSOLUTE)));
 
     // If GPS height usage is specified, height is considered to be inaccurate until the GPS passes all checks
@@ -483,13 +507,13 @@ void  NavEKF3_core::updateFilterStatus(void)
     filterStatus.flags.attitude = !stateStruct.quat.is_nan() && filterHealthy;   // attitude valid (we need a better check)
     filterStatus.flags.horiz_vel = someHorizRefData && filterHealthy;      // horizontal velocity estimate valid
     filterStatus.flags.vert_vel = someVertRefData && filterHealthy;        // vertical velocity estimate valid
-    filterStatus.flags.horiz_pos_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav) && filterHealthy;   // relative horizontal position estimate valid
+    filterStatus.flags.horiz_pos_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav || doingBodyVelNav) && filterHealthy;   // relative horizontal position estimate valid
     filterStatus.flags.horiz_pos_abs = doingNormalGpsNav && filterHealthy; // absolute horizontal position estimate valid
     filterStatus.flags.vert_pos = !hgtTimeout && filterHealthy && !hgtNotAccurate; // vertical position estimate valid
     filterStatus.flags.terrain_alt = gndOffsetValid && filterHealthy;		// terrain height estimate valid
     filterStatus.flags.const_pos_mode = (PV_AidingMode == AID_NONE) && filterHealthy;     // constant position mode
-    filterStatus.flags.pred_horiz_pos_rel = ((optFlowNavPossible || gpsNavPossible) && filterHealthy) || filterStatus.flags.horiz_pos_rel; // we should be able to estimate a relative position when we enter flight mode
-    filterStatus.flags.pred_horiz_pos_abs = (gpsNavPossible && filterHealthy) || filterStatus.flags.horiz_pos_abs; // we should be able to estimate an absolute position when we enter flight mode
+    filterStatus.flags.pred_horiz_pos_rel = filterStatus.flags.horiz_pos_rel; // EKF3 enters the required mode before flight
+    filterStatus.flags.pred_horiz_pos_abs = filterStatus.flags.horiz_pos_abs; // EKF3 enters the required mode before flight
     filterStatus.flags.takeoff_detected = takeOffDetected; // takeoff for optical flow navigation has been detected
     filterStatus.flags.takeoff = expectGndEffectTakeoff; // The EKF has been told to expect takeoff and is in a ground effect mitigation mode
     filterStatus.flags.touchdown = expectGndEffectTouchdown; // The EKF has been told to detect touchdown and is in a ground effect mitigation mode

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -106,6 +106,30 @@ void NavEKF3_core::readRangeFinder(void)
     }
 }
 
+void NavEKF3_core::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset)
+{
+    // limit update rate to maximum allowed by sensor buffers and fusion process
+    // don't try to write to buffer until the filter has been initialised
+    if (((timeStamp_ms - bodyOdmMeasTime_ms) < frontend->sensorIntervalMin_ms) || (delTime < dtEkfAvg) || !statesInitialised) {
+        return;
+    }
+
+    bodyOdmDataNew.body_offset = &posOffset;
+    bodyOdmDataNew.vel = delPos * (1.0f/delTime);
+    bodyOdmDataNew.time_ms = timeStamp_ms;
+    bodyOdmDataNew.angRate = delAng * (1.0f/delTime);
+    bodyOdmMeasTime_ms = timeStamp_ms;
+
+    // simple model of accuracy
+    // TODO move this calculation outside of EKF into the sensor driver
+    const float minVelErr = 0.5f;
+    const float maxVelErr = 10.0f;
+    bodyOdmDataNew.velErr = minVelErr + (maxVelErr - minVelErr) * (1.0f - 0.01f * quality);
+
+    storedBodyOdm.push(bodyOdmDataNew);
+
+}
+
 // write the raw optical flow measurements
 // this needs to be called externally.
 void NavEKF3_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, const Vector3f &posOffset)

libraries/AP_NavEKF3/AP_NavEKF3_OptFlowFusion.cpp

@@ -6,6 +6,7 @@
 #include "AP_NavEKF3_core.h"
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_Vehicle/AP_Vehicle.h>
+#include <GCS_MAVLink/GCS.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -647,7 +648,11 @@ void NavEKF3_core::FuseOptFlow()
         if ((flowTestRatio[obsIndex]) < 1.0f && (ofDataDelayed.flowRadXY.x < frontend->_maxFlowRate) && (ofDataDelayed.flowRadXY.y < frontend->_maxFlowRate)) {
             // record the last time observations were accepted for fusion
             prevFlowFuseTime_ms = imuSampleTime_ms;
-
+            // notify first time only
+            if (!flowFusionActive) {
+                flowFusionActive = true;
+                GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u fusing optical flow",(unsigned)imu_index);
+            }
             // correct the covariance P = (I - K*H)*P
             // take advantage of the empty columns in KH to reduce the
             // number of operations

libraries/AP_NavEKF3/AP_NavEKF3_Outputs.cpp

@@ -67,6 +67,18 @@ void NavEKF3_core::getFlowDebug(float &varFlow, float &gndOffset, float &flowInn
     gndOffsetErr = sqrtf(Popt); // note Popt is constrained to be non-negative in EstimateTerrainOffset()
 }
 
+// return data for debugging body frame odometry fusion
+uint32_t NavEKF3_core::getBodyFrameOdomDebug(Vector3f &velInnov, Vector3f &velInnovVar)
+{
+    velInnov.x = innovBodyVel[0];
+    velInnov.y = innovBodyVel[1];
+    velInnov.z = innovBodyVel[2];
+    velInnovVar.x = varInnovBodyVel[0];
+    velInnovVar.y = varInnovBodyVel[1];
+    velInnovVar.z = varInnovBodyVel[2];
+    return bodyOdmDataDelayed.time_ms;
+}
+
 // return data for debugging range beacon fusion one beacon at a time, incrementing the beacon index after each call
 bool NavEKF3_core::getRangeBeaconDebug(uint8_t &ID, float &rng, float &innov, float &innovVar, float &testRatio, Vector3f &beaconPosNED,
                                        float &offsetHigh, float &offsetLow, Vector3f &posNED)
@@ -339,7 +351,13 @@ bool NavEKF3_core::getLLH(struct Location &loc) const
 // return the scale factor to be applied to navigation velocity gains to compensate for increase in velocity noise with height when using optical flow
 void NavEKF3_core::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const
 {
-    if (PV_AidingMode == AID_RELATIVE) {
+    // If in the last 10 seconds we have received flow data and no odometry data, then we are relying on optical flow
+    bool relyingOnFlowData = (imuSampleTime_ms - prevBodyVelFuseTime_ms > 1000)
+            && (imuSampleTime_ms - flowValidMeaTime_ms <= 10000);
+
+    // If relying on optical flow, limit speed to prevent sensor limit being exceeded and adjust
+    // nav gains to prevent body rate feedback into flow rates destabilising the control loop
+    if (PV_AidingMode == AID_RELATIVE && relyingOnFlowData) {
         // allow 1.0 rad/sec margin for angular motion
         ekfGndSpdLimit = MAX((frontend->_maxFlowRate - 1.0f), 0.0f) * MAX((terrainState - stateStruct.position[2]), rngOnGnd);
         // use standard gains up to 5.0 metres height and reduce above that

libraries/AP_NavEKF3/AP_NavEKF3_PosVelFusion.cpp

@@ -8,6 +8,7 @@
 #include "AP_NavEKF3_core.h"
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_Vehicle/AP_Vehicle.h>
+#include <GCS_MAVLink/GCS.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -886,4 +887,627 @@ void NavEKF3_core::selectHeightForFusion()
     }
 }
 
+/*
+ * Fuse body frame velocity measurements using explicit algebraic equations generated with Matlab symbolic toolbox.
+ * The script file used to generate these and other equations in this filter can be found here:
+ * https://github.com/PX4/ecl/blob/master/matlab/scripts/Inertial%20Nav%20EKF/GenerateNavFilterEquations.m
+*/
+void NavEKF3_core::FuseBodyVel()
+{
+    Vector24 H_VEL;
+    Vector3f bodyVelPred;
+
+    // Copy required states to local variable names
+    float q0  = stateStruct.quat[0];
+    float q1 = stateStruct.quat[1];
+    float q2 = stateStruct.quat[2];
+    float q3 = stateStruct.quat[3];
+    float vn = stateStruct.velocity.x;
+    float ve = stateStruct.velocity.y;
+    float vd = stateStruct.velocity.z;
+
+    // Fuse X, Y and Z axis measurements sequentially assuming observation errors are uncorrelated
+    for (uint8_t obsIndex=0; obsIndex<=2; obsIndex++) {
+
+        // calculate relative velocity in sensor frame including the relative motion due to rotation
+        bodyVelPred = (prevTnb * stateStruct.velocity);
+
+        // correct sensor offset body frame position offset relative to IMU
+        Vector3f posOffsetBody = (*bodyOdmDataDelayed.body_offset) - accelPosOffset;
+
+        // correct prediction for relative motion due to rotation
+        // note - % operator overloaded for cross product
+        if (imuDataDelayed.delAngDT > 0.001f) {
+            bodyVelPred += (imuDataDelayed.delAng * (1.0f / imuDataDelayed.delAngDT)) % posOffsetBody;
+        }
+
+        // calculate observation jacobians and Kalman gains
+        if (obsIndex == 0) {
+            // calculate X axis observation Jacobian
+            H_VEL[0] = q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f;
+            H_VEL[1] = q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f;
+            H_VEL[2] = q0*vd*-2.0f+q1*ve*2.0f-q2*vn*2.0f;
+            H_VEL[3] = q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f;
+            H_VEL[4] = q0*q0+q1*q1-q2*q2-q3*q3;
+            H_VEL[5] = q0*q3*2.0f+q1*q2*2.0f;
+            H_VEL[6] = q0*q2*-2.0f+q1*q3*2.0f;
+            for (uint8_t index = 7; index < 24; index++) {
+                H_VEL[index] = 0.0f;
+            }
+
+            // calculate intermediate expressions for X axis Kalman gains
+            float R_VEL = bodyOdmDataDelayed.velErr;
+            float t2 = q0*q3*2.0f;
+            float t3 = q1*q2*2.0f;
+            float t4 = t2+t3;
+            float t5 = q0*q0;
+            float t6 = q1*q1;
+            float t7 = q2*q2;
+            float t8 = q3*q3;
+            float t9 = t5+t6-t7-t8;
+            float t10 = q0*q2*2.0f;
+            float t25 = q1*q3*2.0f;
+            float t11 = t10-t25;
+            float t12 = q3*ve*2.0f;
+            float t13 = q0*vn*2.0f;
+            float t26 = q2*vd*2.0f;
+            float t14 = t12+t13-t26;
+            float t15 = q3*vd*2.0f;
+            float t16 = q2*ve*2.0f;
+            float t17 = q1*vn*2.0f;
+            float t18 = t15+t16+t17;
+            float t19 = q0*vd*2.0f;
+            float t20 = q2*vn*2.0f;
+            float t27 = q1*ve*2.0f;
+            float t21 = t19+t20-t27;
+            float t22 = q1*vd*2.0f;
+            float t23 = q0*ve*2.0f;
+            float t28 = q3*vn*2.0f;
+            float t24 = t22+t23-t28;
+            float t29 = P[0][0]*t14;
+            float t30 = P[1][1]*t18;
+            float t31 = P[4][5]*t9;
+            float t32 = P[5][5]*t4;
+            float t33 = P[0][5]*t14;
+            float t34 = P[1][5]*t18;
+            float t35 = P[3][5]*t24;
+            float t79 = P[6][5]*t11;
+            float t80 = P[2][5]*t21;
+            float t36 = t31+t32+t33+t34+t35-t79-t80;
+            float t37 = t4*t36;
+            float t38 = P[4][6]*t9;
+            float t39 = P[5][6]*t4;
+            float t40 = P[0][6]*t14;
+            float t41 = P[1][6]*t18;
+            float t42 = P[3][6]*t24;
+            float t81 = P[6][6]*t11;
+            float t82 = P[2][6]*t21;
+            float t43 = t38+t39+t40+t41+t42-t81-t82;
+            float t44 = P[4][0]*t9;
+            float t45 = P[5][0]*t4;
+            float t46 = P[1][0]*t18;
+            float t47 = P[3][0]*t24;
+            float t84 = P[6][0]*t11;
+            float t85 = P[2][0]*t21;
+            float t48 = t29+t44+t45+t46+t47-t84-t85;
+            float t49 = t14*t48;
+            float t50 = P[4][1]*t9;
+            float t51 = P[5][1]*t4;
+            float t52 = P[0][1]*t14;
+            float t53 = P[3][1]*t24;
+            float t86 = P[6][1]*t11;
+            float t87 = P[2][1]*t21;
+            float t54 = t30+t50+t51+t52+t53-t86-t87;
+            float t55 = t18*t54;
+            float t56 = P[4][2]*t9;
+            float t57 = P[5][2]*t4;
+            float t58 = P[0][2]*t14;
+            float t59 = P[1][2]*t18;
+            float t60 = P[3][2]*t24;
+            float t78 = P[2][2]*t21;
+            float t88 = P[6][2]*t11;
+            float t61 = t56+t57+t58+t59+t60-t78-t88;
+            float t62 = P[4][3]*t9;
+            float t63 = P[5][3]*t4;
+            float t64 = P[0][3]*t14;
+            float t65 = P[1][3]*t18;
+            float t66 = P[3][3]*t24;
+            float t90 = P[6][3]*t11;
+            float t91 = P[2][3]*t21;
+            float t67 = t62+t63+t64+t65+t66-t90-t91;
+            float t68 = t24*t67;
+            float t69 = P[4][4]*t9;
+            float t70 = P[5][4]*t4;
+            float t71 = P[0][4]*t14;
+            float t72 = P[1][4]*t18;
+            float t73 = P[3][4]*t24;
+            float t92 = P[6][4]*t11;
+            float t93 = P[2][4]*t21;
+            float t74 = t69+t70+t71+t72+t73-t92-t93;
+            float t75 = t9*t74;
+            float t83 = t11*t43;
+            float t89 = t21*t61;
+            float t76 = R_VEL+t37+t49+t55+t68+t75-t83-t89;
+            float t77;
+
+            // calculate innovation variance for X axis observation and protect against a badly conditioned calculation
+            if (t76 > R_VEL) {
+                t77 = 1.0f/t76;
+                faultStatus.bad_xvel = false;
+            } else {
+                t76 = R_VEL;
+                t77 = 1.0f/R_VEL;
+                faultStatus.bad_xvel = true;
+                return;
+            }
+            varInnovBodyVel[0] = t77;
+
+            // calculate innovation for X axis observation
+            innovBodyVel[0] = bodyVelPred.x - bodyOdmDataDelayed.vel.x;
+
+            // calculate Kalman gains for X-axis observation
+            Kfusion[0] = t77*(t29+P[0][5]*t4+P[0][4]*t9-P[0][6]*t11+P[0][1]*t18-P[0][2]*t21+P[0][3]*t24);
+            Kfusion[1] = t77*(t30+P[1][5]*t4+P[1][4]*t9+P[1][0]*t14-P[1][6]*t11-P[1][2]*t21+P[1][3]*t24);
+            Kfusion[2] = t77*(-t78+P[2][5]*t4+P[2][4]*t9+P[2][0]*t14-P[2][6]*t11+P[2][1]*t18+P[2][3]*t24);
+            Kfusion[3] = t77*(t66+P[3][5]*t4+P[3][4]*t9+P[3][0]*t14-P[3][6]*t11+P[3][1]*t18-P[3][2]*t21);
+            Kfusion[4] = t77*(t69+P[4][5]*t4+P[4][0]*t14-P[4][6]*t11+P[4][1]*t18-P[4][2]*t21+P[4][3]*t24);
+            Kfusion[5] = t77*(t32+P[5][4]*t9+P[5][0]*t14-P[5][6]*t11+P[5][1]*t18-P[5][2]*t21+P[5][3]*t24);
+            Kfusion[6] = t77*(-t81+P[6][5]*t4+P[6][4]*t9+P[6][0]*t14+P[6][1]*t18-P[6][2]*t21+P[6][3]*t24);
+            Kfusion[7] = t77*(P[7][5]*t4+P[7][4]*t9+P[7][0]*t14-P[7][6]*t11+P[7][1]*t18-P[7][2]*t21+P[7][3]*t24);
+            Kfusion[8] = t77*(P[8][5]*t4+P[8][4]*t9+P[8][0]*t14-P[8][6]*t11+P[8][1]*t18-P[8][2]*t21+P[8][3]*t24);
+            Kfusion[9] = t77*(P[9][5]*t4+P[9][4]*t9+P[9][0]*t14-P[9][6]*t11+P[9][1]*t18-P[9][2]*t21+P[9][3]*t24);
+            Kfusion[10] = t77*(P[10][5]*t4+P[10][4]*t9+P[10][0]*t14-P[10][6]*t11+P[10][1]*t18-P[10][2]*t21+P[10][3]*t24);
+            Kfusion[11] = t77*(P[11][5]*t4+P[11][4]*t9+P[11][0]*t14-P[11][6]*t11+P[11][1]*t18-P[11][2]*t21+P[11][3]*t24);
+            Kfusion[12] = t77*(P[12][5]*t4+P[12][4]*t9+P[12][0]*t14-P[12][6]*t11+P[12][1]*t18-P[12][2]*t21+P[12][3]*t24);
+            Kfusion[13] = t77*(P[13][5]*t4+P[13][4]*t9+P[13][0]*t14-P[13][6]*t11+P[13][1]*t18-P[13][2]*t21+P[13][3]*t24);
+            Kfusion[14] = t77*(P[14][5]*t4+P[14][4]*t9+P[14][0]*t14-P[14][6]*t11+P[14][1]*t18-P[14][2]*t21+P[14][3]*t24);
+            Kfusion[15] = t77*(P[15][5]*t4+P[15][4]*t9+P[15][0]*t14-P[15][6]*t11+P[15][1]*t18-P[15][2]*t21+P[15][3]*t24);
+            if (!inhibitMagStates) {
+                Kfusion[16] = t77*(P[16][5]*t4+P[16][4]*t9+P[16][0]*t14-P[16][6]*t11+P[16][1]*t18-P[16][2]*t21+P[16][3]*t24);
+                Kfusion[17] = t77*(P[17][5]*t4+P[17][4]*t9+P[17][0]*t14-P[17][6]*t11+P[17][1]*t18-P[17][2]*t21+P[17][3]*t24);
+                Kfusion[18] = t77*(P[18][5]*t4+P[18][4]*t9+P[18][0]*t14-P[18][6]*t11+P[18][1]*t18-P[18][2]*t21+P[18][3]*t24);
+                Kfusion[19] = t77*(P[19][5]*t4+P[19][4]*t9+P[19][0]*t14-P[19][6]*t11+P[19][1]*t18-P[19][2]*t21+P[19][3]*t24);
+                Kfusion[20] = t77*(P[20][5]*t4+P[20][4]*t9+P[20][0]*t14-P[20][6]*t11+P[20][1]*t18-P[20][2]*t21+P[20][3]*t24);
+                Kfusion[21] = t77*(P[21][5]*t4+P[21][4]*t9+P[21][0]*t14-P[21][6]*t11+P[21][1]*t18-P[21][2]*t21+P[21][3]*t24);
+            } else {
+                for (uint8_t i = 16; i <= 21; i++) {
+                    Kfusion[i] = 0.0f;
+                }
+            }
+            if (!inhibitWindStates) {
+                Kfusion[22] = t77*(P[22][5]*t4+P[22][4]*t9+P[22][0]*t14-P[22][6]*t11+P[22][1]*t18-P[22][2]*t21+P[22][3]*t24);
+                Kfusion[23] = t77*(P[23][5]*t4+P[23][4]*t9+P[23][0]*t14-P[23][6]*t11+P[23][1]*t18-P[23][2]*t21+P[23][3]*t24);
+            } else {
+                Kfusion[22] = 0.0f;
+                Kfusion[23] = 0.0f;
+            }
+        } else if (obsIndex == 1) {
+            // calculate Y axis observation Jacobian
+            H_VEL[0] = q1*vd*2.0f+q0*ve*2.0f-q3*vn*2.0f;
+            H_VEL[1] = q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f;
+            H_VEL[2] = q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f;
+            H_VEL[3] = q2*vd*2.0f-q3*ve*2.0f-q0*vn*2.0f;
+            H_VEL[4] = q0*q3*-2.0f+q1*q2*2.0f;
+            H_VEL[5] = q0*q0-q1*q1+q2*q2-q3*q3;
+            H_VEL[6] = q0*q1*2.0f+q2*q3*2.0f;
+            for (uint8_t index = 7; index < 24; index++) {
+                H_VEL[index] = 0.0f;
+            }
+
+            // calculate intermediate expressions for Y axis Kalman gains
+            float R_VEL = bodyOdmDataDelayed.velErr;
+            float t2 = q0*q3*2.0f;
+            float t9 = q1*q2*2.0f;
+            float t3 = t2-t9;
+            float t4 = q0*q0;
+            float t5 = q1*q1;
+            float t6 = q2*q2;
+            float t7 = q3*q3;
+            float t8 = t4-t5+t6-t7;
+            float t10 = q0*q1*2.0f;
+            float t11 = q2*q3*2.0f;
+            float t12 = t10+t11;
+            float t13 = q1*vd*2.0f;
+            float t14 = q0*ve*2.0f;
+            float t26 = q3*vn*2.0f;
+            float t15 = t13+t14-t26;
+            float t16 = q0*vd*2.0f;
+            float t17 = q2*vn*2.0f;
+            float t27 = q1*ve*2.0f;
+            float t18 = t16+t17-t27;
+            float t19 = q3*vd*2.0f;
+            float t20 = q2*ve*2.0f;
+            float t21 = q1*vn*2.0f;
+            float t22 = t19+t20+t21;
+            float t23 = q3*ve*2.0f;
+            float t24 = q0*vn*2.0f;
+            float t28 = q2*vd*2.0f;
+            float t25 = t23+t24-t28;
+            float t29 = P[0][0]*t15;
+            float t30 = P[1][1]*t18;
+            float t31 = P[5][4]*t8;
+            float t32 = P[6][4]*t12;
+            float t33 = P[0][4]*t15;
+            float t34 = P[1][4]*t18;
+            float t35 = P[2][4]*t22;
+            float t78 = P[4][4]*t3;
+            float t79 = P[3][4]*t25;
+            float t36 = t31+t32+t33+t34+t35-t78-t79;
+            float t37 = P[5][6]*t8;
+            float t38 = P[6][6]*t12;
+            float t39 = P[0][6]*t15;
+            float t40 = P[1][6]*t18;
+            float t41 = P[2][6]*t22;
+            float t81 = P[4][6]*t3;
+            float t82 = P[3][6]*t25;
+            float t42 = t37+t38+t39+t40+t41-t81-t82;
+            float t43 = t12*t42;
+            float t44 = P[5][0]*t8;
+            float t45 = P[6][0]*t12;
+            float t46 = P[1][0]*t18;
+            float t47 = P[2][0]*t22;
+            float t83 = P[4][0]*t3;
+            float t84 = P[3][0]*t25;
+            float t48 = t29+t44+t45+t46+t47-t83-t84;
+            float t49 = t15*t48;
+            float t50 = P[5][1]*t8;
+            float t51 = P[6][1]*t12;
+            float t52 = P[0][1]*t15;
+            float t53 = P[2][1]*t22;
+            float t85 = P[4][1]*t3;
+            float t86 = P[3][1]*t25;
+            float t54 = t30+t50+t51+t52+t53-t85-t86;
+            float t55 = t18*t54;
+            float t56 = P[5][2]*t8;
+            float t57 = P[6][2]*t12;
+            float t58 = P[0][2]*t15;
+            float t59 = P[1][2]*t18;
+            float t60 = P[2][2]*t22;
+            float t87 = P[4][2]*t3;
+            float t88 = P[3][2]*t25;
+            float t61 = t56+t57+t58+t59+t60-t87-t88;
+            float t62 = t22*t61;
+            float t63 = P[5][3]*t8;
+            float t64 = P[6][3]*t12;
+            float t65 = P[0][3]*t15;
+            float t66 = P[1][3]*t18;
+            float t67 = P[2][3]*t22;
+            float t89 = P[4][3]*t3;
+            float t90 = P[3][3]*t25;
+            float t68 = t63+t64+t65+t66+t67-t89-t90;
+            float t69 = P[5][5]*t8;
+            float t70 = P[6][5]*t12;
+            float t71 = P[0][5]*t15;
+            float t72 = P[1][5]*t18;
+            float t73 = P[2][5]*t22;
+            float t92 = P[4][5]*t3;
+            float t93 = P[3][5]*t25;
+            float t74 = t69+t70+t71+t72+t73-t92-t93;
+            float t75 = t8*t74;
+            float t80 = t3*t36;
+            float t91 = t25*t68;
+            float t76 = R_VEL+t43+t49+t55+t62+t75-t80-t91;
+            float t77;
+
+            // calculate innovation variance for Y axis observation and protect against a badly conditioned calculation
+            if (t76 > R_VEL) {
+                t77 = 1.0f/t76;
+                faultStatus.bad_yvel = false;
+            } else {
+                t76 = R_VEL;
+                t77 = 1.0f/R_VEL;
+                faultStatus.bad_yvel = true;
+                return;
+            }
+            varInnovBodyVel[1] = t77;
+
+            // calculate innovation for Y axis observation
+            innovBodyVel[1] = bodyVelPred.y - bodyOdmDataDelayed.vel.y;
+
+            // calculate Kalman gains for Y-axis observation
+            Kfusion[0] = t77*(t29-P[0][4]*t3+P[0][5]*t8+P[0][6]*t12+P[0][1]*t18+P[0][2]*t22-P[0][3]*t25);
+            Kfusion[1] = t77*(t30-P[1][4]*t3+P[1][5]*t8+P[1][0]*t15+P[1][6]*t12+P[1][2]*t22-P[1][3]*t25);
+            Kfusion[2] = t77*(t60-P[2][4]*t3+P[2][5]*t8+P[2][0]*t15+P[2][6]*t12+P[2][1]*t18-P[2][3]*t25);
+            Kfusion[3] = t77*(-t90-P[3][4]*t3+P[3][5]*t8+P[3][0]*t15+P[3][6]*t12+P[3][1]*t18+P[3][2]*t22);
+            Kfusion[4] = t77*(-t78+P[4][5]*t8+P[4][0]*t15+P[4][6]*t12+P[4][1]*t18+P[4][2]*t22-P[4][3]*t25);
+            Kfusion[5] = t77*(t69-P[5][4]*t3+P[5][0]*t15+P[5][6]*t12+P[5][1]*t18+P[5][2]*t22-P[5][3]*t25);
+            Kfusion[6] = t77*(t38-P[6][4]*t3+P[6][5]*t8+P[6][0]*t15+P[6][1]*t18+P[6][2]*t22-P[6][3]*t25);
+            Kfusion[7] = t77*(-P[7][4]*t3+P[7][5]*t8+P[7][0]*t15+P[7][6]*t12+P[7][1]*t18+P[7][2]*t22-P[7][3]*t25);
+            Kfusion[8] = t77*(-P[8][4]*t3+P[8][5]*t8+P[8][0]*t15+P[8][6]*t12+P[8][1]*t18+P[8][2]*t22-P[8][3]*t25);
+            Kfusion[9] = t77*(-P[9][4]*t3+P[9][5]*t8+P[9][0]*t15+P[9][6]*t12+P[9][1]*t18+P[9][2]*t22-P[9][3]*t25);
+            Kfusion[10] = t77*(-P[10][4]*t3+P[10][5]*t8+P[10][0]*t15+P[10][6]*t12+P[10][1]*t18+P[10][2]*t22-P[10][3]*t25);
+            Kfusion[11] = t77*(-P[11][4]*t3+P[11][5]*t8+P[11][0]*t15+P[11][6]*t12+P[11][1]*t18+P[11][2]*t22-P[11][3]*t25);
+            Kfusion[12] = t77*(-P[12][4]*t3+P[12][5]*t8+P[12][0]*t15+P[12][6]*t12+P[12][1]*t18+P[12][2]*t22-P[12][3]*t25);
+            Kfusion[13] = t77*(-P[13][4]*t3+P[13][5]*t8+P[13][0]*t15+P[13][6]*t12+P[13][1]*t18+P[13][2]*t22-P[13][3]*t25);
+            Kfusion[14] = t77*(-P[14][4]*t3+P[14][5]*t8+P[14][0]*t15+P[14][6]*t12+P[14][1]*t18+P[14][2]*t22-P[14][3]*t25);
+            Kfusion[15] = t77*(-P[15][4]*t3+P[15][5]*t8+P[15][0]*t15+P[15][6]*t12+P[15][1]*t18+P[15][2]*t22-P[15][3]*t25);
+            if (!inhibitMagStates) {
+                Kfusion[16] = t77*(-P[16][4]*t3+P[16][5]*t8+P[16][0]*t15+P[16][6]*t12+P[16][1]*t18+P[16][2]*t22-P[16][3]*t25);
+                Kfusion[17] = t77*(-P[17][4]*t3+P[17][5]*t8+P[17][0]*t15+P[17][6]*t12+P[17][1]*t18+P[17][2]*t22-P[17][3]*t25);
+                Kfusion[18] = t77*(-P[18][4]*t3+P[18][5]*t8+P[18][0]*t15+P[18][6]*t12+P[18][1]*t18+P[18][2]*t22-P[18][3]*t25);
+                Kfusion[19] = t77*(-P[19][4]*t3+P[19][5]*t8+P[19][0]*t15+P[19][6]*t12+P[19][1]*t18+P[19][2]*t22-P[19][3]*t25);
+                Kfusion[20] = t77*(-P[20][4]*t3+P[20][5]*t8+P[20][0]*t15+P[20][6]*t12+P[20][1]*t18+P[20][2]*t22-P[20][3]*t25);
+                Kfusion[21] = t77*(-P[21][4]*t3+P[21][5]*t8+P[21][0]*t15+P[21][6]*t12+P[21][1]*t18+P[21][2]*t22-P[21][3]*t25);
+            } else {
+                for (uint8_t i = 16; i <= 21; i++) {
+                    Kfusion[i] = 0.0f;
+                }
+            }
+            if (!inhibitWindStates) {
+                Kfusion[22] = t77*(-P[22][4]*t3+P[22][5]*t8+P[22][0]*t15+P[22][6]*t12+P[22][1]*t18+P[22][2]*t22-P[22][3]*t25);
+                Kfusion[23] = t77*(-P[23][4]*t3+P[23][5]*t8+P[23][0]*t15+P[23][6]*t12+P[23][1]*t18+P[23][2]*t22-P[23][3]*t25);
+            } else {
+                Kfusion[22] = 0.0f;
+                Kfusion[23] = 0.0f;
+            }
+        } else if (obsIndex == 2) {
+            // calculate Z axis observation Jacobian
+            H_VEL[0] = q0*vd*2.0f-q1*ve*2.0f+q2*vn*2.0f;
+            H_VEL[1] = q1*vd*-2.0f-q0*ve*2.0f+q3*vn*2.0f;
+            H_VEL[2] = q2*vd*-2.0f+q3*ve*2.0f+q0*vn*2.0f;
+            H_VEL[3] = q3*vd*2.0f+q2*ve*2.0f+q1*vn*2.0f;
+            H_VEL[4] = q0*q2*2.0f+q1*q3*2.0f;
+            H_VEL[5] = q0*q1*-2.0f+q2*q3*2.0f;
+            H_VEL[6] = q0*q0-q1*q1-q2*q2+q3*q3;
+            for (uint8_t index = 7; index < 24; index++) {
+                H_VEL[index] = 0.0f;
+            }
+
+            // calculate intermediate expressions for Z axis Kalman gains
+            float R_VEL = bodyOdmDataDelayed.velErr;
+            float t2 = q0*q2*2.0f;
+            float t3 = q1*q3*2.0f;
+            float t4 = t2+t3;
+            float t5 = q0*q0;
+            float t6 = q1*q1;
+            float t7 = q2*q2;
+            float t8 = q3*q3;
+            float t9 = t5-t6-t7+t8;
+            float t10 = q0*q1*2.0f;
+            float t25 = q2*q3*2.0f;
+            float t11 = t10-t25;
+            float t12 = q0*vd*2.0f;
+            float t13 = q2*vn*2.0f;
+            float t26 = q1*ve*2.0f;
+            float t14 = t12+t13-t26;
+            float t15 = q1*vd*2.0f;
+            float t16 = q0*ve*2.0f;
+            float t27 = q3*vn*2.0f;
+            float t17 = t15+t16-t27;
+            float t18 = q3*ve*2.0f;
+            float t19 = q0*vn*2.0f;
+            float t28 = q2*vd*2.0f;
+            float t20 = t18+t19-t28;
+            float t21 = q3*vd*2.0f;
+            float t22 = q2*ve*2.0f;
+            float t23 = q1*vn*2.0f;
+            float t24 = t21+t22+t23;
+            float t29 = P[0][0]*t14;
+            float t30 = P[6][4]*t9;
+            float t31 = P[4][4]*t4;
+            float t32 = P[0][4]*t14;
+            float t33 = P[2][4]*t20;
+            float t34 = P[3][4]*t24;
+            float t78 = P[5][4]*t11;
+            float t79 = P[1][4]*t17;
+            float t35 = t30+t31+t32+t33+t34-t78-t79;
+            float t36 = t4*t35;
+            float t37 = P[6][5]*t9;
+            float t38 = P[4][5]*t4;
+            float t39 = P[0][5]*t14;
+            float t40 = P[2][5]*t20;
+            float t41 = P[3][5]*t24;
+            float t80 = P[5][5]*t11;
+            float t81 = P[1][5]*t17;
+            float t42 = t37+t38+t39+t40+t41-t80-t81;
+            float t43 = P[6][0]*t9;
+            float t44 = P[4][0]*t4;
+            float t45 = P[2][0]*t20;
+            float t46 = P[3][0]*t24;
+            float t83 = P[5][0]*t11;
+            float t84 = P[1][0]*t17;
+            float t47 = t29+t43+t44+t45+t46-t83-t84;
+            float t48 = t14*t47;
+            float t49 = P[6][1]*t9;
+            float t50 = P[4][1]*t4;
+            float t51 = P[0][1]*t14;
+            float t52 = P[2][1]*t20;
+            float t53 = P[3][1]*t24;
+            float t85 = P[5][1]*t11;
+            float t86 = P[1][1]*t17;
+            float t54 = t49+t50+t51+t52+t53-t85-t86;
+            float t55 = P[6][2]*t9;
+            float t56 = P[4][2]*t4;
+            float t57 = P[0][2]*t14;
+            float t58 = P[2][2]*t20;
+            float t59 = P[3][2]*t24;
+            float t88 = P[5][2]*t11;
+            float t89 = P[1][2]*t17;
+            float t60 = t55+t56+t57+t58+t59-t88-t89;
+            float t61 = t20*t60;
+            float t62 = P[6][3]*t9;
+            float t63 = P[4][3]*t4;
+            float t64 = P[0][3]*t14;
+            float t65 = P[2][3]*t20;
+            float t66 = P[3][3]*t24;
+            float t90 = P[5][3]*t11;
+            float t91 = P[1][3]*t17;
+            float t67 = t62+t63+t64+t65+t66-t90-t91;
+            float t68 = t24*t67;
+            float t69 = P[6][6]*t9;
+            float t70 = P[4][6]*t4;
+            float t71 = P[0][6]*t14;
+            float t72 = P[2][6]*t20;
+            float t73 = P[3][6]*t24;
+            float t92 = P[5][6]*t11;
+            float t93 = P[1][6]*t17;
+            float t74 = t69+t70+t71+t72+t73-t92-t93;
+            float t75 = t9*t74;
+            float t82 = t11*t42;
+            float t87 = t17*t54;
+            float t76 = R_VEL+t36+t48+t61+t68+t75-t82-t87;
+            float t77;
+
+            // calculate innovation variance for Z axis observation and protect against a badly conditioned calculation
+            if (t76 > R_VEL) {
+                t77 = 1.0f/t76;
+                faultStatus.bad_zvel = false;
+            } else {
+                t76 = R_VEL;
+                t77 = 1.0f/R_VEL;
+                faultStatus.bad_zvel = true;
+                return;
+            }
+            varInnovBodyVel[2] = t77;
+
+            // calculate innovation for Z axis observation
+            innovBodyVel[2] = bodyVelPred.z - bodyOdmDataDelayed.vel.z;
+
+            // calculate Kalman gains for X-axis observation
+            Kfusion[0] = t77*(t29+P[0][4]*t4+P[0][6]*t9-P[0][5]*t11-P[0][1]*t17+P[0][2]*t20+P[0][3]*t24);
+            Kfusion[1] = t77*(P[1][4]*t4+P[1][0]*t14+P[1][6]*t9-P[1][5]*t11-P[1][1]*t17+P[1][2]*t20+P[1][3]*t24);
+            Kfusion[2] = t77*(t58+P[2][4]*t4+P[2][0]*t14+P[2][6]*t9-P[2][5]*t11-P[2][1]*t17+P[2][3]*t24);
+            Kfusion[3] = t77*(t66+P[3][4]*t4+P[3][0]*t14+P[3][6]*t9-P[3][5]*t11-P[3][1]*t17+P[3][2]*t20);
+            Kfusion[4] = t77*(t31+P[4][0]*t14+P[4][6]*t9-P[4][5]*t11-P[4][1]*t17+P[4][2]*t20+P[4][3]*t24);
+            Kfusion[5] = t77*(-t80+P[5][4]*t4+P[5][0]*t14+P[5][6]*t9-P[5][1]*t17+P[5][2]*t20+P[5][3]*t24);
+            Kfusion[6] = t77*(t69+P[6][4]*t4+P[6][0]*t14-P[6][5]*t11-P[6][1]*t17+P[6][2]*t20+P[6][3]*t24);
+            Kfusion[7] = t77*(P[7][4]*t4+P[7][0]*t14+P[7][6]*t9-P[7][5]*t11-P[7][1]*t17+P[7][2]*t20+P[7][3]*t24);
+            Kfusion[8] = t77*(P[8][4]*t4+P[8][0]*t14+P[8][6]*t9-P[8][5]*t11-P[8][1]*t17+P[8][2]*t20+P[8][3]*t24);
+            Kfusion[9] = t77*(P[9][4]*t4+P[9][0]*t14+P[9][6]*t9-P[9][5]*t11-P[9][1]*t17+P[9][2]*t20+P[9][3]*t24);
+            Kfusion[10] = t77*(P[10][4]*t4+P[10][0]*t14+P[10][6]*t9-P[10][5]*t11-P[10][1]*t17+P[10][2]*t20+P[10][3]*t24);
+            Kfusion[11] = t77*(P[11][4]*t4+P[11][0]*t14+P[11][6]*t9-P[11][5]*t11-P[11][1]*t17+P[11][2]*t20+P[11][3]*t24);
+            Kfusion[12] = t77*(P[12][4]*t4+P[12][0]*t14+P[12][6]*t9-P[12][5]*t11-P[12][1]*t17+P[12][2]*t20+P[12][3]*t24);
+            Kfusion[13] = t77*(P[13][4]*t4+P[13][0]*t14+P[13][6]*t9-P[13][5]*t11-P[13][1]*t17+P[13][2]*t20+P[13][3]*t24);
+            Kfusion[14] = t77*(P[14][4]*t4+P[14][0]*t14+P[14][6]*t9-P[14][5]*t11-P[14][1]*t17+P[14][2]*t20+P[14][3]*t24);
+            Kfusion[15] = t77*(P[15][4]*t4+P[15][0]*t14+P[15][6]*t9-P[15][5]*t11-P[15][1]*t17+P[15][2]*t20+P[15][3]*t24);
+            if (!inhibitMagStates) {
+                Kfusion[16] = t77*(P[16][4]*t4+P[16][0]*t14+P[16][6]*t9-P[16][5]*t11-P[16][1]*t17+P[16][2]*t20+P[16][3]*t24);
+                Kfusion[17] = t77*(P[17][4]*t4+P[17][0]*t14+P[17][6]*t9-P[17][5]*t11-P[17][1]*t17+P[17][2]*t20+P[17][3]*t24);
+                Kfusion[18] = t77*(P[18][4]*t4+P[18][0]*t14+P[18][6]*t9-P[18][5]*t11-P[18][1]*t17+P[18][2]*t20+P[18][3]*t24);
+                Kfusion[19] = t77*(P[19][4]*t4+P[19][0]*t14+P[19][6]*t9-P[19][5]*t11-P[19][1]*t17+P[19][2]*t20+P[19][3]*t24);
+                Kfusion[20] = t77*(P[20][4]*t4+P[20][0]*t14+P[20][6]*t9-P[20][5]*t11-P[20][1]*t17+P[20][2]*t20+P[20][3]*t24);
+                Kfusion[21] = t77*(P[21][4]*t4+P[21][0]*t14+P[21][6]*t9-P[21][5]*t11-P[21][1]*t17+P[21][2]*t20+P[21][3]*t24);
+            } else {
+                for (uint8_t i = 16; i <= 21; i++) {
+                    Kfusion[i] = 0.0f;
+                }
+            }
+            if (!inhibitWindStates) {
+                Kfusion[22] = t77*(P[22][4]*t4+P[22][0]*t14+P[22][6]*t9-P[22][5]*t11-P[22][1]*t17+P[22][2]*t20+P[22][3]*t24);
+                Kfusion[23] = t77*(P[23][4]*t4+P[23][0]*t14+P[23][6]*t9-P[23][5]*t11-P[23][1]*t17+P[23][2]*t20+P[23][3]*t24);
+            } else {
+                Kfusion[22] = 0.0f;
+                Kfusion[23] = 0.0f;
+            }
+        } else {
+            return;
+        }
+
+        // calculate the innovation consistency test ratio
+        // TODO add tuning parameter for gate
+        bodyVelTestRatio[obsIndex] = sq(innovBodyVel[obsIndex]) / (sq(5.0f) * varInnovBodyVel[obsIndex]);
+
+        // Check the innovation for consistency and don't fuse if out of bounds
+        // TODO also apply angular velocity magnitude check
+        if ((bodyVelTestRatio[obsIndex]) < 1.0f) {
+            // record the last time observations were accepted for fusion
+            prevBodyVelFuseTime_ms = imuSampleTime_ms;
+            // notify first time only
+            if (!bodyVelFusionActive) {
+                bodyVelFusionActive = true;
+                GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_INFO, "EKF3 IMU%u fusing odometry",(unsigned)imu_index);
+            }
+            // correct the covariance P = (I - K*H)*P
+            // take advantage of the empty columns in KH to reduce the
+            // number of operations
+            for (unsigned i = 0; i<=stateIndexLim; i++) {
+                for (unsigned j = 0; j<=6; j++) {
+                    KH[i][j] = Kfusion[i] * H_VEL[j];
+                }
+                for (unsigned j = 7; j<=stateIndexLim; j++) {
+                    KH[i][j] = 0.0f;
+                }
+            }
+            for (unsigned j = 0; j<=stateIndexLim; j++) {
+                for (unsigned i = 0; i<=stateIndexLim; i++) {
+                    ftype res = 0;
+                    res += KH[i][0] * P[0][j];
+                    res += KH[i][1] * P[1][j];
+                    res += KH[i][2] * P[2][j];
+                    res += KH[i][3] * P[3][j];
+                    res += KH[i][4] * P[4][j];
+                    res += KH[i][5] * P[5][j];
+                    res += KH[i][6] * P[6][j];
+                    KHP[i][j] = res;
+                }
+            }
+
+            // Check that we are not going to drive any variances negative and skip the update if so
+            bool healthyFusion = true;
+            for (uint8_t i= 0; i<=stateIndexLim; i++) {
+                if (KHP[i][i] > P[i][i]) {
+                    healthyFusion = false;
+                }
+            }
+
+            if (healthyFusion) {
+                // update the covariance matrix
+                for (uint8_t i= 0; i<=stateIndexLim; i++) {
+                    for (uint8_t j= 0; j<=stateIndexLim; j++) {
+                        P[i][j] = P[i][j] - KHP[i][j];
+                    }
+                }
+
+                // force the covariance matrix to be symmetrical and limit the variances to prevent ill-condiioning.
+                ForceSymmetry();
+                ConstrainVariances();
+
+                // correct the state vector
+                for (uint8_t j= 0; j<=stateIndexLim; j++) {
+                    statesArray[j] = statesArray[j] - Kfusion[j] * innovBodyVel[obsIndex];
+                }
+                stateStruct.quat.normalize();
+
+            } else {
+                // record bad axis
+                if (obsIndex == 0) {
+                    faultStatus.bad_xvel = true;
+                } else if (obsIndex == 1) {
+                    faultStatus.bad_yvel = true;
+                } else if (obsIndex == 2) {
+                    faultStatus.bad_zvel = true;
+                }
+
+            }
+        }
+    }
+}
+
+// select fusion of body odometry measurements
+void NavEKF3_core::SelectBodyOdomFusion()
+{
+    // Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
+    // If so, don't fuse measurements on this time step to reduce frame over-runs
+    // Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements
+    if (magFusePerformed && (dtIMUavg < 0.005f) && !bodyVelFusionDelayed) {
+        bodyVelFusionDelayed = true;
+        return;
+    } else {
+        bodyVelFusionDelayed = false;
+    }
+
+    // Check for data at the fusion time horizon
+    if (storedBodyOdm.recall(bodyOdmDataDelayed, imuDataDelayed.time_ms)) {
+
+        // start performance timer
+        hal.util->perf_begin(_perf_FuseBodyOdom);
+
+        // Fuse data into the main filter
+        FuseBodyVel();
+
+        // stop the performance timer
+        hal.util->perf_end(_perf_FuseBodyOdom);
+    }
+}
+
 #endif // HAL_CPU_CLASS

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -23,7 +23,8 @@ NavEKF3_core::NavEKF3_core(void) :
     _perf_FuseAirspeed(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseAirspeed")),
     _perf_FuseSideslip(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseSideslip")),
     _perf_TerrainOffset(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_TerrainOffset")),
-    _perf_FuseOptFlow(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseOptFlow"))
+    _perf_FuseOptFlow(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseOptFlow")),
+    _perf_FuseBodyOdom(hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_FuseBodyOdom"))
 {
     _perf_test[0] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_Test0");
     _perf_test[1] = hal.util->perf_alloc(AP_HAL::Util::PC_ELAPSED, "EK3_Test1");
@@ -122,6 +123,9 @@ bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _c
     if(!storedOF.init(obs_buffer_length)) {
         return false;
     }
+    if(!storedBodyOdm.init(obs_buffer_length)) {
+        return false;
+    }
     // Note: the use of dual range finders potentially doubles the amount of data to be stored
     if(!storedRange.init(MIN(2*obs_buffer_length , imu_buffer_length))) {
         return false;
@@ -279,6 +283,7 @@ void NavEKF3_core::InitialiseVariables()
     posDown = 0.0f;
     posVelFusionDelayed = false;
     optFlowFusionDelayed = false;
+    flowFusionActive = false;
     airSpdFusionDelayed = false;
     sideSlipFusionDelayed = false;
     posResetNE.zero();
@@ -356,6 +361,19 @@ void NavEKF3_core::InitialiseVariables()
     bcnPosOffsetNED.zero();
     bcnOriginEstInit = false;
 
+    // body frame displacement fusion
+    memset(&bodyOdmDataNew, 0, sizeof(bodyOdmDataNew));
+    memset(&bodyOdmDataDelayed, 0, sizeof(bodyOdmDataDelayed));
+    bodyOdmStoreIndex = 0;
+    lastbodyVelPassTime_ms = 0;
+    memset(&bodyVelTestRatio, 0, sizeof(bodyVelTestRatio));
+    memset(&varInnovBodyVel, 0, sizeof(varInnovBodyVel));
+    memset(&innovBodyVel, 0, sizeof(innovBodyVel));
+    prevBodyVelFuseTime_ms = 0;
+    bodyOdmMeasTime_ms = 0;
+    bodyVelFusionDelayed = false;
+    bodyVelFusionActive = false;
+
     // zero data buffers
     storedIMU.reset();
     storedGPS.reset();
@@ -365,6 +383,7 @@ void NavEKF3_core::InitialiseVariables()
     storedRange.reset();
     storedOutput.reset();
     storedRangeBeacon.reset();
+    storedBodyOdm.reset();
 }
 
 // Initialise the states from accelerometer and magnetometer data (if present)
@@ -548,6 +567,9 @@ void NavEKF3_core::UpdateFilter(bool predict)
         // Update states using optical flow data
         SelectFlowFusion();
 
+        // Update states using body frame odometry data
+        SelectBodyOdomFusion();
+
         // Update states using airspeed data
         SelectTasFusion();
 

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -202,6 +202,28 @@ public:
     // return data for debugging optical flow fusion
     void getFlowDebug(float &varFlow, float &gndOffset, float &flowInnovX, float &flowInnovY, float &auxInnov, float &HAGL, float &rngInnov, float &range, float &gndOffsetErr) const;
 
+    /*
+     * Write body frame linear and angular displacement measurements from a visual odometry sensor
+     *
+     * quality is a normalised confidence value from 0 to 100
+     * delPos is the XYZ change in linear position meaasured in body frame and relative to the inertial reference at time_ms (m)
+     * delAng is the XYZ angular rotation measured in body frame and relative to the inertial reference at time_ms (rad)
+     * delTime is the time interval for the measurement of delPos and delAng (sec)
+     * timeStamp_ms is the timestamp of the last image used to calculate delPos and delAng (msec)
+     * posOffset is the XYZ body frame position of the camera focal point (m)
+    */
+    void writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset);
+
+    /*
+     * Return data for debugging body frame odometry fusion:
+     *
+     * velInnov are the XYZ body frame velocity innovations (m/s)
+     * velInnovVar are the XYZ body frame velocity innovation variances (m/s)**2
+     *
+     * Return the time stamp of the last odometry fusion update (msec)
+     */
+    uint32_t getBodyFrameOdomDebug(Vector3f &velInnov, Vector3f &velInnovVar);
+
     /*
         Returns the following data for debugging range beacon fusion
         ID : beacon identifier
@@ -376,73 +398,81 @@ private:
     // memory)
     Vector24 statesArray;
     struct state_elements {
-        Quaternion  quat;           // 0..3
-        Vector3f    velocity;       // 4..6
-        Vector3f    position;       // 7..9
-        Vector3f    gyro_bias;      // 10..12
-        Vector3f    accel_bias;     // 13..15
-        Vector3f    earth_magfield; // 16..18
-        Vector3f    body_magfield;  // 19..21
-        Vector2f    wind_vel;       // 22..23
+        Quaternion  quat;           // quaternion defining rotation from local NED earth frame to body frame
+        Vector3f    velocity;       // velocity of IMU in local NED earth frame (m/sec)
+        Vector3f    position;       // position of IMU in local NED earth frame (m)
+        Vector3f    gyro_bias;      // body frame delta angle IMU bias vector (rad)
+        Vector3f    accel_bias;     // body frame delta velocity IMU bias vector (m/sec)
+        Vector3f    earth_magfield; // earth frame magnetic field vector (Gauss)
+        Vector3f    body_magfield;  // body frame magnetic field vector (Gauss)
+        Vector2f    wind_vel;       // horizontal North East wind velocity vector in local NED earth frame (m/sec)
     } &stateStruct;
 
     struct output_elements {
-        Quaternion  quat;           // 0..3
-        Vector3f    velocity;       // 4..6
-        Vector3f    position;       // 7..9
+        Quaternion  quat;           // quaternion defining rotation from local NED earth frame to body frame
+        Vector3f    velocity;       // velocity of body frame origin in local NED earth frame (m/sec)
+        Vector3f    position;       // position of body frame origin in local NED earth frame (m)
     };
 
     struct imu_elements {
-        Vector3f    delAng;         // 0..2
-        Vector3f    delVel;         // 3..5
-        float       delAngDT;       // 6
-        float       delVelDT;       // 7
-        uint32_t    time_ms;        // 8
+        Vector3f    delAng;         // IMU delta angle measurements in body frame (rad)
+        Vector3f    delVel;         // IMU delta velocity measurements in body frame (m/sec)
+        float       delAngDT;       // time interval over which delAng has been measured (sec)
+        float       delVelDT;       // time interval over which delVelDT has been measured (sec)
+        uint32_t    time_ms;        // measurement timestamp (msec)
     };
 
     struct gps_elements {
-        Vector2f    pos;         // 0..1
-        float       hgt;         // 2
-        Vector3f    vel;         // 3..5
-        uint32_t    time_ms;     // 6
-        uint8_t     sensor_idx;  // 7
+        Vector2f    pos;            // horizontal North East position of the GPS antenna in local NED earth frame (m)
+        float       hgt;            // height of the GPS antenna in local NED earth frame (m)
+        Vector3f    vel;            // velocity of the GPS antenna in local NED earth frame (m/sec)
+        uint32_t    time_ms;        // measurement timestamp (msec)
+        uint8_t     sensor_idx;     // unique integer identifying the GPS sensor
     };
 
     struct mag_elements {
-        Vector3f    mag;         // 0..2
-        uint32_t    time_ms;     // 3
+        Vector3f    mag;            // body frame magnetic field measurements (Gauss)
+        uint32_t    time_ms;        // measurement timestamp (msec)
     };
 
     struct baro_elements {
-        float       hgt;         // 0
-        uint32_t    time_ms;     // 1
+        float       hgt;            // height of the pressure sensor in local NED earth frame (m)
+        uint32_t    time_ms;        // measurement timestamp (msec)
     };
 
     struct range_elements {
-        float       rng;         // 0
-        uint32_t    time_ms;     // 1
-        uint8_t     sensor_idx;  // 2
+        float       rng;            // distance measured by the range sensor (m)
+        uint32_t    time_ms;        // measurement timestamp (msec)
+        uint8_t     sensor_idx;     // integer either 0 or 1 uniquely identifying up to two range sensors
     };
 
     struct rng_bcn_elements {
-        float       rng;                // range measurement to each beacon (m)
-        Vector3f    beacon_posNED;      // NED position of the beacon (m)
-        float       rngErr;             // range measurement error 1-std (m)
-        uint8_t     beacon_ID;          // beacon identification number
-        uint32_t    time_ms;            // measurement timestamp (msec)
+        float       rng;            // range measurement to each beacon (m)
+        Vector3f    beacon_posNED;  // NED position of the beacon (m)
+        float       rngErr;         // range measurement error 1-std (m)
+        uint8_t     beacon_ID;      // beacon identification number
+        uint32_t    time_ms;        // measurement timestamp (msec)
     };
 
     struct tas_elements {
-        float       tas;         // 0
-        uint32_t    time_ms;     // 1
+        float       tas;            // true airspeed measurement (m/sec)
+        uint32_t    time_ms;        // measurement timestamp (msec)
     };
 
     struct of_elements {
-        Vector2f    flowRadXY;      // 0..1
-        Vector2f    flowRadXYcomp;  // 2..3
-        uint32_t    time_ms;        // 4
-        Vector3f    bodyRadXYZ;     //8..10
-        const Vector3f *body_offset;// 5..7
+        Vector2f    flowRadXY;      // raw (non motion compensated) optical flow angular rates about the XY body axes (rad/sec)
+        Vector2f    flowRadXYcomp;  // motion compensated XY optical flow angular rates about the XY body axes (rad/sec)
+        uint32_t    time_ms;        // measurement timestamp (msec)
+        Vector3f    bodyRadXYZ;     // body frame XYZ axis angular rates averaged across the optical flow measurement interval (rad/sec)
+        const Vector3f *body_offset;// pointer to XYZ position of the optical flow sensor in body frame (m)
+    };
+
+    struct bfodm_elements {
+        Vector3f        vel;        // XYZ velocity measured in body frame (m/s)
+        float           velErr;     // velocity measurement error 1-std (m/s)
+        const Vector3f *body_offset;// pointer to XYZ position of the velocity sensor in body frame (m)
+        Vector3f        angRate;    // angular rate estimated from odometry (rad/sec)
+        uint32_t        time_ms;    // measurement timestamp (msec)
     };
 
     // update the navigation filter status
@@ -469,6 +499,9 @@ private:
     // fuse selected position, velocity and height measurements
     void FuseVelPosNED();
 
+    // fuse body frame velocity measurements
+    void FuseBodyVel();
+
     // fuse range beacon measurements
     void FuseRngBcn();
 
@@ -624,12 +657,18 @@ private:
     // return true if the filter is ready to start using optical flow measurements
     bool readyToUseOptFlow(void) const;
 
+    // return true if the filter is ready to start using body frame odometry measurements
+    bool readyToUseBodyOdm(void) const;
+
     // return true if we should use the range finder sensor
     bool useRngFinder(void) const;
 
     // determine when to perform fusion of optical flow measurements
     void SelectFlowFusion();
 
+    // determine when to perform fusion of body frame odometry measurements
+    void SelectBodyOdomFusion();
+
     // Estimate terrain offset using a single state EKF
     void EstimateTerrainOffset();
 
@@ -986,8 +1025,9 @@ private:
     bool gndOffsetValid;            // true when the ground offset state can still be considered valid
     Vector3f delAngBodyOF;          // bias corrected delta angle of the vehicle IMU measured summed across the time since the last OF measurement
     float delTimeOF;                // time that delAngBodyOF is summed across
-    Vector3f accelPosOffset;        // position of IMU accelerometer unit in body frame (m)
+    bool flowFusionActive;          // true when optical flow fusion is active
 
+    Vector3f accelPosOffset;        // position of IMU accelerometer unit in body frame (m)
 
     // Range finder
     float baroHgtOffset;                    // offset applied when when switching to use of Baro height
@@ -999,6 +1039,21 @@ private:
     bool terrainHgtStable;                  // true when the terrain height is stable enough to be used as a height reference
     uint32_t terrainHgtStableSet_ms;        // system time at which terrainHgtStable was set
 
+    // body frame odometry fusion
+    obs_ring_buffer_t<bfodm_elements> storedBodyOdm;    // body velocity data buffer
+    bfodm_elements bodyOdmDataNew;       // Body frame odometry data at the current time horizon
+    bfodm_elements bodyOdmDataDelayed;  // Body  frame odometry data at the fusion time horizon
+    uint8_t bodyOdmStoreIndex;          // Body  frame odometry  data storage index
+    uint32_t lastbodyVelPassTime_ms;    // time stamp when the body velocity measurement last passed innvovation consistency checks (msec)
+    Vector3 bodyVelTestRatio;           // Innovation test ratios for body velocity XYZ measurements
+    Vector3 varInnovBodyVel;            // Body velocity XYZ innovation variances (rad/sec)^2
+    Vector3 innovBodyVel;               // Body velocity XYZ innovations (rad/sec)
+    uint32_t prevBodyVelFuseTime_ms;    // previous time all body velocity measurement components passed their innovation consistency checks (msec)
+    uint32_t bodyOdmMeasTime_ms;        // time body velocity measurements were accepted for input to the data buffer (msec)
+    bool bodyVelFusionDelayed;          // true when body frame velocity fusion has been delayed
+    bool bodyVelFusionActive;           // true when body frame velocity fusion is active
+
+
     // Range Beacon Sensor Fusion
     obs_ring_buffer_t<rng_bcn_elements> storedRangeBeacon; // Beacon range buffer
     rng_bcn_elements rngBcnDataNew;     // Range beacon data at the current time horizon
@@ -1094,6 +1149,9 @@ private:
         bool bad_xflow:1;
         bool bad_yflow:1;
         bool bad_rngbcn:1;
+        bool bad_xvel:1;
+        bool bad_yvel:1;
+        bool bad_zvel:1;
     } faultStatus;
 
     // flags indicating which GPS quality checks are failing
@@ -1144,6 +1202,7 @@ private:
     AP_HAL::Util::perf_counter_t  _perf_FuseSideslip;
     AP_HAL::Util::perf_counter_t  _perf_TerrainOffset;
     AP_HAL::Util::perf_counter_t  _perf_FuseOptFlow;
+    AP_HAL::Util::perf_counter_t  _perf_FuseBodyOdom;
     AP_HAL::Util::perf_counter_t  _perf_test[10];
 
     // should we assume zero sideslip?