fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-30 15:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Don't allow tilt alignment if vehicle is moving excessively (#768)

Browse Source 
Also remove unnecessary update of the _accel_lpf after alignment.
This commit is contained in:
Paul Riseborough
2020-03-05 21:42:45 +11:00
committed by GitHub
parent 230c865fa9
commit 8ce285cdfa
2 changed files with 22 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf.cpp
+16 -12
View File
@@ -105,9 +105,6 @@ bool Ekf::update()
// Only run the filter if IMU data in the buffer has been updated // Only run the filter if IMU data in the buffer has been updated
if (_imu_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 // perform state and covariance prediction for the main filter
predictState(); predictState();
predictCovariance(); predictCovariance();
@@ -130,18 +127,21 @@ bool Ekf::update()
bool Ekf::initialiseFilter() bool Ekf::initialiseFilter()
{ {
// Keep accumulating measurements until we have a minimum of 10 samples for the required sensors
// Filter accel for tilt initialization // Filter accel for tilt initialization
const imuSample &imu_init = _imu_buffer.get_newest(); const imuSample &imu_init = _imu_buffer.get_newest();
if(_is_first_imu_sample){ // protect against zero data
_accel_lpf.reset(imu_init.delta_vel); if (imu_init.delta_vel_dt < 1e-4f || imu_init.delta_ang_dt < 1e-4f) {
_is_first_imu_sample = false; return false;
} }
else
{ if (_is_first_imu_sample) {
_accel_lpf.update(imu_init.delta_vel); _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 // Sum the magnetometer measurements
@@ -222,7 +222,11 @@ bool Ekf::initialiseFilter()
bool Ekf::initialiseTilt() 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; return false;
} }
EKF/ekf.h
+6 -3
View File
@@ -445,12 +445,15 @@ private:
uint64_t _last_gps_origin_time_us{0}; ///< time the origin was last set (uSec) 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) 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}; bool _is_first_imu_sample{true};
uint32_t _baro_counter{0}; ///< number of baro samples read during initialisation uint32_t _baro_counter{0}; ///< number of baro samples read during initialisation
uint32_t _mag_counter{0}; ///< number of magnetometer 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 used to align tilt (m/s/s)
AlphaFilterVector3f _accel_lpf; ///< filtered accelerometer measurement for instant reset(Gauss) 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 _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) float _baro_hgt_offset{0.0f}; ///< baro height reading at the local NED origin (m)