Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: added inactive bias learning

Browse Source 
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
This commit is contained in:
Andrew Tridgell 2019-07-05 18:29:19 +10:00
parent 1c4c5a6850
commit 209bca162c
3 changed files with 148 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

120
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -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();
}
imuDataNew.delAngDT = MAX(ins.get_delta_angle_dt(imu_index),1.0e-4f);
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;
}
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);
}
}
}

25
libraries/AP_NavEKF3/AP_NavEKF3_core.cpp
View File

@ -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;

26
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -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();