AP_NavEKF3: added inactive bias learning

this allows for biases on inactive IMUs to be learned so that an IMU
switch within an EKF lane does not cause a jump in the IMU data

libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

@@ -351,22 +351,46 @@ void NavEKF3_core::readIMUData()
     // the imu sample time is used as a common time reference throughout the filter
     imuSampleTime_ms = frontend->imuSampleTime_us / 1000;
 
-    // use the nominated imu or primary if not available
+    uint8_t accel_active, gyro_active;
+
     if (ins.use_accel(imu_index)) {
-        readDeltaVelocity(imu_index, imuDataNew.delVel, imuDataNew.delVelDT);
-        accelPosOffset = ins.get_imu_pos_offset(imu_index);
+        accel_active = imu_index;
     } else {
-        readDeltaVelocity(ins.get_primary_accel(), imuDataNew.delVel, imuDataNew.delVelDT);
-        accelPosOffset = ins.get_imu_pos_offset(ins.get_primary_accel());
+        accel_active = ins.get_primary_accel();
     }
 
-    // Get delta angle data from primary gyro or primary if not available
     if (ins.use_gyro(imu_index)) {
-        readDeltaAngle(imu_index, imuDataNew.delAng);
+        gyro_active = imu_index;
     } else {
-        readDeltaAngle(ins.get_primary_gyro(), imuDataNew.delAng);
+        gyro_active = ins.get_primary_gyro();
+    }
+
+    if (gyro_active != gyro_index_active) {
+        // we are switching active gyro at runtime. Copy over the
+        // bias we have learned from the previously inactive
+        // gyro. We don't re-init the bias uncertainty as it should
+        // have the same uncertainty as the previously active gyro
+        stateStruct.gyro_bias = inactiveBias[gyro_active].gyro_bias;
+        gyro_index_active = gyro_active;
     }
-    imuDataNew.delAngDT = MAX(ins.get_delta_angle_dt(imu_index),1.0e-4f);
+
+    if (accel_active != accel_index_active) {
+        // switch to the learned accel bias for this IMU
+        stateStruct.accel_bias = inactiveBias[accel_active].accel_bias;
+        accel_index_active = accel_active;
+    }
+
+    // update the inactive bias states
+    learnInactiveBiases();
+
+    readDeltaVelocity(accel_index_active, imuDataNew.delVel, imuDataNew.delVelDT);
+    accelPosOffset = ins.get_imu_pos_offset(accel_index_active);
+    imuDataNew.accel_index = accel_index_active;
+    
+    // Get delta angle data from primary gyro or primary if not available
+    readDeltaAngle(gyro_index_active, imuDataNew.delAng);
+    imuDataNew.delAngDT = MAX(ins.get_delta_angle_dt(gyro_index_active),1.0e-4f);
+    imuDataNew.gyro_index = gyro_index_active;
 
     // Get current time stamp
     imuDataNew.time_ms = imuSampleTime_ms;
@@ -375,6 +399,12 @@ void NavEKF3_core::readIMUData()
     imuDataDownSampledNew.delAngDT += imuDataNew.delAngDT;
     imuDataDownSampledNew.delVelDT += imuDataNew.delVelDT;
 
+    // use the most recent IMU index for the downsampled IMU
+    // data. This isn't strictly correct if we switch IMUs between
+    // samples
+    imuDataDownSampledNew.gyro_index = imuDataNew.gyro_index;
+    imuDataDownSampledNew.accel_index = imuDataNew.accel_index;
+
     // Rotate quaternon atitude from previous to new and normalise.
     // Accumulation using quaternions prevents introduction of coning errors due to downsampling
     imuQuatDownSampleNew.rotate(imuDataNew.delAng);
@@ -439,8 +469,8 @@ void NavEKF3_core::readIMUData()
         // correct the extracted IMU data for sensor errors
         delAngCorrected = imuDataDelayed.delAng;
         delVelCorrected = imuDataDelayed.delVel;
-        correctDeltaAngle(delAngCorrected, imuDataDelayed.delAngDT);
-        correctDeltaVelocity(delVelCorrected, imuDataDelayed.delVelDT);
+        correctDeltaAngle(delAngCorrected, imuDataDelayed.delAngDT, imuDataDelayed.gyro_index);
+        correctDeltaVelocity(delVelCorrected, imuDataDelayed.delVelDT, imuDataDelayed.accel_index);
 
     } else {
         // we don't have new IMU data in the buffer so don't run filter updates on this time step
@@ -870,3 +900,71 @@ void NavEKF3_core::getTimingStatistics(struct ekf_timing &_timing)
     memset(&timing, 0, sizeof(timing));
 }
 
