From b19a6ee3b52103227b8f4a76eeb14f6a23a0ba6b Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 2 Oct 2024 09:54:08 +0200 Subject: [PATCH] ekf2: store position state as lat/lon/alt The position error state is still defined in a body-fixed NED frame but the position state itself is latitude-longitude-altitude. --- msg/EstimatorAidSource2d.msg | 2 +- src/modules/ekf2/EKF/CMakeLists.txt | 1 + .../aux_global_position.cpp | 35 ++-- .../barometer/baro_height_control.cpp | 14 +- .../external_vision/ev_height_control.cpp | 14 +- .../external_vision/ev_pos_control.cpp | 16 +- .../EKF/aid_sources/fake_height_control.cpp | 8 +- .../ekf2/EKF/aid_sources/fake_pos_control.cpp | 14 +- .../aid_sources/gnss/gnss_height_control.cpp | 18 +- .../ekf2/EKF/aid_sources/gnss/gps_checks.cpp | 15 +- .../ekf2/EKF/aid_sources/gnss/gps_control.cpp | 28 +-- .../aid_sources/magnetometer/mag_control.cpp | 9 +- .../optical_flow/optical_flow_control.cpp | 6 +- .../optical_flow/optical_flow_fusion.cpp | 3 +- .../range_finder/range_height_control.cpp | 4 +- src/modules/ekf2/EKF/ekf.cpp | 63 +++--- src/modules/ekf2/EKF/ekf.h | 51 +++-- src/modules/ekf2/EKF/ekf_helper.cpp | 188 ++++++++++-------- src/modules/ekf2/EKF/estimator_interface.h | 34 +++- .../ekf2/EKF/lat_lon_alt/CMakeLists.txt | 1 + .../ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp | 154 ++++++++++++++ .../ekf2/EKF/lat_lon_alt/test_lat_lon_alt.cpp | 107 ++++++++++ .../EKF/output_predictor/output_predictor.cpp | 49 ++--- .../EKF/output_predictor/output_predictor.h | 18 +- src/modules/ekf2/EKF/position_fusion.cpp | 151 ++++++++++---- src/modules/ekf2/EKF/terrain_control.cpp | 4 +- src/modules/ekf2/EKF2.cpp | 10 +- src/modules/ekf2/test/test_EKF_airspeed.cpp | 19 +- src/modules/ekf2/test/test_EKF_basics.cpp | 15 +- src/modules/ekf2/test/test_EKF_flow.cpp | 46 +++++ src/modules/ekf2/test/test_EKF_gps.cpp | 2 +- .../ekf2/test/test_EKF_height_fusion.cpp | 19 +- .../ekf2/test/test_EKF_yaw_estimator.cpp | 13 +- 33 files changed, 783 insertions(+), 348 deletions(-) create mode 100644 src/modules/ekf2/EKF/lat_lon_alt/CMakeLists.txt create mode 100644 src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp create mode 100644 src/modules/ekf2/EKF/lat_lon_alt/test_lat_lon_alt.cpp diff --git a/msg/EstimatorAidSource2d.msg b/msg/EstimatorAidSource2d.msg index 14e3ac3f84..8f3a9f0cda 100644 --- a/msg/EstimatorAidSource2d.msg +++ b/msg/EstimatorAidSource2d.msg @@ -7,7 +7,7 @@ uint32 device_id uint64 time_last_fuse -float32[2] observation +float64[2] observation float32[2] observation_variance float32[2] innovation diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 954123b250..27804ebcd8 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -33,6 +33,7 @@ add_subdirectory(bias_estimator) add_subdirectory(output_predictor) +add_subdirectory(lat_lon_alt) set(EKF_LIBS) set(EKF_SRCS) diff --git a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp index dc8f318c97..973a70dbe4 100644 --- a/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp +++ b/src/modules/ekf2/EKF/aid_sources/aux_global_position/aux_global_position.cpp @@ -74,24 +74,21 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed } estimator_aid_source2d_s aid_src{}; - Vector2f position; + const LatLonAlt position(sample.latitude, sample.longitude, sample.altitude_amsl); + const Vector2f innovation = (ekf.getLatLonAlt() - position).xy(); // altitude measurements are not used - if (ekf.global_origin_valid()) { - position = ekf.global_origin().project(sample.latitude, sample.longitude); - //const float hgt = ekf.getEkfGlobalOriginAltitude() - (float)sample.altitude; - // relax the upper observation noise limit which prevents bad measurements perturbing the position estimate - float pos_noise = math::max(sample.eph, _param_ekf2_agp_noise.get(), 0.01f); - const float pos_var = sq(pos_noise); - const Vector2f pos_obs_var(pos_var, pos_var); + // relax the upper observation noise limit which prevents bad measurements perturbing the position estimate + float pos_noise = math::max(sample.eph, _param_ekf2_agp_noise.get(), 0.01f); + const float pos_var = sq(pos_noise); + const Vector2f pos_obs_var(pos_var, pos_var); - ekf.updateAidSourceStatus(aid_src, - sample.time_us, // sample timestamp - position, // observation - pos_obs_var, // observation variance - Vector2f(ekf.state().pos) - position, // innovation - Vector2f(ekf.getPositionVariance()) + pos_obs_var, // innovation variance - math::max(_param_ekf2_agp_gate.get(), 1.f)); // innovation gate - } + ekf.updateAidSourceStatus(aid_src, + sample.time_us, // sample timestamp + matrix::Vector2d(sample.latitude, sample.longitude), // observation + pos_obs_var, // observation variance + innovation, // innovation + Vector2f(ekf.getPositionVariance()) + pos_obs_var, // innovation variance + math::max(_param_ekf2_agp_gate.get(), 1.f)); // innovation gate const bool starting_conditions = PX4_ISFINITE(sample.latitude) && PX4_ISFINITE(sample.longitude) && ekf.control_status_flags().yaw_align; @@ -113,8 +110,8 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed } else { // Try to initialize using measurement - if (ekf.setEkfGlobalOriginFromCurrentPos(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph, - sample.epv)) { + if (ekf.resetGlobalPositionTo(sample.latitude, sample.longitude, sample.altitude_amsl, sample.eph, + sample.epv)) { ekf.enableControlStatusAuxGpos(); _reset_counters.lat_lon = sample.lat_lon_reset_counter; _state = State::active; @@ -131,7 +128,7 @@ void AuxGlobalPosition::update(Ekf &ekf, const estimator::imuSample &imu_delayed if (isTimedOut(aid_src.time_last_fuse, imu_delayed.time_us, ekf._params.no_aid_timeout_max) || (_reset_counters.lat_lon != sample.lat_lon_reset_counter)) { - ekf.resetHorizontalPositionTo(Vector2f(aid_src.observation), Vector2f(aid_src.observation_variance)); + ekf.resetHorizontalPositionTo(sample.latitude, sample.longitude, Vector2f(aid_src.observation_variance)); ekf.resetAidSourceStatusZeroInnovation(aid_src); diff --git a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp index f6ab66f736..0f5bcd610c 100644 --- a/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/barometer/baro_height_control.cpp @@ -73,7 +73,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) if (_baro_counter <= _obs_buffer_length) { // Initialize the pressure offset (included in the baro bias) - bias_est.setBias(_state.pos(2) + _baro_lpf.getState()); + bias_est.setBias(-_gpos.altitude() + _baro_lpf.getState()); } } @@ -106,7 +106,7 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) if (measurement_valid) { bias_est.setMaxStateNoise(sqrtf(measurement_var)); bias_est.setProcessNoiseSpectralDensity(_params.baro_bias_nsd); - bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2)); + bias_est.fuseBias(measurement - _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2)); } // determine if we should use height aiding @@ -131,8 +131,8 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); _information_events.flags.reset_hgt_to_baro = true; - resetVerticalPositionTo(-(_baro_lpf.getState() - bias_est.getBias()), measurement_var); - bias_est.setBias(_state.pos(2) + _baro_lpf.getState()); + resetHeightTo(_baro_lpf.getState() - bias_est.getBias(), measurement_var); + bias_est.setBias(-_gpos.altitude() + _baro_lpf.getState()); // reset vertical velocity if no valid sources available if (!isVerticalVelocityAidingActive()) { @@ -163,12 +163,12 @@ void Ekf::controlBaroHeightFusion(const imuSample &imu_sample) _height_sensor_ref = HeightSensor::BARO; _information_events.flags.reset_hgt_to_baro = true; - resetVerticalPositionTo(-(_baro_lpf.getState() - bias_est.getBias()), measurement_var); - bias_est.setBias(_state.pos(2) + _baro_lpf.getState()); + resetAltitudeTo(measurement, measurement_var); + bias_est.reset(); } else { ECL_INFO("starting %s height fusion", HGT_SRC_NAME); - bias_est.setBias(_state.pos(2) + _baro_lpf.getState()); + bias_est.setBias(-_gpos.altitude() + _baro_lpf.getState()); } aid_src.time_last_fuse = imu_sample.time_us; diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp index 440c0fe7ac..f62be7575f 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp @@ -99,7 +99,7 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp if (measurement_valid && quality_sufficient) { bias_est.setMaxStateNoise(sqrtf(measurement_var)); bias_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd); - bias_est.fuseBias(measurement - _state.pos(2), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2)); + bias_est.fuseBias(measurement + _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2)); } const bool continuing_conditions_passing = (_params.ev_ctrl & static_cast(EvCtrl::VPOS)) @@ -117,11 +117,11 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp if (_height_sensor_ref == HeightSensor::EV) { _information_events.flags.reset_hgt_to_ev = true; - resetVerticalPositionTo(measurement, measurement_var); + resetHeightTo(-measurement, measurement_var); bias_est.reset(); } else { - bias_est.setBias(-_state.pos(2) + measurement); + bias_est.setBias(_gpos.altitude() + measurement); } aid_src.time_last_fuse = _time_delayed_us; @@ -146,8 +146,8 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp // All height sources are failing ECL_WARN("%s fusion reset required, all height sources failing", AID_SRC_NAME); _information_events.flags.reset_hgt_to_ev = true; - resetVerticalPositionTo(measurement - bias_est.getBias(), measurement_var); - bias_est.setBias(-_state.pos(2) + measurement); + resetHeightTo(-measurement - bias_est.getBias(), measurement_var); + bias_est.setBias(_gpos.altitude() + measurement); aid_src.time_last_fuse = _time_delayed_us; @@ -170,14 +170,14 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp if (_params.height_sensor_ref == static_cast(HeightSensor::EV)) { ECL_INFO("starting %s fusion, resetting state", AID_SRC_NAME); _information_events.flags.reset_hgt_to_ev = true; - resetVerticalPositionTo(measurement, measurement_var); + resetHeightTo(-measurement, measurement_var); _height_sensor_ref = HeightSensor::EV; bias_est.reset(); } else { ECL_INFO("starting %s fusion", AID_SRC_NAME); - bias_est.setBias(-_state.pos(2) + measurement); + bias_est.setBias(_gpos.altitude() + measurement); } aid_src.time_last_fuse = _time_delayed_us; diff --git a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp index 590a289da3..266b4cf548 100644 --- a/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp @@ -137,6 +137,8 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample #endif // CONFIG_EKF2_GNSS + const Vector2f position_estimate = getLocalHorizontalPosition(); + const Vector2f measurement{pos(0), pos(1)}; const Vector2f measurement_var{ @@ -150,7 +152,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample if (!bias_fusion_was_active && _ev_pos_b_est.fusionActive()) { if (quality_sufficient) { // reset the bias estimator - _ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement); + _ev_pos_b_est.setBias(-position_estimate + measurement); } else if (isOtherSourceOfHorizontalAidingThan(_control_status.flags.ev_pos)) { // otherwise stop EV position, when quality is good again it will restart with reset bias @@ -165,7 +167,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample ev_sample.time_us, // sample timestamp position, // observation pos_obs_var, // observation variance - Vector2f(_state.pos) - position, // innovation + position_estimate - position, // innovation Vector2f(getStateVariance()) + pos_obs_var, // innovation variance math::max(_params.ev_pos_innov_gate, 1.f)); // innovation gate @@ -174,7 +176,7 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample if (measurement_valid && quality_sufficient) { _ev_pos_b_est.setMaxStateNoise(Vector2f(sqrtf(measurement_var(0)), sqrtf(measurement_var(1)))); _ev_pos_b_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd); // TODO - _ev_pos_b_est.fuseBias(measurement - Vector2f(_state.pos.xy()), + _ev_pos_b_est.fuseBias(measurement - position_estimate, measurement_var + Vector2f(getStateVariance())); } @@ -213,7 +215,7 @@ void Ekf::startEvPosFusion(const Vector2f &measurement, const Vector2f &measurem // TODO: (_params.position_sensor_ref == PositionSensor::EV) if (_control_status.flags.gps) { ECL_INFO("starting %s fusion", EV_AID_SRC_NAME); - _ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement); + _ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement); _ev_pos_b_est.setFusionActive(); } else { @@ -245,7 +247,7 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure _ev_pos_b_est.reset(); } else { - _ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement); + _ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement); } aid_src.time_last_fuse = _time_delayed_us; @@ -275,14 +277,14 @@ void Ekf::updateEvPosFusion(const Vector2f &measurement, const Vector2f &measure if (_control_status.flags.gps && !pos_xy_fusion_failing) { // reset EV position bias - _ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement); + _ev_pos_b_est.setBias(-Vector2f(getLocalHorizontalPosition()) + measurement); } else { _information_events.flags.reset_pos_to_vision = true; if (_control_status.flags.gps) { resetHorizontalPositionTo(measurement - _ev_pos_b_est.getBias(), measurement_var + _ev_pos_b_est.getBiasVar()); - _ev_pos_b_est.setBias(-Vector2f(_state.pos.xy()) + measurement); + _ev_pos_b_est.setBias(-getLocalHorizontalPosition() + measurement); } else { resetHorizontalPositionTo(measurement, measurement_var); diff --git a/src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp index d10fe57d44..4ff4e67d6c 100644 --- a/src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/fake_height_control.cpp @@ -51,7 +51,7 @@ void Ekf::controlFakeHgtFusion() const float obs_var = sq(_params.pos_noaid_noise); const float innov_gate = 3.f; - updateVerticalPositionAidStatus(aid_src, _time_delayed_us, _last_known_pos(2), obs_var, innov_gate); + updateVerticalPositionAidStatus(aid_src, _time_delayed_us, -_last_known_gpos.altitude(), obs_var, innov_gate); const bool continuing_conditions_passing = !isVerticalAidingActive(); const bool starting_conditions_passing = continuing_conditions_passing @@ -98,7 +98,7 @@ void Ekf::controlFakeHgtFusion() void Ekf::resetFakeHgtFusion() { ECL_INFO("reset fake height fusion"); - _last_known_pos(2) = _state.pos(2); + _last_known_gpos.setAltitude(_gpos.altitude()); resetVerticalVelocityToZero(); resetHeightToLastKnown(); @@ -109,8 +109,8 @@ void Ekf::resetFakeHgtFusion() void Ekf::resetHeightToLastKnown() { _information_events.flags.reset_pos_to_last_known = true; - ECL_INFO("reset height to last known (%.3f)", (double)_last_known_pos(2)); - resetVerticalPositionTo(_last_known_pos(2), sq(_params.pos_noaid_noise)); + ECL_INFO("reset height to last known (%.3f)", (double)_last_known_gpos.altitude()); + resetHeightTo(_last_known_gpos.altitude(), sq(_params.pos_noaid_noise)); } void Ekf::stopFakeHgtFusion() diff --git a/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp b/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp index ecc6cdfacb..3e244412ab 100644 --- a/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/fake_pos_control.cpp @@ -63,17 +63,17 @@ void Ekf::controlFakePosFusion() obs_var(0) = obs_var(1) = sq(0.5f); } - const Vector2f position(_last_known_pos); + const Vector2f innovation = (_gpos - _last_known_gpos).xy(); const float innov_gate = 3.f; updateAidSourceStatus(aid_src, _time_delayed_us, - position, // observation - obs_var, // observation variance - Vector2f(_state.pos) - position, // innovation - Vector2f(getStateVariance()) + obs_var, // innovation variance - innov_gate); // innovation gate + Vector2f(_gpos.latitude_deg(), _gpos.longitude_deg()), // observation + obs_var, // observation variance + innovation, // innovation + Vector2f(getStateVariance()) + obs_var, // innovation variance + innov_gate); // innovation gate const bool enable_valid_fake_pos = _control_status.flags.constant_pos || _control_status.flags.vehicle_at_rest; const bool enable_fake_pos = !enable_valid_fake_pos @@ -95,7 +95,7 @@ void Ekf::controlFakePosFusion() void Ekf::resetFakePosFusion() { ECL_INFO("reset fake position fusion"); - _last_known_pos.xy() = _state.pos.xy(); + _last_known_gpos.setLatLon(_gpos); resetHorizontalPositionToLastKnown(); resetHorizontalVelocityToZero(); diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp index cc165d4533..78a6d12f90 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_height_control.cpp @@ -62,9 +62,9 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body; const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; - const float gnss_alt = _gps_sample_delayed.alt + pos_offset_earth(2); + const float gnss_alt = gps_sample.alt + pos_offset_earth(2); - const float measurement = gnss_alt - getEkfGlobalOriginAltitude(); + const float measurement = gnss_alt; const float measurement_var = sq(noise); const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var); @@ -81,13 +81,13 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) if (measurement_valid) { bias_est.setMaxStateNoise(sqrtf(measurement_var)); bias_est.setProcessNoiseSpectralDensity(_params.gps_hgt_bias_nsd); - bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2)); + bias_est.fuseBias(measurement - _gpos.altitude(), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2)); } // determine if we should use height aiding const bool continuing_conditions_passing = (_params.gnss_ctrl & static_cast(GnssCtrl::VPOS)) && measurement_valid - && _pos_ref.isInitialized() + && _local_origin_lat_lon.isInitialized() && _gps_checks_passed; const bool starting_conditions_passing = continuing_conditions_passing @@ -105,8 +105,8 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); _information_events.flags.reset_hgt_to_gps = true; - resetVerticalPositionTo(aid_src.observation, measurement_var); - bias_est.setBias(_state.pos(2) + measurement); + resetHeightTo(measurement, measurement_var); + bias_est.setBias(-_gpos.altitude() + measurement); aid_src.time_last_fuse = _time_delayed_us; @@ -128,13 +128,13 @@ void Ekf::controlGnssHeightFusion(const gnssSample &gps_sample) _height_sensor_ref = HeightSensor::GNSS; _information_events.flags.reset_hgt_to_gps = true; - resetVerticalPositionTo(-measurement, measurement_var); - _gpos_origin_epv = 0.f; // The uncertainty of the global origin is now contained in the local position uncertainty + + resetAltitudeTo(measurement, measurement_var); bias_est.reset(); } else { ECL_INFO("starting %s height fusion", HGT_SRC_NAME); - bias_est.setBias(_state.pos(2) + measurement); + bias_est.setBias(-_gpos.altitude() + measurement); } aid_src.time_last_fuse = _time_delayed_us; diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp index 236fd2dd63..c1b47c224f 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_checks.cpp @@ -57,25 +57,14 @@ void Ekf::collect_gps(const gnssSample &gps) { - if (_filter_initialised && !_pos_ref.isInitialized() && _gps_checks_passed) { - // If we have good GPS data set the origin's WGS-84 position to the last gps fix - setLatLonOriginFromCurrentPos(gps.lat, gps.lon, gps.hacc); - - // Take the current GPS height and subtract the filter height above origin to estimate the GPS height of the origin - if (!PX4_ISFINITE(_gps_alt_ref)) { - setAltOriginFromCurrentPos(gps.alt, gps.vacc); - } - + if (_filter_initialised && !_local_origin_lat_lon.isInitialized() && _gps_checks_passed) { _information_events.flags.gps_checks_passed = true; - ECL_INFO("GPS origin set to lat=%.6f, lon=%.6f", - _pos_ref.getProjectionReferenceLat(), _pos_ref.getProjectionReferenceLon()); - } else { // a rough 2D fix is sufficient to lookup earth spin rate const bool gps_rough_2d_fix = (gps.fix_type >= 2) && (gps.hacc < 1000); - if (gps_rough_2d_fix && (_gps_checks_passed || !_pos_ref.isInitialized())) { + if (gps_rough_2d_fix && (_gps_checks_passed || !_local_origin_lat_lon.isInitialized())) { _earth_rate_NED = calcEarthRateNED((float)math::radians(gps.lat)); } } diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp index 9799c8bb71..de8dad37b5 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gps_control.cpp @@ -82,10 +82,7 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) } } - if (_pos_ref.isInitialized()) { - updateGnssPos(gnss_sample, _aid_src_gnss_pos); - } - + updateGnssPos(gnss_sample, _aid_src_gnss_pos); updateGnssVel(imu_delayed, gnss_sample, _aid_src_gnss_vel); } else if (_control_status.flags.gps) { @@ -108,9 +105,9 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) const bool continuing_conditions_passing = (gnss_vel_enabled || gnss_pos_enabled) && _control_status.flags.tilt_align - && _control_status.flags.yaw_align - && _pos_ref.isInitialized(); + && _control_status.flags.yaw_align; const bool starting_conditions_passing = continuing_conditions_passing && _gps_checks_passed; + const bool gpos_init_conditions_passing = gnss_pos_enabled && _gps_checks_passed; if (_control_status.flags.gps) { if (continuing_conditions_passing) { @@ -174,6 +171,9 @@ void Ekf::controlGpsFusion(const imuSample &imu_delayed) } _control_status.flags.gps = true; + + } else if (gpos_init_conditions_passing && !_local_origin_lat_lon.isInitialized()) { + resetHorizontalPositionToGnss(_aid_src_gnss_pos); } } } @@ -221,8 +221,10 @@ void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s { // correct position and height for offset relative to IMU const Vector3f pos_offset_body = _params.gps_pos_body - _params.imu_pos_body; - const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; - const Vector2f position = _pos_ref.project(gnss_sample.lat, gnss_sample.lon) - pos_offset_earth.xy(); + const Vector3f pos_offset_earth = Vector3f(_R_to_earth * pos_offset_body); + const LatLonAlt measurement(gnss_sample.lat, gnss_sample.lon, gnss_sample.alt); + const LatLonAlt measurement_corrected = measurement + (-pos_offset_earth); + const Vector2f innovation = (_gpos - measurement_corrected).xy(); // relax the upper observation noise limit which prevents bad GPS perturbing the position estimate float pos_noise = math::max(gnss_sample.hacc, _params.gps_pos_noise); @@ -237,12 +239,13 @@ void Ekf::updateGnssPos(const gnssSample &gnss_sample, estimator_aid_source2d_s const float pos_var = math::max(sq(pos_noise), sq(0.01f)); const Vector2f pos_obs_var(pos_var, pos_var); + const matrix::Vector2d observation(measurement_corrected.latitude_deg(), measurement_corrected.longitude_deg()); updateAidSourceStatus(aid_src, gnss_sample.time_us, // sample timestamp - position, // observation + observation, // observation pos_obs_var, // observation variance - Vector2f(_state.pos) - position, // innovation + innovation, // innovation Vector2f(getStateVariance()) + pos_obs_var, // innovation variance math::max(_params.gps_pos_innov_gate, 1.f)); // innovation gate } @@ -322,8 +325,9 @@ void Ekf::resetVelocityToGnss(estimator_aid_source3d_s &aid_src) void Ekf::resetHorizontalPositionToGnss(estimator_aid_source2d_s &aid_src) { _information_events.flags.reset_pos_to_gps = true; - resetHorizontalPositionTo(Vector2f(aid_src.observation), Vector2f(aid_src.observation_variance)); - _gpos_origin_eph = 0.f; // The uncertainty of the global origin is now contained in the local position uncertainty + resetLatLonTo(aid_src.observation[0], aid_src.observation[1], + aid_src.observation_variance[0] + + aid_src.observation_variance[1]); resetAidSourceStatusZeroInnovation(aid_src); } diff --git a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp index cff897d106..85979a2447 100644 --- a/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/magnetometer/mag_control.cpp @@ -90,12 +90,7 @@ void Ekf::controlMagFusion(const imuSample &imu_sample) if (global_origin_valid() && (origin_newer_than_last_mag || (isLocalHorizontalPositionValid() && isTimedOut(_wmm_mag_time_last_checked, 10e6))) ) { - // position of local NED origin in GPS / WGS84 frame - double latitude_deg; - double longitude_deg; - global_origin().reproject(_state.pos(0), _state.pos(1), latitude_deg, longitude_deg); - - if (updateWorldMagneticModel(latitude_deg, longitude_deg)) { + if (updateWorldMagneticModel(_gpos.latitude_deg(), _gpos.longitude_deg())) { wmm_updated = true; } @@ -368,7 +363,7 @@ bool Ekf::checkHaglYawResetReq() const // Check if height has increased sufficiently to be away from ground magnetic anomalies // and request a yaw reset if not already requested. static constexpr float mag_anomalies_max_hagl = 1.5f; - const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl; + const bool above_mag_anomalies = (getTerrainVPos() + _gpos.altitude()) > mag_anomalies_max_hagl; return above_mag_anomalies; } diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp index dbbc1a7296..60238af105 100644 --- a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp @@ -233,7 +233,9 @@ void Ekf::resetFlowFusion(const flowSample &flow_sample) // reset position, estimate is relative to initial position in this mode, so we start with zero error if (!_control_status.flags.in_air) { ECL_INFO("reset position to zero"); - resetHorizontalPositionTo(Vector2f(0.f, 0.f), 0.f); + //TODO: reset origin instead? + resetHorizontalPositionToLastKnown(); + // resetHorizontalPositionTo(Vector2f(0.f, 0.f), 0.f); } resetAidSourceStatusZeroInnovation(_aid_src_optical_flow); @@ -247,7 +249,7 @@ void Ekf::resetTerrainToFlow() ECL_INFO("reset hagl to flow"); // TODO: use the flow data - const float new_terrain = fmaxf(0.0f, _state.pos(2)); + const float new_terrain = -_gpos.altitude() + _params.rng_gnd_clearance; const float delta_terrain = new_terrain - _state.terrain; _state.terrain = new_terrain; P.uncorrelateCovarianceSetVariance(State::terrain.idx, 100.f); diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp index ea722569d6..747fac96f1 100644 --- a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp +++ b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_fusion.cpp @@ -67,7 +67,8 @@ bool Ekf::fuseOptFlow(VectorState &H, const bool update_terrain) // recalculate the innovation using the updated state const Vector3f flow_gyro_corrected = _flow_sample_delayed.gyro_rate - _flow_gyro_bias; - _aid_src_optical_flow.innovation[1] = predictFlow(flow_gyro_corrected)(1) - _aid_src_optical_flow.observation[1]; + _aid_src_optical_flow.innovation[1] = predictFlow(flow_gyro_corrected)(1) - static_cast + (_aid_src_optical_flow.observation[1]); } if (_aid_src_optical_flow.innovation_variance[index] < _aid_src_optical_flow.observation_variance[index]) { diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index 3a848f7d46..2598f3a331 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp @@ -148,7 +148,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) _height_sensor_ref = HeightSensor::RANGE; _information_events.flags.reset_hgt_to_rng = true; - resetVerticalPositionTo(-aid_src.observation, aid_src.observation_variance); + resetHeightTo(aid_src.observation, aid_src.observation_variance); _state.terrain = 0.f; _control_status.flags.rng_hgt = true; stopRngTerrFusion(); @@ -180,7 +180,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); _information_events.flags.reset_hgt_to_rng = true; - resetVerticalPositionTo(-(aid_src.observation - _state.terrain)); + resetHeightTo(aid_src.observation - _state.terrain); // reset vertical velocity if no valid sources available if (!isVerticalVelocityAidingActive()) { diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index f453559c0e..e1fb96784f 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -74,7 +74,7 @@ void Ekf::reset() // #if defined(CONFIG_EKF2_TERRAIN) // assume a ground clearance - _state.terrain = _state.pos(2) + _params.rng_gnd_clearance; + _state.terrain = -_gpos.altitude() + _params.rng_gnd_clearance; #endif // CONFIG_EKF2_TERRAIN #if defined(CONFIG_EKF2_RANGE_FINDER) @@ -97,7 +97,7 @@ void Ekf::reset() resetGpsDriftCheckFilters(); _gps_checks_passed = false; #endif // CONFIG_EKF2_GNSS - _gps_alt_ref = NAN; + _local_origin_alt = NAN; _output_predictor.reset(); @@ -113,7 +113,7 @@ void Ekf::reset() _time_last_heading_fuse = 0; _time_last_terrain_fuse = 0; - _last_known_pos.setZero(); + _last_known_gpos.setZero(); #if defined(CONFIG_EKF2_BAROMETER) _baro_counter = 0; @@ -168,7 +168,7 @@ bool Ekf::update() // control fusion of observation data controlFusionModes(imu_sample_delayed); - _output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _state.pos, + _output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _gpos, _state.gyro_bias, _state.accel_bias); return true; @@ -205,7 +205,7 @@ bool Ekf::initialiseFilter() initialiseCovariance(); // reset the output predictor state history to match the EKF initial values - _output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos); + _output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _gpos); return true; } @@ -258,11 +258,11 @@ void Ekf::predictState(const imuSample &imu_delayed) _state.vel(2) += CONSTANTS_ONE_G * imu_delayed.delta_vel_dt; // predict position states via trapezoidal integration of velocity - _state.pos += (vel_last + _state.vel) * imu_delayed.delta_vel_dt * 0.5f; + _gpos += (vel_last + _state.vel) * imu_delayed.delta_vel_dt * 0.5f; + _state.pos(2) = -_gpos.altitude(); // constrain states _state.vel = matrix::constrain(_state.vel, -_params.velocity_limit, _params.velocity_limit); - _state.pos = matrix::constrain(_state.pos, -1.e6f, 1.e6f); // calculate a filtered horizontal acceleration with a 1 sec time constant @@ -283,14 +283,12 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl return false; } - if (!_pos_ref.isInitialized()) { - if (!setLatLonOriginFromCurrentPos(latitude, longitude, eph)) { + if (!_local_origin_lat_lon.isInitialized()) { + if (!resetLatLonTo(latitude, longitude, sq(eph))) { return false; } - if (!PX4_ISFINITE(_gps_alt_ref)) { - setAltOriginFromCurrentPos(altitude, epv); - } + resetAltitudeTo(altitude, sq(epv)); return true; } @@ -315,12 +313,20 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl pos_correction = _state.vel * dt_s; } - { - const Vector2f hpos = _pos_ref.project(latitude, longitude) + pos_correction.xy(); + LatLonAlt gpos(latitude, longitude, altitude); + bool alt_valid = true; + if (!checkAltitudeValidity(gpos.altitude())) { + gpos.setAltitude(_gpos.altitude()); + alt_valid = false; + } + + const LatLonAlt gpos_corrected = gpos + pos_correction; + + { const float obs_var = math::max(sq(eph), sq(0.01f)); - const Vector2f innov = Vector2f(_state.pos.xy()) - hpos; + const Vector2f innov = (_gpos - gpos_corrected).xy(); const Vector2f innov_var = Vector2f(getStateVariance()) + obs_var; const float sq_gate = sq(5.f); // magic hardcoded gate @@ -334,8 +340,8 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl ECL_INFO("reset position to external observation"); _information_events.flags.reset_pos_to_ext_obs = true; - resetHorizontalPositionTo(hpos, obs_var); - _last_known_pos.xy() = _state.pos.xy(); + resetHorizontalPositionTo(gpos_corrected.latitude_deg(), gpos_corrected.longitude_deg(), obs_var); + _last_known_gpos.setLatLon(gpos_corrected); } else { ECL_INFO("fuse external observation as position measurement"); @@ -348,24 +354,16 @@ bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const doubl _state_reset_status.posNE_change.zero(); _time_last_hor_pos_fuse = _time_delayed_us; - _last_known_pos.xy() = _state.pos.xy(); + _last_known_gpos.setLatLon(gpos_corrected); } } - if (checkAltitudeValidity(altitude)) { - const float altitude_corrected = altitude - pos_correction(2); + if (alt_valid) { + const float obs_var = math::max(sq(epv), sq(0.01f)); - if (!PX4_ISFINITE(_gps_alt_ref)) { - setAltOriginFromCurrentPos(altitude_corrected, epv); - - } else { - const float vpos = -(altitude_corrected - _gps_alt_ref); - const float obs_var = math::max(sq(epv), sq(0.01f)); - - ECL_INFO("reset height to external observation"); - resetVerticalPositionTo(vpos, obs_var); - _last_known_pos(2) = _state.pos(2); - } + ECL_INFO("reset height to external observation"); + resetAltitudeTo(gpos_corrected.altitude(), obs_var); + _last_known_gpos.setAltitude(gpos_corrected.altitude()); } return true; @@ -425,9 +423,10 @@ void Ekf::print_status() (double)getStateVariance()(2) ); + const Vector3f position = getPosition(); printf("Position (%d-%d): [%.3f, %.3f, %.3f] var: [%.1e, %.1e, %.1e]\n", State::pos.idx, State::pos.idx + State::pos.dof - 1, - (double)_state.pos(0), (double)_state.pos(1), (double)_state.pos(2), + (double)position(0), (double)position(1), (double) position(2), (double)getStateVariance()(0), (double)getStateVariance()(1), (double)getStateVariance()(2) ); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index ec9a00154f..a76c0e3517 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -66,6 +66,8 @@ # include "aid_sources/aux_global_position/aux_global_position.hpp" #endif // CONFIG_EKF2_AUX_GLOBAL_POSITION +#include "lat_lon_alt/lat_lon_alt.hpp" + enum class Likelihood { LOW, MEDIUM, HIGH }; class ExternalVisionVel; @@ -102,8 +104,8 @@ public: bool isTerrainEstimateValid() const { return _terrain_valid; } // get the estimated terrain vertical position relative to the NED origin - float getTerrainVertPos() const { return _state.terrain; }; - float getHagl() const { return _state.terrain - _state.pos(2); } + float getTerrainVertPos() const { return _state.terrain + getEkfGlobalOriginAltitude(); }; + float getHagl() const { return _state.terrain + _gpos.altitude(); } // get the terrain variance float getTerrainVariance() const { return P(State::terrain.idx, State::terrain.idx); } @@ -192,8 +194,8 @@ public: bool checkLatLonValidity(double latitude, double longitude); bool checkAltitudeValidity(float altitude); bool setEkfGlobalOrigin(double latitude, double longitude, float altitude, float eph = NAN, float epv = NAN); - bool setEkfGlobalOriginFromCurrentPos(double latitude, double longitude, float altitude, float eph = NAN, - float epv = NAN); + bool resetGlobalPositionTo(double latitude, double longitude, float altitude, float eph = NAN, + float epv = NAN); // 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; @@ -217,17 +219,14 @@ public: void resetAccelBias(); void resetAccelBiasCov(); - // return true if the global position estimate is valid - // return true if the origin is set we are not doing unconstrained free inertial navigation - // and have not started using synthetic position observations to constrain drift bool isGlobalHorizontalPositionValid() const { - return _pos_ref.isInitialized() && isLocalHorizontalPositionValid(); + return _local_origin_lat_lon.isInitialized() && isLocalHorizontalPositionValid(); } bool isGlobalVerticalPositionValid() const { - return _pos_ref.isInitialized() && isLocalVerticalPositionValid(); + return PX4_ISFINITE(_local_origin_alt) && isLocalVerticalPositionValid(); } bool isLocalHorizontalPositionValid() const @@ -473,6 +472,8 @@ private: StateSample _state{}; ///< state struct of the ekf running at the delayed time horizon + LatLonAlt _gpos{0.0, 0.0, 0.f}; + bool _filter_initialised{false}; ///< true when the EKF sttes and covariances been initialised uint64_t _time_last_horizontal_aiding{0}; ///< amount of time we have been doing inertial only deadreckoning (uSec) @@ -486,7 +487,7 @@ private: uint64_t _time_last_heading_fuse{0}; uint64_t _time_last_terrain_fuse{0}; - Vector3f _last_known_pos{}; ///< last known local position vector (m) + LatLonAlt _last_known_gpos{}; Vector3f _earth_rate_NED{}; ///< earth rotation vector (NED) in rad/s @@ -645,11 +646,11 @@ private: P.slice(S.idx, S.idx) = cov; } - bool setLatLonOrigin(double latitude, double longitude, float eph = NAN); - bool setAltOrigin(float altitude, float epv = NAN); + bool setLatLonOrigin(double latitude, double longitude, float hpos_var = NAN); + bool setAltOrigin(float altitude, float vpos_var = NAN); - bool setLatLonOriginFromCurrentPos(double latitude, double longitude, float eph = NAN); - bool setAltOriginFromCurrentPos(float altitude, float epv = NAN); + bool resetLatLonTo(double latitude, double longitude, float hpos_var = NAN); + bool resetAltitudeTo(float altitude, float vpos_var = NAN); // update quaternion states and covariances using an innovation, observation variance and Jacobian vector bool fuseYaw(estimator_aid_source1d_s &aid_src_status, const VectorState &H_YAW); @@ -711,14 +712,22 @@ private: void resetHorizontalPositionToLastKnown(); - void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var); - void resetHorizontalPositionTo(const Vector2f &new_horz_pos, const float pos_var = NAN) { resetHorizontalPositionTo(new_horz_pos, Vector2f(pos_var, pos_var)); } + void resetHorizontalPositionTo(const double &new_latitude, const double &new_longitude, + const Vector2f &new_horz_pos_var); + void resetHorizontalPositionTo(const double &new_latitude, const double &new_longitude, const float pos_var = NAN) { resetHorizontalPositionTo(new_latitude, new_longitude, Vector2f(pos_var, pos_var)); } + void resetHorizontalPositionTo(const Vector2f &new_pos, const Vector2f &new_horz_pos_var); + + Vector2f getLocalHorizontalPosition() const; + + Vector2f computeDeltaHorizontalPosition(const double &new_latitude, const double &new_longitude) const; + void updateHorizontalPositionResetStatus(const Vector2f &delta); void resetWindTo(const Vector2f &wind, const Vector2f &wind_var); bool isHeightResetRequired() const; - void resetVerticalPositionTo(float new_vert_pos, float new_vert_pos_var = NAN); + void resetHeightTo(float new_altitude, float new_vert_pos_var = NAN); + void updateVerticalPositionResetStatus(const float delta_z); void resetVerticalVelocityToZero(); @@ -740,6 +749,7 @@ private: void controlTerrainFakeFusion(); void updateTerrainValidity(); + void updateTerrainResetStatus(const float delta_z); # if defined(CONFIG_EKF2_RANGE_FINDER) // update the terrain vertical position estimate using a height above ground measurement from the range finder @@ -832,6 +842,7 @@ private: void startEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, estimator_aid_source2d_s &aid_src); void updateEvPosFusion(const Vector2f &measurement, const Vector2f &measurement_var, bool quality_sufficient, bool reset, estimator_aid_source2d_s &aid_src); + void stopEvPosFusion(); void stopEvHgtFusion(); void stopEvVelFusion(); @@ -1049,9 +1060,9 @@ private: } // helper used for populating and filtering estimator aid source struct for logging - template + template void updateAidSourceStatus(T &status, const uint64_t ×tamp_sample, - const S &observation, const S &observation_variance, + const D &observation, const S &observation_variance, const S &innovation, const S &innovation_variance, float innovation_gate = 1.f) const { @@ -1106,7 +1117,7 @@ private: status.test_ratio[i] = test_ratio; - status.observation[i] = observation(i); + status.observation[i] = static_cast(observation(i)); status.observation_variance[i] = observation_variance(i); status.innovation[i] = innovation(i); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index d4b3e6031e..33e63a6eb6 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -65,9 +65,9 @@ Vector3f Ekf::calcEarthRateNED(float lat_rad) const void Ekf::getEkfGlobalOrigin(uint64_t &origin_time, double &latitude, double &longitude, float &origin_alt) const { - origin_time = _pos_ref.getProjectionReferenceTimestamp(); - latitude = _pos_ref.getProjectionReferenceLat(); - longitude = _pos_ref.getProjectionReferenceLon(); + origin_time = _local_origin_lat_lon.getProjectionReferenceTimestamp(); + latitude = _local_origin_lat_lon.getProjectionReferenceLat(); + longitude = _local_origin_lat_lon.getProjectionReferenceLon(); origin_alt = getEkfGlobalOriginAltitude(); } @@ -85,156 +85,170 @@ bool Ekf::checkAltitudeValidity(const float altitude) return (PX4_ISFINITE(altitude) && ((altitude > -12'000.f) && (altitude < 100'000.f))); } -bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude, const float eph, - const float epv) +bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, const float altitude, const float hpos_var, + const float vpos_var) { - if (!setLatLonOrigin(latitude, longitude, eph)) { + if (!setLatLonOrigin(latitude, longitude, hpos_var)) { return false; } // altitude is optional - setAltOrigin(altitude, epv); + setAltOrigin(altitude, vpos_var); return true; } -bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const float eph) +bool Ekf::setLatLonOrigin(const double latitude, const double longitude, const float hpos_var) { if (!checkLatLonValidity(latitude, longitude)) { return false; } - bool current_pos_available = false; - double current_lat = static_cast(NAN); - double current_lon = static_cast(NAN); + if (!_local_origin_lat_lon.isInitialized() && isLocalHorizontalPositionValid()) { + // Already navigating in a local frame, use the origin to initialize global position + const Vector2f pos_prev = getLocalHorizontalPosition(); + _local_origin_lat_lon.initReference(latitude, longitude, _time_delayed_us); + double new_latitude; + double new_longitude; + _local_origin_lat_lon.reproject(pos_prev(0), pos_prev(1), new_latitude, new_longitude); + resetHorizontalPositionTo(new_latitude, new_longitude, hpos_var); - // if we are already doing aiding, correct for the change in position since the EKF started navigating - if (_pos_ref.isInitialized() && isLocalHorizontalPositionValid()) { - _pos_ref.reproject(_state.pos(0), _state.pos(1), current_lat, current_lon); - current_pos_available = true; - } - - // reinitialize map projection to latitude, longitude, altitude, and reset position - _pos_ref.initReference(latitude, longitude, _time_delayed_us); - - if (PX4_ISFINITE(eph) && (eph >= 0.f)) { - _gpos_origin_eph = eph; - } - - if (current_pos_available) { - // reset horizontal position if we already have a global origin - Vector2f position = _pos_ref.project(current_lat, current_lon); - resetHorizontalPositionTo(position); + } else { + // Simply move the origin and compute the change in local position + const Vector2f pos_prev = getLocalHorizontalPosition(); + _local_origin_lat_lon.initReference(latitude, longitude, _time_delayed_us); + const Vector2f pos_new = getLocalHorizontalPosition(); + const Vector2f delta_pos = pos_new - pos_prev; + updateHorizontalPositionResetStatus(delta_pos); } return true; } -bool Ekf::setAltOrigin(const float altitude, const float epv) +bool Ekf::setAltOrigin(const float altitude, const float vpos_var) { if (!checkAltitudeValidity(altitude)) { return false; } - const float gps_alt_ref_prev = _gps_alt_ref; - _gps_alt_ref = altitude; + ECL_INFO("EKF origin altitude %.1fm -> %.1fm", (double)_local_origin_alt, + (double)altitude); - if (PX4_ISFINITE(epv) && (epv >= 0.f)) { - _gpos_origin_epv = epv; - } + if (!PX4_ISFINITE(_local_origin_alt) && isLocalVerticalPositionValid()) { + const float local_alt_prev = _gpos.altitude(); + _local_origin_alt = altitude; + resetHeightTo(local_alt_prev + _local_origin_alt); - if (PX4_ISFINITE(gps_alt_ref_prev) && isLocalVerticalPositionValid()) { - // determine current z - const float z_prev = _state.pos(2); - const float current_alt = -z_prev + gps_alt_ref_prev; -#if defined(CONFIG_EKF2_GNSS) - const float gps_hgt_bias = _gps_hgt_b_est.getBias(); -#endif // CONFIG_EKF2_GNSS - resetVerticalPositionTo(_gps_alt_ref - current_alt); - ECL_DEBUG("EKF global origin updated, resetting vertical position %.1fm -> %.1fm", (double)z_prev, - (double)_state.pos(2)); -#if defined(CONFIG_EKF2_GNSS) - // adjust existing GPS height bias - _gps_hgt_b_est.setBias(gps_hgt_bias); -#endif // CONFIG_EKF2_GNSS + } else { + const float delta_origin_alt = altitude - _local_origin_alt; + _local_origin_alt = altitude; + updateVerticalPositionResetStatus(-delta_origin_alt); + +#if defined(CONFIG_EKF2_TERRAIN) + updateTerrainResetStatus(-delta_origin_alt); +#endif // CONFIG_EKF2_TERRAIN } return true; } -bool Ekf::setEkfGlobalOriginFromCurrentPos(const double latitude, const double longitude, const float altitude, - const float eph, const float epv) +bool Ekf::resetGlobalPositionTo(const double latitude, const double longitude, const float altitude, + const float hpos_var, const float vpos_var) { - if (!setLatLonOriginFromCurrentPos(latitude, longitude, eph)) { + if (!resetLatLonTo(latitude, longitude, hpos_var)) { return false; } // altitude is optional - setAltOriginFromCurrentPos(altitude, epv); + resetAltitudeTo(altitude, vpos_var); return true; } -bool Ekf::setLatLonOriginFromCurrentPos(const double latitude, const double longitude, const float eph) +bool Ekf::resetLatLonTo(const double latitude, const double longitude, const float hpos_var) { if (!checkLatLonValidity(latitude, longitude)) { return false; } - _pos_ref.initReference(latitude, longitude, _time_delayed_us); + Vector2f pos_prev; - // if we are already doing aiding, correct for the change in position since the EKF started navigating - if (isLocalHorizontalPositionValid()) { - double est_lat; - double est_lon; - _pos_ref.reproject(-_state.pos(0), -_state.pos(1), est_lat, est_lon); - _pos_ref.initReference(est_lat, est_lon, _time_delayed_us); + if (!_local_origin_lat_lon.isInitialized()) { + MapProjection zero_ref; + zero_ref.initReference(0.0, 0.0); + pos_prev = zero_ref.project(_gpos.latitude_deg(), _gpos.longitude_deg()); + + _local_origin_lat_lon.initReference(latitude, longitude, _time_delayed_us); + + // if we are already doing aiding, correct for the change in position since the EKF started navigating + if (isLocalHorizontalPositionValid()) { + double est_lat; + double est_lon; + _local_origin_lat_lon.reproject(-pos_prev(0), -pos_prev(1), est_lat, est_lon); + _local_origin_lat_lon.initReference(est_lat, est_lon, _time_delayed_us); + } + + ECL_INFO("Origin set to lat=%.6f, lon=%.6f", + _local_origin_lat_lon.getProjectionReferenceLat(), _local_origin_lat_lon.getProjectionReferenceLon()); + + } else { + pos_prev = _local_origin_lat_lon.project(_gpos.latitude_deg(), _gpos.longitude_deg()); } - if (PX4_ISFINITE(eph) && (eph >= 0.f)) { - _gpos_origin_eph = eph; + _gpos.setLatLonDeg(latitude, longitude); + _output_predictor.resetLatLonTo(latitude, longitude); + + const Vector2f delta_horz_pos = getLocalHorizontalPosition() - pos_prev; + +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + _ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - delta_horz_pos); +#endif // CONFIG_EKF2_EXTERNAL_VISION + + updateHorizontalPositionResetStatus(delta_horz_pos); + + if (PX4_ISFINITE(hpos_var)) { + P.uncorrelateCovarianceSetVariance<2>(State::pos.idx, math::max(sq(0.01f), hpos_var)); } + // Reset the timout timer + _time_last_hor_pos_fuse = _time_delayed_us; + return true; } -bool Ekf::setAltOriginFromCurrentPos(const float altitude, const float epv) +bool Ekf::resetAltitudeTo(const float altitude, const float vpos_var) { if (!checkAltitudeValidity(altitude)) { return false; } - _gps_alt_ref = altitude + _state.pos(2); + if (!PX4_ISFINITE(_local_origin_alt)) { + const float local_alt_prev = _gpos.altitude(); - if (PX4_ISFINITE(epv) && (epv >= 0.f)) { - _gpos_origin_epv = epv; + if (isLocalVerticalPositionValid()) { + _local_origin_alt = altitude - local_alt_prev; + + } else { + _local_origin_alt = altitude; + } + + ECL_INFO("Origin alt=%.3f", (double)_local_origin_alt); } + resetHeightTo(altitude, vpos_var); + return true; } void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) const { - float eph = INFINITY; - float epv = INFINITY; - if (global_origin_valid()) { - // report absolute accuracy taking into account the uncertainty in location of the origin - eph = sqrtf(P.trace<2>(State::pos.idx + 0) + sq(_gpos_origin_eph)); - epv = sqrtf(P.trace<1>(State::pos.idx + 2) + sq(_gpos_origin_epv)); + get_ekf_lpos_accuracy(ekf_eph, ekf_epv); - if (_horizontal_deadreckon_time_exceeded) { - float lpos_eph = 0.f; - float lpos_epv = 0.f; - get_ekf_lpos_accuracy(&lpos_eph, &lpos_epv); - - eph = math::max(eph, lpos_eph); - epv = math::max(epv, lpos_epv); - } + } else { + *ekf_eph = INFINITY; + *ekf_epv = INFINITY; } - - *ekf_eph = eph; - *ekf_epv = epv; } void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) const @@ -726,7 +740,13 @@ void Ekf::fuse(const VectorState &K, float innovation) _state.vel = matrix::constrain(_state.vel - K.slice(State::vel.idx, 0) * innovation, -1.e3f, 1.e3f); // pos - _state.pos = matrix::constrain(_state.pos - K.slice(State::pos.idx, 0) * innovation, -1.e6f, 1.e6f); + const Vector3f pos_correction = K.slice(State::pos.idx, 0) * (-innovation); + + // Accumulate position in global coordinates + _gpos += pos_correction; + _state.pos.zero(); + // Also store altitude in the state vector as this is used for optical flow fusion + _state.pos(2) = -_gpos.altitude(); // gyro_bias _state.gyro_bias = matrix::constrain(_state.gyro_bias - K.slice(State::gyro_bias.idx, diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index a188d2f88e..5322fe0997 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -42,6 +42,7 @@ #ifndef EKF_ESTIMATOR_INTERFACE_H #define EKF_ESTIMATOR_INTERFACE_H +#include "lat_lon_alt/lat_lon_alt.hpp" #if defined(MODULE_NAME) #include # define ECL_INFO PX4_DEBUG @@ -241,7 +242,26 @@ public: Vector3f getVelocity() const { return _output_predictor.getVelocity(); } const Vector3f &getVelocityDerivative() const { return _output_predictor.getVelocityDerivative(); } float getVerticalPositionDerivative() const { return _output_predictor.getVerticalPositionDerivative(); } - Vector3f getPosition() const { return _output_predictor.getPosition(); } + Vector3f getPosition() const + { + LatLonAlt lla = _output_predictor.getLatLonAlt(); + float x; + float y; + + if (_local_origin_lat_lon.isInitialized()) { + _local_origin_lat_lon.project(lla.latitude_deg(), lla.longitude_deg(), x, y); + + } else { + MapProjection zero_ref; + zero_ref.initReference(0.0, 0.0); + zero_ref.project(lla.latitude_deg(), lla.longitude_deg(), x, y); + } + + const float z = -(lla.altitude() - getEkfGlobalOriginAltitude()); + + return Vector3f(x, y, z); + } + LatLonAlt getLatLonAlt() const { return _output_predictor.getLatLonAlt(); } const Vector3f &getOutputTrackingError() const { return _output_predictor.getOutputTrackingError(); } #if defined(CONFIG_EKF2_MAGNETOMETER) @@ -307,9 +327,9 @@ public: const imuSample &get_imu_sample_delayed() const { return _imu_buffer.get_oldest(); } const uint64_t &time_delayed_us() const { return _time_delayed_us; } - bool global_origin_valid() const { return _pos_ref.isInitialized(); } - const MapProjection &global_origin() const { return _pos_ref; } - float getEkfGlobalOriginAltitude() const { return PX4_ISFINITE(_gps_alt_ref) ? _gps_alt_ref : 0.f; } + bool global_origin_valid() const { return _local_origin_lat_lon.isInitialized(); } + const MapProjection &global_origin() const { return _local_origin_lat_lon; } + float getEkfGlobalOriginAltitude() const { return PX4_ISFINITE(_local_origin_alt) ? _local_origin_alt : 0.f; } OutputPredictor &output_predictor() { return _output_predictor; }; @@ -379,10 +399,8 @@ protected: bool _initialised{false}; // true if the ekf interface instance (data buffering) is initialized // Variables used to publish the WGS-84 location of the EKF local NED origin - MapProjection _pos_ref{}; // Contains WGS-84 position latitude and longitude of the EKF origin - float _gps_alt_ref{NAN}; ///< WGS-84 height (m) - float _gpos_origin_eph{0.0f}; // horizontal position uncertainty of the global origin - float _gpos_origin_epv{0.0f}; // vertical position uncertainty of the global origin + MapProjection _local_origin_lat_lon{}; + float _local_origin_alt{NAN}; #if defined(CONFIG_EKF2_GNSS) RingBuffer *_gps_buffer {nullptr}; diff --git a/src/modules/ekf2/EKF/lat_lon_alt/CMakeLists.txt b/src/modules/ekf2/EKF/lat_lon_alt/CMakeLists.txt new file mode 100644 index 0000000000..bf2b3ebf53 --- /dev/null +++ b/src/modules/ekf2/EKF/lat_lon_alt/CMakeLists.txt @@ -0,0 +1 @@ +px4_add_unit_gtest(SRC test_lat_lon_alt.cpp LINKLIBS geo) diff --git a/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp b/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp new file mode 100644 index 0000000000..0fa7115df7 --- /dev/null +++ b/src/modules/ekf2/EKF/lat_lon_alt/lat_lon_alt.hpp @@ -0,0 +1,154 @@ +/**************************************************************************** + * + * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "mathlib/math/Limits.hpp" +#include + +class LatLonAlt +{ +public: + LatLonAlt() = default; + LatLonAlt(const LatLonAlt &lla) + { + _latitude_rad = lla.latitude_rad(); + _longitude_rad = lla.longitude_rad(); + _altitude = lla.altitude(); + } + + LatLonAlt(const double latitude_deg, const double longitude_deg, const float altitude_m) + { + _latitude_rad = math::radians(latitude_deg); + _longitude_rad = math::radians(longitude_deg); + _altitude = altitude_m; + } + + void setZero() { _latitude_rad = 0.0; _longitude_rad = 0.0; _altitude = 0.f; } + + double latitude_deg() const { return math::degrees(latitude_rad()); } + double longitude_deg() const { return math::degrees(longitude_rad()); } + + const double &latitude_rad() const { return _latitude_rad; } + const double &longitude_rad() const { return _longitude_rad; } + float altitude() const { return _altitude; } + + void setLatitudeDeg(const double &latitude_deg) { _latitude_rad = math::radians(latitude_deg); } + void setLongitudeDeg(const double &longitude_deg) { _longitude_rad = math::radians(longitude_deg); } + void setAltitude(const float altitude) { _altitude = altitude; } + + void setLatLon(const LatLonAlt &lla) { _latitude_rad = lla.latitude_rad(); _longitude_rad = lla.longitude_rad(); } + void setLatLonDeg(const double latitude, const double longitude) { _latitude_rad = math::radians(latitude); _longitude_rad = math::radians(longitude); } + void setLatLonRad(const double latitude, const double longitude) { _latitude_rad = latitude; _longitude_rad = longitude; } + + void print() const { printf("latitude = %f (deg), longitude = %f (deg), altitude = %f (m)\n", _latitude_rad, _longitude_rad, (double)_altitude); } + + void operator+=(const matrix::Vector3f &delta_pos) + { + matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(_latitude_rad, _altitude); + _latitude_rad = matrix::wrap_pi(_latitude_rad + static_cast(delta_pos(0)) / d_lat_lon_to_d_xy(0)); + _longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast(delta_pos(1)) / d_lat_lon_to_d_xy(1)); + _altitude -= delta_pos(2); + } + + void operator+=(const matrix::Vector2f &delta_pos) + { + matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(_latitude_rad, _altitude); + _latitude_rad = matrix::wrap_pi(_latitude_rad + static_cast(delta_pos(0)) / d_lat_lon_to_d_xy(0)); + _longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast(delta_pos(1)) / d_lat_lon_to_d_xy(1)); + } + + LatLonAlt operator+(const matrix::Vector3f &delta_pos) const + { + matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(latitude_rad(), altitude()); + const double latitude_rad = matrix::wrap_pi(_latitude_rad + static_cast(delta_pos(0)) / d_lat_lon_to_d_xy(0)); + const double longitude_rad = matrix::wrap_pi(_longitude_rad + static_cast(delta_pos(1)) / d_lat_lon_to_d_xy(1)); + const float altitude = _altitude - delta_pos(2); + + LatLonAlt lla_new; + lla_new.setLatLonRad(latitude_rad, longitude_rad); + lla_new.setAltitude(altitude); + return lla_new; + } + + void operator=(const LatLonAlt &lla) + { + _latitude_rad = lla.latitude_rad(); + _longitude_rad = lla.longitude_rad(); + _altitude = lla.altitude(); + } + + matrix::Vector3f operator-(const LatLonAlt &lla) const + { + const double delta_lat = matrix::wrap_pi(_latitude_rad - lla.latitude_rad()); + const double delta_lon = matrix::wrap_pi(_longitude_rad - lla.longitude_rad()); + const float delta_alt = _altitude - lla.altitude(); + + const matrix::Vector2d d_lat_lon_to_d_xy = deltaLatLonToDeltaXY(_latitude_rad, _altitude); + return matrix::Vector3f(static_cast(delta_lat * d_lat_lon_to_d_xy(0)), + static_cast(delta_lon * d_lat_lon_to_d_xy(1)), + -delta_alt); + } + +private: + // Convert between curvilinear and cartesian errors + static matrix::Vector2d deltaLatLonToDeltaXY(const double latitude, const float altitude) + { + double r_n; + double r_e; + computeRadiiOfCurvature(latitude, r_n, r_e); + const double dn_dlat = r_n + static_cast(altitude); + const double de_dlon = (r_e + static_cast(altitude)) * cos(latitude); + + return matrix::Vector2d(dn_dlat, de_dlon); + } + + static void computeRadiiOfCurvature(const double latitude, double &meridian_radius_of_curvature, + double &transverse_radius_of_curvature) + { + const double tmp = 1.0 - pow(Wgs84::eccentricity * sin(latitude), 2); + const double sqrt_tmp = std::sqrt(tmp); + meridian_radius_of_curvature = Wgs84::meridian_radius_of_curvature_numerator / (tmp * tmp * sqrt_tmp); + transverse_radius_of_curvature = Wgs84::equatorial_radius / sqrt_tmp; + } + + struct Wgs84 { + static constexpr double equatorial_radius = 6378137.0; + static constexpr double eccentricity = 0.0818191908425; + static constexpr double meridian_radius_of_curvature_numerator = equatorial_radius * (1.0 - eccentricity *eccentricity); + }; + + double _latitude_rad{0.0}; + double _longitude_rad{0.0}; + float _altitude{0.0}; +}; diff --git a/src/modules/ekf2/EKF/lat_lon_alt/test_lat_lon_alt.cpp b/src/modules/ekf2/EKF/lat_lon_alt/test_lat_lon_alt.cpp new file mode 100644 index 0000000000..b7a42ebd48 --- /dev/null +++ b/src/modules/ekf2/EKF/lat_lon_alt/test_lat_lon_alt.cpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (C) 2024 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include +#include +#include + +#include "lat_lon_alt.hpp" + +using namespace matrix; +using math::radians; +using math::degrees; + +TEST(TestLatLonAlt, init) +{ + LatLonAlt lla(5.7, -2.3, 420); + ASSERT_FLOAT_EQ(lla.latitude_deg(), 5.7); + ASSERT_FLOAT_EQ(lla.longitude_deg(), -2.3); + ASSERT_EQ(lla.altitude(), 420); +} + +TEST(TestLatLonAlt, set) +{ + LatLonAlt lla(0.0, 0.0, 0); + ASSERT_EQ(lla.latitude_rad(), 0.0); + ASSERT_EQ(lla.longitude_rad(), 0.0); + ASSERT_EQ(lla.altitude(), 0); + + lla.setLatLonRad(0.1, -0.5); + lla.setAltitude(420); + ASSERT_EQ(lla.latitude_rad(), 0.1); + ASSERT_EQ(lla.longitude_rad(), -0.5); + ASSERT_EQ(lla.altitude(), 420); +} + +TEST(TestLatLonAlt, copy) +{ + LatLonAlt lla(-0.8, -0.1, 500); + + LatLonAlt lla_copy = lla; + ASSERT_EQ(lla_copy.latitude_deg(), -0.8); + ASSERT_EQ(lla_copy.longitude_deg(), -0.1); + ASSERT_EQ(lla_copy.altitude(), 500); +} + +TEST(TestLatLonAlt, addDeltaPos) +{ + MapProjection pos_ref(60.0, 5.0); + LatLonAlt lla(pos_ref.getProjectionReferenceLat(), pos_ref.getProjectionReferenceLon(), 400.f); + + Vector3f delta_pos(5.f, -2.f, 3.f); + lla += delta_pos; + + double lat_new, lon_new; + pos_ref.reproject(delta_pos(0), delta_pos(1), lat_new, lon_new); + + EXPECT_NEAR(lla.latitude_deg(), lat_new, 1e-6); + EXPECT_NEAR(lla.longitude_deg(), lon_new, 1e-6); + EXPECT_EQ(lla.altitude(), 397.f); +} + +TEST(TestLatLonAlt, subLatLonAlt) +{ + MapProjection pos_ref(60.0, 5.0); + LatLonAlt lla(pos_ref.getProjectionReferenceLat(), pos_ref.getProjectionReferenceLon(), 0.f); + + const Vector3f delta_pos_true(1.f, -2.f, 3.f); + + double lat_new, lon_new; + pos_ref.reproject(delta_pos_true(0), delta_pos_true(1), lat_new, lon_new); + LatLonAlt lla_new(lat_new, lon_new, -3.f); + Vector3f delta_pos = lla_new - lla; + + EXPECT_NEAR(delta_pos(0), delta_pos_true(0), 1e-2); + EXPECT_NEAR(delta_pos(1), delta_pos_true(1), 1e-2); + EXPECT_EQ(delta_pos(2), delta_pos_true(2)); +} diff --git a/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp b/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp index d16e20a790..5657d68a20 100644 --- a/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp +++ b/src/modules/ekf2/EKF/output_predictor/output_predictor.cpp @@ -69,7 +69,7 @@ void OutputPredictor::print_status() _output_vert_buffer.entries(), _output_vert_buffer.get_length(), _output_vert_buffer.get_total_size()); } -void OutputPredictor::alignOutputFilter(const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state) +void OutputPredictor::alignOutputFilter(const Quatf &quat_state, const Vector3f &vel_state, const LatLonAlt &gpos_state) { const outputSample &output_delayed = _output_buffer.get_oldest(); @@ -77,9 +77,12 @@ void OutputPredictor::alignOutputFilter(const Quatf &quat_state, const Vector3f Quatf q_delta{quat_state * output_delayed.quat_nominal.inversed()}; q_delta.normalize(); - // calculate the velocity and position deltas between the output and EKF at the EKF fusion time horizon + // calculate the velocity delta between the output and EKF at the EKF fusion time horizon const Vector3f vel_delta = vel_state - output_delayed.vel; - const Vector3f pos_delta = pos_state - output_delayed.pos; + + // zero the position error at delayed time and reset the global reference + const Vector3f pos_delta = -output_delayed.pos; + _global_ref = gpos_state; // loop through the output filter state history and add the deltas for (uint8_t i = 0; i < _output_buffer.get_length(); i++) { @@ -156,29 +159,15 @@ void OutputPredictor::resetVerticalVelocityTo(float delta_vert_vel) _output_vert_new.vert_vel += delta_vert_vel; } -void OutputPredictor::resetHorizontalPositionTo(const Vector2f &delta_horz_pos) +void OutputPredictor::resetLatLonTo(const double &new_latitude, const double &new_longitude) { - for (uint8_t index = 0; index < _output_buffer.get_length(); index++) { - _output_buffer[index].pos.xy() += delta_horz_pos; - } - - _output_new.pos.xy() += delta_horz_pos; + _global_ref.setLatitudeDeg(new_latitude); + _global_ref.setLongitudeDeg(new_longitude); } -void OutputPredictor::resetVerticalPositionTo(const float new_vert_pos, const float vert_pos_change) +void OutputPredictor::resetAltitudeTo(const float new_altitude, const float vert_pos_change) { - // 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) += vert_pos_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) += vert_pos_change; - _output_vert_buffer[i].vert_vel_integ += vert_pos_change; - } - - // add the reset amount to the output observer vertical position state - _output_vert_new.vert_vel_integ = new_vert_pos; + _global_ref.setAltitude(new_altitude); } void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector3f &delta_angle, @@ -261,7 +250,7 @@ void OutputPredictor::calculateOutputStates(const uint64_t time_us, const Vector } void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us, - const Quatf &quat_state, const Vector3f &vel_state, const Vector3f &pos_state, const matrix::Vector3f &gyro_bias, + const Quatf &quat_state, const Vector3f &vel_state, const LatLonAlt &gpos_state, const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias) { // calculate an average filter update time @@ -317,6 +306,8 @@ void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us, const float vel_gain = _dt_correct_states_avg / math::constrain(_vel_tau, _dt_correct_states_avg, 10.f); const float pos_gain = _dt_correct_states_avg / math::constrain(_pos_tau, _dt_correct_states_avg, 10.f); + const Vector3f pos_state = gpos_state - _global_ref; + // calculate down velocity and position tracking errors const float vert_vel_err = (vel_state(2) - output_vert_delayed.vert_vel); const float vert_vel_integ_err = (pos_state(2) - output_vert_delayed.vert_vel_integ); @@ -325,7 +316,7 @@ void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us, // using a PD feedback tuned to a 5% overshoot const float vert_vel_correction = vert_vel_integ_err * pos_gain + vert_vel_err * vel_gain * 1.1f; - applyCorrectionToVerticalOutputBuffer(vert_vel_correction); + applyCorrectionToVerticalOutputBuffer(vert_vel_correction, pos_state(2)); // calculate velocity and position tracking errors const Vector3f vel_err(vel_state - output_delayed.vel); @@ -342,10 +333,14 @@ void OutputPredictor::correctOutputStates(const uint64_t time_delayed_us, _pos_err_integ += pos_err; const Vector3f pos_correction = pos_err * pos_gain + _pos_err_integ * sq(pos_gain) * 0.1f; - applyCorrectionToOutputBuffer(vel_correction, pos_correction); + // as the reference changes, adjust the position correction to keep a constant global position + const Vector3f pos_correction_with_ref_change = pos_correction - pos_state; + applyCorrectionToOutputBuffer(vel_correction, pos_correction_with_ref_change); + + _global_ref = gpos_state; } -void OutputPredictor::applyCorrectionToVerticalOutputBuffer(float vert_vel_correction) +void OutputPredictor::applyCorrectionToVerticalOutputBuffer(const float vert_vel_correction, const float pos_ref_change) { // loop through the vertical output filter state history starting at the oldest and apply the corrections to the // vert_vel states and propagate vert_vel_integ forward using the corrected vert_vel @@ -367,7 +362,7 @@ void OutputPredictor::applyCorrectionToVerticalOutputBuffer(float vert_vel_corre // position is propagated forward using the corrected velocity and a trapezoidal integrator next_state.vert_vel_integ = current_state.vert_vel_integ + (current_state.vert_vel + next_state.vert_vel) * 0.5f * - next_state.dt; + next_state.dt - pos_ref_change; // advance the index index = (index + 1) % size; diff --git a/src/modules/ekf2/EKF/output_predictor/output_predictor.h b/src/modules/ekf2/EKF/output_predictor/output_predictor.h index 07248f3dce..33b7f127bc 100644 --- a/src/modules/ekf2/EKF/output_predictor/output_predictor.h +++ b/src/modules/ekf2/EKF/output_predictor/output_predictor.h @@ -37,6 +37,7 @@ #include #include "../RingBuffer.h" +#include "../lat_lon_alt/lat_lon_alt.hpp" #include @@ -52,7 +53,7 @@ public: // modify output filter to match the the EKF state at the fusion time horizon void alignOutputFilter(const matrix::Quatf &quat_state, const matrix::Vector3f &vel_state, - const matrix::Vector3f &pos_state); + const LatLonAlt &gpos_state); /* * Implement a strapdown INS algorithm using the latest IMU data at the current time horizon. * Buffer the INS states and calculate the difference with the EKF states at the delayed fusion time horizon. @@ -67,7 +68,7 @@ public: const matrix::Vector3f &delta_velocity, const float delta_velocity_dt); void correctOutputStates(const uint64_t time_delayed_us, - const matrix::Quatf &quat_state, const matrix::Vector3f &vel_state, const matrix::Vector3f &pos_state, + const matrix::Quatf &quat_state, const matrix::Vector3f &vel_state, const LatLonAlt &gpos_state, const matrix::Vector3f &gyro_bias, const matrix::Vector3f &accel_bias); void resetQuaternion(const matrix::Quatf &quat_change); @@ -75,8 +76,8 @@ public: void resetHorizontalVelocityTo(const matrix::Vector2f &delta_horz_vel); void resetVerticalVelocityTo(float delta_vert_vel); - void resetHorizontalPositionTo(const matrix::Vector2f &delta_horz_pos); - void resetVerticalPositionTo(const float new_vert_pos, const float vert_pos_change); + void resetLatLonTo(const double &new_latitude, const double &new_longitude); + void resetAltitudeTo(float new_altitude, float vert_pos_change); void print_status(); @@ -106,13 +107,12 @@ public: // get the derivative of the vertical position of the body frame origin in local NED earth frame float getVerticalPositionDerivative() const { return _output_vert_new.vert_vel - _vel_imu_rel_body_ned(2); } - // get the position of the body frame origin in local earth frame - matrix::Vector3f getPosition() const + LatLonAlt getLatLonAlt() const { // rotate the position of the IMU relative to the boy origin into earth frame const matrix::Vector3f pos_offset_earth{_R_to_earth_now * _imu_pos_body}; // subtract from the EKF position (which is at the IMU) to get position at the body origin - return _output_new.pos - pos_offset_earth; + return _global_ref + (_output_new.pos - pos_offset_earth); } // return an array containing the output predictor angular, velocity and position tracking @@ -133,7 +133,7 @@ private: * This provides an alternative vertical velocity output that is closer to the first derivative * of the position but does degrade tracking relative to the EKF state. */ - void applyCorrectionToVerticalOutputBuffer(float vert_vel_correction); + void applyCorrectionToVerticalOutputBuffer(float vert_vel_correction, const float pos_ref_change); /* * Calculate corrections to be applied to vel and pos output state history. @@ -159,6 +159,8 @@ private: float dt{0.f}; ///< delta time (sec) }; + LatLonAlt _global_ref{0.0, 0.0, 0.f}; + RingBuffer _output_buffer{12}; RingBuffer _output_vert_buffer{12}; diff --git a/src/modules/ekf2/EKF/position_fusion.cpp b/src/modules/ekf2/EKF/position_fusion.cpp index 9008045842..e4c16b4941 100644 --- a/src/modules/ekf2/EKF/position_fusion.cpp +++ b/src/modules/ekf2/EKF/position_fusion.cpp @@ -36,7 +36,7 @@ void Ekf::updateVerticalPositionAidStatus(estimator_aid_source1d_s &aid_src, const uint64_t &time_us, const float observation, const float observation_variance, const float innovation_gate) const { - float innovation = _state.pos(2) - observation; + float innovation = -_gpos.altitude() - observation; float innovation_variance = getStateVariance()(2) + observation_variance; updateAidSourceStatus(aid_src, time_us, @@ -93,10 +93,20 @@ bool Ekf::fuseVerticalPosition(estimator_aid_source1d_s &aid_src) return aid_src.fused; } -void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f &new_horz_pos_var) +void Ekf::resetHorizontalPositionTo(const double &new_latitude, const double &new_longitude, + const Vector2f &new_horz_pos_var) { - const Vector2f delta_horz_pos{new_horz_pos - Vector2f{_state.pos}}; - _state.pos.xy() = new_horz_pos; + const Vector2f delta_horz_pos = computeDeltaHorizontalPosition(new_latitude, new_longitude); + + updateHorizontalPositionResetStatus(delta_horz_pos); + +#if defined(CONFIG_EKF2_EXTERNAL_VISION) + _ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - delta_horz_pos); +#endif // CONFIG_EKF2_EXTERNAL_VISION + //_gps_pos_b_est.setBias(_gps_pos_b_est.getBias() + _state_reset_status.posNE_change); + + _gpos.setLatLonDeg(new_latitude, new_longitude); + _output_predictor.resetLatLonTo(new_latitude, new_longitude); if (PX4_ISFINITE(new_horz_pos_var(0))) { P.uncorrelateCovarianceSetVariance<1>(State::pos.idx, math::max(sq(0.01f), new_horz_pos_var(0))); @@ -106,54 +116,89 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos, const Vector2f P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 1, math::max(sq(0.01f), new_horz_pos_var(1))); } - _output_predictor.resetHorizontalPositionTo(delta_horz_pos); - - // record the state change - if (_state_reset_status.reset_count.posNE == _state_reset_count_prev.posNE) { - _state_reset_status.posNE_change = delta_horz_pos; - - } else { - // there's already a reset this update, accumulate total delta - _state_reset_status.posNE_change += delta_horz_pos; - } - - _state_reset_status.reset_count.posNE++; - -#if defined(CONFIG_EKF2_EXTERNAL_VISION) - _ev_pos_b_est.setBias(_ev_pos_b_est.getBias() - _state_reset_status.posNE_change); -#endif // CONFIG_EKF2_EXTERNAL_VISION - //_gps_pos_b_est.setBias(_gps_pos_b_est.getBias() + _state_reset_status.posNE_change); - // Reset the timout timer _time_last_hor_pos_fuse = _time_delayed_us; } -void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_var) +Vector2f Ekf::computeDeltaHorizontalPosition(const double &new_latitude, const double &new_longitude) const { - const float old_vert_pos = _state.pos(2); - _state.pos(2) = new_vert_pos; + Vector2f pos; + Vector2f pos_new; + + if (_local_origin_lat_lon.isInitialized()) { + _local_origin_lat_lon.project(_gpos.latitude_deg(), _gpos.longitude_deg(), pos(0), pos(1)); + _local_origin_lat_lon.project(new_latitude, new_longitude, pos_new(0), pos_new(1)); + + } else { + MapProjection zero_ref; + zero_ref.initReference(0.0, 0.0); + zero_ref.project(_gpos.latitude_deg(), _gpos.longitude_deg(), pos(0), pos(1)); + zero_ref.project(new_latitude, new_longitude, pos_new(0), pos_new(1)); + } + + return pos_new - pos; +} + +Vector2f Ekf::getLocalHorizontalPosition() const +{ + if (_local_origin_lat_lon.isInitialized()) { + return _local_origin_lat_lon.project(_gpos.latitude_deg(), _gpos.longitude_deg()); + + } else { + MapProjection zero_ref; + zero_ref.initReference(0.0, 0.0); + return zero_ref.project(_gpos.latitude_deg(), _gpos.longitude_deg()); + } +} + +void Ekf::updateHorizontalPositionResetStatus(const Vector2f &delta) +{ + if (_state_reset_status.reset_count.posNE == _state_reset_count_prev.posNE) { + _state_reset_status.posNE_change = delta; + + } else { + // there's already a reset this update, accumulate total delta + _state_reset_status.posNE_change += delta; + } + + _state_reset_status.reset_count.posNE++; +} + +void Ekf::resetHorizontalPositionTo(const Vector2f &new_pos, + const Vector2f &new_horz_pos_var) +{ + double new_latitude; + double new_longitude; + + if (_local_origin_lat_lon.isInitialized()) { + _local_origin_lat_lon.reproject(new_pos(0), new_pos(1), new_latitude, new_longitude); + + } else { + MapProjection zero_ref; + zero_ref.initReference(0.0, 0.0); + zero_ref.reproject(new_pos(0), new_pos(1), new_latitude, new_longitude); + } + + resetHorizontalPositionTo(new_latitude, new_longitude, new_horz_pos_var); +} + +void Ekf::resetHeightTo(const float new_altitude, float new_vert_pos_var) +{ + const float old_altitude = _gpos.altitude(); + _gpos.setAltitude(new_altitude); if (PX4_ISFINITE(new_vert_pos_var)) { // the state variance is the same as the observation P.uncorrelateCovarianceSetVariance<1>(State::pos.idx + 2, math::max(sq(0.01f), new_vert_pos_var)); } - const float delta_z = new_vert_pos - old_vert_pos; + const float delta_z = -(new_altitude - old_altitude); // 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_predictor.resetVerticalPositionTo(new_vert_pos, delta_z); + _output_predictor.resetAltitudeTo(new_altitude, delta_z); - // record the state change - if (_state_reset_status.reset_count.posD == _state_reset_count_prev.posD) { - _state_reset_status.posD_change = delta_z; - - } else { - // there's already a reset this update, accumulate total delta - _state_reset_status.posD_change += delta_z; - } - - _state_reset_status.reset_count.posD++; + updateVerticalPositionResetStatus(delta_z); #if defined(CONFIG_EKF2_BAROMETER) _baro_b_est.setBias(_baro_b_est.getBias() + delta_z); @@ -166,9 +211,29 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_TERRAIN) + updateTerrainResetStatus(delta_z); _state.terrain += delta_z; +#endif // CONFIG_EKF2_TERRAIN - // record the state change + // Reset the timout timer + _time_last_hgt_fuse = _time_delayed_us; +} + +void Ekf::updateVerticalPositionResetStatus(const float delta_z) +{ + if (_state_reset_status.reset_count.posD == _state_reset_count_prev.posD) { + _state_reset_status.posD_change = delta_z; + + } else { + // there's already a reset this update, accumulate total delta + _state_reset_status.posD_change += delta_z; + } + + _state_reset_status.reset_count.posD++; +} + +void Ekf::updateTerrainResetStatus(const float delta_z) +{ if (_state_reset_status.reset_count.hagl == _state_reset_count_prev.hagl) { _state_reset_status.hagl_change = delta_z; @@ -178,17 +243,15 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v } _state_reset_status.reset_count.hagl++; -#endif // CONFIG_EKF2_TERRAIN - - // Reset the timout timer - _time_last_hgt_fuse = _time_delayed_us; } void Ekf::resetHorizontalPositionToLastKnown() { - ECL_INFO("reset position to last known (%.3f, %.3f)", (double)_last_known_pos(0), (double)_last_known_pos(1)); + ECL_INFO("reset position to last known (%.3f, %.3f)", (double)_last_known_gpos.latitude_deg(), + (double)_last_known_gpos.longitude_deg()); _information_events.flags.reset_pos_to_last_known = true; // Used when falling back to non-aiding mode of operation - resetHorizontalPositionTo(_last_known_pos.xy(), sq(_params.pos_noaid_noise)); + resetHorizontalPositionTo(_last_known_gpos.latitude_deg(), _last_known_gpos.longitude_deg(), + sq(_params.pos_noaid_noise)); } diff --git a/src/modules/ekf2/EKF/terrain_control.cpp b/src/modules/ekf2/EKF/terrain_control.cpp index 323d5ac553..afb845a662 100644 --- a/src/modules/ekf2/EKF/terrain_control.cpp +++ b/src/modules/ekf2/EKF/terrain_control.cpp @@ -43,7 +43,7 @@ void Ekf::initTerrain() { // assume a ground clearance - _state.terrain = _state.pos(2) + _params.rng_gnd_clearance; + _state.terrain = -_gpos.altitude() + _params.rng_gnd_clearance; // use the ground clearance value as our uncertainty P.uncorrelateCovarianceSetVariance(State::terrain.idx, sq(_params.rng_gnd_clearance)); @@ -53,7 +53,7 @@ void Ekf::controlTerrainFakeFusion() { // If we are on ground, store the local position and time to use as a reference if (!_control_status.flags.in_air) { - _last_on_ground_posD = _state.pos(2); + _last_on_ground_posD = -_gpos.altitude(); _control_status.flags.rng_fault = false; } else if (!_control_status_prev.flags.in_air) { diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index a40289f7b3..93c9ed617a 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1166,17 +1166,17 @@ void EKF2::PublishEventFlags(const hrt_abstime ×tamp) void EKF2::PublishGlobalPosition(const hrt_abstime ×tamp) { if (_ekf.global_origin_valid() && _ekf.control_status().flags.yaw_align) { - const Vector3f position{_ekf.getPosition()}; - // generate and publish global position data vehicle_global_position_s global_pos{}; global_pos.timestamp_sample = timestamp; - // Position of local NED origin in GPS / WGS84 frame - _ekf.global_origin().reproject(position(0), position(1), global_pos.lat, global_pos.lon); + // Position GPS / WGS84 frame + const LatLonAlt lla = _ekf.getLatLonAlt(); + global_pos.lat = lla.latitude_deg(); + global_pos.lon = lla.longitude_deg(); global_pos.lat_lon_valid = _ekf.isGlobalHorizontalPositionValid(); - global_pos.alt = -position(2) + _ekf.getEkfGlobalOriginAltitude(); // Altitude AMSL in meters + global_pos.alt = lla.altitude(); global_pos.alt_valid = _ekf.isGlobalVerticalPositionValid(); #if defined(CONFIG_EKF2_GNSS) diff --git a/src/modules/ekf2/test/test_EKF_airspeed.cpp b/src/modules/ekf2/test/test_EKF_airspeed.cpp index d1a9e91896..d905fb8570 100644 --- a/src/modules/ekf2/test/test_EKF_airspeed.cpp +++ b/src/modules/ekf2/test/test_EKF_airspeed.cpp @@ -180,15 +180,31 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoning) _ekf->set_vehicle_at_rest(false); _ekf->set_is_fixed_wing(true); + const Vector3f pos_prev = _ekf->getPosition(); + const double latitude_new = -15.0000005; const double longitude_new = -115.0000005; const float altitude_new = 1500.0; const float eph = 50.f; const float epv = 10.f; - _ekf->setEkfGlobalOrigin(latitude_new, longitude_new, altitude_new); _ekf->resetGlobalPosToExternalObservation(latitude_new, longitude_new, altitude_new, eph, epv, 0); + const Vector3f pos = _ekf->getPosition(); + + // lat/lon is initialized so the local horizontal position remains constant + EXPECT_NEAR(pos(0), pos_prev(0), 1e-3f); + EXPECT_NEAR(pos(1), pos_prev(1), 1e-3f); + + // alt is updated as the local altitude origin was already set + EXPECT_NEAR(pos(2), pos_prev(2) - (altitude_new - _ekf->getEkfGlobalOriginAltitude()), 1e-3f); + + const LatLonAlt lla = _ekf->getLatLonAlt(); + EXPECT_NEAR(lla.latitude_deg(), latitude_new, 1e-6f); + EXPECT_NEAR(lla.longitude_deg(), longitude_new, 1e-6f); + EXPECT_NEAR(lla.altitude(), altitude_new, 1e-3f); + + // Simulate the fact that the sideslip can start immediately, without // waiting for a measurement sample. _ekf_wrapper.enableBetaFusion(); @@ -227,7 +243,6 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoningLatLonAltReset) const float eph = 50.f; const float epv = 1.f; - _ekf->setEkfGlobalOrigin(latitude, longitude, altitude); _ekf->resetGlobalPosToExternalObservation(latitude, longitude, altitude, eph, epv, 0); _ekf_wrapper.enableBetaFusion(); diff --git a/src/modules/ekf2/test/test_EKF_basics.cpp b/src/modules/ekf2/test/test_EKF_basics.cpp index afe5699205..d67cac0e52 100644 --- a/src/modules/ekf2/test/test_EKF_basics.cpp +++ b/src/modules/ekf2/test/test_EKF_basics.cpp @@ -219,19 +219,22 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_initialized) _altitude_new = 100.0; _sensor_simulator.startGps(); - _ekf->set_min_required_gps_health_time(1e6); - _sensor_simulator.runSeconds(1); + _ekf_wrapper.enableGpsHeightFusion(); _sensor_simulator.setGpsLatitude(_latitude_new); _sensor_simulator.setGpsLongitude(_longitude_new); _sensor_simulator.setGpsAltitude(_altitude_new); + _ekf->set_min_required_gps_health_time(1e6); + _sensor_simulator.runSeconds(1); _sensor_simulator.runSeconds(5); _ekf->getEkfGlobalOrigin(_origin_time, _latitude, _longitude, _altitude); EXPECT_DOUBLE_EQ(_latitude, _latitude_new); EXPECT_DOUBLE_EQ(_longitude, _longitude_new); - EXPECT_NEAR(_altitude, _altitude_new, 0.01f); + + // In baro height ref the origin is set using baro data and not GNSS altitude + EXPECT_NEAR(_altitude, _sensor_simulator._baro.getData(), 0.01f); // Note: we cannot reset too far since the local position is limited to 1e6m _latitude_new = 14.0000005; @@ -261,11 +264,13 @@ TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_initialized) TEST_F(EkfBasicsTest, reset_ekf_global_origin_gps_uninitialized) { - _ekf->getEkfGlobalOrigin(_origin_time, _latitude_new, _longitude_new, _altitude_new); + _ekf->getEkfGlobalOrigin(_origin_time, _latitude, _longitude, _altitude); EXPECT_DOUBLE_EQ(_latitude, _latitude_new); EXPECT_DOUBLE_EQ(_longitude, _longitude_new); - EXPECT_FLOAT_EQ(_altitude, _altitude_new); + + // In baro height ref the origin is set using baro data and not GNSS altitude + EXPECT_NEAR(_altitude, _sensor_simulator._baro.getData(), 0.01f); EXPECT_FALSE(_ekf->global_origin_valid()); diff --git a/src/modules/ekf2/test/test_EKF_flow.cpp b/src/modules/ekf2/test/test_EKF_flow.cpp index 650d168a98..bc33c350e7 100644 --- a/src/modules/ekf2/test/test_EKF_flow.cpp +++ b/src/modules/ekf2/test/test_EKF_flow.cpp @@ -345,3 +345,49 @@ TEST_F(EkfFlowTest, yawMotionNoMagFusion) _ekf->state().vector().print(); _ekf->covariances().print(); } + +TEST_F(EkfFlowTest, deadReckoning) +{ + ResetLoggingChecker reset_logging_checker(_ekf); + + // WHEN: simulate being 5m above ground + const float simulated_distance_to_ground = 5.f; + _sensor_simulator._trajectory[2].setCurrentPosition(-simulated_distance_to_ground); + startRangeFinderFusion(simulated_distance_to_ground); + + _ekf->set_in_air_status(true); + _ekf->set_vehicle_at_rest(false); + + // WHEN: moving a couple of meters while doing flow dead_reckoning + Vector3f simulated_velocity(0.5f, -0.2f, 0.f); + _sensor_simulator._trajectory[0].setCurrentVelocity(simulated_velocity(0)); + _sensor_simulator._trajectory[1].setCurrentVelocity(simulated_velocity(1)); + _sensor_simulator.setTrajectoryTargetVelocity(simulated_velocity); + _ekf_wrapper.enableFlowFusion(); + _sensor_simulator.startFlow(); + + _sensor_simulator.runTrajectorySeconds(5.f); + + simulated_velocity = Vector3f(0.f, 0.f, 0.f); + _sensor_simulator.setTrajectoryTargetVelocity(simulated_velocity); + + _sensor_simulator.runTrajectorySeconds(_sensor_simulator._trajectory[0].getTotalTime()); + + EXPECT_FALSE(_ekf->isGlobalHorizontalPositionValid()); + EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); + const Vector3f lpos_before_reset = _ekf->getPosition(); + const float altitude_ref_prev = _ekf->getEkfGlobalOriginAltitude(); + + const double latitude_new = -15.0000005; + const double longitude_new = -115.0000005; + const float altitude_new = 1500.0; + const float eph = 50.f; + const float epv = 10.f; + _ekf->setEkfGlobalOrigin(latitude_new, longitude_new, altitude_new, eph, epv); + + const Vector3f lpos_after_reset = _ekf->getPosition(); + + EXPECT_NEAR(lpos_after_reset(0), lpos_before_reset(0), 1e-3); + EXPECT_NEAR(lpos_after_reset(1), lpos_before_reset(1), 1e-3); + EXPECT_NEAR(lpos_after_reset(2), lpos_before_reset(2) + (altitude_new - altitude_ref_prev), 1e-3); +} diff --git a/src/modules/ekf2/test/test_EKF_gps.cpp b/src/modules/ekf2/test/test_EKF_gps.cpp index ac683564a4..f6aae937df 100644 --- a/src/modules/ekf2/test/test_EKF_gps.cpp +++ b/src/modules/ekf2/test/test_EKF_gps.cpp @@ -164,7 +164,7 @@ TEST_F(EkfGpsTest, resetToGpsPosition) // AND: simulate jump in position _sensor_simulator.startGps(); - const Vector3f simulated_position_change(2.0f, -1.0f, 0.f); + const Vector3f simulated_position_change(20.0f, -1.0f, 0.f); _sensor_simulator._gps.stepHorizontalPositionByMeters( Vector2f(simulated_position_change)); _sensor_simulator.runSeconds(6); diff --git a/src/modules/ekf2/test/test_EKF_height_fusion.cpp b/src/modules/ekf2/test/test_EKF_height_fusion.cpp index b322a28182..17c95cad45 100644 --- a/src/modules/ekf2/test/test_EKF_height_fusion.cpp +++ b/src/modules/ekf2/test/test_EKF_height_fusion.cpp @@ -123,7 +123,7 @@ TEST_F(EkfHeightFusionTest, baroRef) /* EXPECT_EQ(status.bias, _sensor_simulator._baro.getData()); */ // This is the real bias, but the estimator isn't running so the status isn't updated EXPECT_EQ(baro_status.bias, 0.f); const BiasEstimator::status &gps_status = _ekf->getGpsHgtBiasEstimatorStatus(); - EXPECT_NEAR(gps_status.bias, -baro_increment, 0.2f); + EXPECT_NEAR(gps_status.bias, _sensor_simulator._gps.getData().alt - _sensor_simulator._baro.getData(), 0.2f); const float terrain = _ekf->getTerrainVertPos(); EXPECT_NEAR(terrain, -baro_increment, 1.2f); @@ -170,8 +170,13 @@ TEST_F(EkfHeightFusionTest, gpsRef) EXPECT_TRUE(_ekf_wrapper.isIntendingRangeHeightFusion()); EXPECT_FALSE(_ekf_wrapper.isIntendingExternalVisionHeightFusion()); - // AND WHEN: the baro data increases const float baro_initial = _sensor_simulator._baro.getData(); + + const BiasEstimator::status &baro_status_initial = _ekf->getBaroBiasEstimatorStatus(); + const float baro_rel_initial = baro_initial - _sensor_simulator._gps.getData().alt; + EXPECT_NEAR(baro_status_initial.bias, baro_rel_initial, 0.6f); + + // AND WHEN: the baro data increases const float baro_increment = 5.f; _sensor_simulator._baro.setData(baro_initial + baro_increment); _sensor_simulator.runSeconds(100); @@ -180,7 +185,7 @@ TEST_F(EkfHeightFusionTest, gpsRef) // the GPS height value and the baro gets its bias estimated EXPECT_NEAR(_ekf->getPosition()(2), 0.f, 1.f); const BiasEstimator::status &baro_status = _ekf->getBaroBiasEstimatorStatus(); - EXPECT_NEAR(baro_status.bias, baro_initial + baro_increment, 1.3f); + EXPECT_NEAR(baro_status.bias, baro_rel_initial + baro_increment, 1.3f); const float terrain = _ekf->getTerrainVertPos(); EXPECT_NEAR(terrain, 0.f, 1.1f); // TODO: why? @@ -196,7 +201,7 @@ TEST_F(EkfHeightFusionTest, gpsRef) EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion()); EXPECT_TRUE(_ekf_wrapper.isIntendingRangeHeightFusion()); - EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, baro_initial + baro_increment - gps_step, 0.2f); + EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, baro_rel_initial + baro_increment - gps_step, 0.2f); // and the innovations are close to zero EXPECT_NEAR(_ekf->aid_src_baro_hgt().innovation, 0.f, 0.2f); @@ -367,6 +372,7 @@ TEST_F(EkfHeightFusionTest, changeEkfOriginAlt) ResetLoggingChecker reset_logging_checker(_ekf); reset_logging_checker.capturePreResetState(); + const float baro_bias_prev = _ekf->getBaroBiasEstimatorStatus().bias; const float alt_increment = 4478.f; _ekf->setEkfGlobalOrigin(lat, lon, alt + alt_increment); @@ -376,9 +382,12 @@ TEST_F(EkfHeightFusionTest, changeEkfOriginAlt) EXPECT_NEAR(_ekf->getPosition()(2), alt_increment, 1.f); reset_logging_checker.capturePostResetState(); - EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, _sensor_simulator._baro.getData() + alt_increment, 0.2f); + + // An origin reset doesn't change the baro bias as it is relative to the height reference (GNSS) + EXPECT_NEAR(_ekf->getBaroBiasEstimatorStatus().bias, baro_bias_prev, 0.3f); EXPECT_NEAR(_ekf->getTerrainVertPos(), alt_increment, 1.f); + EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(0)); EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1)); } diff --git a/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp b/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp index 1a132e5db5..1d1fdfb6ce 100644 --- a/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp +++ b/src/modules/ekf2/test/test_EKF_yaw_estimator.cpp @@ -40,7 +40,6 @@ #include "EKF/ekf.h" #include "sensor_simulator/sensor_simulator.h" #include "sensor_simulator/ekf_wrapper.h" -#include "test_helper/reset_logging_checker.h" class EKFYawEstimatorTest : public ::testing::Test { @@ -91,8 +90,6 @@ TEST_F(EKFYawEstimatorTest, inAirYawAlignment) // WHEN: the vehicle starts accelerating in the horizontal plane _sensor_simulator.setTrajectoryTargetVelocity(Vector3f(2.f, -2.f, -1.f)); _ekf->set_in_air_status(true); - ResetLoggingChecker reset_logging_checker(_ekf); - reset_logging_checker.capturePreResetState(); _sensor_simulator.runTrajectorySeconds(3.f); @@ -106,10 +103,12 @@ TEST_F(EKFYawEstimatorTest, inAirYawAlignment) EXPECT_NEAR(yaw_est, yaw, tolerance_rad); EXPECT_LT(yaw_est_var, tolerance_rad); - // 1 reset when starting GNSS aiding - reset_logging_checker.capturePostResetState(); - EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(1)); - EXPECT_TRUE(reset_logging_checker.isHorizontalPositionResetCounterIncreasedBy(1)); + EXPECT_LT(_ekf->aid_src_gnss_pos().test_ratio[0], 0.5f); + EXPECT_LT(_ekf->aid_src_gnss_pos().test_ratio[1], 0.5f); + + EXPECT_LT(_ekf->aid_src_gnss_vel().test_ratio[0], 0.1f); + EXPECT_LT(_ekf->aid_src_gnss_vel().test_ratio[1], 0.1f); + EXPECT_LT(_ekf->aid_src_gnss_vel().test_ratio[2], 0.1f); EXPECT_TRUE(_ekf->isLocalHorizontalPositionValid()); EXPECT_TRUE(_ekf->isGlobalHorizontalPositionValid());