diff --git a/EKF/control.cpp b/EKF/control.cpp index 92d40d0131..d1401ee282 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -1131,7 +1131,7 @@ void Ekf::controlHeightFusion() gps_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f); // fuse height information fuseVerticalPosition(_gps_pos_innov,gps_hgt_innov_gate, - gps_hgt_obs_var, _gps_pos_innov_var,_gps_pos_test_ratio); + gps_hgt_obs_var, _gps_pos_innov_var, _gps_pos_test_ratio); } else if (_control_status.flags.rng_hgt) { Vector2f rng_hgt_innov_gate; diff --git a/EKF/ekf.h b/EKF/ekf.h index e2f379eabb..ade36dd280 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -151,7 +151,12 @@ public: // get the ekf WGS-84 origin position and height and the system time it was last set // return true if the origin is valid - bool get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) const; + bool getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const; + bool setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude); + + float getEkfGlobalOriginAltitude() const { return _gps_alt_ref; } + bool setEkfGlobalOriginAltitude(const float altitude); + // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const; @@ -621,6 +626,8 @@ private: inline void resetHorizontalPositionTo(const Vector2f &new_horz_pos); + inline void resetVerticalPositionTo(const float &new_vert_pos); + void resetHeight(); // fuse optical flow line of sight rate measurements diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index e82618adab..8de58a8782 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -210,7 +210,7 @@ void Ekf::resetHorizontalPositionToVision() void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos) { - const Vector2f delta_horz_pos = new_horz_pos - Vector2f(_state.pos); + const Vector2f delta_horz_pos{new_horz_pos - Vector2f{_state.pos}}; _state.pos.xy() = new_horz_pos; for (uint8_t index = 0; index < _output_buffer.get_length(); index++) { @@ -223,28 +223,43 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos) _state_reset_status.posNE_counter++; } +void Ekf::resetVerticalPositionTo(const float &new_vert_pos) +{ + const float old_vert_pos = _state.pos(2); + _state.pos(2) = new_vert_pos; + + // store the reset amount and time to be published + _state_reset_status.posD_change = new_vert_pos - old_vert_pos; + _state_reset_status.posD_counter++; + + // apply the change in height / height rate to our newest height / height rate estimate + // which have already been taken out from the output buffer + _output_new.pos(2) += _state_reset_status.posD_change; + + // add the reset amount to the output observer buffered data + for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { + _output_buffer[i].pos(2) += _state_reset_status.posD_change; + _output_vert_buffer[i].vert_vel_integ += _state_reset_status.posD_change; + } + + // add the reset amount to the output observer vertical position state + _output_vert_new.vert_vel_integ = _state.pos(2); +} + // Reset height state using the last height measurement void Ekf::resetHeight() { // Get the most recent GPS data const 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 - const float old_vert_pos = _state.pos(2); - bool vert_pos_reset = false; - // reset the vertical position if (_control_status.flags.rng_hgt) { - const float new_pos_down = _hgt_sensor_offset - _range_sensor.getDistBottom(); - // update the state and associated variance - _state.pos(2) = new_pos_down; + resetVerticalPositionTo(_hgt_sensor_offset - _range_sensor.getDistBottom()); // the state variance is the same as the observation P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.range_noise)); - vert_pos_reset = true; - // reset the baro offset which is subtracted from the baro reading if we need to use it as a backup const baroSample &baro_newest = _baro_buffer.get_newest(); _baro_hgt_offset = baro_newest.hgt + _state.pos(2); @@ -254,13 +269,11 @@ void Ekf::resetHeight() const baroSample &baro_newest = _baro_buffer.get_newest(); if (!_baro_hgt_faulty) { - _state.pos(2) = -baro_newest.hgt + _baro_hgt_offset; + resetVerticalPositionTo(-baro_newest.hgt + _baro_hgt_offset); // the state variance is the same as the observation P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.baro_noise)); - vert_pos_reset = true; - } else { // TODO: reset to last known baro based estimate } @@ -268,13 +281,11 @@ void Ekf::resetHeight() } else if (_control_status.flags.gps_hgt) { // initialize vertical position and velocity with newest gps measurement if (!_gps_hgt_intermittent) { - _state.pos(2) = _hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref; + resetVerticalPositionTo(_hgt_sensor_offset - gps_newest.hgt + _gps_alt_ref); // the state variance is the same as the observation P.uncorrelateCovarianceSetVariance<1>(9, sq(gps_newest.vacc)); - vert_pos_reset = true; - // reset the baro offset which is subtracted from the baro reading if we need to use it as a backup const baroSample &baro_newest = _baro_buffer.get_newest(); _baro_hgt_offset = baro_newest.hgt + _state.pos(2); @@ -288,16 +299,11 @@ void Ekf::resetHeight() const extVisionSample &ev_newest = _ext_vision_buffer.get_newest(); // use the most recent data if it's time offset from the fusion time horizon is smaller - const int32_t dt_newest = ev_newest.time_us - _imu_sample_delayed.time_us; - const int32_t dt_delayed = _ev_sample_delayed.time_us - _imu_sample_delayed.time_us; - - vert_pos_reset = true; - - if (std::abs(dt_newest) < std::abs(dt_delayed)) { - _state.pos(2) = ev_newest.pos(2); + if (ev_newest.time_us >= _ev_sample_delayed.time_us) { + resetVerticalPositionTo(ev_newest.pos(2)); } else { - _state.pos(2) = _ev_sample_delayed.pos(2); + resetVerticalPositionTo(_ev_sample_delayed.pos(2)); } } @@ -317,31 +323,6 @@ void Ekf::resetHeight() // that does not destabilise the filter P.uncorrelateCovarianceSetVariance<1>(6, 10.0f); } - - // store the reset amount and time to be published - if (vert_pos_reset) { - _state_reset_status.posD_change = _state.pos(2) - old_vert_pos; - _state_reset_status.posD_counter++; - } - - // apply the change in height / height rate to our newest height / height rate estimate - // which have already been taken out from the output buffer - if (vert_pos_reset) { - _output_new.pos(2) += _state_reset_status.posD_change; - } - - // add the reset amount to the output observer buffered data - for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { - if (vert_pos_reset) { - _output_buffer[i].pos(2) += _state_reset_status.posD_change; - _output_vert_buffer[i].vert_vel_integ += _state_reset_status.posD_change; - } - } - - // add the reset amount to the output observer vertical position state - if (vert_pos_reset) { - _output_vert_new.vert_vel_integ = _state.pos(2); - } } // align output filter states to match EKF states at the fusion time horizon @@ -693,16 +674,49 @@ matrix::Vector Ekf::getStateAtFusionHorizonAsVector() const return state; } -// get the position and height of the ekf origin in WGS-84 coordinates and time the origin was set -// return true if the origin is valid -bool Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) const +bool Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const { - memcpy(origin_time, &_last_gps_origin_time_us, sizeof(uint64_t)); - memcpy(origin_pos, &_pos_ref, sizeof(map_projection_reference_s)); - memcpy(origin_alt, &_gps_alt_ref, sizeof(float)); + origin_time = _last_gps_origin_time_us; + latitude = math::degrees(_pos_ref.lat_rad); + longitude = math::degrees(_pos_ref.lon_rad); + origin_alt = _gps_alt_ref; return _NED_origin_initialised; } +bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude) +{ + bool current_pos_available = false; + double current_lat = static_cast(NAN); + double current_lon = static_cast(NAN); + float current_alt = NAN; + + // if we are already doing aiding, correct for the change in position since the EKF started navigating + if (map_projection_initialized(&_pos_ref) && isHorizontalAidingActive()) { + map_projection_reproject(&_pos_ref, _state.pos(0), _state.pos(1), ¤t_lat, ¤t_lon); + current_alt = -_state.pos(2) + _gps_alt_ref; + current_pos_available = true; + } + + // reinitialize map projection to latitude, longitude, altitude, and reset position + if (map_projection_init_timestamped(&_pos_ref, latitude, longitude, _time_last_imu) == 0) { + + if (current_pos_available) { + // reset horizontal position + Vector2f position; + map_projection_project(&_pos_ref, current_lat, current_lon, &position(0), &position(1)); + resetHorizontalPositionTo(position); + + // reset altitude + _gps_alt_ref = altitude; + resetVerticalPositionTo(_gps_alt_ref - current_alt); + } + + return true; + } + + return false; +} + /* First argument returns GPS drift metrics in the following array locations 0 : Horizontal position drift rate (m/s) diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index f77f0eebbd..893590fd90 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -229,6 +229,9 @@ public: // Getter for the baro sample on the delayed time horizon const baroSample &get_baro_sample_delayed() const { return _baro_sample_delayed; } + const bool& global_origin_valid() const { return _NED_origin_initialised; } + const map_projection_reference_s& global_origin() const { return _pos_ref; } + void print_status(); static constexpr unsigned FILTER_UPDATE_PERIOD_MS{10}; // ekf prediction period in milliseconds - this should ideally be an integer multiple of the IMU time delta diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index e04c16f55e..6a99179618 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -65,13 +65,17 @@ bool Ekf::collect_gps(const gps_message &gps) // If we have good GPS data set the origin's WGS-84 position to the last gps fix const double lat = gps.lat * 1.0e-7; const double lon = gps.lon * 1.0e-7; - map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu); - // if we are already doing aiding, correct for the change in position since the EKF started navigationg - if (isHorizontalAidingActive()) { - 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); + if (!map_projection_initialized(&_pos_ref)) { + map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu); + + // if we are already doing aiding, correct for the change in position since the EKF started navigating + if (isHorizontalAidingActive()) { + double est_lat; + double 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 @@ -89,7 +93,7 @@ bool Ekf::collect_gps(const gps_message &gps) // request a reset of the yaw using the new declination if (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE) { - // try to reset the yaw using the EKF-GSF yaw esitimator + // try to reset the yaw using the EKF-GSF yaw estimator _do_ekfgsf_yaw_reset = true; _ekfgsf_yaw_reset_time = 0; @@ -104,7 +108,6 @@ bool Ekf::collect_gps(const gps_message &gps) _gps_origin_epv = gps.epv; // if the user has selected GPS as the primary height source, switch across to using it - if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) { startGpsHgtFusion(); } diff --git a/test/sensor_simulator/gps.cpp b/test/sensor_simulator/gps.cpp index 7e0abbbfce..9a15e605d3 100644 --- a/test/sensor_simulator/gps.cpp +++ b/test/sensor_simulator/gps.cpp @@ -86,15 +86,11 @@ void Gps::stepHorizontalPositionByMeters(Vector2f hpos_change) { float hposN_curr; float hposE_curr; - map_projection_reference_s origin; - uint64_t time; - float alt; - _ekf->get_ekf_origin(&time, &origin, &alt); - map_projection_project(&origin, _gps_data.lat * 1e-7, _gps_data.lon * 1e-7, &hposN_curr, &hposE_curr); + map_projection_project(&_ekf->global_origin(), _gps_data.lat * 1e-7, _gps_data.lon * 1e-7, &hposN_curr, &hposE_curr); Vector2f hpos_new = Vector2f{hposN_curr, hposE_curr} + hpos_change; double lat_new; double lon_new; - map_projection_reproject(&origin, hpos_new(0), hpos_new(1), &lat_new, &lon_new); + map_projection_reproject(&_ekf->global_origin(), hpos_new(0), hpos_new(1), &lat_new, &lon_new); _gps_data.lon = static_cast(lon_new * 1e7); _gps_data.lat = static_cast(lat_new * 1e7); } diff --git a/test/test_EKF_basics.cpp b/test/test_EKF_basics.cpp index 65227169bc..7e92c1a2b6 100644 --- a/test/test_EKF_basics.cpp +++ b/test/test_EKF_basics.cpp @@ -31,6 +31,7 @@ * ****************************************************************************/ +#include #include #include #include @@ -187,4 +188,56 @@ TEST_F(EkfBasicsTest, accelBiasEstimation) << "gyro_bias = " << gyro_bias(0) << ", " << gyro_bias(1) << ", " << gyro_bias(2); } +TEST_F(EkfBasicsTest, reset_ekf_global_origin) +{ + double latitude {0.0}; + double longitude {0.0}; + float altitude {0.f}; + + double latitude_new {0.0}; + double longitude_new {0.0}; + float altitude_new {0.f}; + + uint64_t origin_time = 0; + + _ekf->getEkfGlobalOrigin(origin_time, latitude_new, longitude_new, altitude_new); + + EXPECT_DOUBLE_EQ(latitude, latitude_new); + EXPECT_DOUBLE_EQ(longitude, longitude_new); + EXPECT_FLOAT_EQ(altitude, altitude_new); + + _sensor_simulator.startGps(); + _ekf->set_min_required_gps_health_time(1e6); + _sensor_simulator.runSeconds(1); + sleep(1); + + latitude_new = 45.0000005; + longitude_new = -111.0000005; + altitude_new = 1500.0; + + _ekf->setEkfGlobalOrigin(latitude_new, longitude_new, altitude_new); + _ekf->getEkfGlobalOrigin(origin_time, latitude, longitude, altitude); + + // EKF origin MSL altitude cannot be reset without valid MSL origin. + EXPECT_DOUBLE_EQ(latitude, latitude_new); + EXPECT_DOUBLE_EQ(longitude, longitude_new); + + // After the change of origin, the pos and vel innovations should stay small + _sensor_simulator.runSeconds(1); + sleep(1); + + float hpos = 0.f; + float vpos = 0.f; + float hvel = 0.f; + float vvel = 0.f; + + _ekf->getGpsVelPosInnovRatio(hvel, vvel, hpos, vpos); + + EXPECT_NEAR(hvel, 0.f, 0.02f); + EXPECT_NEAR(vvel, 0.f, 0.02f); + + EXPECT_NEAR(hpos, 0.f, 0.05f); + EXPECT_NEAR(vpos, 0.f, 0.05f); +} + // TODO: Add sampling tests