def calc_high_freq_delta_velocity_statistics(
self, imu_metrics: dict, estimator_status_data: dict) -> None:
"""
calculates the statistics for the high frequency delta velocity metric
:param estimator_status_data:
:return:
"""
# max high frequency delta velocity
imu_high_freq_delta_velocity_max = self.add_statistic(
CheckStatisticType.IMU_HIGH_FREQ_DELTA_VELOCITY_MAX,
statistic_instance=0)
imu_high_freq_delta_velocity_max.value = float(
calculate_stat_from_signal(
estimator_status_data,
"estimator_status",
"vibe[2]",
self._in_air_detector_no_ground_effects,
np.amax,
))
imu_high_freq_delta_velocity_max.thresholds.warning = (
thresholds.imu_high_freq_delta_velocity_warning_max())
# avg high frequency delta velocity
imu_high_freq_delta_velocity_avg = self.add_statistic(
CheckStatisticType.IMU_HIGH_FREQ_DELTA_VELOCITY_AVG,
statistic_instance=0)
imu_high_freq_delta_velocity_avg.value = float(0.0)
if imu_high_freq_delta_velocity_max.value > 0.0:
imu_high_freq_delta_velocity_avg.value = float(
calculate_stat_from_signal(
estimator_status_data,
"estimator_status",
"vibe[2]",
self._in_air_detector_no_ground_effects,
np.mean,
))
# windowed avg high frequency delta velocity
imu_high_freq_delta_velocity_windowed_avg = self.add_statistic(
CheckStatisticType.IMU_HIGH_FREQ_DELTA_VELOCITY_WINDOWED_AVG,
statistic_instance=0,
)
imu_high_freq_delta_velocity_windowed_avg.value = float(
max([
np.max(signal)
for _, signal in imu_metrics["imu_hfdvel_windowed_mean"]
]))
imu_high_freq_delta_velocity_windowed_avg.thresholds.warning = (
thresholds.imu_high_freq_delta_velocity_warning_rolling_avg())
def calc_innovation_statistics(self) -> None:
"""
:return:
"""
innovation_metrics = self.calc_innovation_metrics()
for i, innov_fail_name in enumerate(self._innov_fail_names):
innov_stats_fail_pct = self.add_statistic(
CheckStatisticType.FAIL_RATIO_PCT, statistic_instance=i)
innov_stats_fail_pct.value = float(
calculate_stat_from_signal(
self._innov_flags, 'estimator_status', innov_fail_name,
self._in_air_detector_no_ground_effects,
lambda x: 100.0 * np.mean(x > 0.5)))
innov_stats_fail_pct.thresholds.failure = thresholds.ecl_innovation_failure_pct(
self._check_id)
innov_stats_fail_windowed_pct = self.add_statistic(
CheckStatisticType.FAIL_RATIO_WINDOWED_PCT,
statistic_instance=i)
innov_stats_fail_windowed_pct.value = float(
max([
np.max(metric) for _, metric in innovation_metrics[
'{:s}_windowed_mean'.format(innov_fail_name)]
]))
def calc_coning_statistics(self, imu_metrics: dict,
estimator_status_data: dict) -> None:
"""
calculates the statistics for the coning metric
:param estimator_status_data:
:return:
"""
# max coning
imu_coning_max = self.add_statistic(CheckStatisticType.IMU_CONING_MAX,
statistic_instance=0)
imu_coning_max.value = float(
calculate_stat_from_signal(
estimator_status_data,
"estimator_status",
"vibe[0]",
self._in_air_detector_no_ground_effects,
np.amax,
))
imu_coning_max.thresholds.warning = thresholds.imu_coning_warning_max()
# avg coning
imu_coning_avg = self.add_statistic(CheckStatisticType.IMU_CONING_AVG,
statistic_instance=0)
imu_coning_avg.value = float(0.0)
if imu_coning_max.value > 0.0:
imu_coning_avg.value = float(
calculate_stat_from_signal(
estimator_status_data,
"estimator_status",
"vibe[0]",
self._in_air_detector_no_ground_effects,
np.mean,
))
# windowed avg coning
imu_coning_windowed_avg = self.add_statistic(
CheckStatisticType.IMU_CONING_WINDOWED_AVG, statistic_instance=0)
imu_coning_windowed_avg.value = float(
max([
np.max(signal)
for _, signal in imu_metrics["imu_coning_windowed_mean"]
]))
imu_coning_windowed_avg.thresholds.warning = (
thresholds.imu_coning_warning_rolling_avg())
def calc_high_freq_delta_angle_statistics(
self, imu_metrics: dict, estimator_status_data: dict) -> None:
"""
calculates the statistics for the high frequency delta angle metric
:param estimator_status_data:
:return:
"""
# max high frequency delta angle
imu_high_freq_delta_angle_max = self.add_statistic(
CheckStatisticType.IMU_HIGH_FREQ_DELTA_ANGLE_MAX)
imu_high_freq_delta_angle_max.value = float(
calculate_stat_from_signal(estimator_status_data,
'estimator_status', 'vibe[1]',
self._in_air_detector_no_ground_effects,
np.amax))
imu_high_freq_delta_angle_max.thresholds.warning = \
thresholds.imu_high_freq_delta_angle_warning_max()
# avg high frequency delta angle
imu_high_freq_delta_angle_avg = self.add_statistic(
CheckStatisticType.IMU_HIGH_FREQ_DELTA_ANGLE_AVG)
imu_high_freq_delta_angle_avg.value = float(0.0)
if imu_high_freq_delta_angle_max.value > 0.0:
imu_high_freq_delta_angle_avg.value = float(
calculate_stat_from_signal(
estimator_status_data, 'estimator_status', 'vibe[1]',
self._in_air_detector_no_ground_effects, np.mean))
imu_high_freq_delta_angle_avg.thresholds.warning = \
thresholds.imu_high_freq_delta_angle_warning_avg()
# windowed avg high frequency delta angle
imu_high_freq_delta_angle_windowed_avg = self.add_statistic(
CheckStatisticType.IMU_HIGH_FREQ_DELTA_ANGLE_WINDOWED_AVG)
imu_high_freq_delta_angle_windowed_avg.value = float(
max([
np.max(signal)
for _, signal in imu_metrics['imu_hfdang_windowed_mean']
]))
def calc_estimator_statistics(self) -> None:
"""
:return:
"""
if self._test_ratio_name is None:
return
estimator_status_data = self.ulog.get_dataset('estimator_status').data
estimator_status_metrics = self.calc_estimator_status_metrics()
innov_red_pct = self.add_statistic(
CheckStatisticType.INNOVATION_RED_PCT, statistic_instance=0)
innov_red_pct.value = float(
calculate_stat_from_signal(
estimator_status_data, 'estimator_status',
self._test_ratio_name, self._in_air_detector,
lambda x: 100.0 * np.mean(x > params.ecl_red_thresh())))
#TODO: remove subtraction of innov_red_pct and tune parameters
innov_amber_pct = self.add_statistic(
CheckStatisticType.INNOVATION_AMBER_PCT, statistic_instance=0)
innov_amber_pct.value = float(
calculate_stat_from_signal(
estimator_status_data, 'estimator_status',
self._test_ratio_name, self._in_air_detector, lambda x: 100.0 *
np.mean(x > params.ecl_amb_thresh()))) - innov_red_pct.value
innov_amber_pct.thresholds.warning = thresholds.ecl_amber_warning_pct(
self._check_id)
innov_amber_pct.thresholds.failure = thresholds.ecl_amber_failure_pct(
self._check_id)
innov_red_windowed_pct = self.add_statistic(
CheckStatisticType.INNOVATION_RED_WINDOWED_PCT,
statistic_instance=0)
innov_red_windowed_pct.value = float(
max([
np.max(metric) for _, metric in estimator_status_metrics[
'{:s}_percentage_red_windowed'.format(self._check_id)]
]))
innov_amber_windowed_pct = self.add_statistic(
CheckStatisticType.INNOVATION_AMBER_WINDOWED_PCT,
statistic_instance=0)
innov_amber_windowed_pct.value = float(
max([
np.max(metric) for _, metric in estimator_status_metrics[
'{:s}_percentage_amber_windowed'.format(self._check_id)]
]))
# the max and mean ratio of samples above / below std dev
test_ratio_max = self.add_statistic(
CheckStatisticType.ESTIMATOR_FAILURE_MAX, statistic_instance=0)
test_ratio_max.value = float(
calculate_stat_from_signal(estimator_status_data,
'estimator_status',
self._test_ratio_name,
self._in_air_detector, np.amax))
test_ratio_avg = self.add_statistic(
CheckStatisticType.ESTIMATOR_FAILURE_AVG, statistic_instance=0)
test_ratio_avg.value = float(0.0)
if test_ratio_max.value > 0.0:
test_ratio_avg.value = float(
calculate_stat_from_signal(estimator_status_data,
'estimator_status',
self._test_ratio_name,
self._in_air_detector, np.mean))
test_ratio_windowed_avg = self.add_statistic(
CheckStatisticType.ESTIMATOR_FAILURE_WINDOWED_AVG,
statistic_instance=0)
test_ratio_windowed_avg.value = float(
max([
float(np.mean(metric))
for _, metric in estimator_status_metrics[
'{:s}_test_windowed_mean'.format(self._check_id)]
]))
def calc_statistics(self) -> None:
"""
:return:
"""
imu_metrics = self.calculate_metrics()
estimator_states_data = self.ulog.get_dataset(
self._estimator_states_msg).data
# summarize biases from all six possible states
imu_state_metrics = dict()
for signal in [
"states[10]",
"states[11]",
"states[12]",
"states[13]",
"states[14]",
"states[15]",
]:
imu_state_metrics["{:s}_windowed_mean".format(signal)] = float(
max([
np.max(phase) for _, phase in imu_metrics[
"{:s}_windowed_mean".format(signal)]
]))
# delta angle bias windowed
imu_delta_angle_bias_avg = self.add_statistic(
CheckStatisticType.IMU_DELTA_ANGLE_BIAS_AVG, statistic_instance=0)
imu_delta_angle_bias_avg.value = float(
np.sqrt(
np.sum([
np.square(
calculate_stat_from_signal(
estimator_states_data,
self._estimator_states_msg,
signal,
self._in_air_detector_no_ground_effects,
np.median,
))
for signal in ["states[10]", "states[11]", "states[12]"]
])))
imu_delta_angle_bias_avg.thresholds.warning = (
thresholds.imu_delta_angle_bias_warning_avg())
# delta angle bias windowed
imu_delta_angle_bias_windowed_avg = self.add_statistic(
CheckStatisticType.IMU_DELTA_ANGLE_BIAS_WINDOWED_AVG,
statistic_instance=0)
imu_delta_angle_bias_windowed_avg.value = float(
np.sqrt(
np.sum([
np.square(imu_state_metrics[signal]) for signal in [
"states[10]_windowed_mean",
"states[11]_windowed_mean",
"states[12]_windowed_mean",
]
])))
# delta velocity bias
imu_delta_velocity_bias_avg = self.add_statistic(
CheckStatisticType.IMU_DELTA_VELOCITY_BIAS_AVG,
statistic_instance=0)
imu_delta_velocity_bias_avg.value = float(
np.sqrt(
np.sum([
np.square(
calculate_stat_from_signal(
estimator_states_data,
self._estimator_states_msg,
signal,
self._in_air_detector_no_ground_effects,
np.median,
))
for signal in ["states[13]", "states[14]", "states[15]"]
])))
imu_delta_velocity_bias_avg.thresholds.warning = (
thresholds.imu_delta_velocity_bias_warning_avg())
# delta velocity bias windowed
imu_delta_velocity_bias_windowed_avg = self.add_statistic(
CheckStatisticType.IMU_DELTA_VELOCITY_BIAS_WINDOWED_AVG,
statistic_instance=0,
)
imu_delta_velocity_bias_windowed_avg.value = float(
np.sqrt(
np.sum([
np.square(imu_state_metrics[signal]) for signal in [
"states[13]_windowed_mean",
"states[14]_windowed_mean",
"states[15]_windowed_mean",
]
])))
def calc_statistics(self) -> None:
"""
:return:
"""
imu_metrics = self.calculate_metrics()
output_tracking_error_msg = get_output_tracking_error_message(
self.ulog)
output_tracking_error_data = self.ulog.get_dataset(
output_tracking_error_msg).data
# observed angle error statistic average
imu_observed_angle_error_avg = self.add_statistic(
CheckStatisticType.IMU_OBSERVED_ANGLE_ERROR_AVG,
statistic_instance=0)
imu_observed_angle_error_avg.value = float(
calculate_stat_from_signal(
output_tracking_error_data,
output_tracking_error_msg,
"output_tracking_error[0]",
self._in_air_detector_no_ground_effects,
np.median,
))
imu_observed_angle_error_avg.thresholds.warning = (
thresholds.imu_observed_angle_error_warning_avg())
# observed angle error statistic average windowed
imu_observed_angle_error_windowed_avg = self.add_statistic(
CheckStatisticType.IMU_OBSERVED_ANGLE_ERROR_WINDOWED_AVG,
statistic_instance=0,
)
imu_observed_angle_error_windowed_avg.value = float(
max([
metric.max() for _, metric in
imu_metrics["output_obs_ang_err_median_windowed_mean"]
]))
# observed velocity error statistic average
imu_observed_velocity_error_avg = self.add_statistic(
CheckStatisticType.IMU_OBSERVED_VELOCITY_ERROR_AVG,
statistic_instance=0)
imu_observed_velocity_error_avg.value = float(
calculate_stat_from_signal(
output_tracking_error_data,
output_tracking_error_msg,
"output_tracking_error[1]",
self._in_air_detector_no_ground_effects,
np.median,
))
imu_observed_velocity_error_avg.thresholds.warning = (
thresholds.imu_observed_velocity_error_warning_avg())
# observed velocity error statistic average windowed
imu_observed_velocity_error_windowed_avg = self.add_statistic(
CheckStatisticType.IMU_OBSERVED_VELOCITY_ERROR_WINDOWED_AVG,
statistic_instance=0,
)
imu_observed_velocity_error_windowed_avg.value = float(
max([
metric.max() for _, metric in
imu_metrics["output_obs_vel_err_median_windowed_mean"]
]))
# observed position error statistic average
imu_observed_position_error_avg = self.add_statistic(
CheckStatisticType.IMU_OBSERVED_POSITION_ERROR_AVG,
statistic_instance=0)
imu_observed_position_error_avg.value = float(
calculate_stat_from_signal(
output_tracking_error_data,
output_tracking_error_msg,
"output_tracking_error[2]",
self._in_air_detector_no_ground_effects,
np.median,
))
imu_observed_position_error_avg.thresholds.warning = (
thresholds.imu_observed_position_error_warning_avg())
# observed position error statistic average windowed
imu_observed_position_error_windowed_avg = self.add_statistic(
CheckStatisticType.IMU_OBSERVED_POSITION_ERROR_WINDOWED_AVG,
statistic_instance=0,
)
imu_observed_position_error_windowed_avg.value = float(
max([
metric.max() for _, metric in
imu_metrics["output_obs_pos_err_median_windowed_mean"]
]))
