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; 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 a6729eeb8d..3ad036e769 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; } } } @@ -166,7 +170,24 @@ 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)) { + // 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 @@ -176,17 +197,45 @@ 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)) { - // declare the baro as unhealthy + // check for inertial sensing errors in the last 10 seconds + bool prev_bad_vert_accel = (_time_last_imu - _time_bad_vert_accel < 10E6); + + // 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; - // 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"); + // 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; } } @@ -202,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; } } @@ -225,22 +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; + } + } // 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 74a9562dc8..02caeea693 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), @@ -85,7 +85,12 @@ 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), + _time_bad_vert_accel(0) { _state = {}; _last_known_posNE.setZero(); @@ -384,12 +389,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; } } @@ -402,12 +407,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; } } @@ -465,17 +470,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 @@ -484,6 +484,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; } } diff --git a/EKF/ekf.h b/EKF/ekf.h index e2df78fdac..754786161e 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; @@ -169,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 @@ -208,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; // set as necessary if desired to maintain the same height after a height reset (m) gps_check_fail_status_u _gps_check_fail_status; @@ -231,7 +234,14 @@ 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) + 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 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); + + } } 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