Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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
This commit is contained in:
Andrew Tridgell 2019-07-06 08:33:59 +10:00
parent a91af12364
commit 9f4085b0bc
3 changed files with 156 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

126
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -293,24 +293,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;
@ -357,6 +394,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;
@ -380,8 +419,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
@ -540,7 +579,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;
}
@ -832,4 +871,71 @@ void NavEKF2_core::writeExtNavData(const Vector3f &sensOffset, const Vector3f &p
}
/*
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<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 and scale
inactiveBias[i].gyro_bias = stateStruct.gyro_bias;
inactiveBias[i].gyro_scale = stateStruct.gyro_scale;
} 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 estimate from main filter
inactiveBias[i].accel_zbias = stateStruct.accel_zbias;
} 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
float filtered_accel_active = ins.get_accel(accel_index_active).z - (stateStruct.accel_zbias/dtEkfAvg);
float filtered_accel_inactive = ins.get_accel(i).z - (inactiveBias[i].accel_zbias/dtEkfAvg);
float error = filtered_accel_active - filtered_accel_inactive;
// prevent a single large error from contaminating bias estimate
const float bias_limit = 1; // m/s/s
error = constrain_float(error, -bias_limit, bias_limit);
// slowly bring the inactive accel in line with the active accel
// this learns 0.5m/s/s bias in about 1 minute
inactiveBias[i].accel_zbias -= error * (1.0e-4f * dtEkfAvg);
}
}
}
#endif // HAL_CPU_CLASS

25
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -49,6 +49,8 @@ bool NavEKF2_core::setup_core(NavEKF2 *_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;
@ -378,7 +380,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();
@ -434,6 +436,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<INS_MAX_INSTANCES; i++) {
inactiveBias[i].gyro_bias.zero();
inactiveBias[i].accel_zbias = 0;
inactiveBias[i].gyro_scale.x = 1;
inactiveBias[i].gyro_scale.y = 1;
inactiveBias[i].gyro_scale.z = 1;
}
// we initially return false to wait for the IMU buffer to fill
return false;
}
@ -553,17 +564,17 @@ void NavEKF2_core::UpdateFilter(bool predict)
#endif
}
void NavEKF2_core::correctDeltaAngle(Vector3f &delAng, float delAngDT)
void NavEKF2_core::correctDeltaAngle(Vector3f &delAng, float delAngDT, uint8_t gyro_index)
{
delAng.x = delAng.x * stateStruct.gyro_scale.x;
delAng.y = delAng.y * stateStruct.gyro_scale.y;
delAng.z = delAng.z * stateStruct.gyro_scale.z;
delAng -= stateStruct.gyro_bias * (delAngDT / dtEkfAvg);
delAng -= inactiveBias[gyro_index].gyro_bias * (delAngDT / dtEkfAvg);
}
void NavEKF2_core::correctDeltaVelocity(Vector3f &delVel, float delVelDT)
void NavEKF2_core::correctDeltaVelocity(Vector3f &delVel, float delVelDT, uint8_t accel_index)
{
delVel.z -= stateStruct.accel_zbias * (delVelDT / dtEkfAvg);
delVel.z -= inactiveBias[accel_index].accel_zbias * (delVelDT / dtEkfAvg);
}
/*
@ -646,8 +657,8 @@ void NavEKF2_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 corections to track EKF solution
Vector3f delAng = delAngNewCorrected + delAngCorrection;

27
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -30,6 +30,7 @@
#include <stdio.h>
#include <AP_Math/vectorN.h>
#include <AP_NavEKF2/AP_NavEKF2_Buffer.h>
#include <AP_InertialSensor/AP_InertialSensor.h>
// GPS pre-flight check bit locations
#define MASK_GPS_NSATS (1<<0)
@ -301,8 +302,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);
@ -324,7 +326,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;
@ -409,6 +413,8 @@ private:
float delAngDT; // 6
float delVelDT; // 7
uint32_t time_ms; // 8
uint8_t gyro_index;
uint8_t accel_index;
};
struct gps_elements {
@ -468,6 +474,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);
@ -574,12 +588,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();