From c58ab3e25612f163d3819d9b841b2f97d1be037a Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sun, 13 Mar 2016 19:43:20 +1100 Subject: [PATCH] EKF: Enable fallback to baro alt when using range finder for height --- EKF/control.cpp | 4 ++-- EKF/ekf.cpp | 53 +++++++++++++++++++++++++++++------------- EKF/ekf.h | 2 ++ EKF/ekf_helper.cpp | 9 +++++++ EKF/vel_pos_fusion.cpp | 4 ++-- 5 files changed, 52 insertions(+), 20 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index b0cd91b935..026cb7a1de 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -159,8 +159,8 @@ void Ekf::controlFusionModes() // Handle the case where we have rejected height measurements for an extended period // This excessive vibration levels can cause this so a reset gives the filter a chance to recover - // After 10 seconds without aiding we reset to the height measurement provided the data is fresh - if ((_time_last_imu - _time_last_hgt_fuse > 10e6) && (_time_last_imu - _time_last_baro < 5e5)) { + // After 10 seconds without aiding we reset to the height measurement + if (_time_last_imu - _time_last_hgt_fuse > 10e6) { // Reset vertical position and velocity states to the last measurement resetHeight(); } diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 001f77f795..62f4e1cbbc 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -40,6 +40,7 @@ */ #include "ekf.h" +#include "mathlib.h" Ekf::Ekf(): _filter_initialised(false), @@ -78,7 +79,8 @@ Ekf::Ekf(): _terrain_vpos(0.0f), _hagl_innov(0.0f), _hagl_innov_var(0.0f), - _time_last_hagl_fuse(0) + _time_last_hagl_fuse(0), + _baro_hgt_offset(0.0f) { _control_status = {}; _control_status_prev = {}; @@ -101,7 +103,7 @@ Ekf::Ekf(): _mag_sum = {}; _delVel_sum = {}; _flow_gyro_bias = {}; - _imu_del_ang_of = {}; + _imu_del_ang_of = {}; } Ekf::~Ekf() @@ -209,7 +211,7 @@ bool Ekf::update() // determine if range finder data has fallen behind the fusin time horizon fuse it if we are // not tilted too much to use it if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed) - && (_R_prev(2, 2) > 0.7071f)) { + && (_R_prev(2, 2) > 0.7071f)) { // if we have range data we always try to estimate terrain height _fuse_hagl_data = true; @@ -217,20 +219,34 @@ bool Ekf::update() if (_control_status.flags.rng_hgt) { _fuse_height = true; } - } - // If we are supposed to be using range finder data as the primary height sensor and haven't received it for a while - // and are on the ground, then set the measurement to the expected on ground value - if ((_time_last_imu - _time_last_hgt_fuse) > 5e5 && _control_status.flags.rng_hgt && !_in_air) { - _range_sample_delayed.rng = _params.rng_gnd_clearance; - _range_sample_delayed.time_us = _imu_sample_delayed.time_us; + } else if ((_time_last_imu - _time_last_hgt_fuse) > 5e5 && _control_status.flags.rng_hgt) { + // If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements + // and are on the ground, then synthesise a measurement at the expected on ground value + if (!_in_air) { + _range_sample_delayed.rng = _params.rng_gnd_clearance; + _range_sample_delayed.time_us = _imu_sample_delayed.time_us; + + } else { + // if this happens in the air, fuse the baro measurement to constrain drift + // use the baro for this update only + _control_status.flags.baro_hgt = true; + _control_status.flags.rng_hgt = false; + } + _fuse_height = true; + } // determine if baro data has fallen behind the fuson time horizon and fuse it in the main filter if enabled - if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed) - && _control_status.flags.baro_hgt) { - _fuse_height = true; + if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) { + if (_control_status.flags.baro_hgt) { + _fuse_height = true; + + } else { + // calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference + _baro_hgt_offset += 0.02f * math::constrain((_baro_sample_delayed.hgt + _state.pos(2) - _baro_hgt_offset), -5.0f, 5.0f); + } } // If we are using GPS aiding and data has fallen behind the fusion time horizon then fuse it @@ -242,15 +258,15 @@ bool Ekf::update() // If we are using optical flow aiding and data has fallen behind the fusion time horizon, then fuse it if (_flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed) - && _control_status.flags.opt_flow && (_time_last_imu - _time_last_optflow) < 2e5 - && (_R_prev(2, 2) > 0.7071f)) { + && _control_status.flags.opt_flow && (_time_last_imu - _time_last_optflow) < 2e5 + && (_R_prev(2, 2) > 0.7071f)) { _fuse_flow = true; } // if we aren't doing any aiding, fake GPS measurements at the last known position to constrain drift // Coincide fake measurements with baro data for efficiency with a minimum fusion rate of 5Hz if (!_control_status.flags.gps && !_control_status.flags.opt_flow - && ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) { + && ((_time_last_imu - _time_last_fake_gps > 2e5) || _fuse_height)) { _fuse_pos = true; _gps_sample_delayed.pos(0) = _last_known_posNE(0); _gps_sample_delayed.pos(1) = _last_known_posNE(1); @@ -375,9 +391,14 @@ bool Ekf::initialiseFilter(void) // calculate the initial magnetic field and yaw alignment resetMagHeading(mag_init); - // calculate the averaged height reading + // calculate the averaged height reading to calulate the height of the origin _hgt_at_alignment = _hgt_sum / (float)_hgt_counter; + // if we are not using the baro height as the primary source, then calculate an offset relative to the origin + // so it can be used as a backup + baroSample baro_newest = _baro_buffer.get_newest(); + _baro_hgt_offset = baro_newest.hgt; + // set the velocity to the GPS measurement (by definition, the initial position and height is at the origin) resetVelocity(); diff --git a/EKF/ekf.h b/EKF/ekf.h index d4f13fb7c0..cc3d7cb67d 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -216,6 +216,8 @@ private: uint64_t _time_last_hagl_fuse; // last system time in usec that the hagl measurement failed it's checks bool _terrain_initialised; // true when the terrain estimator has been intialised + float _baro_hgt_offset; // number of metres the baro height origin is above the local NED origin (m) + // update the real time complementary filter states. This includes the prediction // and the correction step void calculateOutputStates(); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 2f91cd8170..ae60f8b342 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -96,6 +96,11 @@ void Ekf::resetHeight() } else { // TODO: reset to last known range based estimate } + + // reset the baro offset which is subtracted from the baro reading if we need to use it as a backup + baroSample baro_newest = _baro_buffer.get_newest(); + _baro_hgt_offset = baro_newest.hgt + _state.pos(2); + } else if (_control_status.flags.baro_hgt) { // initialize vertical position with newest baro measurement baroSample baro_newest = _baro_buffer.get_newest(); @@ -106,6 +111,10 @@ void Ekf::resetHeight() } else { // TODO: reset to last known baro based estimate } + + // the baro height offset should be zero if baro is our primary height source + _baro_hgt_offset = 0.0f; + } else { // TODO: reset to GPS height } diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index ade2dcdb0c..44c491b498 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -110,7 +110,7 @@ void Ekf::fuseVelPosHeight() if (_control_status.flags.baro_hgt) { fuse_map[5] = true; // vertical position innovation - baro measurement has opposite sign to earth z axis - _vel_pos_innov[5] = _state.pos(2) - (_hgt_at_alignment - _baro_sample_delayed.hgt); + _vel_pos_innov[5] = _state.pos(2) - (_hgt_at_alignment - _baro_sample_delayed.hgt + _baro_hgt_offset); // observation variance - user parameter defined R[5] = fmaxf(_params.baro_noise, 0.01f); R[5] = R[5] * R[5]; @@ -120,7 +120,7 @@ void Ekf::fuseVelPosHeight() } else if (_control_status.flags.rng_hgt && (_R_prev(2, 2) > 0.7071f)) { fuse_map[5] = true; // use range finder with tilt correction - _vel_pos_innov[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng *_R_prev(2, 2), + _vel_pos_innov[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_prev(2, 2), _params.rng_gnd_clearance)); // observation variance - user parameter defined R[5] = fmaxf(_params.range_noise, 0.01f);