From ffea65e1a4168d7ec20084dc7934be7b5ac13a94 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 2 May 2016 21:25:50 +1000 Subject: [PATCH] EKF: fix bug in state prediction --- EKF/ekf.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 3e808b2835..421cdf5efa 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -518,8 +518,7 @@ void Ekf::predictState() Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel - _state.accel_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;