From 1c4c5a6850fd0f66c05f3bc02b66f5ee2247150f Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 4 Jul 2019 17:11:57 +1000 Subject: [PATCH] AP_NavEKF2: learn gyro biases for inactive gyros this allows us to learn the gyro biases each lane would need if it had to switch to another gyro due to a sensor failure. This prevents a sudden change in gyro bias on IMU failure --- .../AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 127 ++++++++++++++++-- libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 25 +++- libraries/AP_NavEKF2/AP_NavEKF2_core.h | 27 +++- 3 files changed, 157 insertions(+), 22 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index b418cdbcff..d64b87b225 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -307,24 +307,61 @@ void NavEKF2_core::readIMUData() imuSampleTime_ms = AP_HAL::millis(); // 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, imuDataNew.delAngDT); + gyro_active = imu_index; } else { - readDeltaAngle(ins.get_primary_gyro(), imuDataNew.delAng, imuDataNew.delAngDT); + gyro_active = ins.get_primary_gyro(); + } + + if (gyro_active != gyro_index_active) { + // we are switching active gyro at runtime. Copy over the + // biases 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; + + // use the gyro scale factor we have previously used on this + // IMU (if any). We don't reset the variances as we don't want + // errors after switching to be mis-assigned to the gyro scale + // factor + stateStruct.gyro_scale = inactiveBias[gyro_active].gyro_scale; } + if (accel_active != accel_index_active) { + // switch to the learned accel bias for this IMU + stateStruct.accel_zbias = inactiveBias[accel_active].accel_zbias; + 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); + imuDataNew.gyro_index = gyro_index_active; + // Get current time stamp imuDataNew.time_ms = imuSampleTime_ms; + // 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; + // Accumulate the measurement time interval for the delta velocity and angle data imuDataDownSampledNew.delAngDT += imuDataNew.delAngDT; imuDataDownSampledNew.delVelDT += imuDataNew.delVelDT; @@ -371,6 +408,8 @@ void NavEKF2_core::readIMUData() imuDataDownSampledNew.delVel.zero(); imuDataDownSampledNew.delAngDT = 0.0f; imuDataDownSampledNew.delVelDT = 0.0f; + imuDataDownSampledNew.gyro_index = gyro_index_active; + imuDataDownSampledNew.accel_index = accel_index_active; imuQuatDownSampleNew[0] = 1.0f; imuQuatDownSampleNew[3] = imuQuatDownSampleNew[2] = imuQuatDownSampleNew[1] = 0.0f; @@ -394,8 +433,8 @@ void NavEKF2_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 @@ -568,7 +607,7 @@ bool NavEKF2_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng, float &dAng if (ins_index < ins.get_gyro_count()) { ins.get_delta_angle(ins_index,dAng); frontend->logging.log_imu = true; - dAng_dt = MAX(ins.get_delta_angle_dt(imu_index),1.0e-4f); + dAng_dt = MAX(ins.get_delta_angle_dt(ins_index),1.0e-4f); dAng_dt = MIN(dAng_dt,1.0e-1f); return true; } @@ -885,3 +924,71 @@ float NavEKF2_core::MagDeclination(void) const } return _ahrs->get_compass()->get_declination(); } + +/* + 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 NavEKF2_core::learnInactiveBiases(void) +{ + const AP_InertialSensor &ins = AP::ins(); + + // learn gyro biases + for (uint8_t i=0; i_ahrs; @@ -393,7 +395,7 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void) Vector3f initAccVec; // TODO we should average accel readings over several cycles - initAccVec = ins.get_accel(imu_index); + initAccVec = ins.get_accel(accel_index_active); // read the magnetometer data readMagData(); @@ -449,6 +451,15 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void) // set to true now that states have be initialised statesInitialised = true; + // reset inactive biases + for (uint8_t i=0; i #include #include +#include // GPS pre-flight check bit locations #define MASK_GPS_NSATS (1<<0) @@ -302,8 +303,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); @@ -325,7 +327,9 @@ public: private: // Reference to the global EKF frontend for parameters NavEKF2 *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; @@ -414,6 +418,8 @@ private: float delAngDT; // 6 float delVelDT; // 7 uint32_t time_ms; // 8 + uint8_t gyro_index; + uint8_t accel_index; }; struct gps_elements { @@ -473,6 +479,14 @@ private: bool posReset; // true when the position measurement has been reset }; + // bias estimates for the IMUs that are enabled but not being used + // by this core. + struct { + Vector3f gyro_bias; + Vector3f gyro_scale; + float accel_zbias; + } inactiveBias[INS_MAX_INSTANCES]; + // update the navigation filter status void updateFilterStatus(void); @@ -582,12 +596,15 @@ private: bool readDeltaAngle(uint8_t ins_index, Vector3f &dAng, float &dAng_dt); // 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();