|
|
|
@ -236,25 +236,29 @@ void Ekf::fuseOptFlow()
@@ -236,25 +236,29 @@ void Ekf::fuseOptFlow()
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// run the innovation consistency check and record result
|
|
|
|
|
bool flow_fail = false; |
|
|
|
|
bool all_innovation_checks_passed = true; |
|
|
|
|
float test_ratio[2]; |
|
|
|
|
test_ratio[0] = sq(_flow_innov(0)) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var(0)); |
|
|
|
|
test_ratio[1] = sq(_flow_innov(1)) / (sq(math::max(_params.flow_innov_gate, 1.0f)) * _flow_innov_var(1)); |
|
|
|
|
_optflow_test_ratio = math::max(test_ratio[0], test_ratio[1]); |
|
|
|
|
|
|
|
|
|
for (uint8_t obs_index = 0; obs_index <= 1; obs_index++) { |
|
|
|
|
if (test_ratio[obs_index] > 1.0f) { |
|
|
|
|
flow_fail = true; |
|
|
|
|
_innov_check_fail_status.value |= (1 << (obs_index + 10)); |
|
|
|
|
const bool innov_check_fail = (test_ratio[obs_index] > 1.0f); |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
_innov_check_fail_status.value &= ~(1 << (obs_index + 10)); |
|
|
|
|
if (innov_check_fail) { |
|
|
|
|
all_innovation_checks_passed = false; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
if (obs_index == 0) { |
|
|
|
|
_innov_check_fail_status.flags.reject_optflow_X = innov_check_fail; |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
_innov_check_fail_status.flags.reject_optflow_Y = innov_check_fail; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// if either axis fails we abort the fusion
|
|
|
|
|
if (flow_fail) { |
|
|
|
|
if (!all_innovation_checks_passed) { |
|
|
|
|
return; |
|
|
|
|
|
|
|
|
|
} |
|
|
|
|