forked from Archive/PX4-Autopilot
EKF: Reduce EKF-GSF vulnerability to large yaw gyro bias errors (#831)
* EKF: Reduce EKF-GSF vulnerability to large yaw gyro bias errors * Update EKF/ekf_helper.cpp * EKF: Fix comment typo Co-authored-by: Roman Bapst <bapstroman@gmail.com> Co-authored-by: Mathieu Bresciani <brescianimathieu@gmail.com> Co-authored-by: Roman Bapst <bapstroman@gmail.com>
This commit is contained in:
parent
d0ddda8917
commit
59183f70ba
|
@ -14,8 +14,9 @@ EKFGSF_yaw::EKFGSF_yaw()
|
|||
}
|
||||
|
||||
void EKFGSF_yaw::update(const imuSample& imu_sample,
|
||||
bool run_EKF, // set to true when flying or movement is suitable for yaw estimation
|
||||
float airspeed) // true airspeed used for centripetal accel compensation - set to 0 when not required.
|
||||
bool run_EKF, // set to true when flying or movement is suitable for yaw estimation
|
||||
float airspeed, // true airspeed used for centripetal accel compensation - set to 0 when not required.
|
||||
const Vector3f &imu_gyro_bias) // estimated rate gyro bias (rad/sec)
|
||||
{
|
||||
// copy to class variables
|
||||
_delta_ang = imu_sample.delta_ang;
|
||||
|
@ -60,6 +61,11 @@ void EKFGSF_yaw::update(const imuSample& imu_sample,
|
|||
if (!_ekf_gsf_vel_fuse_started) {
|
||||
initialiseEKFGSF();
|
||||
ahrsAlignYaw();
|
||||
// Initialise to gyro bias estimate from main filter because there could be a large
|
||||
// uncorrected rate gyro bias error about the gravity vector
|
||||
for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index ++) {
|
||||
_ahrs_ekf_gsf[model_index].gyro_bias = imu_gyro_bias;
|
||||
}
|
||||
_ekf_gsf_vel_fuse_started = true;
|
||||
} else {
|
||||
bool bad_update = false;
|
||||
|
|
|
@ -31,11 +31,12 @@ public:
|
|||
|
||||
// Update Filter States - this should be called whenever new IMU data is available
|
||||
void update(const imuSample &imu_sample,
|
||||
bool run_EKF, // set to true when flying or movement is suitable for yaw estimation
|
||||
float airspeed); // true airspeed used for centripetal accel compensation - set to 0 when not required.
|
||||
bool run_EKF, // set to true when flying or movement is suitable for yaw estimation
|
||||
float airspeed, // true airspeed used for centripetal accel compensation - set to 0 when not required.
|
||||
const Vector3f &imu_gyro_bias); // estimated rate gyro bias (rad/sec)
|
||||
|
||||
void setVelocity(const Vector2f &velocity, // NE velocity measurement (m/s)
|
||||
float accuracy); // 1-sigma accuracy of velocity measurement (m/s)
|
||||
void setVelocity(const Vector2f &velocity, // NE velocity measurement (m/s)
|
||||
float accuracy); // 1-sigma accuracy of velocity measurement (m/s)
|
||||
|
||||
// get solution data for logging
|
||||
bool getLogData(float *yaw_composite,
|
||||
|
|
|
@ -1858,7 +1858,8 @@ void Ekf::runYawEKFGSF()
|
|||
TAS = _airspeed_sample_delayed.true_airspeed;
|
||||
}
|
||||
|
||||
yawEstimator.update(_imu_sample_delayed, _control_status.flags.in_air, TAS);
|
||||
const Vector3f imu_gyro_bias = getGyroBias();
|
||||
yawEstimator.update(_imu_sample_delayed, _control_status.flags.in_air, TAS, imu_gyro_bias);
|
||||
|
||||
// basic sanity check on GPS velocity data
|
||||
if (_gps_data_ready && _gps_sample_delayed.vacc > FLT_EPSILON && ISFINITE(_gps_sample_delayed.vel(0)) && ISFINITE(_gps_sample_delayed.vel(1))) {
|
||||
|
|
Loading…
Reference in New Issue