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

ekf2: perform GNSS checks at delayed-time horizon

Browse Source 
- never fuse a measurement that is not passing the checks
- cleanup and simplify GNSS vel/pos control logic
This commit is contained in:
bresch
2023-12-18 18:11:20 +01:00
committed by Mathieu Bresciani
parent b0566cd8aa
commit a653073d4f
15 changed files with 371 additions and 419 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/test/sensor_simulator/gps.cpp
+20 -22
View File
@@ -15,9 +15,9 @@ Gps::~Gps()
void Gps::send(const uint64_t time)
{
const float dt = static_cast<float>(time - _gps_data.time_usec) * 1e-6f;
const float dt = static_cast<float>(time - _gps_data.time_us) * 1e-6f;
_gps_data.time_usec = time;
_gps_data.time_us = time;
if (fabsf(_gps_pos_rate(0)) > FLT_EPSILON || fabsf(_gps_pos_rate(1)) > FLT_EPSILON) {
stepHorizontalPositionByMeters(Vector2f(_gps_pos_rate) * dt);
@@ -30,29 +30,29 @@ void Gps::send(const uint64_t time)
_ekf->setGpsData(_gps_data);
}
void Gps::setData(const gpsMessage &gps)
void Gps::setData(const gnssSample &gps)
{
_gps_data = gps;
}
void Gps::setAltitude(const int32_t alt)
void Gps::setAltitude(const float alt)
{
_gps_data.alt = alt;
}
void Gps::setLatitude(const int32_t lat)
void Gps::setLatitude(const double lat)
{
_gps_data.lat = lat;
}
void Gps::setLongitude(const int32_t lon)
void Gps::setLongitude(const double lon)
{
_gps_data.lon = lon;
}
void Gps::setVelocity(const Vector3f &vel)
{
_gps_data.vel_ned = vel;
_gps_data.vel = vel;
}
void Gps::setYaw(const float yaw)
@@ -87,7 +87,7 @@ void Gps::setPositionRateNED(const Vector3f &rate)
void Gps::stepHeightByMeters(const float hgt_change)
{
_gps_data.alt += hgt_change * 1e3f;
_gps_data.alt += hgt_change;
}
void Gps::stepHorizontalPositionByMeters(const Vector2f hpos_change)
@@ -98,32 +98,30 @@ void Gps::stepHorizontalPositionByMeters(const Vector2f hpos_change)
double lat_new {0.0};
double lon_new {0.0};
_ekf->global_origin().project(_gps_data.lat * 1e-7, _gps_data.lon * 1e-7, hposN_curr, hposE_curr);
_ekf->global_origin().project(_gps_data.lat, _gps_data.lon, hposN_curr, hposE_curr);
Vector2f hpos_new = Vector2f{hposN_curr, hposE_curr} + hpos_change;
_ekf->global_origin().reproject(hpos_new(0), hpos_new(1), lat_new, lon_new);
_gps_data.lon = static_cast<int32_t>(lon_new * 1e7);
_gps_data.lat = static_cast<int32_t>(lat_new * 1e7);
_gps_data.lon = lon_new;
_gps_data.lat = lat_new;
}
gpsMessage Gps::getDefaultGpsData()
gnssSample Gps::getDefaultGpsData()
{
gpsMessage gps_data{};
gps_data.time_usec = 0;
gps_data.lat = 473566094;
gps_data.lon = 85190237;
gps_data.alt = 422056;
gnssSample gps_data{};
gps_data.time_us = 0;
gps_data.lat = 47.3566094;
gps_data.lon = 8.5190237;
gps_data.alt = 422.056f;
gps_data.yaw = NAN;
gps_data.yaw_offset = 0.0f;
gps_data.fix_type = 3;
gps_data.eph = 0.5f;
gps_data.epv = 0.8f;
gps_data.hacc = 0.5f;
gps_data.vacc = 0.8f;
gps_data.sacc = 0.2f;
gps_data.vel_m_s = 0.0;
gps_data.vel_ned.setZero();
gps_data.vel_ned_valid = 1;
gps_data.vel.setZero();
gps_data.nsats = 16;
gps_data.pdop = 0.0f;
src/modules/ekf2/test/sensor_simulator/gps.h
+6 -6
View File
@@ -51,13 +51,13 @@ public:
Gps(std::shared_ptr<Ekf> ekf);
~Gps();
void setData(const gpsMessage &gps);
void setData(const gnssSample &gps);
void stepHeightByMeters(const float hgt_change);
void stepHorizontalPositionByMeters(const Vector2f hpos_change);
void setPositionRateNED(const Vector3f &rate);
void setAltitude(const int32_t alt);
void setLatitude(const int32_t lat);
void setLongitude(const int32_t lon);
void setAltitude(const float alt);
void setLatitude(const double lat);
void setLongitude(const double lon);
void setVelocity(const Vector3f &vel);
void setYaw(const float yaw);
void setYawOffset(const float yaw);
@@ -65,12 +65,12 @@ public:
void setNumberOfSatellites(const int num_satellites);
void setPdop(const float pdop);
gpsMessage getDefaultGpsData();
gnssSample getDefaultGpsData();
private:
void send(uint64_t time) override;
gpsMessage _gps_data{};
gnssSample _gps_data{};
Vector3f _gps_pos_rate{};
};
src/modules/ekf2/test/sensor_simulator/sensor_simulator.cpp
+3 -6
View File
@@ -395,20 +395,17 @@ void SensorSimulator::setSensorDataFromTrajectory()
void SensorSimulator::setGpsLatitude(const double latitude)
{
int32_t lat = static_cast<int32_t>(latitude * 1e7);
_gps.setLatitude(lat);
_gps.setLatitude(latitude);
}
void SensorSimulator::setGpsLongitude(const double longitude)
{
int32_t lon = static_cast<int32_t>(longitude * 1e7);
_gps.setLongitude(lon);
_gps.setLongitude(longitude);
}
void SensorSimulator::setGpsAltitude(const float altitude)
{
int32_t alt = static_cast<int32_t>(altitude * 1e3f);
_gps.setAltitude(alt);
_gps.setAltitude(altitude);
}
void SensorSimulator::setImuBias(Vector3f accel_bias, Vector3f gyro_bias)
src/modules/ekf2/test/test_EKF_fusionLogic.cpp
+4 -4
View File
@@ -195,15 +195,15 @@ TEST_F(EkfFusionLogicTest, fallbackFromGpsToFlow)
// WHEN: GPS data stops
_sensor_simulator.stopGps();
_sensor_simulator.runSeconds(2);
_sensor_simulator.runSeconds(10);
// THEN: immediately switch to flow only
// THEN: GNSS fusion stops after a timing out
EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion());
EXPECT_TRUE(_ekf_wrapper.isIntendingFlowFusion());
// BUT WHEN: GPS starts again
// BUT WHEN: GPS starts after passing the checks again
_sensor_simulator.startGps();
_sensor_simulator.runSeconds(1);
_sensor_simulator.runSeconds(6);
// THEN: use it again
EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
src/modules/ekf2/test/test_EKF_gps.cpp
+11 -22
View File
@@ -84,28 +84,16 @@ TEST_F(EkfGpsTest, gpsTimeout)
// WHEN: the number of satellites drops below the minimum
_sensor_simulator._gps.setNumberOfSatellites(3);
// THEN: the GNSS fusion continues because it is the only source of position/velocity
_sensor_simulator.runSeconds(20);
EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
// BUT: if we have another velocity aiding source
const float max_flow_rate = 5.f;
const float min_ground_distance = 0.f;
const float max_ground_distance = 50.f;
_ekf->set_optical_flow_limits(max_flow_rate, min_ground_distance, max_ground_distance);
_sensor_simulator._flow.setData(_sensor_simulator._flow.dataAtRest());
_ekf_wrapper.enableFlowFusion();
_sensor_simulator.startFlow();
_sensor_simulator._rng.setData(0.2, 100);
_sensor_simulator._rng.setLimits(0.1f, 9.f);
_sensor_simulator.startRangeFinder();
_sensor_simulator.runSeconds(5);
// THEN: the GNSS fusion stops
EXPECT_TRUE(_ekf_wrapper.isIntendingFlowFusion());
// THEN: the GNSS fusion stops after some time
_sensor_simulator.runSeconds(8);
EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion());
// BUT WHEN: the number of satellites is good again
_sensor_simulator._gps.setNumberOfSatellites(16);
// THEN: the GNSS fusion restarts
_sensor_simulator.runSeconds(6);
EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
}
TEST_F(EkfGpsTest, gpsFixLoss)
@@ -148,6 +136,7 @@ TEST_F(EkfGpsTest, resetToGpsVelocity)
_ekf->set_in_air_status(true);
_ekf->set_vehicle_at_rest(false);
_sensor_simulator.runSeconds(5.2); // required to pass the checks
_sensor_simulator.runMicroseconds(dt_us);
// THEN: a reset to GPS velocity should be done
@@ -178,7 +167,7 @@ TEST_F(EkfGpsTest, resetToGpsPosition)
const Vector3f simulated_position_change(2.0f, -1.0f, 0.f);
_sensor_simulator._gps.stepHorizontalPositionByMeters(
Vector2f(simulated_position_change));
_sensor_simulator.runMicroseconds(1e5);
_sensor_simulator.runSeconds(6);
// THEN: a reset to the new GPS position should be done
const Vector3f estimated_position = _ekf->getPosition();
src/modules/ekf2/test/test_EKF_terrain_estimator.cpp
+1 -2
View File
@@ -81,8 +81,7 @@ public:
_ekf->set_vehicle_at_rest(true);
_ekf_wrapper.enableGpsFusion();
_sensor_simulator.runSeconds(2); // Run to pass the GPS checks
_sensor_simulator.runSeconds(3.5); // And a bit more to start the GPS fusion TODO: this shouldn't be necessary
_sensor_simulator.runSeconds(1.5); // Run to pass the GPS checks
EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
const Vector3f simulated_velocity(0.5f, -1.0f, 0.f);