Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 16:56:25 +08:00
Code Issues Actions 4 Packages Projects Releases Wiki Activity

ekf2: EV velocity control should own vertical velocity reset if height failing

Browse Source
This commit is contained in:
Daniel Agar
2024-07-10 13:20:16 -04:00
parent 177613eb68
commit 40349fa3dc
2 changed files with 11 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/external_vision/ev_height_control.cpp
-28
View File
@@ -149,34 +149,6 @@ void Ekf::controlEvHeightFusion(const imuSample &imu_sample, const extVisionSamp
resetVerticalPositionTo(measurement - bias_est.getBias(), measurement_var);
bias_est.setBias(-_state.pos(2) + measurement);
// reset vertical velocity
if (ev_sample.vel.isAllFinite() && (_params.ev_ctrl & static_cast<int32_t>(EvCtrl::VEL))) {
// 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 vel_offset_body = angular_velocity % pos_offset_body;
const Vector3f vel_offset_earth = _R_to_earth * vel_offset_body;
switch (ev_sample.vel_frame) {
case VelocityFrame::LOCAL_FRAME_NED:
case VelocityFrame::LOCAL_FRAME_FRD: {
const Vector3f reset_vel = ev_sample.vel - vel_offset_earth;
resetVerticalVelocityTo(reset_vel(2), math::max(ev_sample.velocity_var(2), sq(_params.ev_vel_noise)));
}
break;
case VelocityFrame::BODY_FRAME_FRD: {
const Vector3f reset_vel = _R_to_earth * (ev_sample.vel - vel_offset_body);
const Matrix3f reset_vel_cov = _R_to_earth * matrix::diag(ev_sample.velocity_var) * _R_to_earth.transpose();
resetVerticalVelocityTo(reset_vel(2), math::max(reset_vel_cov(2, 2), sq(_params.ev_vel_noise)));
}
break;
}
} else {
resetVerticalVelocityToZero();
}
aid_src.time_last_fuse = _time_delayed_us;
} else if (is_fusion_failing) {
src/modules/ekf2/EKF/aid_sources/external_vision/ev_vel_control.cpp
+11
View File
@@ -212,6 +212,17 @@ void Ekf::controlEvVelFusion(const imuSample &imu_sample, const extVisionSample
stopEvVelFusion();
}
} else if (isHeightResetRequired()) {
// reset vertical velocity if height is failing
if (ev_sample.vel_frame == VelocityFrame::BODY_FRAME_FRD) {
const Vector3f measurement_ekf_frame = _R_to_earth * measurement;
const Vector3f measurement_var_ekf_frame = rotateVarianceToEkf(measurement_var);
resetVerticalVelocityTo(measurement_ekf_frame(2), measurement_var_ekf_frame(2));
} else {
resetVerticalVelocityTo(measurement(2), measurement_var(2));
}
}
} else {