diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index a81cc39d62..24734dcfb3 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -276,67 +276,100 @@ void Ekf::predictState(const imuSample &imu_delayed) _height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf; } -bool Ekf::resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, - uint64_t timestamp_observation) +bool Ekf::resetGlobalPosToExternalObservation(const double latitude, const double longitude, const float altitude, + const float eph, + const float epv, uint64_t timestamp_observation) { - if (!_pos_ref.isInitialized()) { - return setLatLonOriginFromCurrentPos(lat_deg, lon_deg, accuracy); + if (!checkLatLonValidity(latitude, longitude)) { + return false; } - Vector2f pos_corrected = _pos_ref.project(lat_deg, lon_deg); + if (!_pos_ref.isInitialized()) { + if (!setLatLonOriginFromCurrentPos(latitude, longitude, eph)) { + return false; + } + + if (!PX4_ISFINITE(_gps_alt_ref)) { + setAltOriginFromCurrentPos(altitude, epv); + } + + return true; + } + + Vector3f pos_correction; // apply a first order correction using velocity at the delayed time horizon and the delta time - if ((timestamp_observation > 0) && (isHorizontalAidingActive() || !_horizontal_deadreckon_time_exceeded)) { + if ((timestamp_observation > 0) && local_position_is_valid()) { timestamp_observation = math::min(_time_latest_us, timestamp_observation); - float diff_us = 0.f; + float dt_us; if (_time_delayed_us >= timestamp_observation) { - diff_us = static_cast(_time_delayed_us - timestamp_observation); + dt_us = static_cast(_time_delayed_us - timestamp_observation); } else { - diff_us = -static_cast(timestamp_observation - _time_delayed_us); + dt_us = -static_cast(timestamp_observation - _time_delayed_us); } - const float dt_s = diff_us * 1e-6f; - pos_corrected += _state.vel.xy() * dt_s; + const float dt_s = dt_us * 1e-6f; + pos_correction = _state.vel * dt_s; } - const float obs_var = math::max(sq(accuracy), sq(0.01f)); + { + const Vector2f hpos = _pos_ref.project(latitude, longitude) + pos_correction.xy(); - const Vector2f innov = Vector2f(_state.pos.xy()) - pos_corrected; - const Vector2f innov_var = Vector2f(getStateVariance()) + obs_var; + const float obs_var = math::max(sq(eph), sq(0.01f)); - const float sq_gate = sq(5.f); // magic hardcoded gate - const Vector2f test_ratio{sq(innov(0)) / (sq_gate * innov_var(0)), - sq(innov(1)) / (sq_gate * innov_var(1))}; + const Vector2f innov = Vector2f(_state.pos.xy()) - hpos; + const Vector2f innov_var = Vector2f(getStateVariance()) + obs_var; - const bool innov_rejected = (test_ratio.max() > 1.f); + const float sq_gate = sq(5.f); // magic hardcoded gate + const float test_ratio = sq(innov(0)) / (sq_gate * innov_var(0)) + sq(innov(1)) / (sq_gate * innov_var(1)); - if (!_control_status.flags.in_air || (accuracy > 0.f && accuracy < 1.f) || innov_rejected) { - // when on ground or accuracy chosen to be very low, we hard reset position - // this allows the user to still send hard resets at any time - ECL_INFO("reset position to external observation"); - _information_events.flags.reset_pos_to_ext_obs = true; + const bool innov_rejected = (test_ratio > 1.f); - resetHorizontalPositionTo(pos_corrected, obs_var); - _last_known_pos.xy() = _state.pos.xy(); - return true; + if (!_control_status.flags.in_air || (eph > 0.f && eph < 1.f) || innov_rejected) { + // when on ground or accuracy chosen to be very low, we hard reset position + // this allows the user to still send hard resets at any time + ECL_INFO("reset position to external observation"); + _information_events.flags.reset_pos_to_ext_obs = true; - } else { - if (fuseDirectStateMeasurement(innov(0), innov_var(0), obs_var, State::pos.idx + 0) - && fuseDirectStateMeasurement(innov(1), innov_var(1), obs_var, State::pos.idx + 1) - ) { - ECL_INFO("fused external observation as position measurement"); + resetHorizontalPositionTo(hpos, obs_var); + _last_known_pos.xy() = _state.pos.xy(); + + } else { + ECL_INFO("fuse external observation as position measurement"); + fuseDirectStateMeasurement(innov(0), innov_var(0), obs_var, State::pos.idx + 0); + fuseDirectStateMeasurement(innov(1), innov_var(1), obs_var, State::pos.idx + 1); + + // Use the reset counters to inform the controllers about a potential large position jump + // TODO: compute the actual position change _state_reset_status.reset_count.posNE++; + _state_reset_status.posNE_change.zero(); + _time_last_hor_pos_fuse = _time_delayed_us; _last_known_pos.xy() = _state.pos.xy(); - return true; } } - return false; + if (checkAltitudeValidity(altitude)) { + const float altitude_corrected = altitude - pos_correction(2); + + 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); + } + } + + return true; } void Ekf::updateParameters() diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 5f2ba8c95f..5be5cee3bb 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -533,7 +533,7 @@ public: return true; } - bool resetGlobalPosToExternalObservation(double lat_deg, double lon_deg, float accuracy, + bool resetGlobalPosToExternalObservation(double latitude, double longitude, float altitude, float eph, float epv, uint64_t timestamp_observation); /** diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index c339548b8c..7769543d2b 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -545,9 +545,8 @@ void EKF2::Run() accuracy = vehicle_command.param3; } - // TODO add check for lat and long validity - if (_ekf.resetGlobalPosToExternalObservation(vehicle_command.param5, vehicle_command.param6, - accuracy, timestamp_observation) + if (_ekf.resetGlobalPosToExternalObservation(vehicle_command.param5, vehicle_command.param6, vehicle_command.param7, + accuracy, accuracy, timestamp_observation) ) { command_ack.result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED; diff --git a/src/modules/ekf2/test/CMakeLists.txt b/src/modules/ekf2/test/CMakeLists.txt index c1ecb992b9..7e200c4c54 100644 --- a/src/modules/ekf2/test/CMakeLists.txt +++ b/src/modules/ekf2/test/CMakeLists.txt @@ -37,7 +37,7 @@ add_subdirectory(sensor_simulator) add_subdirectory(test_helper) px4_add_unit_gtest(SRC test_EKF_accelerometer.cpp LINKLIBS ecl_EKF ecl_sensor_sim) -px4_add_unit_gtest(SRC test_EKF_airspeed.cpp LINKLIBS ecl_EKF ecl_sensor_sim) +px4_add_unit_gtest(SRC test_EKF_airspeed.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) px4_add_unit_gtest(SRC test_EKF_basics.cpp LINKLIBS ecl_EKF ecl_sensor_sim) px4_add_unit_gtest(SRC test_EKF_externalVision.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) px4_add_unit_gtest(SRC test_EKF_fake_pos.cpp LINKLIBS ecl_EKF ecl_sensor_sim ecl_test_helper) diff --git a/src/modules/ekf2/test/test_EKF_airspeed.cpp b/src/modules/ekf2/test/test_EKF_airspeed.cpp index d1bd54a13b..b9dc3e3672 100644 --- a/src/modules/ekf2/test/test_EKF_airspeed.cpp +++ b/src/modules/ekf2/test/test_EKF_airspeed.cpp @@ -40,6 +40,7 @@ #include "EKF/ekf.h" #include "sensor_simulator/sensor_simulator.h" #include "sensor_simulator/ekf_wrapper.h" +#include "test_helper/reset_logging_checker.h" class EkfAirspeedTest : public ::testing::Test @@ -182,9 +183,11 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoning) 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, 50.f, 0); + _ekf->resetGlobalPosToExternalObservation(latitude_new, longitude_new, altitude_new, eph, epv, 0); // Simulate the fact that the sideslip can start immediately, without // waiting for a measurement sample. @@ -203,4 +206,78 @@ TEST_F(EkfAirspeedTest, testAirspeedDeadReckoning) const Vector2f vel_wind_earth = _ekf->getWindVelocity(); EXPECT_NEAR(vel_wind_earth(0), 0.f, .1f); EXPECT_NEAR(vel_wind_earth(1), 0.f, .1f); + + EXPECT_TRUE(_ekf->global_position_is_valid()); +} + +TEST_F(EkfAirspeedTest, testAirspeedDeadReckoningLatLonAltReset) +{ + // GIVEN: a flying fixed-wing dead-reckoning with airspeed and sideslip fusion + const Vector3f simulated_velocity_earth(-3.6f, 8.f, 0.0f); + const Vector2f airspeed_body(15.f, 0.0f); + _sensor_simulator.runSeconds(10); + + _ekf->set_in_air_status(true); + _ekf->set_vehicle_at_rest(false); + _ekf->set_is_fixed_wing(true); + + double latitude = -15.0000005; + double longitude = -115.0000005; + float altitude = 1500.0; + 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(); + _sensor_simulator.runSeconds(1.f); + EXPECT_TRUE(_ekf_wrapper.isIntendingBetaFusion()); + + _sensor_simulator.startAirspeedSensor(); + _sensor_simulator._airspeed.setData(airspeed_body(0), airspeed_body(0)); + _sensor_simulator.runSeconds(10.f); + EXPECT_TRUE(_ekf_wrapper.isIntendingAirspeedFusion()); + + EXPECT_TRUE(_ekf->global_position_is_valid()); + + // WHEN: an external position reset is sent + ResetLoggingChecker reset_logging_checker(_ekf); + reset_logging_checker.capturePreResetState(); + + double latitude_new = -16.0000005; + double longitude_new = -116.0000005; + float altitude_new = 1602.0; + _ekf->resetGlobalPosToExternalObservation(latitude_new, longitude_new, altitude_new, eph, epv, 0); + + const Vector3f pos_new = _ekf->getPosition(); + const float altitude_est = -pos_new(2) + _ekf->getEkfGlobalOriginAltitude(); + + double latitude_est, longitude_est; + _ekf->global_origin().reproject(pos_new(0), pos_new(1), latitude_est, longitude_est); + + // THEN: the global position is adjusted accordingly + EXPECT_NEAR(altitude_est, altitude_new, 0.01f); + EXPECT_NEAR(latitude_est, latitude_new, 1e-3f); + EXPECT_NEAR(longitude_est, longitude_new, 1e-3f); + EXPECT_TRUE(_ekf->global_position_is_valid()); + + reset_logging_checker.capturePostResetState(); + EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(0)); + EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1)); + EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(0)); + EXPECT_TRUE(reset_logging_checker.isHorizontalPositionResetCounterIncreasedBy(1)); + + // AND WHEN: only the lat/lon is valid + latitude_new = -16.0000005; + longitude_new = -116.0000005; + altitude_new = NAN; + _ekf->resetGlobalPosToExternalObservation(latitude_new, longitude_new, altitude_new, eph, epv, 0); + + // THEN: lat/lon are reset but not the altitude + reset_logging_checker.capturePostResetState(); + EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(0)); + EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1)); + EXPECT_TRUE(reset_logging_checker.isHorizontalVelocityResetCounterIncreasedBy(0)); + EXPECT_TRUE(reset_logging_checker.isHorizontalPositionResetCounterIncreasedBy(2)); }