diff --git a/EKF/control.cpp b/EKF/control.cpp index e2583f5af3..8ca8eae32c 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -125,6 +125,7 @@ void Ekf::controlFusionModes() if (_control_status.flags.yaw_align) { _control_status.flags.gps = true; _time_last_gps = _time_last_imu; + // if we are not already aiding with optical flow, then we need to reset the position and velocity if (!_control_status.flags.opt_flow) { _control_status.flags.gps = resetPosition(); @@ -159,8 +160,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(); } @@ -247,10 +248,22 @@ void Ekf::controlFusionModes() } // Control the soure of height measurements for the main filter - _control_status.flags.baro_hgt = true; - _control_status.flags.rng_hgt = false; - _control_status.flags.gps_hgt = false; + if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) { + _control_status.flags.baro_hgt = true; + _control_status.flags.rng_hgt = false; + _control_status.flags.gps_hgt = false; + + } else if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE) { + _control_status.flags.baro_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 // TODO add methods for true airspeed and/or sidelsip fusion or some type of drag force measurement diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 8fe2e016ff..51f3a64fa9 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -40,6 +40,7 @@ */ #include "ekf.h" +#include "mathlib.h" Ekf::Ekf(): _filter_initialised(false), @@ -70,13 +71,16 @@ Ekf::Ekf(): _last_gps_origin_time_us(0), _gps_alt_ref(0.0f), _hgt_counter(0), + _time_last_hgt(0), _hgt_sum(0.0f), _mag_counter(0), + _time_last_mag(0), _hgt_at_alignment(0.0f), _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 = {}; @@ -99,7 +103,7 @@ Ekf::Ekf(): _mag_sum = {}; _delVel_sum = {}; _flow_gyro_bias = {}; - _imu_del_ang_of = {}; + _imu_del_ang_of = {}; } Ekf::~Ekf() @@ -156,8 +160,6 @@ bool Ekf::init(uint64_t timestamp) bool Ekf::update() { - // Only run the filter if IMU data in the buffer has been updated - if (_imu_updated) { if (!_filter_initialised) { _filter_initialised = initialiseFilter(); @@ -166,6 +168,9 @@ bool Ekf::update() } } + // Only run the filter if IMU data in the buffer has been updated + if (_imu_updated) { + // perform state and covariance prediction for the main filter predictState(); predictCovariance(); @@ -207,21 +212,43 @@ 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; // only use range finder as a height observation in the main filter if specifically enabled - if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE) { + if (_control_status.flags.rng_hgt) { _fuse_height = true; } + } 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) - && _params.vdist_sensor_type == VDIST_SENSOR_BARO) { - _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 + float offset_error = _baro_sample_delayed.hgt + _state.pos(2) - _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 @@ -233,15 +260,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); @@ -303,22 +330,27 @@ bool Ekf::initialiseFilter(void) if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE) { rangeSample range_init = _range_buffer.get_newest(); - if (range_init.time_us != 0) { + if (range_init.time_us != _time_last_hgt) { _hgt_counter ++; _hgt_sum += range_init.rng; + _time_last_hgt = range_init.time_us; } - } else { + } else if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) { // initialize vertical position with newest baro measurement baroSample baro_init = _baro_buffer.get_newest(); - if (baro_init.time_us != 0) { + if (baro_init.time_us != _time_last_hgt) { _hgt_counter ++; _hgt_sum += baro_init.hgt; + _time_last_hgt = baro_init.time_us; } + + } else { + return false; } - // check to see if we have enough measruements and return false if not + // check to see if we have enough measurements and return false if not if (_hgt_counter < 10 || _mag_counter < 10) { return false; @@ -361,9 +393,14 @@ bool Ekf::initialiseFilter(void) // calculate the initial magnetic field and yaw alignment resetMagHeading(mag_init); - // calculate the averaged barometer 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 89a590d732..cc3d7cb67d 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -197,9 +197,11 @@ private: float _gps_alt_ref; // WGS-84 height (m) // Variables used to initialise the filter states - uint8_t _hgt_counter; // number of baro samples averaged - float _hgt_sum; // summed baro measurement + 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 + uint64_t _time_last_mag; // measurement time of last magnetomter sample Vector3f _mag_sum; // summed magnetometer measurement Vector3f _delVel_sum; // summed delta velocity float _hgt_at_alignment; // baro offset relative to alignment position @@ -214,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 43713992e4..ae60f8b342 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -87,7 +87,7 @@ bool Ekf::resetPosition() // Reset height state using the last height measurement void Ekf::resetHeight() { - if (_params.vdist_sensor_type == VDIST_SENSOR_RANGE) { + if (_control_status.flags.rng_hgt) { rangeSample range_newest = _range_buffer.get_newest(); if (_time_last_imu - range_newest.time_us < 200000) { @@ -96,7 +96,12 @@ void Ekf::resetHeight() } else { // TODO: reset to last known range based estimate } - } else { + + // 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,12 @@ 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/gps_checks.cpp b/EKF/gps_checks.cpp index 3a2a5a220c..e30c676b22 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -56,7 +56,7 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps) { // If we have defined the WGS-84 position of the NED origin, run gps quality checks until they pass, then define the origins WGS-84 position using the last GPS fix - if (!_NED_origin_initialised ) { + 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"); @@ -64,12 +64,14 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps) double lat = gps->lat / 1.0e7; double lon = gps->lon / 1.0e7; map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu); + // if we are already doing aiding, corect for the change in posiiton since the EKF started navigating if (_control_status.flags.opt_flow || _control_status.flags.gps) { double est_lat, est_lon; map_projection_reproject(&_pos_ref, -_state.pos(0), -_state.pos(1), &est_lat, &est_lon); map_projection_init_timestamped(&_pos_ref, est_lat, est_lon, _time_last_imu); } + // Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin _gps_alt_ref = 1e-3f * (float)gps->alt + _state.pos(2); _NED_origin_initialised = true; diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index c4f3655d71..9879bbecb4 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -78,7 +78,8 @@ void Ekf::predictHagl() _terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_p_noise); // process noise due to terrain gradient - _terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel(1))); + _terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_gradient) * (sq(_state.vel(0)) + sq(_state.vel( + 1))); // limit the variance to prevent it becoming badly conditioned _terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f); 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);