Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 13:02:25 +08:00
Code Issues Actions 3 Packages Projects Releases Wiki Activity

ekf2: handle external altitude resets

Browse Source
This commit is contained in:
bresch
2024-08-23 15:33:21 +02:00
committed by Mathieu Bresciani
parent cd2170deea
commit bab256bfe6
5 changed files with 148 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf.cpp
+66 -33
View File
@@ -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<float>(_time_delayed_us - timestamp_observation);
dt_us = static_cast<float>(_time_delayed_us - timestamp_observation);
} else {
diff_us = -static_cast<float>(timestamp_observation - _time_delayed_us);
dt_us = -static_cast<float>(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<State::pos>()) + 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<State::pos>()) + 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()
src/modules/ekf2/EKF/ekf.h
+1 -1
View File
@@ -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);
/**
src/modules/ekf2/EKF2.cpp
+2 -3
View File
@@ -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;
src/modules/ekf2/test/CMakeLists.txt
+1 -1
View File
@@ -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)
src/modules/ekf2/test/test_EKF_airspeed.cpp
+78 -1
View File
@@ -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));
}