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

EKF2: vision attitude error filter (#21791)

Browse Source 
* ekf2-ev: filter q_error for frame correction
* ekf2: filter EV attitude error centrally
This commit is contained in:
Mathieu Bresciani
2023-07-03 11:13:59 -04:00
committed by GitHub
parent 24665f10f2
commit 288e3ae74a
5 changed files with 24 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ev_control.cpp
+18
View File
@@ -58,6 +58,7 @@ void Ekf::controlExternalVisionFusion()
&& ((_params.ev_quality_minimum <= 0) || (_ext_vision_buffer->get_newest().quality >= _params.ev_quality_minimum)) // newest quality sufficient
&& isNewestSampleRecent(_time_last_ext_vision_buffer_push, EV_MAX_INTERVAL);
updateEvAttitudeErrorFilter(ev_sample, ev_reset);
controlEvYawFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_yaw);
controlEvVelFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_vel);
controlEvPosFusion(ev_sample, starting_conditions_passing, ev_reset, quality_sufficient, _aid_src_ev_pos);
@@ -84,3 +85,20 @@ void Ekf::controlExternalVisionFusion()
ECL_WARN("vision data stopped");
}
}
void Ekf::updateEvAttitudeErrorFilter(extVisionSample &ev_sample, bool ev_reset)
{
const AxisAnglef q_error((_state.quat_nominal * ev_sample.quat.inversed()).normalized());
if (!q_error.isAllFinite()) {
return;
}
if (!_ev_q_error_initialized || ev_reset) {
_ev_q_error_filt.reset(q_error);
_ev_q_error_initialized = true;
} else {
_ev_q_error_filt.update(q_error);
}
}