aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_AHRS: use current EKF2 IMU core in gyro estimate

Browse Source 
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
This commit is contained in:
Andrew Tridgell 2016-09-03 17:12:39 +10:00
parent dd812cfc0c
commit c62fc336cb
2 changed files with 36 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
libraries/AP_AHRS/AP_AHRS_NavEKF.cpp
View File

@ -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++) {
if (_ins.get_gyro_health(i) && healthy_count < 2 && _ins.use_gyro(i)) {
_gyro_estimate += _ins.get_gyro(i);
healthy_count++;
}
}
if (healthy_count > 1) {
_gyro_estimate /= healthy_count;
// the gyro bias applies only to the IMU associated with the primary EKF2
// core
int8_t primary_imu = EKF2.getPrimaryCoreIMUIndex();
if (primary_imu == -1) {
_gyro_estimate = _ins.get_gyro();
} else {
_gyro_estimate = _ins.get_gyro(primary_imu);
}
_gyro_estimate += _gyro_bias;
float abias;
float abias = 0;
EKF2.getAccelZBias(-1,abias);
// This EKF uses the primary 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
// This EKF is currently using primary_imu, and abias applies to only that IMU
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

12
libraries/AP_AHRS/AP_AHRS_NavEKF.h
View File

@ -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 Matrix3f &get_rotation_body_to_ned(void) const;
const Vector3f &get_gyro(void) const override;
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() const {
return get_accel_ef(_ins.get_primary_accel());
};
const Vector3f &get_accel_ef(uint8_t i) const override;
const Vector3f &get_accel_ef() const override;
// blended accelerometer values in the earth frame in m/s/s
const Vector3f &get_accel_ef_blended(void) const;