AP_AHRS: use current EKF2 IMU core in gyro estimate

The EKF2 implementation uses one IMU per EKF2 core. When reporting the
gyro estimate, accel_ef estimate and gyro bias estimate we need to use
the values associated with the current IMU index being used by the
current EKF2 core. Otherwise we will have an inconsistency between the
gyro estimate and attitude estimate

This affects all multi-IMU systems using EKF2

libraries/AP_AHRS/AP_AHRS_NavEKF.cpp

@@ -217,39 +217,37 @@ void AP_AHRS_NavEKF::update_EKF2(void)
             update_trig();
 
             // keep _gyro_bias for get_gyro_drift()
+            _gyro_bias.zero();            
             EKF2.getGyroBias(-1,_gyro_bias);
             _gyro_bias = -_gyro_bias;
 
             // calculate corrected gryo estimate for get_gyro()
             _gyro_estimate.zero();
-            uint8_t healthy_count = 0;
+
-            for (uint8_t i=0; i<_ins.get_gyro_count(); i++) {
+            // the gyro bias applies only to the IMU associated with the primary EKF2
-                if (_ins.get_gyro_health(i) && healthy_count < 2 && _ins.use_gyro(i)) {
+            // core
-                    _gyro_estimate += _ins.get_gyro(i);
+            int8_t primary_imu = EKF2.getPrimaryCoreIMUIndex();
-                    healthy_count++;
+            if (primary_imu == -1) {
-                }
+                _gyro_estimate = _ins.get_gyro();
-            }
+            } else {
-            if (healthy_count > 1) {
+                _gyro_estimate = _ins.get_gyro(primary_imu);
-                _gyro_estimate /= healthy_count;
             }
             _gyro_estimate += _gyro_bias;
 
-            float abias;
+            float abias = 0;
             EKF2.getAccelZBias(-1,abias);
 
-            // This EKF uses the primary IMU
+            // This EKF is currently using primary_imu, and abias applies to only that IMU
-            // Eventually we will run a separate instance of the EKF for each IMU and do the selection and blending of EKF outputs upstream
-            // update _accel_ef_ekf
             for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
                 Vector3f accel = _ins.get_accel(i);
-                if (i==_ins.get_primary_accel()) {
+                if (i == primary_imu) {
                     accel.z -= abias;
                 }
                 if (_ins.get_accel_health(i)) {
                     _accel_ef_ekf[i] = _dcm_matrix * accel;
                 }
             }
-            _accel_ef_ekf_blended = _accel_ef_ekf[_ins.get_primary_accel()];
+            _accel_ef_ekf_blended = _accel_ef_ekf[primary_imu>=0?primary_imu:_ins.get_primary_accel()];
         }
     }
 }
@@ -1397,5 +1395,23 @@ bool AP_AHRS_NavEKF::have_ekf_logging(void) const
     return false;
 }
 
+// get earth-frame accel vector for primary IMU
+const Vector3f &AP_AHRS_NavEKF::get_accel_ef() const
+{
+    int8_t imu = -1;
+    switch (ekf_type()) {
+    case 2:
+        // let EKF2 choose primary IMU
+        imu = EKF2.getPrimaryCoreIMUIndex();
+        break;
+    default:
+        break;
+    }
+    if (imu == -1) {
+        imu = _ins.get_primary_accel();
+    }
+    return get_accel_ef(imu);
+}
+
 #endif // AP_AHRS_NAVEKF_AVAILABLE
 

libraries/AP_AHRS/AP_AHRS_NavEKF.h

@@ -63,11 +63,11 @@ public:
                    NavEKF &_EKF1, NavEKF2 &_EKF2, Flags flags = FLAG_NONE);
 
     // return the smoothed gyro vector corrected for drift
-    const Vector3f &get_gyro(void) const;
+    const Vector3f &get_gyro(void) const override;
-    const Matrix3f &get_rotation_body_to_ned(void) const;
+    const Matrix3f &get_rotation_body_to_ned(void) const override;
 
     // return the current drift correction integrator value
-    const Vector3f &get_gyro_drift(void) const;
+    const Vector3f &get_gyro_drift(void) const override;
 
     // reset the current gyro drift estimate
     //  should be called if gyro offsets are recalculated
@@ -123,10 +123,8 @@ public:
     // EKF has a better ground speed vector estimate
     Vector2f groundspeed_vector(void);
 
-    const Vector3f &get_accel_ef(uint8_t i) const;
+    const Vector3f &get_accel_ef(uint8_t i) const override;
-    const Vector3f &get_accel_ef() const {
+    const Vector3f &get_accel_ef() const override;
-        return get_accel_ef(_ins.get_primary_accel());
-    };
 
     // blended accelerometer values in the earth frame in m/s/s
     const Vector3f &get_accel_ef_blended(void) const;