From 6c25ac5731ad2f90c2e35f9365fdb68d12e05807 Mon Sep 17 00:00:00 2001 From: kritz Date: Tue, 17 Dec 2019 11:01:25 +0100 Subject: [PATCH] Tilt Initialisation: Average Filter -> LowPass Filter (#686) * Tilt Initialisation: Average Filter -> LowPass Filter * Add _is_first_imu_sample variable * Remove not needed comments --- EKF/control.cpp | 10 +++---- EKF/ekf.cpp | 60 ++++++++++++++++++++----------------- EKF/ekf.h | 7 +++-- EKF/estimator_interface.cpp | 1 + 4 files changed, 43 insertions(+), 35 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index bf80f6020d..0943af3304 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -61,18 +61,18 @@ void Ekf::controlFusionModes() // send alignment status message to the console if (_control_status.flags.baro_hgt) { - ECL_INFO("EKF aligned, (pressure height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length); + ECL_INFO("%llu: EKF aligned, (pressure height, IMU buf: %i, OBS buf: %i)", (unsigned long long)_imu_sample_delayed.time_us, (int)_imu_buffer_length, (int)_obs_buffer_length); } else if (_control_status.flags.ev_hgt) { - ECL_INFO("EKF aligned, (EV height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length); + ECL_INFO("%llu: EKF aligned, (EV height, IMU buf: %i, OBS buf: %i)", (unsigned long long)_imu_sample_delayed.time_us, (int)_imu_buffer_length, (int)_obs_buffer_length); } else if (_control_status.flags.gps_hgt) { - ECL_INFO("EKF aligned, (GPS height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length); + ECL_INFO("%llu: EKF aligned, (GPS height, IMU buf: %i, OBS buf: %i)", (unsigned long long)_imu_sample_delayed.time_us, (int)_imu_buffer_length, (int)_obs_buffer_length); } else if (_control_status.flags.rng_hgt) { - ECL_INFO("EKF aligned, (range height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length); + ECL_INFO("%llu: EKF aligned, (range height, IMU buf: %i, OBS buf: %i)", (unsigned long long)_imu_sample_delayed.time_us, (int)_imu_buffer_length, (int)_obs_buffer_length); } else { - ECL_ERR("EKF aligned, (unknown height, IMU buf: %i, OBS buf: %i)", (int)_imu_buffer_length, (int)_obs_buffer_length); + ECL_ERR("%llu: EKF aligned, (unknown height, IMU buf: %i, OBS buf: %i)", (unsigned long long)_imu_sample_delayed.time_us, (int)_imu_buffer_length, (int)_obs_buffer_length); } } diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 354eeafac4..8251bd9c83 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -114,6 +114,8 @@ 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(); @@ -139,9 +141,17 @@ bool Ekf::initialiseFilter() { // Keep accumulating measurements until we have a minimum of 10 samples for the required sensors - // Sum the IMU delta velocity measurements + // Filter accel for tilt initialization const imuSample &imu_init = _imu_buffer.get_newest(); - _delVel_sum += imu_init.delta_vel; + + if(_is_first_imu_sample){ + _accel_lpf.reset(imu_init.delta_vel); + _is_first_imu_sample = false; + } + else + { + _accel_lpf.update(imu_init.delta_vel); + } // Sum the magnetometer measurements if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) { @@ -199,35 +209,10 @@ bool Ekf::initialiseFilter() _gps_drift_velD = 0.0f; _gps_alt_ref = 0.0f; - // Zero all of the states - _state.vel.setZero(); - _state.pos.setZero(); - _state.delta_ang_bias.setZero(); - _state.delta_vel_bias.setZero(); - _state.mag_I.setZero(); - _state.mag_B.setZero(); - _state.wind_vel.setZero(); - - // get initial roll and pitch estimate from delta velocity vector, assuming vehicle is static - float pitch = 0.0f; - float roll = 0.0f; - - if (_delVel_sum.norm() > 0.001f) { - _delVel_sum.normalize(); - pitch = asinf(_delVel_sum(0)); - roll = atan2f(-_delVel_sum(1), -_delVel_sum(2)); - - } else { + if(!initialiseTilt()){ return false; } - // calculate initial tilt alignment - Eulerf euler_init(roll, pitch, 0.0f); - _state.quat_nominal = Quatf(euler_init); - - // update transformation matrix from body to world frame - _R_to_earth = Dcmf(_state.quat_nominal); - // calculate the initial magnetic field and yaw alignment _control_status.flags.yaw_align = resetMagHeading(_mag_lpf.getState(), false, false); @@ -258,6 +243,25 @@ bool Ekf::initialiseFilter() } } +bool Ekf::initialiseTilt() +{ + if (_accel_lpf.getState().norm() < 0.001f) { + return false; + } + + // get initial roll and pitch estimate from delta velocity vector, assuming vehicle is static + Vector3f gravity_in_body = _accel_lpf.getState(); + gravity_in_body.normalize(); + const float pitch = asinf(gravity_in_body(0)); + const float roll = atan2f(-gravity_in_body(1), -gravity_in_body(2)); + + const Eulerf euler_init(roll, pitch, 0.0f); + _state.quat_nominal = Quatf(euler_init); + _R_to_earth = Dcmf(_state.quat_nominal); + + return true; +} + void Ekf::predictState() { if (!_earth_rate_initialised) { diff --git a/EKF/ekf.h b/EKF/ekf.h index 8b1ef38918..c674a58f00 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -57,6 +57,8 @@ public: // set the internal states and status to their default value void reset(uint64_t timestamp) override; + bool initialiseTilt(); + // should be called every time new data is pushed into the filter bool update() override; @@ -462,10 +464,11 @@ private: // Variables used to initialise the filter states uint32_t _hgt_counter{0}; ///< number of height samples read during initialisation float _rng_filt_state{0.0f}; ///< filtered height measurement (m) + bool _is_first_imu_sample{true}; uint32_t _mag_counter{0}; ///< number of magnetometer samples read during initialisation uint64_t _time_last_mag{0}; ///< measurement time of last magnetomter sample (uSec) - AlphaFilterVector3f _mag_lpf; ///< filtered magnetometer measurement (Gauss) - Vector3f _delVel_sum; ///< summed delta velocity (m/sec) + AlphaFilterVector3f _mag_lpf; ///< filtered magnetometer measurement for instant reset(Gauss) + AlphaFilterVector3f _accel_lpf; ///< filtered accelerometer 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) diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 79d1d18d6f..ceb45b597d 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -48,6 +48,7 @@ // Accumulate imu data and store to buffer at desired rate void EstimatorInterface::setIMUData(const imuSample &imu_sample) { + // TODO: resolve misplaced responsibility if (!_initialised) { init(imu_sample.time_us); _initialised = true;