fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 01:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: store position state as lat/lon/alt

Browse Source 
The position error state is still defined in a body-fixed NED frame but the
position state itself is latitude-longitude-altitude.
This commit is contained in:
bresch
2024-11-22 15:13:47 +01:00
committed by Mathieu Bresciani
parent cf34b9d574
commit b19a6ee3b5
33 changed files with 783 additions and 348 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/test/test_EKF_airspeed.cpp
+17 -2
View File
@@ -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();
src/modules/ekf2/test/test_EKF_basics.cpp
+10 -5
View File
@@ -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());
src/modules/ekf2/test/test_EKF_flow.cpp
+46
View File
@@ -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);
}
src/modules/ekf2/test/test_EKF_gps.cpp
+1 -1
View File
@@ -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);
src/modules/ekf2/test/test_EKF_height_fusion.cpp
+14 -5
View File
@@ -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));
}
src/modules/ekf2/test/test_EKF_yaw_estimator.cpp
+6 -7
View File
@@ -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());