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

ekf2: enable multiple height sources fusion

Browse Source 
Instead of having a single height source fused into the EKF and the
other ones "waiting" for a failure or the primary sensor, fuse all
sources in EKF2 at the same time. To prevent the sources from fighting against each
other, the "primary" source is set as reference and the other ones are
running a bias estimator in order to make all the secondary height
sources converge to the primary one.

If the reference isn't available, another one is automatically selected
from a priority list. This secondary reference keeps its current bias
estimate but stops updating it in order to be the new reference as close
as possible to the primary one.
This commit is contained in:
bresch
2022-08-24 09:16:11 -04:00
committed by Daniel Agar
parent f5f31006a0
commit 8fd79688c0
17 changed files with 573 additions and 456 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp
+48 -8
View File
@@ -10,9 +10,19 @@ EkfWrapper::~EkfWrapper()
{
}
void EkfWrapper::setBaroHeight()
void EkfWrapper::setBaroHeightRef()
{
_ekf_params->vdist_sensor_type = VerticalHeightSensor::BARO;
_ekf_params->height_sensor_ref |= HeightSensorRef::BARO;
}
void EkfWrapper::enableBaroHeightFusion()
{
_ekf_params->fusion_mode |= SensorFusionMask::USE_BARO_HGT;
}
void EkfWrapper::disableBaroHeightFusion()
{
_ekf_params->fusion_mode &= ~SensorFusionMask::USE_BARO_HGT;
}
bool EkfWrapper::isIntendingBaroHeightFusion() const
@@ -20,9 +30,19 @@ bool EkfWrapper::isIntendingBaroHeightFusion() const
return _ekf->control_status_flags().baro_hgt;
}
void EkfWrapper::setGpsHeight()
void EkfWrapper::setGpsHeightRef()
{
_ekf_params->vdist_sensor_type = VerticalHeightSensor::GPS;
_ekf_params->height_sensor_ref |= HeightSensorRef::GPS;
}
void EkfWrapper::enableGpsHeightFusion()
{
_ekf_params->fusion_mode |= SensorFusionMask::USE_GPS_HGT;
}
void EkfWrapper::disableGpsHeightFusion()
{
_ekf_params->fusion_mode &= ~SensorFusionMask::USE_GPS_HGT;
}
bool EkfWrapper::isIntendingGpsHeightFusion() const
@@ -30,9 +50,19 @@ bool EkfWrapper::isIntendingGpsHeightFusion() const
return _ekf->control_status_flags().gps_hgt;
}
void EkfWrapper::setRangeHeight()
void EkfWrapper::setRangeHeightRef()
{
_ekf_params->vdist_sensor_type = VerticalHeightSensor::RANGE;
_ekf_params->height_sensor_ref |= HeightSensorRef::RANGE;
}
void EkfWrapper::enableRangeHeightFusion()
{
_ekf_params->fusion_mode |= SensorFusionMask::USE_RNG_HGT;
}
void EkfWrapper::disableRangeHeightFusion()
{
_ekf_params->fusion_mode &= ~SensorFusionMask::USE_RNG_HGT;
}
bool EkfWrapper::isIntendingRangeHeightFusion() const
@@ -40,9 +70,19 @@ bool EkfWrapper::isIntendingRangeHeightFusion() const
return _ekf->control_status_flags().rng_hgt;
}
void EkfWrapper::setVisionHeight()
void EkfWrapper::setVisionHeightRef()
{
_ekf_params->vdist_sensor_type = VerticalHeightSensor::EV;
_ekf_params->height_sensor_ref |= HeightSensorRef::EV;
}
void EkfWrapper::enableVisionHeightFusion()
{
_ekf_params->fusion_mode |= SensorFusionMask::USE_EXT_VIS_HGT;
}
void EkfWrapper::disableVisionHeightFusion()
{
_ekf_params->fusion_mode &= ~SensorFusionMask::USE_EXT_VIS_HGT;
}
bool EkfWrapper::isIntendingVisionHeightFusion() const
src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h
+12 -4
View File
@@ -49,16 +49,24 @@ public:
~EkfWrapper();
void setBaroHeight();
void setBaroHeightRef();
void enableBaroHeightFusion();
void disableBaroHeightFusion();
bool isIntendingBaroHeightFusion() const;
void setGpsHeight();
void setGpsHeightRef();
void enableGpsHeightFusion();
void disableGpsHeightFusion();
bool isIntendingGpsHeightFusion() const;
void setRangeHeight();
void setRangeHeightRef();
void enableRangeHeightFusion();
void disableRangeHeightFusion();
bool isIntendingRangeHeightFusion() const;
void setVisionHeight();
void setVisionHeightRef();
void enableVisionHeightFusion();
void disableVisionHeightFusion();
bool isIntendingVisionHeightFusion() const;
void enableGpsFusion();
src/modules/ekf2/test/test_EKF_fusionLogic.cpp
+9 -6
View File
@@ -358,6 +358,7 @@ TEST_F(EkfFusionLogicTest, doBaroHeightFusion)
// GIVEN: EKF that receives baro and GPS data
_sensor_simulator.startGps();
_sensor_simulator.runSeconds(11);
_ekf_wrapper.enableGpsHeightFusion();
// THEN: EKF should intend to fuse baro by default
EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
@@ -396,26 +397,28 @@ TEST_F(EkfFusionLogicTest, doBaroHeightFusionTimeout)
// BUT WHEN: GPS height data is also available
_sensor_simulator.startGps();
_ekf_wrapper.enableGpsHeightFusion();
_sensor_simulator.runSeconds(11);
reset_logging_checker.capturePostResetState();
EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(2));
EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1));
// AND: the baro data jumps by a lot
_sensor_simulator._baro.setData(800.f);
_sensor_simulator.runSeconds(20);
reset_logging_checker.capturePostResetState();
// THEN: EKF should fallback to GPS height
// THEN: EKF should fallback to GPS height (without reset)
EXPECT_FALSE(_ekf_wrapper.isIntendingBaroHeightFusion());
EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion());
EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(3));
EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(2));
EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(2));
EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1));
}
TEST_F(EkfFusionLogicTest, doGpsHeightFusion)
{
// WHEN: commanding GPS height and sending GPS data
_ekf_wrapper.setGpsHeight();
_ekf_wrapper.enableGpsHeightFusion();
_sensor_simulator.startGps();
_sensor_simulator.runSeconds(11);
@@ -434,7 +437,7 @@ TEST_F(EkfFusionLogicTest, doGpsHeightFusion)
TEST_F(EkfFusionLogicTest, doRangeHeightFusion)
{
// WHEN: commanding range height and sending range data
_ekf_wrapper.setRangeHeight();
_ekf_wrapper.enableRangeHeightFusion();
_sensor_simulator.startRangeFinder();
_sensor_simulator.runSeconds(2.5f);
// THEN: EKF should intend to fuse range height
@@ -462,7 +465,7 @@ TEST_F(EkfFusionLogicTest, doRangeHeightFusion)
TEST_F(EkfFusionLogicTest, doVisionHeightFusion)
{
// WHEN: commanding vision height and sending vision data
_ekf_wrapper.setVisionHeight();
_ekf_wrapper.enableVisionHeightFusion();
_sensor_simulator.startExternalVision();
_sensor_simulator.runSeconds(2);
src/modules/ekf2/test/test_EKF_gps.cpp
+2 -2
View File
@@ -157,12 +157,12 @@ TEST_F(EkfGpsTest, gpsHgtToBaroFallback)
_ekf_wrapper.enableFlowFusion();
_sensor_simulator.startFlow();
_ekf_wrapper.setGpsHeight();
_ekf_wrapper.enableGpsHeightFusion();
_sensor_simulator.runSeconds(1);
EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion());
EXPECT_TRUE(_ekf_wrapper.isIntendingFlowFusion());
EXPECT_FALSE(_ekf_wrapper.isIntendingBaroHeightFusion());
EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion());
// WHEN: stopping GPS fusion
_sensor_simulator.stopGps();
src/modules/ekf2/test/test_EKF_terrain_estimator.cpp
-1
View File
@@ -80,7 +80,6 @@ public:
_ekf->set_vehicle_at_rest(true);
_ekf_wrapper.enableGpsFusion();
_ekf_wrapper.setBaroHeight();
_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
EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());