From 6c57bea1c3de411c6e993253b53320d6eb1f7107 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 30 Apr 2016 15:11:12 +1000 Subject: [PATCH] EKF: fix bug in IMU error application timing patch --- EKF/ekf.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 83d68e56f8..917504a521 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -530,8 +530,7 @@ void Ekf::predictState() corrected_delta_vel(2) -= _state.accel_z_bias; // correct delta angles for earth rotation rate - corrected_delta_ang = _imu_sample_delayed.delta_ang - _R_to_earth.transpose() * _earth_rate_NED * - _imu_sample_delayed.delta_ang_dt; + corrected_delta_ang -= _R_to_earth.transpose() * _earth_rate_NED * _imu_sample_delayed.delta_ang_dt; // convert the delta angle to a delta quaternion Quaternion dq;