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

EKF2: EV vel/pos only use EV q if enabled and valid

Browse Source
This commit is contained in:
Daniel Agar
2024-05-14 12:23:04 -04:00
parent fdfe43471e
commit b42fa1dee0
12 changed files with 68 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/external_vision/ev_control.cpp
+2 -1
View File
@@ -92,7 +92,8 @@ void Ekf::updateEvAttitudeErrorFilter(extVisionSample &ev_sample, bool ev_reset)
{
const Quatf q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized());
if (!q_error.isAllFinite()) {
if (!ev_sample.quat.isAllFinite() || !q_error.isAllFinite()) {
_ev_q_error_initialized = false;
return;
}
src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp
+5 -3
View File
@@ -59,10 +59,12 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
// rotate EV to the EKF reference frame unless we're operating entirely in vision frame
if (!(_control_status.flags.ev_yaw && _control_status.flags.ev_pos)) {
const Quatf q_error(_ev_q_error_filt.getState());
const bool ev_q_available = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::ORIENTATION))
&& ev_sample.quat.isAllFinite();
if (q_error.isAllFinite()) {
const Dcmf R_ev_to_ekf(q_error);
if (ev_q_available && _ev_q_error_filt.getState().isAllFinite()) {
// rotate EV to the EKF reference frame
const Dcmf R_ev_to_ekf(_ev_q_error_filt.getState());
pos = R_ev_to_ekf * ev_sample.pos;
pos_cov = R_ev_to_ekf * matrix::diag(ev_sample.position_var) * R_ev_to_ekf.transpose();
src/modules/ekf2/EKF/aid_sources/external_vision/ev_pos_control.cpp
+20 -13
View File
@@ -45,15 +45,19 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
const bool common_starting_conditions_passing, const bool ev_reset, const bool quality_sufficient,
estimator_aid_source2d_s &aid_src)
{
const bool yaw_alignment_changed = (!_control_status_prev.flags.ev_yaw && _control_status.flags.ev_yaw)
|| (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
// determine if we should use EV position aiding
bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::HPOS))
&& _control_status.flags.tilt_align
&& PX4_ISFINITE(ev_sample.pos(0))
&& PX4_ISFINITE(ev_sample.pos(1));
const bool ev_q_available = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::ORIENTATION))
&& ev_sample.quat.isAllFinite();
const bool yaw_alignment_changed = (!_control_status_prev.flags.ev_yaw && _control_status.flags.ev_yaw)
|| (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
// correct position for offset relative to IMU
const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body;
@@ -86,17 +90,9 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
break;
case PositionFrame::LOCAL_FRAME_FRD:
if (_control_status.flags.ev_yaw) {
// using EV frame
pos = ev_sample.pos - pos_offset_earth;
pos_cov = matrix::diag(ev_sample.position_var);
_ev_pos_b_est.setFusionInactive();
_ev_pos_b_est.reset();
} else {
if (ev_q_available && !_control_status.flags.ev_yaw) {
// rotate EV to the EKF reference frame
const Dcmf R_ev_to_ekf = Dcmf(_ev_q_error_filt.getState());
const Dcmf R_ev_to_ekf(_ev_q_error_filt.getState());
pos = R_ev_to_ekf * ev_sample.pos - pos_offset_earth;
pos_cov = R_ev_to_ekf * matrix::diag(ev_sample.position_var) * R_ev_to_ekf.transpose();
@@ -115,6 +111,17 @@ void Ekf::controlEvPosFusion(const imuSample &imu_sample, const extVisionSample
} else {
_ev_pos_b_est.setFusionInactive();
}
} else if (!_control_status.flags.yaw_align) {
// using EV frame
pos = ev_sample.pos - pos_offset_earth;
pos_cov = matrix::diag(ev_sample.position_var);
_ev_pos_b_est.setFusionInactive();
_ev_pos_b_est.reset();
} else {
continuing_conditions_passing = false;
}
break;
src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp
+17 -10
View File
@@ -47,14 +47,18 @@ void Ekf::controlEvVelFusion(const imuSample &imu_sample, const extVisionSample
{
static constexpr const char *AID_SRC_NAME = "EV velocity";
const bool yaw_alignment_changed = (!_control_status_prev.flags.ev_yaw && _control_status.flags.ev_yaw)
|| (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
// determine if we should use EV velocity aiding
bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))
&& _control_status.flags.tilt_align
&& ev_sample.vel.isAllFinite();
const bool ev_q_available = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::ORIENTATION))
&& ev_sample.quat.isAllFinite();
const bool yaw_alignment_changed = (!_control_status_prev.flags.ev_yaw && _control_status.flags.ev_yaw)
|| (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align);
// correct velocity for offset relative to IMU
const Vector3f angular_velocity = imu_sample.delta_ang / imu_sample.delta_ang_dt - _state.gyro_bias;
const Vector3f pos_offset_body = _params.ev_pos_body - _params.imu_pos_body;
@@ -80,19 +84,22 @@ void Ekf::controlEvVelFusion(const imuSample &imu_sample, const extVisionSample
break;
case VelocityFrame::LOCAL_FRAME_FRD:
if (_control_status.flags.ev_yaw) {
// using EV frame
measurement = ev_sample.vel - vel_offset_earth;
measurement_var = ev_sample.velocity_var;
} else {
if (ev_q_available && !_control_status.flags.ev_yaw) {
// rotate EV to the EKF reference frame
const Dcmf R_ev_to_ekf = Dcmf(_ev_q_error_filt.getState());
const Dcmf R_ev_to_ekf(_ev_q_error_filt.getState());
measurement = R_ev_to_ekf * ev_sample.vel - vel_offset_earth;
measurement_var = matrix::SquareMatrix3f(R_ev_to_ekf * matrix::diag(ev_sample.velocity_var) *
R_ev_to_ekf.transpose()).diag();
minimum_variance = math::max(minimum_variance, ev_sample.orientation_var.max());
} else if (!_control_status.flags.yaw_align) {
// using EV frame
measurement = ev_sample.vel - vel_offset_earth;
measurement_var = ev_sample.velocity_var;
} else {
continuing_conditions_passing = false;
}
break;
src/modules/ekf2/EKF/aid_sources/external_vision/ev_yaw_control.cpp
+1 -1
View File
@@ -66,7 +66,7 @@ void Ekf::controlEvYawFusion(const imuSample &imu_sample, const extVisionSample
}
// determine if we should use EV yaw aiding
bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::YAW))
bool continuing_conditions_passing = (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::ORIENTATION))
&& _control_status.flags.tilt_align
&& !_control_status.flags.ev_yaw_fault
&& PX4_ISFINITE(aid_src.observation)
src/modules/ekf2/EKF/common.h
+4 -4
View File
@@ -155,10 +155,10 @@ enum class RngCtrl : uint8_t {
};
enum class EvCtrl : uint8_t {
HPOS = (1 << 0),
VPOS = (1 << 1),
VEL = (1 << 2),
YAW = (1 << 3)
HPOS = (1 << 0),
VPOS = (1 << 1),
VEL = (1 << 2),
ORIENTATION = (1 << 3)
};
enum class MagCheckMask : uint8_t {
src/modules/ekf2/EKF2.cpp
+1 -1
View File
@@ -300,7 +300,7 @@ void EKF2::AdvertiseTopics()
_estimator_aid_src_ev_vel_pub.advertise();
}
if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::YAW)) {
if (_param_ekf2_ev_ctrl.get() & static_cast<int32_t>(EvCtrl::ORIENTATION)) {
_estimator_aid_src_ev_yaw_pub.advertise();
}
src/modules/ekf2/module.yaml
+2 -2
View File
@@ -508,13 +508,13 @@ parameters:
description:
short: External vision (EV) sensor aiding
long: 'Set bits in the following positions to enable: 0 : Horizontal position
fusion 1 : Vertical position fusion 2 : 3D velocity fusion 3 : Yaw'
fusion 1 : Vertical position fusion 2 : 3D velocity fusion 3 : Orientation'
type: bitmask
bit:
0: Horizontal position
1: Vertical position
2: 3D velocity
3: Yaw
3: Orientation
default: 0
min: 0
max: 15
src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp
+2 -2
View File
@@ -187,12 +187,12 @@ bool EkfWrapper::isIntendingExternalVisionVelocityFusion() const
void EkfWrapper::enableExternalVisionHeadingFusion()
{
_ekf_params->ev_ctrl |= static_cast<int32_t>(EvCtrl::YAW);
_ekf_params->ev_ctrl |= static_cast<int32_t>(EvCtrl::ORIENTATION);
}
void EkfWrapper::disableExternalVisionHeadingFusion()
{
_ekf_params->ev_ctrl &= ~static_cast<int32_t>(EvCtrl::YAW);
_ekf_params->ev_ctrl &= ~static_cast<int32_t>(EvCtrl::ORIENTATION);
}
bool EkfWrapper::isIntendingExternalVisionHeadingFusion() const
src/modules/ekf2/test/test_EKF_accelerometer.cpp
+2
View File
@@ -207,6 +207,7 @@ TEST_F(EkfAccelerometerTest, imuFallingDetectionBaroEvVel)
{
// GIVEN: baro and EV vel fusion
_ekf_wrapper.enableExternalVisionVelocityFusion();
_ekf_wrapper.enableExternalVisionHeadingFusion();
_sensor_simulator.startExternalVision();
_sensor_simulator.runSeconds(1);
@@ -227,6 +228,7 @@ TEST_F(EkfAccelerometerTest, imuFallingDetectionEvVelHgt)
{
// GIVEN: EV height and vel fusion
_ekf_wrapper.enableExternalVisionVelocityFusion();
_ekf_wrapper.enableExternalVisionHeadingFusion();
_ekf_wrapper.enableExternalVisionHeightFusion();
_sensor_simulator.startExternalVision();
_ekf_wrapper.disableBaroHeightFusion();
src/modules/ekf2/test/test_EKF_externalVision.cpp
+10 -1
View File
@@ -89,6 +89,7 @@ TEST_F(EkfExternalVisionTest, checkVisionFusionLogic)
EXPECT_FALSE(_ekf->global_position_is_valid());
_ekf_wrapper.enableExternalVisionVelocityFusion();
_sensor_simulator._vio.setVelocityFrameToLocalNED();
_sensor_simulator.runSeconds(2);
EXPECT_TRUE(_ekf_wrapper.isIntendingExternalVisionPositionFusion());
@@ -119,6 +120,7 @@ TEST_F(EkfExternalVisionTest, visionVelocityReset)
_sensor_simulator._vio.setVelocity(simulated_velocity);
_ekf_wrapper.enableExternalVisionVelocityFusion();
_sensor_simulator._vio.setVelocityFrameToLocalNED();
_sensor_simulator.startExternalVision();
// Note: test duration needs to allow time for tilt alignment to complete
_ekf->set_vehicle_at_rest(false);
@@ -152,8 +154,11 @@ TEST_F(EkfExternalVisionTest, visionVelocityResetWithAlignment)
const Vector3f simulated_velocity_in_ekf_frame =
Dcmf(vision_to_ekf) * simulated_velocity_in_vision_frame;
_sensor_simulator._vio.setVelocity(simulated_velocity_in_vision_frame);
_sensor_simulator._vio.setVelocityFrameToLocalFRD();
_ekf_wrapper.enableExternalVisionVelocityFusion();
_ekf_wrapper.enableGpsFusion();
_sensor_simulator.startExternalVision();
_sensor_simulator.startGps();
_ekf->set_vehicle_at_rest(false);
_sensor_simulator.runMicroseconds(2e5);
@@ -203,6 +208,7 @@ TEST_F(EkfExternalVisionTest, visionHorizontalPositionResetWithAlignment)
Dcmf(vision_to_ekf) * simulated_position_in_vision_frame;
_sensor_simulator._vio.setPosition(simulated_position_in_vision_frame);
_ekf_wrapper.enableExternalVisionPositionFusion();
_ekf_wrapper.setMagFuseTypeNone();
_sensor_simulator.startExternalVision();
_sensor_simulator.runMicroseconds(2e5);
@@ -240,12 +246,15 @@ TEST_F(EkfExternalVisionTest, visionAlignment)
// the y EKF frame axis
_sensor_simulator._vio.setVelocityVariance(Vector3f{2.0f, 0.01f, 0.01f});
_ekf_wrapper.enableExternalVisionVelocityFusion();
_ekf_wrapper.enableExternalVisionHeadingFusion();
_ekf_wrapper.enableGpsFusion();
_sensor_simulator.startExternalVision();
_sensor_simulator.startGps();
const Vector3f velVar_init = _ekf->getVelocityVariance();
EXPECT_NEAR(velVar_init(0), velVar_init(1), 0.0001);
_sensor_simulator.runSeconds(4);
_sensor_simulator.runSeconds(20);
// THEN: velocity uncertainty in y should be bigger
const Vector3f velVar_new = _ekf->getVelocityVariance();
src/modules/ekf2/test/test_EKF_fusionLogic.cpp
+2
View File
@@ -271,6 +271,7 @@ TEST_F(EkfFusionLogicTest, doVisionPositionFusion)
{
// WHEN: allow vision position to be fused and we send vision data
_ekf_wrapper.enableExternalVisionPositionFusion();
_ekf_wrapper.setMagFuseTypeNone();
_sensor_simulator.startExternalVision();
_sensor_simulator.runSeconds(4);
@@ -299,6 +300,7 @@ TEST_F(EkfFusionLogicTest, doVisionVelocityFusion)
{
// WHEN: allow vision position to be fused and we send vision data
_ekf_wrapper.enableExternalVisionVelocityFusion();
_ekf_wrapper.setMagFuseTypeNone();
_sensor_simulator.startExternalVision();
_sensor_simulator.runSeconds(4);