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

ekf2: EV overhaul yaw and position fusion (#20501)

Browse Source 
- move EV yaw and EV position to new state machines
 - EV yaw and EV pos now configured via EKF2_EV_CTRL (migrated from EKF2_AID_MASK)
 - new EV position offset estimator to enable EV position while GPS position is active (no more EV pos delta fusion)
 - yaw_align now strictly means north (no more rotate external vision aid mask)
 - automatic switching between EV yaw, and yaw align north based on GPS quality
This commit is contained in:
Daniel Agar
2022-12-20 10:23:56 -05:00
committed by GitHub
parent 20342216e2
commit 54a32eb2f7
34 changed files with 1019 additions and 553 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp
+4 -14
View File
@@ -137,12 +137,12 @@ void EkfWrapper::setFlowOffset(const Vector3f &offset)
void EkfWrapper::enableExternalVisionPositionFusion()
{
_ekf_params->fusion_mode |= SensorFusionMask::USE_EXT_VIS_POS;
_ekf_params->ev_ctrl |= static_cast<int32_t>(EvCtrl::HPOS);
}
void EkfWrapper::disableExternalVisionPositionFusion()
{
_ekf_params->fusion_mode &= ~SensorFusionMask::USE_EXT_VIS_POS;
_ekf_params->ev_ctrl &= ~static_cast<int32_t>(EvCtrl::HPOS);
}
bool EkfWrapper::isIntendingExternalVisionPositionFusion() const
@@ -167,12 +167,12 @@ bool EkfWrapper::isIntendingExternalVisionVelocityFusion() const
void EkfWrapper::enableExternalVisionHeadingFusion()
{
_ekf_params->fusion_mode |= SensorFusionMask::USE_EXT_VIS_YAW;
_ekf_params->ev_ctrl |= static_cast<int32_t>(EvCtrl::YAW);
}
void EkfWrapper::disableExternalVisionHeadingFusion()
{
_ekf_params->fusion_mode &= ~SensorFusionMask::USE_EXT_VIS_YAW;
_ekf_params->ev_ctrl &= ~static_cast<int32_t>(EvCtrl::YAW);
}
bool EkfWrapper::isIntendingExternalVisionHeadingFusion() const
@@ -180,16 +180,6 @@ bool EkfWrapper::isIntendingExternalVisionHeadingFusion() const
return _ekf->control_status_flags().ev_yaw;
}
void EkfWrapper::enableExternalVisionAlignment()
{
_ekf_params->fusion_mode |= SensorFusionMask::ROTATE_EXT_VIS;
}
void EkfWrapper::disableExternalVisionAlignment()
{
_ekf_params->fusion_mode &= ~SensorFusionMask::ROTATE_EXT_VIS;
}
bool EkfWrapper::isIntendingMagHeadingFusion() const
{
return _ekf->control_status_flags().mag_hdg;
src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h
-3
View File
@@ -98,9 +98,6 @@ public:
bool isIntendingMag3DFusion() const;
void setMagFuseTypeNone();
void enableExternalVisionAlignment();
void disableExternalVisionAlignment();
bool isWindVelocityEstimated() const;
void enableTerrainRngFusion();
src/modules/ekf2/test/sensor_simulator/vio.cpp
+16 -4
View File
@@ -8,6 +8,7 @@ namespace sensor
Vio::Vio(std::shared_ptr<Ekf> ekf): Sensor(ekf)
{
_vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD;
_vio_data.pos_frame = PositionFrame::LOCAL_FRAME_FRD;
}
Vio::~Vio()
@@ -32,7 +33,7 @@ void Vio::setVelocityVariance(const Vector3f &velVar)
void Vio::setPositionVariance(const Vector3f &posVar)
{
_vio_data.posVar = posVar;
_vio_data.position_var = posVar;
}
void Vio::setAngularVariance(float angVar)
@@ -70,16 +71,27 @@ void Vio::setVelocityFrameToLocalNED()
_vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_NED;
}
void Vio::setPositionFrameToLocalNED()
{
_vio_data.pos_frame = PositionFrame::LOCAL_FRAME_NED;
}
void Vio::setPositionFrameToLocalFRD()
{
_vio_data.pos_frame = PositionFrame::LOCAL_FRAME_FRD;
}
extVisionSample Vio::dataAtRest()
{
extVisionSample vio_data;
vio_data.pos = Vector3f{0.0f, 0.0f, 0.0f};;
vio_data.vel = Vector3f{0.0f, 0.0f, 0.0f};;
vio_data.pos = Vector3f{0.0f, 0.0f, 0.0f};
vio_data.vel = Vector3f{0.0f, 0.0f, 0.0f};
vio_data.quat = Quatf{1.0f, 0.0f, 0.0f, 0.0f};
vio_data.posVar = Vector3f{0.1f, 0.1f, 0.1f};
vio_data.position_var = Vector3f{0.1f, 0.1f, 0.1f};
vio_data.velocity_var = Vector3f{0.1f, 0.1f, 0.1f};
vio_data.orientation_var(2) = 0.05f;
vio_data.vel_frame = VelocityFrame::LOCAL_FRAME_FRD;
vio_data.pos_frame = PositionFrame::LOCAL_FRAME_FRD;
return vio_data;
}
src/modules/ekf2/test/sensor_simulator/vio.h
+3
View File
@@ -63,6 +63,9 @@ public:
void setVelocityFrameToLocalFRD();
void setVelocityFrameToBody();
void setPositionFrameToLocalNED();
void setPositionFrameToLocalFRD();
extVisionSample dataAtRest();
private:
src/modules/ekf2/test/test_EKF_externalVision.cpp
+5 -7
View File
@@ -78,6 +78,7 @@ TEST_F(EkfExternalVisionTest, checkVisionFusionLogic)
{
_sensor_simulator.runSeconds(_tilt_align_time); // Let the tilt align
_ekf_wrapper.enableExternalVisionPositionFusion();
_sensor_simulator._vio.setPositionFrameToLocalNED();
_sensor_simulator.startExternalVision();
_sensor_simulator.runSeconds(2);
@@ -147,7 +148,6 @@ TEST_F(EkfExternalVisionTest, visionVelocityResetWithAlignment)
// WHEN: Vision frame is rotate +90°. The reported heading is -90°
Quatf vision_to_ekf(Eulerf(0.0f, 0.0f, math::radians(-90.0f)));
_sensor_simulator._vio.setOrientation(vision_to_ekf.inversed());
_ekf_wrapper.enableExternalVisionAlignment();
const Vector3f simulated_velocity_in_vision_frame(0.3f, -1.0f, 0.4f);
const Vector3f simulated_velocity_in_ekf_frame =
@@ -179,6 +179,7 @@ TEST_F(EkfExternalVisionTest, visionHorizontalPositionReset)
const Vector3f simulated_position(8.3f, -1.0f, 0.0f);
_sensor_simulator._vio.setPosition(simulated_position);
_sensor_simulator._vio.setPositionFrameToLocalNED();
_ekf_wrapper.enableExternalVisionPositionFusion();
_sensor_simulator.startExternalVision();
_sensor_simulator.runMicroseconds(2e5);
@@ -197,7 +198,6 @@ TEST_F(EkfExternalVisionTest, visionHorizontalPositionResetWithAlignment)
// WHEN: Vision frame is rotate +90°. The reported heading is -90°
Quatf vision_to_ekf(Eulerf(0.0f, 0.0f, math::radians(-90.0f)));
_sensor_simulator._vio.setOrientation(vision_to_ekf.inversed());
_ekf_wrapper.enableExternalVisionAlignment();
const Vector3f simulated_position_in_vision_frame(8.3f, -1.0f, 0.0f);
const Vector3f simulated_position_in_ekf_frame =
@@ -236,7 +236,6 @@ TEST_F(EkfExternalVisionTest, visionAlignment)
// WHEN: Vision frame is rotate +90°. The reported heading is -90°
Quatf externalVisionFrameOffset(Eulerf(0.0f, 0.0f, math::radians(90.0f)));
_sensor_simulator._vio.setOrientation(externalVisionFrameOffset.inversed());
_ekf_wrapper.enableExternalVisionAlignment();
// Simulate high uncertainty on vision x axis which is in this case
// the y EKF frame axis
@@ -254,9 +253,9 @@ TEST_F(EkfExternalVisionTest, visionAlignment)
EXPECT_TRUE(velVar_new(1) > velVar_new(0));
// THEN: the frame offset should be estimated correctly
Quatf estimatedExternalVisionFrameOffset = _ekf->getVisionAlignmentQuaternion();
EXPECT_TRUE(matrix::isEqual(externalVisionFrameOffset.canonical(),
estimatedExternalVisionFrameOffset.canonical()));
// Quatf estimatedExternalVisionFrameOffset = _ekf->getVisionAlignmentQuaternion();
// EXPECT_TRUE(matrix::isEqual(externalVisionFrameOffset.canonical(),
// estimatedExternalVisionFrameOffset.canonical()));
}
TEST_F(EkfExternalVisionTest, velocityFrameBody)
@@ -340,7 +339,6 @@ TEST_F(EkfExternalVisionTest, positionFrameLocal)
// WHEN: using EV yaw fusion and rotate EV is set
Quatf vision_to_ekf(Eulerf(0.0f, 0.0f, 0.f));
_sensor_simulator._vio.setOrientation(vision_to_ekf.inversed());
_ekf_wrapper.enableExternalVisionAlignment(); // ROTATE_EV
_ekf_wrapper.enableExternalVisionHeadingFusion(); // EV_YAW
// AND WHEN: using EV position aiding