diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 5ee6ee34e5..2aaa41ebc6 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -261,7 +261,8 @@ bool Ekf::initialiseTilt() void Ekf::predictState() { // apply imu bias corrections - Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _state.delta_ang_bias; + const Vector3f delta_ang_bias_scaled = (_state.delta_ang_bias / _dt_ekf_avg) * _imu_sample_delayed.delta_ang_dt; + Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - delta_ang_bias_scaled; // subtract component of angular rate due to earth rotation corrected_delta_ang -= _R_to_earth.transpose() * _earth_rate_NED * _imu_sample_delayed.delta_ang_dt; @@ -273,7 +274,8 @@ void Ekf::predictState() _R_to_earth = Dcmf(_state.quat_nominal); // Calculate an earth frame delta velocity - const Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel - _state.delta_vel_bias; + const Vector3f delta_vel_bias_scaled = (_state.delta_vel_bias / _dt_ekf_avg) * _imu_sample_delayed.delta_vel_dt; + const Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel - delta_vel_bias_scaled; const Vector3f corrected_delta_vel_ef = _R_to_earth * corrected_delta_vel; // calculate a filtered horizontal acceleration with a 1 sec time constant @@ -326,10 +328,10 @@ void Ekf::calculateOutputStates(const imuSample &imu) // Use full rate IMU data at the current time horizon // correct delta angles for bias offsets - const float dt_scale_correction = _dt_imu_avg / _dt_ekf_avg; + const Vector3f delta_ang_bias_scaled = (_state.delta_ang_bias / _dt_ekf_avg) * imu.delta_ang_dt; // Apply corrections to the delta angle required to track the quaternion states at the EKF fusion time horizon - const Vector3f delta_angle(imu.delta_ang - _state.delta_ang_bias * dt_scale_correction + _delta_angle_corr); + const Vector3f delta_angle(imu.delta_ang - delta_ang_bias_scaled + _delta_angle_corr); // calculate a yaw change about the earth frame vertical const float spin_del_ang_D = delta_angle.dot(Vector3f(_R_to_earth_now.row(2))); @@ -355,7 +357,8 @@ void Ekf::calculateOutputStates(const imuSample &imu) _R_to_earth_now = Dcmf(_output_new.quat_nominal); // correct delta velocity for bias offsets - const Vector3f delta_vel_body{imu.delta_vel - _state.delta_vel_bias * dt_scale_correction}; + const Vector3f delta_vel_bias_scaled = (_state.delta_vel_bias / _dt_ekf_avg) * _imu_sample_delayed.delta_vel_dt; + const Vector3f delta_vel_body{imu.delta_vel - delta_vel_bias_scaled}; // rotate the delta velocity to earth frame Vector3f delta_vel_earth{_R_to_earth_now * delta_vel_body}; @@ -536,7 +539,9 @@ Quatf Ekf::calculate_quaternion() const { // Correct delta angle data for bias errors using bias state estimates from the EKF and also apply // corrections required to track the EKF quaternion states - const Vector3f delta_angle{_newest_high_rate_imu_sample.delta_ang - _state.delta_ang_bias * (_dt_imu_avg / _dt_ekf_avg) + _delta_angle_corr}; + const Vector3f delta_ang_bias_scaled = (_state.delta_ang_bias / _dt_ekf_avg) * _newest_high_rate_imu_sample.delta_ang_dt; + + const Vector3f delta_angle{_newest_high_rate_imu_sample.delta_ang - delta_ang_bias_scaled + _delta_angle_corr}; // increment the quaternions using the corrected delta angle vector // the quaternions must always be normalised after modification