From 15df20831a9e7a6a420aaddc4bed8a4165421c6d Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 30 Apr 2016 11:03:45 +1000 Subject: [PATCH] EKF: improve output complementary filter tracking Optimise for height tracking and adjust gains automatically with changes in time delay. --- EKF/ekf.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index c4fdd1f6cb..83d68e56f8 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -690,15 +690,19 @@ void Ekf::calculateOutputStates() delta_ang_error(1) = scalar * q_error(2); delta_ang_error(2) = scalar * q_error(3); + // calculate gains that provides tight tracking of the estimator states and + // adjust for changes in time delay to mantain consistent overshoot + float omega = 1e6f / (_imu_sample_new.time_us - _imu_sample_delayed.time_us); + // calculate a corrrection to the delta angle - // that will cause the INS to track the EKF quaternions with a 1 sec time constant - _delta_angle_corr = delta_ang_error * imu_new.delta_ang_dt; + // that will cause the INS to track the EKF quaternions + _delta_angle_corr = delta_ang_error * imu_new.delta_ang_dt * omega * 0.5f; // calculate a correction to the delta velocity - // that will cause the INS to track the EKF velocity with a 1 sec time constant - _delta_vel_corr = (_state.vel - _output_sample_delayed.vel) * imu_new.delta_vel_dt; + // that will cause the INS to track the EKF velocity + _delta_vel_corr = (_state.vel - _output_sample_delayed.vel) * imu_new.delta_vel_dt * omega * 0.5f; // calculate a correction to the INS velocity - // that will cause the INS to track the EKF position with a 1 sec time constant - _vel_corr = (_state.pos - _output_sample_delayed.pos); + // that will cause the INS to track the EKF position + _vel_corr = (_state.pos - _output_sample_delayed.pos) * omega * 0.6f; }