From 4d7cc41921101cf9ee91793cf12e21c3d94a7c19 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Oct 2015 12:12:58 +0200 Subject: [PATCH] EKF: Retain minimum Kalman update rate --- .../ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h | 1 + .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 4 +++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h index 57df913069..421e109eb3 100644 --- a/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h +++ b/src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h @@ -177,6 +177,7 @@ private: hrt_abstime _last_accel; hrt_abstime _last_mag; unsigned _prediction_steps; + uint64_t _prediction_last; struct sensor_combined_s _sensor_combined; diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 587d22bac4..afb7cfbac5 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -157,6 +157,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _last_accel(0), _last_mag(0), _prediction_steps(0), + _prediction_last(0), _sensor_combined{}, @@ -997,11 +998,12 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const _covariancePredictionDt += _ekf->dtIMU; // only fuse every few steps - if (_prediction_steps < MAX_PREDICITION_STEPS) { + if (_prediction_steps < MAX_PREDICITION_STEPS && ((hrt_absolute_time() - _prediction_last) < 20 * 1000)) { _prediction_steps++; return; } else { _prediction_steps = 0; + _prediction_last = hrt_absolute_time(); } // perform a covariance prediction if the total delta angle has exceeded the limit