From 2dcc6e2053d3861b9a6132c10b9403d4587f68cc Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 12 Apr 2016 06:58:10 +1000 Subject: [PATCH] EKF: Improve accuracy of state prediction Use an a common estimator value for gravity Use average orientation across update interval when rotating delta velocities --- EKF/ekf.cpp | 15 ++++++++++++--- EKF/ekf.h | 3 ++- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 4c49a30207..874f68a4cf 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -511,9 +511,18 @@ void Ekf::predictState() // save the previous value of velocity so we can use trapzoidal integration Vector3f vel_last = _state.vel; - // predict velocity states - _state.vel += _R_to_earth * _imu_sample_delayed.delta_vel; - _state.vel(2) += 9.81f * _imu_sample_delayed.delta_vel_dt; + // 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; + + // compensate for acceleration due to gravity + _state.vel(2) += _gravity_mss * _imu_sample_delayed.delta_vel_dt; // predict position states via trapezoidal integration of velocity _state.pos += (vel_last + _state.vel) * _imu_sample_delayed.delta_vel_dt * 0.5f; diff --git a/EKF/ekf.h b/EKF/ekf.h index c2b91eb546..30801ad6dd 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -124,7 +124,8 @@ public: private: static const uint8_t _k_num_states = 24; - static constexpr float _k_earth_rate = 0.000072921f; + const float _k_earth_rate = 0.000072921f; + const float _gravity_mss = 9.80665f; stateSample _state; // state struct of the ekf running at the delayed time horizon