From 1562a82dc23e5210e426d31aeb5ae8d4f58e23a3 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 16 May 2018 16:51:10 +1000 Subject: [PATCH] EKF: Add parameter to adjust on-ground movement detector sensitivity --- EKF/common.h | 3 +++ EKF/estimator_interface.cpp | 6 +++--- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 251c694069..5cfcfeff62 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -340,6 +340,9 @@ struct parameters { // auxilliary velocity fusion float auxvel_noise{0.5f}; ///< minimum observation noise, uses reported noise if greater (m/s) float auxvel_gate{5.0f}; ///< velocity fusion innovation consistency gate size (STD) + + // control of on-ground movement check + float is_moving_scaler{1.0f}; ///< gain scaler used to adjust the threshold for the on-ground movement detection. Larger values make the test less sensitive. }; struct stateSample { diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 0cb1db08a1..4fd543ad4b 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -89,9 +89,9 @@ void EstimatorInterface::setIMUData(uint64_t time_usec, uint64_t delta_ang_dt, u // detect if the vehicle is not moving when on ground if (!_control_status.flags.in_air) { - if ((_vibe_metrics[1] * 4.0E4f > 1.0f) - || (_vibe_metrics[2] * 2.1E2f > 1.0f) - || ((imu_sample_new.delta_ang.norm() / dt) > 0.05f)) { + if ((_vibe_metrics[1] * 4.0E4f > _params.is_moving_scaler) + || (_vibe_metrics[2] * 2.1E2f > _params.is_moving_scaler) + || ((imu_sample_new.delta_ang.norm() / dt) > 0.05f * _params.is_moving_scaler)) { _time_last_move_detect_us = _imu_sample_new.time_us; } _vehicle_at_rest = ((_imu_sample_new.time_us - _time_last_move_detect_us) > (uint64_t)1E6);