|
|
|
@ -195,11 +195,12 @@ void FailureDetector::updateImbalancedPropStatus()
@@ -195,11 +195,12 @@ void FailureDetector::updateImbalancedPropStatus()
|
|
|
|
|
|
|
|
|
|
_imbalanced_prop_lpf.setParameters(dt, _imbalanced_prop_lpf_time_constant); |
|
|
|
|
|
|
|
|
|
const float var_x = imu_status.var_accel[0]; |
|
|
|
|
const float var_y = imu_status.var_accel[1]; |
|
|
|
|
const float var_z = imu_status.var_accel[2]; |
|
|
|
|
const float std_x = sqrtf(imu_status.var_accel[0]); |
|
|
|
|
const float std_y = sqrtf(imu_status.var_accel[1]); |
|
|
|
|
const float std_z = sqrtf(imu_status.var_accel[2]); |
|
|
|
|
|
|
|
|
|
const float metric = var_x + var_y - var_z; |
|
|
|
|
// Note: the metric is done using standard deviations instead of variances to be linear
|
|
|
|
|
const float metric = (std_x + std_y) / 2.f - std_z; |
|
|
|
|
const float metric_lpf = _imbalanced_prop_lpf.update(metric); |
|
|
|
|
|
|
|
|
|
const bool is_imbalanced = metric_lpf > _param_fd_imb_prop_thr.get(); |
|
|
|
|