From 8ce285cdfa6a3aae20bb28cca5cc9c15fd52bd04 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 5 Mar 2020 21:42:45 +1100 Subject: [PATCH] EKF: Don't allow tilt alignment if vehicle is moving excessively (#768) Also remove unnecessary update of the _accel_lpf after alignment. --- EKF/ekf.cpp | 28 ++++++++++++++++------------ EKF/ekf.h | 9 ++++++--- 2 files changed, 22 insertions(+), 15 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 4bec23d36a..a5a2894858 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -105,9 +105,6 @@ bool Ekf::update() // Only run the filter if IMU data in the buffer has been updated if (_imu_updated) { - const imuSample &imu_init = _imu_buffer.get_newest(); - _accel_lpf.update(imu_init.delta_vel); - // perform state and covariance prediction for the main filter predictState(); predictCovariance(); @@ -130,18 +127,21 @@ bool Ekf::update() bool Ekf::initialiseFilter() { - // Keep accumulating measurements until we have a minimum of 10 samples for the required sensors - // Filter accel for tilt initialization const imuSample &imu_init = _imu_buffer.get_newest(); - if(_is_first_imu_sample){ - _accel_lpf.reset(imu_init.delta_vel); - _is_first_imu_sample = false; + // protect against zero data + if (imu_init.delta_vel_dt < 1e-4f || imu_init.delta_ang_dt < 1e-4f) { + return false; } - else - { - _accel_lpf.update(imu_init.delta_vel); + + if (_is_first_imu_sample) { + _accel_lpf.reset(imu_init.delta_vel / imu_init.delta_vel_dt); + _gyro_lpf.reset(imu_init.delta_ang / imu_init.delta_ang_dt); + _is_first_imu_sample = false; + } else { + _accel_lpf.update(imu_init.delta_vel / imu_init.delta_vel_dt); + _gyro_lpf.update(imu_init.delta_ang / imu_init.delta_ang_dt); } // Sum the magnetometer measurements @@ -222,7 +222,11 @@ bool Ekf::initialiseFilter() bool Ekf::initialiseTilt() { - if (_accel_lpf.getState().norm() < 0.001f) { + const float accel_norm = _accel_lpf.getState().norm(); + const float gyro_norm = _gyro_lpf.getState().norm(); + if (accel_norm < 0.9f * CONSTANTS_ONE_G || + accel_norm > 1.1f * CONSTANTS_ONE_G || + gyro_norm > math::radians(15.0f)) { return false; } diff --git a/EKF/ekf.h b/EKF/ekf.h index ea2a061329..18b705892c 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -445,12 +445,15 @@ private: uint64_t _last_gps_origin_time_us{0}; ///< time the origin was last set (uSec) float _gps_alt_ref{0.0f}; ///< WGS-84 height (m) - // Variables used to initialise the filter states + // Variables used by the initial filter alignment bool _is_first_imu_sample{true}; uint32_t _baro_counter{0}; ///< number of baro samples read during initialisation uint32_t _mag_counter{0}; ///< number of magnetometer samples read during initialisation - AlphaFilterVector3f _mag_lpf; ///< filtered magnetometer measurement for instant reset(Gauss) - AlphaFilterVector3f _accel_lpf; ///< filtered accelerometer measurement for instant reset(Gauss) + AlphaFilterVector3f _accel_lpf; ///< filtered accelerometer measurement used to align tilt (m/s/s) + AlphaFilterVector3f _gyro_lpf; ///< filtered gyro measurement used for alignment excessive movement check (rad/sec) + + // Variables used to perform in flight resets and switch between height sources + AlphaFilterVector3f _mag_lpf; ///< filtered magnetometer measurement for instant reset (Gauss) float _hgt_sensor_offset{0.0f}; ///< set as necessary if desired to maintain the same height after a height reset (m) float _baro_hgt_offset{0.0f}; ///< baro height reading at the local NED origin (m)