From eb70aca2e8bf0e153c38bd41aa21082d3777e791 Mon Sep 17 00:00:00 2001 From: CarlOlsson Date: Sun, 22 May 2016 16:03:30 +0200 Subject: [PATCH] deleted second update of transformation matrix --- EKF/ekf.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index f37adb933d..1debc0108e 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -570,8 +570,10 @@ void Ekf::predictState() // save the previous value of velocity so we can use trapzoidal integration Vector3f vel_last = _state.vel; - // update the rotation matrix and calculate the increment in velocity using the current orientation + // update transformation matrix from body to world frame _R_to_earth = quat_to_invrotmat(_state.quat_nominal); + + // calculate the increment in velocity using the current orientation _state.vel += _R_to_earth * corrected_delta_vel; // compensate for acceleration due to gravity @@ -580,9 +582,6 @@ void Ekf::predictState() // predict position states via trapezoidal integration of velocity _state.pos += (vel_last + _state.vel) * _imu_sample_delayed.delta_vel_dt * 0.5f; - // update transformation matrix from body to world frame - _R_to_earth = quat_to_invrotmat(_state.quat_nominal); - constrainStates(); // calculate an average filter update time