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

hackity hack hack

Browse Source
This commit is contained in:
Daniel Agar
2023-03-14 17:13:27 -04:00
parent e1a4473118
commit 6016b7ec19
5 changed files with 17 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/optflow_fusion.cpp
+2 -14
View File
@@ -77,24 +77,12 @@ 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