+/*
+  update estimates of inactive bias states. This keeps inactive IMUs
+  as hot-spares so we can switch to them without causing a jump in the
+  error
+ */
+void NavEKF3_core::learnInactiveBiases(void)
+{
+    const AP_InertialSensor &ins = AP::ins();
+
+    // learn gyro biases
+    for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
+        if (!ins.use_gyro(i)) {
+            // can't use this gyro
+            continue;
+        }
+        if (gyro_index_active == i) {
+            // use current estimates from main filter of gyro bias
+            inactiveBias[i].gyro_bias = stateStruct.gyro_bias;
+        } else {
+            // get filtered gyro and use the difference between the
+            // corrected gyro on the active IMU and the inactive IMU
+            // to move the inactive bias towards the right value
+            Vector3f filtered_gyro_active = ins.get_gyro(gyro_index_active) - (stateStruct.gyro_bias/dtEkfAvg);
+            Vector3f filtered_gyro_inactive = ins.get_gyro(i) - (inactiveBias[i].gyro_bias/dtEkfAvg);
+            Vector3f error = filtered_gyro_active - filtered_gyro_inactive;
+
+            // prevent a single large error from contaminating bias estimate
+            const float bias_limit = radians(5);
+            error.x = constrain_float(error.x, -bias_limit, bias_limit);
+            error.y = constrain_float(error.y, -bias_limit, bias_limit);
+            error.z = constrain_float(error.z, -bias_limit, bias_limit);
+
+            // slowly bring the inactive gyro in line with the active gyro. This corrects a 5 deg/sec
+            // gyro bias error in around 1 minute
+            inactiveBias[i].gyro_bias -= error * (1.0e-4f * dtEkfAvg);
+        }
+    }
+
+    // learn accel biases
+    for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
+        if (!ins.use_accel(i)) {
+            // can't use this accel
+            continue;
+        }
+        if (accel_index_active == i) {
+            // use current estimates from main filter of accel bias
+            inactiveBias[i].accel_bias = stateStruct.accel_bias;
+        } else {
+            // get filtered accel and use the difference between the
+            // corrected accel on the active IMU and the inactive IMU
+            // to move the inactive bias towards the right value
+            Vector3f filtered_accel_active = ins.get_accel(accel_index_active) - (stateStruct.accel_bias/dtEkfAvg);
+            Vector3f filtered_accel_inactive = ins.get_accel(i) - (inactiveBias[i].accel_bias/dtEkfAvg);
+            Vector3f error = filtered_accel_active - filtered_accel_inactive;
+
+            // prevent a single large error from contaminating bias estimate
+            const float bias_limit = 1.0; // m/s/s
+            error.x = constrain_float(error.x, -bias_limit, bias_limit);
+            error.y = constrain_float(error.y, -bias_limit, bias_limit);
+            error.z = constrain_float(error.z, -bias_limit, bias_limit);
+
+            // slowly bring the inactive accel in line with the active
+            // accel. This corrects a 0.5 m/s/s accel bias error in
+            // around 1 minute
+            inactiveBias[i].accel_bias -= error * (1.0e-4f * dtEkfAvg);
+        }
+    }
+}

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -40,6 +40,8 @@ bool NavEKF3_core::setup_core(NavEKF3 *_frontend, uint8_t _imu_index, uint8_t _c
 {
     frontend = _frontend;
     imu_index = _imu_index;
+    gyro_index_active = imu_index;
+    accel_index_active = imu_index;
     core_index = _core_index;
     _ahrs = frontend->_ahrs;
 
@@ -290,6 +292,8 @@ void NavEKF3_core::InitialiseVariables()
     imuDataDownSampledNew.delVel.zero();
     imuDataDownSampledNew.delAngDT = 0.0f;
     imuDataDownSampledNew.delVelDT = 0.0f;
+    imuDataDownSampledNew.gyro_index = gyro_index_active;
+    imuDataDownSampledNew.accel_index = accel_index_active;
     runUpdates = false;
     framesSincePredict = 0;
     gpsYawResetRequest = false;
@@ -445,7 +449,7 @@ bool NavEKF3_core::InitialiseFilterBootstrap(void)
     Vector3f initAccVec;
 
     // TODO we should average accel readings over several cycles
-    initAccVec = AP::ins().get_accel(imu_index);
+    initAccVec = AP::ins().get_accel(accel_index_active);
 
     // normalise the acceleration vector
     float pitch=0, roll=0;
@@ -489,6 +493,13 @@ bool NavEKF3_core::InitialiseFilterBootstrap(void)
 
     // set to true now that states have be initialised
     statesInitialised = true;
+
+    // reset inactive biases
+    for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
+        inactiveBias[i].gyro_bias.zero();
+        inactiveBias[i].accel_bias.zero();
+    }
+
     gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u initialised",(unsigned)imu_index);
 
     // we initially return false to wait for the IMU buffer to fill
@@ -613,14 +624,14 @@ void NavEKF3_core::UpdateFilter(bool predict)
 #endif
 }
 
