From 4fa1e9c651e6532c8144a2a51210226edf803118 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 30 Apr 2016 08:44:07 +1000 Subject: [PATCH] EKF: Fix timing errors in state prediction IMU corrections were being applied at the wrong time horizon to the EKF states IMU downsampling rotates the delta velocities into the orientation produced by the last delta angle, but this wasn't consistent with the way the state prediction was using the delta velocity data. --- EKF/ekf.cpp | 41 +++++++++++++++++++---------------------- 1 file changed, 19 insertions(+), 22 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 9826627baa..c4fdd1f6cb 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -519,8 +519,18 @@ void Ekf::predictState() } } + // correct delta angles for bias offsets and scale factors + Vector3f corrected_delta_ang; + corrected_delta_ang(0) = _imu_sample_delayed.delta_ang(0) * _state.gyro_scale(0) - _state.gyro_bias(0); + corrected_delta_ang(1) = _imu_sample_delayed.delta_ang(1) * _state.gyro_scale(1) - _state.gyro_bias(1); + corrected_delta_ang(2) = _imu_sample_delayed.delta_ang(2) * _state.gyro_scale(2) - _state.gyro_bias(2); + + // correct delta velocity for bias offsets + Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel; + corrected_delta_vel(2) -= _state.accel_z_bias; + // correct delta angles for earth rotation rate - Vector3f corrected_delta_ang = _imu_sample_delayed.delta_ang - _R_to_earth.transpose() * _earth_rate_NED * + corrected_delta_ang = _imu_sample_delayed.delta_ang - _R_to_earth.transpose() * _earth_rate_NED * _imu_sample_delayed.delta_ang_dt; // convert the delta angle to a delta quaternion @@ -536,15 +546,9 @@ void Ekf::predictState() // save the previous value of velocity so we can use trapzoidal integration Vector3f vel_last = _state.vel; - // calculate the increment in velocity using the previous orientation - Vector3f delta_vel_earth_1 = _R_to_earth * _imu_sample_delayed.delta_vel; - // update the rotation matrix and calculate the increment in velocity using the current orientation _R_to_earth = quat_to_invrotmat(_state.quat_nominal); - Vector3f delta_vel_earth_2 = _R_to_earth * _imu_sample_delayed.delta_vel; - - // Update the velocity assuming constant angular rate and acceleration across the same delta time interval - _state.vel += (delta_vel_earth_1 + delta_vel_earth_2) * 0.5f; + _state.vel += _R_to_earth * corrected_delta_vel; // compensate for acceleration due to gravity _state.vel(2) += _gravity_mss * _imu_sample_delayed.delta_vel_dt; @@ -560,13 +564,6 @@ void Ekf::predictState() bool Ekf::collect_imu(imuSample &imu) { - imu.delta_ang(0) = imu.delta_ang(0) * _state.gyro_scale(0); - imu.delta_ang(1) = imu.delta_ang(1) * _state.gyro_scale(1); - imu.delta_ang(2) = imu.delta_ang(2) * _state.gyro_scale(2); - - imu.delta_ang -= _state.gyro_bias * imu.delta_ang_dt / (_dt_imu_avg > 0 ? _dt_imu_avg : 0.01f); - imu.delta_vel(2) -= _state.accel_z_bias * imu.delta_vel_dt / (_dt_imu_avg > 0 ? _dt_imu_avg : 0.01f);; - _imu_sample_new.delta_ang = imu.delta_ang; _imu_sample_new.delta_vel = imu.delta_vel; _imu_sample_new.delta_ang_dt = imu.delta_ang_dt; @@ -621,15 +618,15 @@ void Ekf::calculateOutputStates() // use latest IMU data imuSample imu_new = _imu_sample_new; - // Get the delta angle and velocity from the latest IMU data - // Note: We do no not need to consider any bias or scale correction here - // since the base class has already corrected the imu sample + // correct delta angles for bias offsets and scale factors Vector3f delta_angle; - delta_angle(0) = imu_new.delta_ang(0); - delta_angle(1) = imu_new.delta_ang(1); - delta_angle(2) = imu_new.delta_ang(2); + delta_angle(0) = _imu_sample_new.delta_ang(0) * _state.gyro_scale(0) - _state.gyro_bias(0); + delta_angle(1) = _imu_sample_new.delta_ang(1) * _state.gyro_scale(1) - _state.gyro_bias(1); + delta_angle(2) = _imu_sample_new.delta_ang(2) * _state.gyro_scale(2) - _state.gyro_bias(2); - Vector3f delta_vel = imu_new.delta_vel; + // correct delta velocity for bias offsets + Vector3f delta_vel = _imu_sample_new.delta_vel; + delta_vel(2) -= _state.accel_z_bias; // Apply corrections to the delta angle required to track the quaternion states at the EKF fusion time horizon delta_angle += _delta_angle_corr;