From 62c6d40f1f54bbd809ba678b7bb68f2ff9684ec2 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 22 Apr 2016 08:21:36 +1000 Subject: [PATCH 01/10] EKF: Add methods to ring buffer to access specific indices --- EKF/RingBuffer.h | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/EKF/RingBuffer.h b/EKF/RingBuffer.h index 376d8e5851..b16f25cde1 100644 --- a/EKF/RingBuffer.h +++ b/EKF/RingBuffer.h @@ -160,6 +160,30 @@ public: return _buffer[index]; } + // return data at the specified index + data_type get_from_index(unsigned index) + { + if (index >= _size) { + index = _size-1; + } + return _buffer[index]; + } + + // push data to the specified index + void push_to_index(unsigned index, data_type sample) + { + if (index >= _size) { + index = _size-1; + } + _buffer[index] = sample; + } + + // return the length of the buffer + unsigned get_length() + { + return _size; + } + private: data_type *_buffer; unsigned _head, _tail, _size; From 0de15b1b2088810397148cbd402d94b76382e4f8 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 22 Apr 2016 08:28:56 +1000 Subject: [PATCH 02/10] EKF: Add variables to monitor vertical position and height offset --- EKF/ekf.cpp | 6 +++++- EKF/ekf.h | 4 ++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 74a9562dc8..b5e8df7487 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -85,7 +85,11 @@ Ekf::Ekf(): _baro_hgt_faulty(false), _gps_hgt_faulty(false), _rng_hgt_faulty(false), - _baro_hgt_offset(0.0f) + _baro_hgt_offset(0.0f), + _vert_pos_reset_delta(0.0f), + _time_vert_pos_reset(0), + _vert_vel_reset_delta(0.0f), + _time_vert_vel_reset(0) { _state = {}; _last_known_posNE.setZero(); diff --git a/EKF/ekf.h b/EKF/ekf.h index e2df78fdac..2c62540d3f 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -232,6 +232,10 @@ private: int _primary_hgt_source; // priary source of height data set at initialisation float _baro_hgt_offset; // number of metres the baro height origin is above the local NED origin (m) + float _vert_pos_reset_delta; // increase in vertical position state at the last reset(m) + uint64_t _time_vert_pos_reset; // last system time in usec that the vertical position state was reset + float _vert_vel_reset_delta; // increase in vertical position velocity at the last reset(m) + uint64_t _time_vert_vel_reset; // last system time in usec that the vertical velocity state was reset // update the real time complementary filter states. This includes the prediction // and the correction step From 10bf05e9a6aa3fc8d79479c47d2844c64febfd19 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 22 Apr 2016 08:30:46 +1000 Subject: [PATCH 03/10] EKF: publish the vertical position offset --- EKF/ekf.h | 3 +++ EKF/estimator_interface.h | 3 +++ 2 files changed, 6 insertions(+) diff --git a/EKF/ekf.h b/EKF/ekf.h index 2c62540d3f..f7ee978113 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -126,6 +126,9 @@ public: // get GPS check status void get_gps_check_status(uint16_t *_gps_check_fail_status); + // return the amount the local vertical position changed in the last height reset and the time of the reset + void get_vert_pos_reset(float *delta, uint64_t *time_us) {*delta = _vert_pos_reset_delta; *time_us = _time_vert_pos_reset;} + private: static const uint8_t _k_num_states = 24; diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index b99dafff6d..a917fa8456 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -220,6 +220,9 @@ public: // get GPS check status virtual void get_gps_check_status(uint16_t *val) = 0; + // return the amount the local vertical position changed in the last height reset and the time of the reset + virtual void get_vert_pos_reset(float *delta, uint64_t *time_us) = 0; + protected: parameters _params; // filter parameters From a7417657c36f4af4f9a30dd7b6ab60eb392b2bcd Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 22 Apr 2016 08:33:11 +1000 Subject: [PATCH 04/10] EKF: ensure fusion timeout counters are reset when required --- EKF/control.cpp | 7 +++++++ EKF/ekf.cpp | 7 +++++++ 2 files changed, 14 insertions(+) diff --git a/EKF/control.cpp b/EKF/control.cpp index a6729eeb8d..d6c4a00809 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -156,6 +156,10 @@ void Ekf::controlFusionModes() // Reset states to the last GPS measurement resetPosition(); resetVelocity(); + + // Reset the timeout counters + _time_last_pos_fuse = _time_last_imu; + _time_last_vel_fuse = _time_last_imu; } } } @@ -241,6 +245,9 @@ void Ekf::controlFusionModes() // Reset vertical position and velocity states to the last measurement resetHeight(); + + // Reset the timout timer + _time_last_hgt_fuse = _time_last_imu; } // handle the case when we are relying on optical flow fusion and lose it diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index b5e8df7487..dd14b991c6 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -488,6 +488,13 @@ bool Ekf::initialiseFilter(void) // initialise the terrain estimator initHagl(); + // reset the essential fusion timeout counters + _time_last_hgt_fuse = _time_last_imu; + _time_last_pos_fuse = _time_last_imu; + _time_last_vel_fuse = _time_last_imu; + _time_last_hagl_fuse = _time_last_imu; + _time_last_of_fuse = _time_last_imu; + return true; } } From 421703c2674a137470cf9c60887b32ba46c917ec Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 22 Apr 2016 08:38:41 +1000 Subject: [PATCH 05/10] EKF: rework initialisation of height state and offsets --- EKF/ekf.cpp | 25 ++++++++++--------------- EKF/ekf.h | 8 ++++---- 2 files changed, 14 insertions(+), 19 deletions(-) diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index dd14b991c6..a73c998e0b 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -74,7 +74,7 @@ Ekf::Ekf(): _last_gps_origin_time_us(0), _gps_alt_ref(0.0f), _hgt_counter(0), - _hgt_filt_state(0.0f), + _rng_filt_state(0.0f), _mag_counter(0), _time_last_mag(0), _hgt_sensor_offset(0.0f), @@ -388,12 +388,12 @@ bool Ekf::initialiseFilter(void) _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = true; - _hgt_filt_state = _range_sample_delayed.rng; + _rng_filt_state = _range_sample_delayed.rng; _hgt_counter = 1; } else if (_hgt_counter != 0) { // increment the sample count and apply a LPF to the measurement _hgt_counter ++; - _hgt_filt_state = 0.9f * _hgt_filt_state + 0.1f * _range_sample_delayed.rng; + _rng_filt_state = 0.9f * _rng_filt_state + 0.1f * _range_sample_delayed.rng; } } @@ -406,12 +406,12 @@ bool Ekf::initialiseFilter(void) _control_status.flags.baro_hgt = true; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; - _hgt_filt_state = _baro_sample_delayed.hgt; + _baro_hgt_offset = _baro_sample_delayed.hgt; _hgt_counter = 1; } else if (_hgt_counter != 0) { // increment the sample count and apply a LPF to the measurement _hgt_counter ++; - _hgt_filt_state = 0.9f * _hgt_filt_state + 0.1f * _baro_sample_delayed.hgt; + _baro_hgt_offset = 0.9f * _baro_hgt_offset + 0.1f * _baro_sample_delayed.hgt; } } @@ -469,17 +469,12 @@ bool Ekf::initialiseFilter(void) // calculate the initial magnetic field and yaw alignment resetMagHeading(mag_init); - // calculate the averaged height reading to calulate the height of the origin - _hgt_sensor_offset = _hgt_filt_state; - - // 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 - if (!_control_status.flags.baro_hgt) { + // if we are using the range finder as the primary source, then calculate the baro height at origin so we can use baro as a backup + // so it can be used as a backup ad set the initial height using the range finder + if (_control_status.flags.rng_hgt) { baroSample baro_newest = _baro_buffer.get_newest(); - _baro_hgt_offset = baro_newest.hgt - _hgt_sensor_offset; - - } else { - _baro_hgt_offset = 0.0f; + _baro_hgt_offset = baro_newest.hgt; + _state.pos(2) = -math::max(_rng_filt_state * _R_to_earth(2, 2),_params.rng_gnd_clearance); } // initialise the state covariance matrix diff --git a/EKF/ekf.h b/EKF/ekf.h index f7ee978113..662aecf4fc 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -172,7 +172,7 @@ private: float _mag_innov_var[3]; // earth magnetic field innovation variance float _airspeed_innov; // airspeed measurement innovation - float _airspeed_innov_var; // airspeed measurement innovation variance + float _airspeed_innov_var; // airspeed measurement innovation variance float _heading_innov; // heading measurement innovation float _heading_innov_var; // heading measurement innovation variance @@ -211,12 +211,12 @@ private: // Variables used to initialise the filter states uint32_t _hgt_counter; // number of height samples taken - float _hgt_filt_state; // filtered height measurement + float _rng_filt_state; // filtered height measurement uint32_t _mag_counter; // number of magnetometer samples taken uint64_t _time_last_mag; // measurement time of last magnetomter sample Vector3f _mag_filt_state; // filtered magnetometer measurement Vector3f _delVel_sum; // summed delta velocity - float _hgt_sensor_offset; // height that needs to be subtracted from the primary height sensor so that it reads zero height at the origin (m) + float _hgt_sensor_offset; // value subtracted from the height measurement to maintain consistency after switching height sensors (m) gps_check_fail_status_u _gps_check_fail_status; @@ -234,7 +234,7 @@ private: bool _rng_hgt_faulty; // true if valid rnage finder height data is unavailable for use int _primary_hgt_source; // priary source of height data set at initialisation - float _baro_hgt_offset; // number of metres the baro height origin is above the local NED origin (m) + float _baro_hgt_offset; // baro height reading at the local NED origin (m) float _vert_pos_reset_delta; // increase in vertical position state at the last reset(m) uint64_t _time_vert_pos_reset; // last system time in usec that the vertical position state was reset float _vert_vel_reset_delta; // increase in vertical position velocity at the last reset(m) From e3365525c22a08c3bf5a75a3dfdfab7ff76e6658 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 22 Apr 2016 08:39:24 +1000 Subject: [PATCH 06/10] EKF: rework height reset logic --- EKF/common.h | 2 +- EKF/control.cpp | 16 +++++++++++----- 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index d8382838b2..891f77d64a 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -267,7 +267,7 @@ struct parameters { baro_innov_gate = 5.0f; posNE_innov_gate = 5.0f; vel_innov_gate = 5.0f; - hgt_reset_lim = 5.0f; + hgt_reset_lim = 0.0f; // magnetometer fusion mag_heading_noise = 3.0e-1f; diff --git a/EKF/control.cpp b/EKF/control.cpp index d6c4a00809..a2a9d45519 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -180,17 +180,23 @@ void Ekf::controlFusionModes() baroSample baro_init = _baro_buffer.get_newest(); bool baro_hgt_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); - // use the gps if it is accurate or there is no baro data available - if (gps_hgt_available && (gps_hgt_accurate || !baro_hgt_available)) { + // check for inertial sensing errors as evidenced by the vertical innovations having the same sign and not stale + bool bad_imu = ((_vel_pos_innov[5] * _vel_pos_innov[2] > 0.0f) && + ((_imu_sample_delayed.time_us - _baro_sample_delayed.time_us) < 2 * BARO_MAX_INTERVAL) && + ((_imu_sample_delayed.time_us - _gps_sample_delayed.time_us) < 2 * GPS_MAX_INTERVAL)); + + // continue to use baro if it is available and we have detected bad IMU data or inadequate GPS + // switch to GPS if GPS data is available or we do not have Baro + if (baro_hgt_available && (bad_imu || !gps_hgt_available || !gps_hgt_accurate)) { + printf("EKF baro hgt timeout - reset to baro\n"); + } else if (gps_hgt_available && !bad_imu) { // declare the baro as unhealthy _baro_hgt_faulty = true; // set the height mode to the GPS _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = true; _control_status.flags.rng_hgt = false; - // adjust the height offset so we can use the GPS - _hgt_sensor_offset = _state.pos(2) + gps_init.hgt - _gps_alt_ref; - printf("EKF baro hgt timeout - switching to gps\n"); + printf("EKF baro hgt timeout - reset to GPS\n"); } } From 469a7d1711e5f518e6ae9303c3dbc23cbd5f2efe Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 22 Apr 2016 08:43:56 +1000 Subject: [PATCH 07/10] EKF: rework height reset calculate and save the amount that the vertical states have changed and the time of the change apply the change delta to the output observer states and buffer --- EKF/ekf_helper.cpp | 49 +++++++++++++++++++++++++++++++++++++++++++++- 1 file changed, 48 insertions(+), 1 deletion(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 09377f9537..cd0d93920e 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -90,12 +90,23 @@ void Ekf::resetHeight() // Get the most recent GPS data gpsSample gps_newest = _gps_buffer.get_newest(); + // store the current vertical position and velocity for reference so we can calculate and publish the reset amount + float old_vert_pos = _state.pos(2); + bool vert_pos_reset = false; + float old_vert_vel = _state.vel(2); + bool vert_vel_reset = false; + + // reset the vertical position if (_control_status.flags.rng_hgt) { rangeSample range_newest = _range_buffer.get_newest(); if (_time_last_imu - range_newest.time_us < 2 * RNG_MAX_INTERVAL) { - _state.pos(2) = _hgt_sensor_offset - range_newest.rng; + // calculate the new vertical position using range sensor + float new_pos_down = _hgt_sensor_offset - range_newest.rng; + // update the state and assoicated variance + _state.pos(2) = new_pos_down; P[8][8] = sq(_params.range_noise); + vert_pos_reset = true; } else { // TODO: reset to last known range based estimate @@ -112,6 +123,7 @@ void Ekf::resetHeight() if (_time_last_imu - baro_newest.time_us < 2 * BARO_MAX_INTERVAL) { _state.pos(2) = _hgt_sensor_offset - baro_newest.hgt + _baro_hgt_offset; P[8][8] = sq(_params.baro_noise); + vert_pos_reset = true; } else { // TODO: reset to last known baro based estimate @@ -122,6 +134,7 @@ void Ekf::resetHeight() if (_time_last_imu - gps_newest.time_us < 2 * GPS_MAX_INTERVAL) { _state.pos(2) = _hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref; P[8][8] = sq(gps_newest.hacc); + vert_pos_reset = true; } else { // TODO: reset to last known gps based estimate @@ -140,6 +153,40 @@ void Ekf::resetHeight() P[5][5] = fminf(sq(_state.vel(2)),1000.0f); _state.vel(2) = 0.0f; } + vert_vel_reset = true; + + // store the reset amount and time to be published + if (vert_pos_reset) { + _vert_pos_reset_delta = _state.pos(2) - old_vert_pos; + _time_vert_pos_reset = _time_last_imu; + } + + if (vert_vel_reset) { + _vert_vel_reset_delta = _state.vel(2) - old_vert_vel; + _time_vert_vel_reset = _time_last_imu; + } + + // add the reset amount to the output observer states + _output_new.pos(2) += _vert_pos_reset_delta; + _output_new.vel(2) += _vert_vel_reset_delta; + + // add the reset amount to the output observer buffered data + outputSample output_states; + unsigned output_length = _output_buffer.get_length(); + for (unsigned i=0; i < output_length; i++) { + output_states = _output_buffer.get_from_index(i); + + if (vert_pos_reset) { + output_states.pos(2) += _vert_pos_reset_delta; + } + + if (vert_vel_reset) { + output_states.vel(2) += _vert_vel_reset_delta; + } + + _output_buffer.push_to_index(i,output_states); + + } } From 874558d1946ca1b9ffdf925296f1fb52b2b1eade Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 22 Apr 2016 09:24:04 +1000 Subject: [PATCH 08/10] EKF: improve detection of bad vert accel data Improve ability to detect if bad vertical accel data has caused loss of height accuracy by using historical data. --- EKF/control.cpp | 24 ++++++++++++++++++------ EKF/ekf.cpp | 3 ++- EKF/ekf.h | 3 +++ 3 files changed, 23 insertions(+), 7 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index a2a9d45519..ad7456cb02 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -170,6 +170,20 @@ void Ekf::controlFusionModes() * measurement source, continue using it after the reset and declare the current * source failed if we have switched. */ + + // check for inertial sensing errors as evidenced by the vertical innovations having the same sign and not stale + bool bad_vert_accel = (_control_status.flags.baro_hgt && // we can only run this check if vertical position and velocity observations are indepedant + (_vel_pos_innov[5] * _vel_pos_innov[2] > 0.0f) && // vertical position and velocity sensors are in agreement + ((_imu_sample_delayed.time_us - _baro_sample_delayed.time_us) < 2 * BARO_MAX_INTERVAL) && // vertical position data is fresh + ((_imu_sample_delayed.time_us - _gps_sample_delayed.time_us) < 2 * GPS_MAX_INTERVAL) && // vertical velocity data is freshs + _vel_pos_test_ratio[2] > 1.0f && // vertical velocty innovations have failed innovation consistency checks + _vel_pos_test_ratio[5] > 1.0f); // vertical position innovations have failed innovation consistency checks + + // record time of last bad vert accel + if (bad_vert_accel) { + _time_bad_vert_accel = _time_last_imu; + } + if ((P[8][8] > sq(_params.hgt_reset_lim)) && ((_time_last_imu - _time_last_hgt_fuse) > 5e6)) { // handle the case where we are using baro for height if (_control_status.flags.baro_hgt) { @@ -180,16 +194,14 @@ void Ekf::controlFusionModes() baroSample baro_init = _baro_buffer.get_newest(); bool baro_hgt_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); - // check for inertial sensing errors as evidenced by the vertical innovations having the same sign and not stale - bool bad_imu = ((_vel_pos_innov[5] * _vel_pos_innov[2] > 0.0f) && - ((_imu_sample_delayed.time_us - _baro_sample_delayed.time_us) < 2 * BARO_MAX_INTERVAL) && - ((_imu_sample_delayed.time_us - _gps_sample_delayed.time_us) < 2 * GPS_MAX_INTERVAL)); + // check for inertial sensing errors in the last 10 seconds + bool prev_bad_vert_accel = (_time_last_imu - _time_bad_vert_accel < 10E6); // continue to use baro if it is available and we have detected bad IMU data or inadequate GPS // switch to GPS if GPS data is available or we do not have Baro - if (baro_hgt_available && (bad_imu || !gps_hgt_available || !gps_hgt_accurate)) { + if (baro_hgt_available && (prev_bad_vert_accel || !gps_hgt_available || !gps_hgt_accurate)) { printf("EKF baro hgt timeout - reset to baro\n"); - } else if (gps_hgt_available && !bad_imu) { + } else if (gps_hgt_available && !prev_bad_vert_accel) { // declare the baro as unhealthy _baro_hgt_faulty = true; // set the height mode to the GPS diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index a73c998e0b..02caeea693 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -89,7 +89,8 @@ Ekf::Ekf(): _vert_pos_reset_delta(0.0f), _time_vert_pos_reset(0), _vert_vel_reset_delta(0.0f), - _time_vert_vel_reset(0) + _time_vert_vel_reset(0), + _time_bad_vert_accel(0) { _state = {}; _last_known_posNE.setZero(); diff --git a/EKF/ekf.h b/EKF/ekf.h index 662aecf4fc..b28d2c9c3f 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -240,6 +240,9 @@ private: float _vert_vel_reset_delta; // increase in vertical position velocity at the last reset(m) uint64_t _time_vert_vel_reset; // last system time in usec that the vertical velocity state was reset + // imu fault status + uint64_t _time_bad_vert_accel; // last time a bad vertical accel was detected (usec) + // update the real time complementary filter states. This includes the prediction // and the correction step void calculateOutputStates(); From 3e81b86280d771b7fb941d73221ebb6b6990f7ea Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 22 Apr 2016 10:50:53 +1000 Subject: [PATCH 09/10] EKF: improve height reset logic Don't use failed sensors unless necessary and handle case where not height sensor is available for reset --- EKF/control.cpp | 118 +++++++++++++++++++++++++++++++++++++++--------- 1 file changed, 97 insertions(+), 21 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index ad7456cb02..3ad036e769 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -185,6 +185,9 @@ void Ekf::controlFusionModes() } if ((P[8][8] > sq(_params.hgt_reset_lim)) && ((_time_last_imu - _time_last_hgt_fuse) > 5e6)) { + // boolean that indicates we will do a height reset + bool reset_height = false; + // handle the case where we are using baro for height if (_control_status.flags.baro_hgt) { // check if GPS height is available @@ -197,18 +200,42 @@ void Ekf::controlFusionModes() // check for inertial sensing errors in the last 10 seconds bool prev_bad_vert_accel = (_time_last_imu - _time_bad_vert_accel < 10E6); - // continue to use baro if it is available and we have detected bad IMU data or inadequate GPS - // switch to GPS if GPS data is available or we do not have Baro - if (baro_hgt_available && (prev_bad_vert_accel || !gps_hgt_available || !gps_hgt_accurate)) { - printf("EKF baro hgt timeout - reset to baro\n"); - } else if (gps_hgt_available && !prev_bad_vert_accel) { - // declare the baro as unhealthy + // reset to GPS if adequate GPS data is available and the timeout cannot be blamed on IMU data + bool reset_to_gps = gps_hgt_available && gps_hgt_accurate && !_gps_hgt_faulty && !prev_bad_vert_accel; + + // reset to GPS if GPS data is available and there is no Baro data + reset_to_gps = reset_to_gps || (gps_hgt_available && !baro_hgt_available); + + // reset to Baro if we are not doing a GPS reset and baro data is available + bool reset_to_baro = !reset_to_gps && baro_hgt_available; + + if (reset_to_gps) { + // set height sensor health _baro_hgt_faulty = true; - // set the height mode to the GPS + _gps_hgt_faulty = false; + // declare the GPS height healthy + _gps_hgt_faulty = false; + // reset the height mode _control_status.flags.baro_hgt = false; _control_status.flags.gps_hgt = true; _control_status.flags.rng_hgt = false; + // request a reset + reset_height = true; printf("EKF baro hgt timeout - reset to GPS\n"); + } else if (reset_to_baro){ + // set height sensor health + _baro_hgt_faulty = false; + // reset the height mode + _control_status.flags.baro_hgt = true; + _control_status.flags.gps_hgt = false; + _control_status.flags.rng_hgt = false; + // request a reset + reset_height = true; + printf("EKF baro hgt timeout - reset to baro\n"); + } else { + // we have nothing we can reset to + // deny a reset + reset_height = false; } } @@ -224,15 +251,39 @@ void Ekf::controlFusionModes() float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset); bool baro_data_consistent = fabsf(baro_innov) < (sq(_params.baro_noise) + P[8][8]) * sq(_params.baro_innov_gate); - // if baro data is consistent and fresh or GPS height is unavailable or inaccurate, we switch to baro for height - if ((baro_data_consistent && baro_data_fresh) || !gps_hgt_available || !gps_hgt_accurate) { - // declare the GPS height unhealthy + // if baro data is acceptable and GPS data is inaccurate, reset height to baro + bool reset_to_baro = baro_data_consistent && baro_data_fresh && !_baro_hgt_faulty && !gps_hgt_accurate; + + // if GPS height is unavailable and baro data is available, reset height to baro + reset_to_baro = reset_to_baro || (!gps_hgt_available && baro_data_fresh); + + // if we cannot switch to baro and GPs data is available, reset height to GPS + bool reset_to_gps = !reset_to_baro && gps_hgt_available; + + if (reset_to_baro) { + // set height sensor health _gps_hgt_faulty = true; - // set the height mode to the baro + _baro_hgt_faulty = false; + // reset the height mode _control_status.flags.baro_hgt = true; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; - printf("EKF gps hgt timeout - switching to baro\n"); + // request a reset + reset_height = true; + printf("EKF gps hgt timeout - reset to baro\n"); + } else if (reset_to_gps) { + // set height sensor health + _gps_hgt_faulty = false; + // reset the height mode + _control_status.flags.baro_hgt = false; + _control_status.flags.gps_hgt = true; + _control_status.flags.rng_hgt = false; + // request a reset + reset_height = true; + printf("EKF gps hgt timeout - reset to GPS\n"); + } else { + // we have nothing to reset to + reset_height = false; } } @@ -247,25 +298,50 @@ void Ekf::controlFusionModes() // check if baro data is consistent float baro_innov = _state.pos(2) - (_hgt_sensor_offset - baro_init.hgt + _baro_hgt_offset); bool baro_data_consistent = sq(baro_innov) < (sq(_params.baro_noise) + P[8][8]) * sq(_params.baro_innov_gate); - // switch to baro if necessary or preferable - bool switch_to_baro = (!rng_data_available && baro_data_available) || (baro_data_consistent && baro_data_available); - if (switch_to_baro) { - // declare the range finder height unhealthy + // reset to baro if data is available and we have no range data + bool reset_to_baro = !rng_data_available && baro_data_available; + + // reset to baro if data is acceptable + reset_to_baro = reset_to_baro || (baro_data_consistent && baro_data_available && !_baro_hgt_faulty); + + // reset to range data if it is available and we cannot switch to baro + bool reset_to_rng = !reset_to_baro && rng_data_available; + + if (reset_to_baro) { + // set height sensor health _rng_hgt_faulty = true; - // set the height mode to the baro + _baro_hgt_faulty = false; + // reset the height mode _control_status.flags.baro_hgt = true; _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = false; - printf("EKF rng hgt timeout - switching to baro\n"); + // request a reset + reset_height = true; + printf("EKF rng hgt timeout - reset to baro\n"); + } else if (reset_to_rng) { + // set height sensor health + _rng_hgt_faulty = false; + // reset the height mode + _control_status.flags.baro_hgt = false; + _control_status.flags.gps_hgt = false; + _control_status.flags.rng_hgt = true; + // request a reset + reset_height = true; + printf("EKF rng hgt timeout - reset to rng hgt\n"); + } else { + // we have nothing to reset to + reset_height = false; } } // Reset vertical position and velocity states to the last measurement - resetHeight(); + if (reset_height) { + resetHeight(); + // Reset the timout timer + _time_last_hgt_fuse = _time_last_imu; + } - // Reset the timout timer - _time_last_hgt_fuse = _time_last_imu; } // handle the case when we are relying on optical flow fusion and lose it From 4fcbfb5d42ae1d1d8a175ba04511df9506539810 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 23 Apr 2016 07:55:25 +1000 Subject: [PATCH 10/10] EKF: correct error in comment --- EKF/ekf.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/ekf.h b/EKF/ekf.h index b28d2c9c3f..754786161e 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -216,7 +216,7 @@ private: uint64_t _time_last_mag; // measurement time of last magnetomter sample Vector3f _mag_filt_state; // filtered magnetometer measurement Vector3f _delVel_sum; // summed delta velocity - float _hgt_sensor_offset; // value subtracted from the height measurement to maintain consistency after switching height sensors (m) + float _hgt_sensor_offset; // set as necessary if desired to maintain the same height after a height reset (m) gps_check_fail_status_u _gps_check_fail_status;