From 54e713969d0276910c838b21fa64b6c2a7f1c5f9 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 30 Apr 2016 15:33:09 +1000 Subject: [PATCH] EKF: Fix rebase error --- EKF/ekf.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 7617880824..20f0a422ce 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -612,13 +612,12 @@ void Ekf::calculateOutputStates() // correct delta angles for bias offsets and scale factors Vector3f delta_angle; - 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); + delta_angle(0) = _imu_sample_new.delta_ang(0) - _state.gyro_bias(0); + delta_angle(1) = _imu_sample_new.delta_ang(1) - _state.gyro_bias(1); + delta_angle(2) = _imu_sample_new.delta_ang(2) - _state.gyro_bias(2); // correct delta velocity for bias offsets - Vector3f delta_vel = _imu_sample_new.delta_vel; - delta_vel(2) -= _state.accel_z_bias; + Vector3f delta_vel = _imu_sample_new.delta_vel - _state.accel_bias; // Apply corrections to the delta angle required to track the quaternion states at the EKF fusion time horizon delta_angle += _delta_angle_corr;