From 6b2e2dba909a44430dc3879e73e990231fcc2206 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 15 Mar 2016 17:07:33 +1100 Subject: [PATCH] EKF: Add GPS height option and improve height recovery --- EKF/common.h | 11 ++++ EKF/control.cpp | 102 ++++++++++++++++++++++++++----- EKF/ekf.cpp | 116 ++++++++++++++++++++++-------------- EKF/ekf.h | 23 ++++--- EKF/ekf_helper.cpp | 26 +++++--- EKF/estimator_interface.cpp | 37 ++++++------ EKF/estimator_interface.h | 9 ++- EKF/gps_checks.cpp | 12 +++- EKF/mag_fusion.cpp | 1 - EKF/vel_pos_fusion.cpp | 36 +++++++---- 10 files changed, 261 insertions(+), 112 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 291e3df73a..8fd9d2652b 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -90,6 +90,9 @@ struct gpsSample { Vector2f pos; // NE earth frame gps horizontal position measurement in m float hgt; // gps height measurement in m Vector3f vel; // NED earth frame gps velocity measurement in m/s + float hacc; // 1-std horizontal position error m + float vacc; // 1-std vertical position error m + float sacc; // 1-std speed error m/s uint64_t time_us; // timestamp in microseconds }; @@ -142,6 +145,11 @@ struct flowSample { #define MAG_FUSE_TYPE_3D 2 // Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions #define MAG_FUSE_TYPE_2D 3 // A 2D fusion that uses the horizontal projection of the magnetic fields measurement will alays be used. This is less accurate, but less affected by earth field distortions. +// Maximum sensor intervals in usec +#define GPS_MAX_INTERVAL 5e5 +#define BARO_MAX_INTERVAL 2e5 +#define RNG_MAX_INTERVAL 2e5 + struct parameters { // measurement source control int fusion_mode; // bitmasked integer that selects which of the GPS and optical flow aiding sources will be used @@ -176,6 +184,7 @@ struct parameters { float baro_innov_gate; // barometric height innovation consistency gate size (STD) float posNE_innov_gate; // GPS horizontal position innovation consistency gate size (STD) float vel_innov_gate; // GPS velocity innovation consistency gate size (STD) + float hgt_reset_lim; // The maximum 1-sigma uncertainty in height that can be tolerated before the height state is reset (m) // magnetometer fusion float mag_heading_noise; // measurement noise used for simple heading fusion (rad) @@ -245,6 +254,7 @@ struct parameters { baro_innov_gate = 3.0f; posNE_innov_gate = 3.0f; vel_innov_gate = 3.0f; + hgt_reset_lim = 5.0f; // magnetometer fusion mag_heading_noise = 1.7e-1f; @@ -341,4 +351,5 @@ union filter_control_status_u { } flags; uint16_t value; }; + } diff --git a/EKF/control.cpp b/EKF/control.cpp index 8ca8eae32c..8fc114a045 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -158,10 +158,85 @@ 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 - if (_time_last_imu - _time_last_hgt_fuse > 10e6) { + /* + * Handle the case where we have not fused height measurements recently and + * uncertainty exceeds the max allowable. Reset using the best available height + * measurement source, continue using it after the reset and declare the current + * source failed if we have switched. + */ + 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) { + // check if GPS height is available + gpsSample gps_init = _gps_buffer.get_newest(); + bool gps_hgt_available = ((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL); + bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc); + 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 + _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"); + } + } + + // handle the case we are using GPS for height + if (_control_status.flags.gps_hgt) { + // check if GPS height is available + gpsSample gps_init = _gps_buffer.get_newest(); + bool gps_hgt_available = ((_time_last_imu - gps_init.time_us) < 2 * GPS_MAX_INTERVAL); + bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc); + // check the baro height source for consistency and freshness + baroSample baro_init = _baro_buffer.get_newest(); + bool baro_data_fresh = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); + 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 + _gps_hgt_faulty = true; + // set the height mode to the baro + _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"); + } + } + + // handle the case we are using range finder for height + if (_control_status.flags.rng_hgt) { + // check if range finder data is available + rangeSample rng_init = _range_buffer.get_newest(); + bool rng_data_available = ((_time_last_imu - rng_init.time_us) < 2 * RNG_MAX_INTERVAL); + // check if baro data is available + baroSample baro_init = _baro_buffer.get_newest(); + bool baro_data_available = ((_time_last_imu - baro_init.time_us) < 2 * BARO_MAX_INTERVAL); + // 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 + _rng_hgt_faulty = true; + // set the height mode to the baro + _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"); + } + } + // Reset vertical position and velocity states to the last measurement resetHeight(); } @@ -248,21 +323,20 @@ void Ekf::controlFusionModes() } // Control the soure of height measurements for the main filter - if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) { + if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO && !_baro_hgt_faulty) || _control_status.flags.baro_hgt) { _control_status.flags.baro_hgt = true; - _control_status.flags.rng_hgt = false; _control_status.flags.gps_hgt = false; + _control_status.flags.rng_hgt = false; - } else if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE) { + } else if ((_params.vdist_sensor_type == VDIST_SENSOR_GPS && !_gps_hgt_faulty) || _control_status.flags.gps_hgt) { _control_status.flags.baro_hgt = false; + _control_status.flags.gps_hgt = true; + _control_status.flags.rng_hgt = false; + + } else if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE && !_rng_hgt_faulty) { + _control_status.flags.baro_hgt = false; + _control_status.flags.gps_hgt = false; _control_status.flags.rng_hgt = true; - _control_status.flags.gps_hgt = false; - - } else { - // TODO functionality to fuse GPS height - _control_status.flags.baro_hgt = false; - _control_status.flags.rng_hgt = false; - _control_status.flags.gps_hgt = false; } // Placeholder for control of wind velocity states estimation diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 252cb54e98..ffe593f924 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -71,15 +71,17 @@ Ekf::Ekf(): _last_gps_origin_time_us(0), _gps_alt_ref(0.0f), _hgt_counter(0), - _time_last_hgt(0), - _hgt_sum(0.0f), + _hgt_filt_state(0.0f), _mag_counter(0), _time_last_mag(0), - _hgt_at_alignment(0.0f), + _hgt_sensor_offset(0.0f), _terrain_vpos(0.0f), _hagl_innov(0.0f), _hagl_innov_var(0.0f), _time_last_hagl_fuse(0), + _baro_hgt_faulty(false), + _gps_hgt_faulty(false), + _rng_hgt_faulty(false), _baro_hgt_offset(0.0f) { _control_status = {}; @@ -100,7 +102,7 @@ Ekf::Ekf(): _last_known_posNE.setZero(); _imu_down_sampled = {}; _q_down_sampled.setZero(); - _mag_sum = {}; + _mag_filt_state = {}; _delVel_sum = {}; _flow_gyro_bias = {}; _imu_del_ang_of = {}; @@ -160,13 +162,13 @@ bool Ekf::init(uint64_t timestamp) bool Ekf::update() { - if (!_filter_initialised) { - _filter_initialised = initialiseFilter(); + if (!_filter_initialised) { + _filter_initialised = initialiseFilter(); - if (!_filter_initialised) { - return false; - } + if (!_filter_initialised) { + return false; } + } // Only run the filter if IMU data in the buffer has been updated if (_imu_updated) { @@ -221,22 +223,16 @@ bool Ekf::update() _fuse_height = true; } - } else if ((_time_last_imu - _time_last_hgt_fuse) > 5e5 && _control_status.flags.rng_hgt) { + } else if ((_time_last_imu - _time_last_hgt_fuse) > 2 * RNG_MAX_INTERVAL && _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 @@ -246,16 +242,25 @@ bool Ekf::update() } else { // calculate a filtered offset between the baro origin and local NED origin if we are not using the baro as a height reference - float offset_error = _baro_sample_delayed.hgt + _state.pos(2) - _baro_hgt_offset; + float offset_error = _state.pos(2) + _baro_sample_delayed.hgt - _hgt_sensor_offset - _baro_hgt_offset; _baro_hgt_offset += 0.02f * math::constrain(offset_error, -5.0f, 5.0f); } } // If we are using GPS aiding and data has fallen behind the fusion time horizon then fuse it - if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed) && _control_status.flags.gps) { - _fuse_pos = true; - _fuse_vert_vel = true; - _fuse_hor_vel = true; + if (_gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed)) { + // Only use GPS data for position and velocity aiding if enabled + if (_control_status.flags.gps) { + _fuse_pos = true; + _fuse_vert_vel = true; + _fuse_hor_vel = true; + } + + // only use gps height observation in the main filter if specifically enabled + if (_control_status.flags.gps_hgt) { + _fuse_height = true; + } + } // If we are using optical flow aiding and data has fallen behind the fusion time horizon, then fuse it @@ -320,30 +325,44 @@ bool Ekf::initialiseFilter(void) _delVel_sum += imu_init.delta_vel; // Sum the magnetometer measurements - magSample mag_init = _mag_buffer.get_newest(); - - if (mag_init.time_us != 0) { - _mag_counter ++; - _mag_sum += mag_init.mag; - } - - if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE) { - rangeSample range_init = _range_buffer.get_newest(); - - if (range_init.time_us != _time_last_hgt) { - _hgt_counter ++; - _hgt_sum += range_init.rng; - _time_last_hgt = range_init.time_us; + if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) { + if (_mag_counter == 0) { + _mag_filt_state = _mag_sample_delayed.mag; } - } else if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) { - // initialize vertical position with newest baro measurement - baroSample baro_init = _baro_buffer.get_newest(); + _mag_counter ++; + _mag_filt_state = _mag_filt_state * 0.9f + _mag_sample_delayed.mag * 0.1f; + } + + // set the default height source from the adjustable parameter + if (_hgt_counter == 0) { + _primary_hgt_source = _params.vdist_sensor_type; + } + + if (_primary_hgt_source == VDIST_SENSOR_RANGE) { + if (_range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)) { + if (_hgt_counter == 0) { + _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; + } - if (baro_init.time_us != _time_last_hgt) { _hgt_counter ++; - _hgt_sum += baro_init.hgt; - _time_last_hgt = baro_init.time_us; + _hgt_filt_state = 0.9f * _hgt_filt_state + 0.1f * _range_sample_delayed.rng; + } + + } else if (_primary_hgt_source == VDIST_SENSOR_BARO || _primary_hgt_source == VDIST_SENSOR_GPS) { + if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) { + if (_hgt_counter == 0) { + _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; + } + + _hgt_counter ++; + _hgt_filt_state = 0.9f * _hgt_filt_state + 0.1f * _baro_sample_delayed.hgt; } } else { @@ -351,10 +370,14 @@ bool Ekf::initialiseFilter(void) } // check to see if we have enough measurements and return false if not - if (_hgt_counter < 10 || _mag_counter < 10) { + if (_hgt_counter <= 10 || _mag_counter <= 10) { return false; } else { + // reset variables that are shared with post alignment GPS checks + _gps_drift_velD = 0.0f; + _gps_alt_ref = 0.0f; + // Zero all of the states _state.ang_error.setZero(); _state.vel.setZero(); @@ -388,19 +411,20 @@ bool Ekf::initialiseFilter(void) _tilt_err_length_filt = 1.0f; // calculate the averaged magnetometer reading - Vector3f mag_init = _mag_sum * (1.0f / (float(_mag_counter))); + Vector3f mag_init = _mag_filt_state; // calculate the initial magnetic field and yaw alignment resetMagHeading(mag_init); // calculate the averaged height reading to calulate the height of the origin - _hgt_at_alignment = _hgt_sum / (float)_hgt_counter; + _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 (_params.vdist_sensor_type != VDIST_SENSOR_BARO) { + if (!_control_status.flags.baro_hgt) { baroSample baro_newest = _baro_buffer.get_newest(); - _baro_hgt_offset = baro_newest.hgt - _hgt_at_alignment; + _baro_hgt_offset = baro_newest.hgt - _hgt_sensor_offset; + } else { _baro_hgt_offset = 0.0f; } diff --git a/EKF/ekf.h b/EKF/ekf.h index cc3d7cb67d..51a01a9be2 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -98,12 +98,6 @@ public: bool collect_gps(uint64_t time_usec, struct gps_message *gps); bool collect_imu(imuSample &imu); - // this is the current status of the filter control modes - filter_control_status_u _control_status; - - // this is the previous status of the filter control modes - used to detect mode transitions - filter_control_status_u _control_status_prev; - // get the ekf WGS-84 origin position and height and the system time it was last set void get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt); @@ -197,14 +191,13 @@ private: float _gps_alt_ref; // WGS-84 height (m) // Variables used to initialise the filter states - uint8_t _hgt_counter; // number of height samples averaged - uint64_t _time_last_hgt; // measurement time of last height sample - float _hgt_sum; // summed height measurement - uint8_t _mag_counter; // number of magnetometer samples averaged + uint32_t _hgt_counter; // number of height samples taken + float _hgt_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_sum; // summed magnetometer measurement + Vector3f _mag_filt_state; // filtered magnetometer measurement Vector3f _delVel_sum; // summed delta velocity - float _hgt_at_alignment; // baro offset relative to alignment position + 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) gps_check_fail_status_u _gps_check_fail_status; @@ -216,6 +209,12 @@ 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 + // height sensor fault status + bool _baro_hgt_faulty; // true if valid baro data is unavailable for use + bool _gps_hgt_faulty; // true if valid gps height data is unavailable for use + 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) // update the real time complementary filter states. This includes the prediction diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index ae60f8b342..f74fb7392d 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -90,8 +90,8 @@ void Ekf::resetHeight() if (_control_status.flags.rng_hgt) { rangeSample range_newest = _range_buffer.get_newest(); - if (_time_last_imu - range_newest.time_us < 200000) { - _state.pos(2) = _hgt_at_alignment - range_newest.rng; + if (_time_last_imu - range_newest.time_us < 2 * RNG_MAX_INTERVAL) { + _state.pos(2) = _hgt_sensor_offset - range_newest.rng; } else { // TODO: reset to last known range based estimate @@ -105,18 +105,28 @@ void Ekf::resetHeight() // initialize vertical position with newest baro measurement baroSample baro_newest = _baro_buffer.get_newest(); - if (_time_last_imu - baro_newest.time_us < 200000) { - _state.pos(2) = _hgt_at_alignment - baro_newest.hgt; + if (_time_last_imu - baro_newest.time_us < 2 * BARO_MAX_INTERVAL) { + _state.pos(2) = _hgt_sensor_offset - baro_newest.hgt + _baro_hgt_offset; } 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 if (_control_status.flags.gps_hgt) { + // initialize vertical position and velocity with newest gps measurement + gpsSample gps_newest = _gps_buffer.get_newest(); - } else { - // TODO: reset to GPS height + if (_time_last_imu - gps_newest.time_us < 2 * GPS_MAX_INTERVAL) { + _state.pos(2) = _hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref; + _state.vel(2) = gps_newest.vel(2); + + } else { + // TODO: reset to last known gps 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); } } diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 766164082c..7be7c9ba58 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -56,8 +56,6 @@ EstimatorInterface::EstimatorInterface(): _in_air(false), _NED_origin_initialised(false), _gps_speed_valid(false), - _gps_speed_accuracy(0.0f), - _gps_hpos_accuracy(0.0f), _gps_origin_eph(0.0f), _gps_origin_epv(0.0f), _mag_healthy(false), @@ -145,12 +143,8 @@ void EstimatorInterface::setMagData(uint64_t time_usec, float *data) void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps) { - if (!collect_gps(time_usec, gps) || !_initialised) { - return; - } - - // Only use GPS data if we have a 3D fix and limit the GPS data rate to a maximum of 14Hz - if (time_usec - _time_last_gps > 70000 && gps->fix_type >= 3) { + // Limit the GPS data rate to a maximum of 14Hz + if (time_usec - _time_last_gps > 70000) { gpsSample gps_sample_new = {}; gps_sample_new.time_us = gps->time_usec - _params.gps_delay_ms * 1000; @@ -162,15 +156,24 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps) memcpy(gps_sample_new.vel._data[0], gps->vel_ned, sizeof(gps_sample_new.vel._data)); _gps_speed_valid = gps->vel_ned_valid; - _gps_speed_accuracy = gps->sacc; - _gps_hpos_accuracy = gps->eph; + gps_sample_new.sacc = gps->sacc; + gps_sample_new.hacc = gps->eph; + gps_sample_new.vacc = gps->epv; - float lpos_x = 0.0f; - float lpos_y = 0.0f; - map_projection_project(&_pos_ref, (gps->lat / 1.0e7), (gps->lon / 1.0e7), &lpos_x, &lpos_y); - gps_sample_new.pos(0) = lpos_x; - gps_sample_new.pos(1) = lpos_y; - gps_sample_new.hgt = gps->alt / 1e3f; + gps_sample_new.hgt = (float)gps->alt * 1e-3f; + + // Only calculate the relative position if the WGS-84 location of the origin is set + if (collect_gps(time_usec, gps)) { + float lpos_x = 0.0f; + float lpos_y = 0.0f; + map_projection_project(&_pos_ref, (gps->lat / 1.0e7), (gps->lon / 1.0e7), &lpos_x, &lpos_y); + gps_sample_new.pos(0) = lpos_x; + gps_sample_new.pos(1) = lpos_y; + + } else { + gps_sample_new.pos(0) = 0.0f; + gps_sample_new.pos(1) = 0.0f; + } _gps_buffer.push(gps_sample_new); } @@ -291,7 +294,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) _airspeed_buffer.allocate(OBS_BUFFER_LENGTH) && _flow_buffer.allocate(OBS_BUFFER_LENGTH) && _output_buffer.allocate(IMU_BUFFER_LENGTH))) { - printf("Estimator Buffer Allocation failed!"); + printf("EKF buffer allocation failed!"); unallocate_buffers(); return false; } diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index 400445dcc6..2e5a3aca1a 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -222,8 +222,6 @@ protected: bool _NED_origin_initialised = false; bool _gps_speed_valid = false; - float _gps_speed_accuracy = 0.0f; // GPS receiver reported 1-sigma speed accuracy (m/s) - float _gps_hpos_accuracy = 0.0f; // GPS receiver reported 1-sigma horizontal accuracy (m) float _gps_origin_eph = 0.0f; // horizontal position uncertainty of the GPS origin float _gps_origin_epv = 0.0f; // vertical position uncertainty of the GPS origin struct map_projection_reference_s _pos_ref = {}; // Contains WGS-84 position latitude and longitude (radians) @@ -262,4 +260,11 @@ protected: float _mag_declination_gps; // magnetic declination returned by the geo library using the last valid GPS position (rad) float _mag_declination_to_save_deg; // magnetic declination to save to EKF2_MAG_DECL (deg) + + // this is the current status of the filter control modes + filter_control_status_u _control_status; + + // this is the previous status of the filter control modes - used to detect mode transitions + filter_control_status_u _control_status_prev; + }; diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index e30c676b22..a5785ed8ee 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -59,7 +59,7 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps) if (!_NED_origin_initialised) { // we have good GPS data so can now set the origin's WGS-84 position if (gps_is_good(gps) && !_NED_origin_initialised) { - printf("gps is good - setting EKF origin\n"); + printf("EKF gps is good - setting origin\n"); // Set the origin's WGS-84 position to the last gps fix double lat = gps->lat / 1.0e7; double lon = gps->lon / 1.0e7; @@ -81,6 +81,16 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps) // save the horizontal and vertical position uncertainty of the origin _gps_origin_eph = gps->eph; _gps_origin_epv = gps->epv; + + // if the user has selected GPS as the primary height source, switch across to using it + if (_primary_hgt_source == VDIST_SENSOR_GPS) { + printf("EKF switching to GPS height\n"); + _control_status.flags.baro_hgt = false; + _control_status.flags.gps_hgt = true; + _control_status.flags.rng_hgt = false; + // zero the sensor offset + _hgt_sensor_offset = 0.0f; + } } } diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index fc08f3817f..689bdf23ca 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -1007,7 +1007,6 @@ void Ekf::fuseMag2D() // we allow to use it when on the ground because the large innovation could be caused // by interference or a large initial gyro bias if (_control_status.flags.in_air) { - printf("return 5\n"); return; } else { diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 44c491b498..dffb767b2b 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -60,7 +60,7 @@ void Ekf::fuseVelPosHeight() _vel_pos_innov[1] = _state.vel(1) - _gps_sample_delayed.vel(1); // observation variance - use receiver reported accuracy with parameter setting the minimum value R[0] = fmaxf(_params.gps_vel_noise, 0.01f); - R[0] = fmaxf(R[0], _gps_speed_accuracy); + R[0] = fmaxf(R[0], _gps_sample_delayed.sacc); R[0] = R[0] * R[0]; R[1] = R[0]; // innovation gate sizes @@ -75,7 +75,7 @@ void Ekf::fuseVelPosHeight() // observation variance - use receiver reported accuracy with parameter setting the minimum value R[2] = fmaxf(_params.gps_vel_noise, 0.01f); // use scaled horizontal speed accuracy assuming typical ratio of VDOP/HDOP - R[2] = 1.5f * fmaxf(R[2], _gps_speed_accuracy); + R[2] = 1.5f * fmaxf(R[2], _gps_sample_delayed.sacc); R[2] = R[2] * R[2]; // innovation gate size gate_size[2] = fmaxf(_params.vel_innov_gate, 1.0f); @@ -95,7 +95,7 @@ void Ekf::fuseVelPosHeight() } else { float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f); float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit); - R[3] = math::constrain(_gps_hpos_accuracy, lower_limit, upper_limit); + R[3] = math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit); } @@ -110,13 +110,26 @@ 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 + _baro_hgt_offset); + _vel_pos_innov[5] = _state.pos(2) + _baro_sample_delayed.hgt - _baro_hgt_offset - _hgt_sensor_offset; // observation variance - user parameter defined R[5] = fmaxf(_params.baro_noise, 0.01f); R[5] = R[5] * R[5]; // innovation gate size gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f); + } else if (_control_status.flags.gps_hgt) { + fuse_map[5] = true; + // vertical position innovation - gps measurement has opposite sign to earth z axis + _vel_pos_innov[5] = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset; + // observation variance - receiver defined and parameter limited + // use scaled horizontal position accuracy assuming typical ratio of VDOP/HDOP + float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f); + float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit); + R[5] = 1.5f * math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit); + R[5] = R[5] * R[5]; + // innovation gate size + gate_size[5] = fmaxf(_params.baro_innov_gate, 1.0f); + } else if (_control_status.flags.rng_hgt && (_R_prev(2, 2) > 0.7071f)) { fuse_map[5] = true; // use range finder with tilt correction @@ -128,9 +141,10 @@ void Ekf::fuseVelPosHeight() // innovation gate size gate_size[5] = fmaxf(_params.range_innov_gate, 1.0f); } + } - // calculate innovation test ratios +// calculate innovation test ratios for (unsigned obs_index = 0; obs_index < 6; obs_index++) { if (fuse_map[obs_index]) { // compute the innovation variance SK = HPH + R @@ -142,9 +156,9 @@ void Ekf::fuseVelPosHeight() } } - // check position, velocity and height innovations - // treat 3D velocity, 2D position and height as separate sensors - // always pass position checks if using synthetic position measurements +// check position, velocity and height innovations +// treat 3D velocity, 2D position and height as separate sensors +// always pass position checks if using synthetic position measurements bool vel_check_pass = (_vel_pos_test_ratio[0] <= 1.0f) && (_vel_pos_test_ratio[1] <= 1.0f) && (_vel_pos_test_ratio[2] <= 1.0f); innov_check_pass_map[2] = innov_check_pass_map[1] = innov_check_pass_map[0] = vel_check_pass; @@ -154,19 +168,19 @@ void Ekf::fuseVelPosHeight() innov_check_pass_map[4] = innov_check_pass_map[3] = pos_check_pass; innov_check_pass_map[5] = (_vel_pos_test_ratio[5] <= 1.0f); - // record the successful velocity fusion time +// record the successful velocity fusion time if (vel_check_pass && _fuse_hor_vel) { _time_last_vel_fuse = _time_last_imu; _tilt_err_vec.setZero(); } - // record the successful position fusion time +// record the successful position fusion time if (pos_check_pass && _fuse_pos) { _time_last_pos_fuse = _time_last_imu; _tilt_err_vec.setZero(); } - // record the successful height fusion time +// record the successful height fusion time if (innov_check_pass_map[5] && _fuse_height) { _time_last_hgt_fuse = _time_last_imu; }