-void NavEKF3_core::correctDeltaAngle(Vector3f &delAng, float delAngDT)
+void NavEKF3_core::correctDeltaAngle(Vector3f &delAng, float delAngDT, uint8_t gyro_index)
 {
-    delAng -= stateStruct.gyro_bias * (delAngDT / dtEkfAvg);
+    delAng -= inactiveBias[gyro_index].gyro_bias * (delAngDT / dtEkfAvg);
 }
 
-void NavEKF3_core::correctDeltaVelocity(Vector3f &delVel, float delVelDT)
+void NavEKF3_core::correctDeltaVelocity(Vector3f &delVel, float delVelDT, uint8_t accel_index)
 {
-    delVel -= stateStruct.accel_bias * (delVelDT / dtEkfAvg);
+    delVel -= inactiveBias[accel_index].accel_bias * (delVelDT / dtEkfAvg);
 }
 
 /*
@@ -708,8 +719,8 @@ void NavEKF3_core::calcOutputStates()
     // apply corrections to the IMU data
     Vector3f delAngNewCorrected = imuDataNew.delAng;
     Vector3f delVelNewCorrected = imuDataNew.delVel;
-    correctDeltaAngle(delAngNewCorrected, imuDataNew.delAngDT);
-    correctDeltaVelocity(delVelNewCorrected, imuDataNew.delVelDT);
+    correctDeltaAngle(delAngNewCorrected, imuDataNew.delAngDT, imuDataNew.gyro_index);
+    correctDeltaVelocity(delVelNewCorrected, imuDataNew.delVelDT, imuDataNew.accel_index);
 
     // apply corrections to track EKF solution
     Vector3f delAng = delAngNewCorrected + delAngCorrection;

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -28,6 +28,7 @@
 #include "AP_NavEKF3.h"
 #include <AP_Math/vectorN.h>
 #include <AP_NavEKF3/AP_NavEKF3_Buffer.h>
+#include <AP_InertialSensor/AP_InertialSensor.h>
 
 // GPS pre-flight check bit locations
 #define MASK_GPS_NSATS      (1<<0)
@@ -344,8 +345,9 @@ public:
     // publish output observer angular, velocity and position tracking error
     void getOutputTrackingError(Vector3f &error) const;
 
-    // get the IMU index
-    uint8_t getIMUIndex(void) const { return imu_index; }
+    // get the IMU index. For now we return the gyro index, as that is most
+    // critical for use by other subsystems.
+    uint8_t getIMUIndex(void) const { return gyro_index_active; }
 
     // get timing statistics structure
     void getTimingStatistics(struct ekf_timing &timing);
@@ -353,7 +355,9 @@ public:
 private:
     // Reference to the global EKF frontend for parameters
     NavEKF3 *frontend;
-    uint8_t imu_index;
+    uint8_t imu_index; // preferred IMU index
+    uint8_t gyro_index_active; // active gyro index (in case preferred fails)
+    uint8_t accel_index_active; // active accel index (in case preferred fails)
     uint8_t core_index;
     uint8_t imu_buffer_length;
     uint8_t obs_buffer_length;
@@ -443,6 +447,8 @@ private:
         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)
+        uint8_t     gyro_index;
+        uint8_t     accel_index;
     };
 
     struct gps_elements {
@@ -506,6 +512,13 @@ private:
         uint32_t        time_ms;    // measurement timestamp (msec)
     };
 
+    // bias estimates for the IMUs that are enabled but not being used
+    // by this core.
+    struct {
+        Vector3f gyro_bias;
+        Vector3f accel_bias;
+    } inactiveBias[INS_MAX_INSTANCES];
+
     // update the navigation filter status
     void updateFilterStatus(void);
 
@@ -612,12 +625,15 @@ private:
     bool readDeltaAngle(uint8_t ins_index, Vector3f &dAng);
 
     // helper functions for correcting IMU data
-    void correctDeltaAngle(Vector3f &delAng, float delAngDT);
-    void correctDeltaVelocity(Vector3f &delVel, float delVelDT);
+    void correctDeltaAngle(Vector3f &delAng, float delAngDT, uint8_t gyro_index);
+    void correctDeltaVelocity(Vector3f &delVel, float delVelDT, uint8_t accel_index);
 
     // update IMU delta angle and delta velocity measurements
     void readIMUData();
 
+    // update estimate of inactive bias states
+    void learnInactiveBiases();
+
     // check for new valid GPS data and update stored measurement if available
     void readGpsData();