fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-09 07:31:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: OF control logic fixes

Browse Source 
- always compute innovation variance and test ratios (updateOptFlow())
 - if horizontal aiding active require good test ratio before starting OF
 - strict on ground (not flying) optical flow fusion workaround to when
vehicle is also at rest (much more conservative)
 - add additional OF reset terrain validity and OF quality check
This commit is contained in:
Daniel Agar
2023-03-14 16:54:53 -04:00
parent 2605562a69
commit e1a4473118
5 changed files with 40 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/optflow_fusion.cpp
+15 -3
View File
@@ -77,12 +77,24 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src)
const float R_LOS = calcOptFlowMeasVar(_flow_sample_delayed);
aid_src.observation_variance[0] = R_LOS;
aid_src.observation_variance[1] = R_LOS;
// compute innovation variance to update test ratios
const Vector24f state_vector = getStateAtFusionHorizonAsVector();
Vector2f innov_var;
Vector24f H;
sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H);
innov_var.copyTo(_aid_src_optical_flow.innovation_variance);
// run the innovation consistency check and record result
setEstimatorAidStatusTestRatio(_aid_src_optical_flow, math::max(_params.flow_innov_gate, 1.f));
_innov_check_fail_status.flags.reject_optflow_X = (_aid_src_optical_flow.test_ratio[0] > 1.f);
_innov_check_fail_status.flags.reject_optflow_Y = (_aid_src_optical_flow.test_ratio[1] > 1.f);
}
void Ekf::fuseOptFlow()
{
_aid_src_optical_flow.fusion_enabled = true;
const float R_LOS = _aid_src_optical_flow.observation_variance[0];
// calculate the height above the ground of the optical flow camera. Since earth frame is NED
@@ -111,7 +123,7 @@ void Ekf::fuseOptFlow()
_innov_check_fail_status.flags.reject_optflow_Y = (_aid_src_optical_flow.test_ratio[1] > 1.f);
// if either axis fails we abort the fusion
if (_aid_src_optical_flow.innovation_rejected) {
if (_aid_src_optical_flow.innovation_rejected || !_aid_src_optical_flow.fusion_enabled) {
return;
}