diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp index a061e2c7f1..198d6a344c 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/src/modules/ekf2/EKF/optflow_fusion.cpp @@ -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; } diff --git a/src/modules/ekf2/EKF/optical_flow_control.cpp b/src/modules/ekf2/EKF/optical_flow_control.cpp index d143cb4684..4984ae0c85 100644 --- a/src/modules/ekf2/EKF/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/optical_flow_control.cpp @@ -59,6 +59,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) // Accumulate autopilot gyro data across the same time interval as the flow sensor const Vector3f delta_angle(imu_delayed.delta_ang - (getGyroBias() * imu_delayed.delta_ang_dt)); + if (_delta_time_of < 0.1f) { _imu_del_ang_of += delta_angle; _delta_time_of += imu_delayed.delta_ang_dt; @@ -110,7 +111,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) // compensate for body motion to give a LOS rate _flow_compensated_XY_rad = _flow_sample_delayed.flow_xy_rad - _flow_sample_delayed.gyro_xyz.xy(); - } else if (!_control_status.flags.in_air) { + } else if (!_control_status.flags.in_air && _control_status.flags.vehicle_at_rest) { if (!is_delta_time_good) { // handle special case of SITL and PX4Flow where dt is forced to @@ -154,7 +155,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) const bool flight_condition_not_ok = _control_status.flags.in_air && !isTerrainEstimateValid(); const bool inhibit_flow_use = ((preflight_motion_not_ok || flight_condition_not_ok) && !is_flow_required) - || !_control_status.flags.tilt_align; + || !_control_status.flags.tilt_align; // Handle cases where we are using optical flow but we should not use it anymore if (_control_status.flags.opt_flow) { @@ -169,7 +170,15 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) // optical flow fusion mode selection logic if ((_params.fusion_mode & SensorFusionMask::USE_OPT_FLOW) // optical flow has been selected by the user && !_control_status.flags.opt_flow // we are not yet using flow data - && !inhibit_flow_use) { + && !inhibit_flow_use + && _range_sensor.isDataHealthy()) { + + if (isHorizontalAidingActive() + && ((_aid_src_optical_flow.test_ratio[0] > 0.1f) || (_aid_src_optical_flow.test_ratio[1] > 0.1f)) + ) { + // if horizontal aiding already active don't allow optical flow to start unless the test ratio is good + return; + } // set the flag and reset the fusion timeout ECL_INFO("starting optical flow fusion"); @@ -205,6 +214,8 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) // Fuse optical flow LOS rate observations into the main filter only if height above ground has been updated recently // but use a relaxed time criteria to enable it to coast through bad range finder data if (isRecent(_time_last_hagl_fuse, (uint64_t)10e6)) { + + _aid_src_optical_flow.fusion_enabled = true; fuseOptFlow(); _last_known_pos.xy() = _state.pos.xy(); } @@ -214,7 +225,10 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) // handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period if (isTimedOut(_aid_src_optical_flow.time_last_fuse, _params.no_aid_timeout_max) - && !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow)) { + && !isOtherSourceOfHorizontalAidingThan(_control_status.flags.opt_flow) + && (_flow_sample_delayed.quality >= _params.flow_qual_min) + && isTerrainEstimateValid() + ) { ECL_INFO("reset velocity to flow"); _information_events.flags.reset_vel_to_flow = true; @@ -229,7 +243,10 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) } } - } else if (_control_status.flags.opt_flow && !isRecent(_flow_sample_delayed.time_us, (uint64_t)10e6)) { + } else if (_control_status.flags.opt_flow + && (!isRecent(_flow_sample_delayed.time_us, (uint64_t)10e6) + || !isRecent(_aid_src_optical_flow.time_last_fuse, (uint64_t)10e6)) + ) { stopFlowFusion(); } diff --git a/src/modules/ekf2/test/test_EKF_flow.cpp b/src/modules/ekf2/test/test_EKF_flow.cpp index 553a0af47c..78b579bfd3 100644 --- a/src/modules/ekf2/test/test_EKF_flow.cpp +++ b/src/modules/ekf2/test/test_EKF_flow.cpp @@ -168,6 +168,7 @@ TEST_F(EkfFlowTest, resetToFlowVelocityOnGround) _sensor_simulator._flow.setData(flow_sample); _ekf_wrapper.enableFlowFusion(); _sensor_simulator.startFlow(); + _sensor_simulator.startRangeFinder(); _sensor_simulator.runSeconds(1.0); // THEN: estimated velocity should match simulated velocity diff --git a/src/modules/ekf2/test/test_EKF_fusionLogic.cpp b/src/modules/ekf2/test/test_EKF_fusionLogic.cpp index cfcc62d660..2431dea5e7 100644 --- a/src/modules/ekf2/test/test_EKF_fusionLogic.cpp +++ b/src/modules/ekf2/test/test_EKF_fusionLogic.cpp @@ -183,7 +183,7 @@ TEST_F(EkfFusionLogicTest, fallbackFromGpsToFlow) const float max_ground_distance = 50.f; _ekf->set_optical_flow_limits(max_flow_rate, min_ground_distance, max_ground_distance); _sensor_simulator.startFlow(); - _sensor_simulator.startFlow(); + _sensor_simulator.startRangeFinder(); _ekf_wrapper.enableFlowFusion(); _ekf->set_in_air_status(true); diff --git a/src/modules/ekf2/test/test_EKF_gps.cpp b/src/modules/ekf2/test/test_EKF_gps.cpp index 9d0355fbfa..68436208b9 100644 --- a/src/modules/ekf2/test/test_EKF_gps.cpp +++ b/src/modules/ekf2/test/test_EKF_gps.cpp @@ -156,6 +156,7 @@ TEST_F(EkfGpsTest, gpsHgtToBaroFallback) _sensor_simulator._flow.setData(_sensor_simulator._flow.dataAtRest()); _ekf_wrapper.enableFlowFusion(); _sensor_simulator.startFlow(); + _sensor_simulator.startRangeFinder(); _ekf_wrapper.enableGpsHeightFusion();