mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-03 07:21:36 +08:00
refactor ecl ekf analysis (#11412)
* refactor ekf analysis part 1: move plotting to functions * add plot_check_flags plot function * put plots in seperate file * use object-oriented programming for plotting * move functions for post processing and pdf report creation to new files * add in_air_detector and description as a csv file * refactor metrics and checks into separate functions * refactor metrics into seperate file, seperate plotting * ecl-ekf tools: re-structure folder and move results table generation * ecl-ekf-tool: fix imports and test_results_table * ecl-ekf tools: bugfix output observer tracking error plot * ecl-ekf-tools: update batch processing to new api, fix exception handling * ecl-ekf-tools: use correct in_air_detector * ecl-ekf-tools: rename csv file containing the bare test results table * ecl-tools: refactor for improving readability * ecl-ekf tools: small plotting bugfixes * ecl-ekf tools: small bugfixes in_air time, on_ground_trans, filenames * ecl-ekf-tools: fix amber metric bug * ecl-ekf-tools: remove custom function in inairdetector * ecl-ekf-tools: remove import of pandas * ecl-ekf-tools: add python interpreter to the script start * ecl-ekf-tools pdf_report: fix python interpreter line * px4-dev-ros-kinetic: update container tag to 2019-02-13 * ecl-ekf-tools python interpreter line: call python3 bin directly * ecl-ekf-tools: change airtime from namedtuple to class for python 3.5 * ecl-ekf-tools: update docker image px4-dev-ros-kinetic * ecl-ekf-tools: fix memory leak by correctly closing matplotlib figures
This commit is contained in:
JohannesBrand
@@ -0,0 +1,143 @@
|
||||
#! /usr/bin/env python3
|
||||
"""
|
||||
function collection for performing ecl ekf checks
|
||||
"""
|
||||
|
||||
from typing import Tuple, List, Dict
|
||||
|
||||
|
||||
def perform_ecl_ekf_checks(
|
||||
metrics: Dict[str, float], sensor_checks: List[str], innov_fail_checks: List[str],
|
||||
check_levels: Dict[str, float]) -> Tuple[Dict[str, str], str]:
|
||||
"""
|
||||
# performs the imu, sensor, amd ekf checks and calculates a master status.
|
||||
:param metrics:
|
||||
:param sensor_checks:
|
||||
:param innov_fail_checks:
|
||||
:param check_levels:
|
||||
:return:
|
||||
"""
|
||||
|
||||
imu_status = perform_imu_checks(metrics, check_levels)
|
||||
|
||||
sensor_status = perform_sensor_innov_checks(
|
||||
metrics, sensor_checks, innov_fail_checks, check_levels)
|
||||
|
||||
ekf_status = dict()
|
||||
ekf_status['filter_fault_status'] = 'Fail' if metrics['filter_faults_max'] > 0 else 'Pass'
|
||||
|
||||
# combine the status from the checks
|
||||
combined_status = dict()
|
||||
combined_status.update(imu_status)
|
||||
combined_status.update(sensor_status)
|
||||
combined_status.update(ekf_status)
|
||||
if any(val == 'Fail' for val in combined_status.values()):
|
||||
master_status = 'Fail'
|
||||
elif any(val == 'Warning' for val in combined_status.values()):
|
||||
master_status = 'Warning'
|
||||
else:
|
||||
master_status = 'Pass'
|
||||
|
||||
return combined_status, master_status
|
||||
|
||||
|
||||
def perform_imu_checks(
|
||||
imu_metrics: Dict[str, float], check_levels: Dict[str, float]) -> Dict[str, str]:
|
||||
"""
|
||||
performs the imu checks.
|
||||
:param imu_metrics:
|
||||
:param check_levels:
|
||||
:return:
|
||||
"""
|
||||
|
||||
# check for IMU sensor warnings
|
||||
imu_status = dict()
|
||||
|
||||
# perform the vibration check
|
||||
imu_status['imu_vibration_check'] = 'Pass'
|
||||
for imu_vibr_metric in ['imu_coning', 'imu_hfdang', 'imu_hfdvel']:
|
||||
mean_metric = '{:s}_mean'.format(imu_vibr_metric)
|
||||
peak_metric = '{:s}_peak'.format(imu_vibr_metric)
|
||||
if imu_metrics[mean_metric] > check_levels['{:s}_warn'.format(mean_metric)] \
|
||||
or imu_metrics[peak_metric] > check_levels['{:s}_warn'.format(peak_metric)]:
|
||||
imu_status['imu_vibration_check'] = 'Warning'
|
||||
|
||||
if imu_status['imu_vibration_check'] == 'Warning':
|
||||
print('IMU vibration check warning.')
|
||||
|
||||
# perform the imu bias check
|
||||
if imu_metrics['imu_dang_bias_median'] > check_levels['imu_dang_bias_median_warn'] \
|
||||
or imu_metrics['imu_dvel_bias_median'] > check_levels['imu_dvel_bias_median_warn']:
|
||||
imu_status['imu_bias_check'] = 'Warning'
|
||||
print('IMU bias check warning.')
|
||||
else:
|
||||
imu_status['imu_bias_check'] = 'Pass'
|
||||
|
||||
# perform output predictor
|
||||
if imu_metrics['output_obs_ang_err_median'] > check_levels['obs_ang_err_median_warn'] \
|
||||
or imu_metrics['output_obs_vel_err_median'] > check_levels['obs_vel_err_median_warn'] \
|
||||
or imu_metrics['output_obs_pos_err_median'] > check_levels['obs_pos_err_median_warn']:
|
||||
imu_status['imu_output_predictor_check'] = 'Warning'
|
||||
print('IMU output predictor check warning.')
|
||||
else:
|
||||
imu_status['imu_output_predictor_check'] = 'Pass'
|
||||
|
||||
imu_status['imu_sensor_status'] = 'Warning' if any(
|
||||
val == 'Warning' for val in imu_status.values()) else 'Pass'
|
||||
|
||||
return imu_status
|
||||
|
||||
|
||||
def perform_sensor_innov_checks(
|
||||
metrics: Dict[str, float], sensor_checks: List[str], innov_fail_checks: List[str],
|
||||
check_levels: Dict[str, float]) -> Dict[str, str]:
|
||||
"""
|
||||
performs the sensor checks.
|
||||
:param metrics:
|
||||
:param sensor_checks:
|
||||
:param innov_fail_checks:
|
||||
:param check_levels:
|
||||
:return:
|
||||
"""
|
||||
|
||||
sensor_status = dict()
|
||||
|
||||
for result_id in ['hgt', 'mag', 'vel', 'pos', 'tas', 'hagl']:
|
||||
|
||||
# only run sensor checks, if they apply.
|
||||
if result_id in sensor_checks:
|
||||
if metrics['{:s}_percentage_amber'.format(result_id)] > check_levels[
|
||||
'{:s}_amber_fail_pct'.format(result_id)]:
|
||||
sensor_status['{:s}_sensor_status'.format(result_id)] = 'Fail'
|
||||
print('{:s} sensor check failure.'.format(result_id))
|
||||
elif metrics['{:s}_percentage_amber'.format(result_id)] > check_levels[
|
||||
'{:s}_amber_warn_pct'.format(result_id)]:
|
||||
sensor_status['{:s}_sensor_status'.format(result_id)] = 'Warning'
|
||||
print('{:s} sensor check warning.'.format(result_id))
|
||||
else:
|
||||
sensor_status['{:s}_sensor_status'.format(result_id)] = 'Pass'
|
||||
|
||||
# perform innovation checks.
|
||||
for signal_id, metric_name, result_id in [('posv', 'hgt_fail_percentage', 'hgt'),
|
||||
('magx', 'magx_fail_percentage', 'mag'),
|
||||
('magy', 'magy_fail_percentage', 'mag'),
|
||||
('magz', 'magz_fail_percentage', 'mag'),
|
||||
('yaw', 'yaw_fail_percentage', 'yaw'),
|
||||
('vel', 'vel_fail_percentage', 'vel'),
|
||||
('posh', 'pos_fail_percentage', 'pos'),
|
||||
('tas', 'tas_fail_percentage', 'tas'),
|
||||
('hagl', 'hagl_fail_percentage', 'hagl'),
|
||||
('ofx', 'ofx_fail_percentage', 'flow'),
|
||||
('ofy', 'ofy_fail_percentage', 'flow')]:
|
||||
|
||||
# only run innov fail checks, if they apply.
|
||||
if signal_id in innov_fail_checks:
|
||||
|
||||
if metrics[metric_name] > check_levels['{:s}_fail_pct'.format(result_id)]:
|
||||
sensor_status['{:s}_sensor_status'.format(result_id)] = 'Fail'
|
||||
print('{:s} sensor check failure.'.format(result_id))
|
||||
else:
|
||||
if not ('{:s}_sensor_status'.format(result_id) in sensor_status):
|
||||
sensor_status['{:s}_sensor_status'.format(result_id)] = 'Pass'
|
||||
|
||||
return sensor_status
|
||||
@@ -0,0 +1,182 @@
|
||||
#! /usr/bin/env python3
|
||||
"""
|
||||
detectors
|
||||
"""
|
||||
from typing import Optional
|
||||
import numpy as np
|
||||
from pyulog import ULog
|
||||
|
||||
|
||||
class PreconditionError(Exception):
|
||||
"""
|
||||
a class for a Precondition Error
|
||||
"""
|
||||
|
||||
|
||||
class Airtime(object):
|
||||
"""
|
||||
Airtime struct.
|
||||
"""
|
||||
def __init__(self, take_off: float, landing: float):
|
||||
self.take_off = take_off
|
||||
self.landing = landing
|
||||
|
||||
|
||||
class InAirDetector(object):
|
||||
"""
|
||||
handles airtime detection.
|
||||
"""
|
||||
|
||||
def __init__(self, ulog: ULog, min_flight_time_seconds: float = 0.0,
|
||||
in_air_margin_seconds: float = 0.0) -> None:
|
||||
"""
|
||||
initializes an InAirDetector instance.
|
||||
:param ulog:
|
||||
:param min_flight_time_seconds: set this value to return only airtimes that are at least
|
||||
min_flight_time_seconds long
|
||||
:param in_air_margin_seconds: removes a margin of in_air_margin_seconds from the airtime
|
||||
to avoid ground effects.
|
||||
"""
|
||||
|
||||
self._ulog = ulog
|
||||
self._min_flight_time_seconds = min_flight_time_seconds
|
||||
self._in_air_margin_seconds = in_air_margin_seconds
|
||||
|
||||
try:
|
||||
self._vehicle_land_detected = ulog.get_dataset('vehicle_land_detected').data
|
||||
self._landed = self._vehicle_land_detected['landed']
|
||||
except:
|
||||
self._in_air = []
|
||||
raise PreconditionError(
|
||||
'InAirDetector: Could not find vehicle land detected message and/or landed field'
|
||||
' and thus not find any airtime.')
|
||||
|
||||
self._log_start = self._ulog.start_timestamp / 1.0e6
|
||||
|
||||
self._in_air = self._detect_airtime()
|
||||
|
||||
|
||||
def _detect_airtime(self) -> Optional[Airtime]:
|
||||
"""
|
||||
detects the airtime take_off and landing of a ulog.
|
||||
:return: a named tuple of ('Airtime', ['take_off', 'landing']) or None.
|
||||
"""
|
||||
|
||||
# test whether flight was in air at all
|
||||
if (self._landed > 0).all():
|
||||
print('InAirDetector: always on ground.')
|
||||
return []
|
||||
|
||||
# find the indices of all take offs and landings
|
||||
take_offs = np.where(np.diff(self._landed) < 0)[0].tolist()
|
||||
landings = np.where(np.diff(self._landed) > 0)[0].tolist()
|
||||
|
||||
# check for start in air.
|
||||
if len(take_offs) == 0 or ((len(landings) > 0) and (landings[0] < take_offs[0])):
|
||||
|
||||
print('Started in air. Take first timestamp value as start point.')
|
||||
take_offs = [-1] + take_offs
|
||||
|
||||
# correct for offset: add 1 to take_off list
|
||||
take_offs = [take_off + 1 for take_off in take_offs]
|
||||
if len(landings) < len(take_offs):
|
||||
print('No final landing detected. Assume last timestamp is landing.')
|
||||
landings += [len(self._landed) - 2]
|
||||
# correct for offset: add 1 to landing list
|
||||
landings = [landing + 1 for landing in landings]
|
||||
|
||||
assert len(landings) == len(take_offs), 'InAirDetector: different number of take offs' \
|
||||
' and landings.'
|
||||
|
||||
in_air = []
|
||||
for take_off, landing in zip(take_offs, landings):
|
||||
if (self._vehicle_land_detected['timestamp'][landing] / 1e6 -
|
||||
self._in_air_margin_seconds) - \
|
||||
(self._vehicle_land_detected['timestamp'][take_off] / 1e6 +
|
||||
self._in_air_margin_seconds) >= self._min_flight_time_seconds:
|
||||
in_air.append(Airtime(
|
||||
take_off=(self._vehicle_land_detected['timestamp'][take_off] -
|
||||
self._ulog.start_timestamp) / 1.0e6 + self._in_air_margin_seconds,
|
||||
landing=(self._vehicle_land_detected['timestamp'][landing] -
|
||||
self._ulog.start_timestamp) / 1.0e6 - self._in_air_margin_seconds))
|
||||
if len(in_air) == 0:
|
||||
print('InAirDetector: no airtime detected.')
|
||||
|
||||
return in_air
|
||||
|
||||
@property
|
||||
def airtimes(self):
|
||||
"""
|
||||
airtimes
|
||||
:return:
|
||||
"""
|
||||
return self._in_air
|
||||
|
||||
@property
|
||||
def take_off(self) -> Optional[float]:
|
||||
"""
|
||||
first take off
|
||||
:return:
|
||||
"""
|
||||
return self._in_air[0].take_off if self._in_air else None
|
||||
|
||||
@property
|
||||
def landing(self) -> Optional[float]:
|
||||
"""
|
||||
last landing
|
||||
:return: the last landing of the flight.
|
||||
"""
|
||||
return self._in_air[-1].landing if self._in_air else None
|
||||
|
||||
@property
|
||||
def log_start(self) -> Optional[float]:
|
||||
"""
|
||||
log start
|
||||
:return: the start time of the log.
|
||||
"""
|
||||
return self._log_start
|
||||
|
||||
def get_take_off_to_last_landing(self, dataset) -> list:
|
||||
"""
|
||||
return all indices of the log file between the first take_off and the
|
||||
last landing.
|
||||
:param dataset:
|
||||
:return:
|
||||
"""
|
||||
try:
|
||||
data = self._ulog.get_dataset(dataset).data
|
||||
except:
|
||||
print('InAirDetector: {:s} not found in log.'.format(dataset))
|
||||
return []
|
||||
|
||||
if self._in_air:
|
||||
airtime = np.where(((data['timestamp'] - self._ulog.start_timestamp) / 1.0e6 >=
|
||||
self._in_air[0].take_off) & (
|
||||
(data['timestamp'] - self._ulog.start_timestamp) /
|
||||
1.0e6 < self._in_air[-1].landing))[0]
|
||||
|
||||
else:
|
||||
airtime = []
|
||||
|
||||
return airtime
|
||||
|
||||
def get_airtime(self, dataset) -> list:
|
||||
"""
|
||||
return all indices of the log file that are in air
|
||||
:param dataset:
|
||||
:return:
|
||||
"""
|
||||
try:
|
||||
data = self._ulog.get_dataset(dataset).data
|
||||
except:
|
||||
raise PreconditionError('InAirDetector: {:s} not found in log.'.format(dataset))
|
||||
|
||||
airtime = []
|
||||
if self._in_air is not None:
|
||||
for i in range(len(self._in_air)):
|
||||
airtime.extend(np.where(((data['timestamp'] - self._ulog.start_timestamp) / 1.0e6 >=
|
||||
self._in_air[i].take_off) & (
|
||||
(data['timestamp'] - self._ulog.start_timestamp) /
|
||||
1.0e6 < self._in_air[i].landing))[0])
|
||||
|
||||
return airtime
|
||||
@@ -0,0 +1,183 @@
|
||||
#! /usr/bin/env python3
|
||||
"""
|
||||
function collection for calculation ecl ekf metrics.
|
||||
"""
|
||||
|
||||
from typing import Dict, List, Tuple, Callable
|
||||
|
||||
from pyulog import ULog
|
||||
import numpy as np
|
||||
|
||||
from analysis.detectors import InAirDetector
|
||||
|
||||
def calculate_ecl_ekf_metrics(
|
||||
ulog: ULog, innov_flags: Dict[str, float], innov_fail_checks: List[str],
|
||||
sensor_checks: List[str], in_air: InAirDetector, in_air_no_ground_effects: InAirDetector,
|
||||
red_thresh: float = 1.0, amb_thresh: float = 0.5) -> Tuple[dict, dict, dict, dict]:
|
||||
|
||||
sensor_metrics = calculate_sensor_metrics(
|
||||
ulog, sensor_checks, in_air, in_air_no_ground_effects,
|
||||
red_thresh=red_thresh, amb_thresh=amb_thresh)
|
||||
|
||||
innov_fail_metrics = calculate_innov_fail_metrics(
|
||||
innov_flags, innov_fail_checks, in_air, in_air_no_ground_effects)
|
||||
|
||||
imu_metrics = calculate_imu_metrics(ulog, in_air_no_ground_effects)
|
||||
|
||||
estimator_status_data = ulog.get_dataset('estimator_status').data
|
||||
|
||||
# Check for internal filter nummerical faults
|
||||
ekf_metrics = {'filter_faults_max': np.amax(estimator_status_data['filter_fault_flags'])}
|
||||
# TODO - process these bitmask's when they have been properly documented in the uORB topic
|
||||
# estimator_status['health_flags']
|
||||
# estimator_status['timeout_flags']
|
||||
|
||||
# combine the metrics
|
||||
combined_metrics = dict()
|
||||
combined_metrics.update(imu_metrics)
|
||||
combined_metrics.update(sensor_metrics)
|
||||
combined_metrics.update(innov_fail_metrics)
|
||||
combined_metrics.update(ekf_metrics)
|
||||
|
||||
return combined_metrics
|
||||
|
||||
|
||||
def calculate_sensor_metrics(
|
||||
ulog: ULog, sensor_checks: List[str], in_air: InAirDetector,
|
||||
in_air_no_ground_effects: InAirDetector, red_thresh: float = 1.0,
|
||||
amb_thresh: float = 0.5) -> Dict[str, float]:
|
||||
|
||||
estimator_status_data = ulog.get_dataset('estimator_status').data
|
||||
|
||||
sensor_metrics = dict()
|
||||
|
||||
# calculates peak, mean, percentage above 0.5 std, and percentage above std metrics for
|
||||
# estimator status variables
|
||||
for signal, result_id in [('hgt_test_ratio', 'hgt'),
|
||||
('mag_test_ratio', 'mag'),
|
||||
('vel_test_ratio', 'vel'),
|
||||
('pos_test_ratio', 'pos'),
|
||||
('tas_test_ratio', 'tas'),
|
||||
('hagl_test_ratio', 'hagl')]:
|
||||
|
||||
# only run sensor checks, if they apply.
|
||||
if result_id in sensor_checks:
|
||||
|
||||
if result_id == 'mag' or result_id == 'hgt':
|
||||
in_air_detector = in_air_no_ground_effects
|
||||
else:
|
||||
in_air_detector = in_air
|
||||
|
||||
# the percentage of samples above / below std dev
|
||||
sensor_metrics['{:s}_percentage_red'.format(result_id)] = calculate_stat_from_signal(
|
||||
estimator_status_data, 'estimator_status', signal, in_air_detector,
|
||||
lambda x: 100.0 * np.mean(x > red_thresh))
|
||||
sensor_metrics['{:s}_percentage_amber'.format(result_id)] = calculate_stat_from_signal(
|
||||
estimator_status_data, 'estimator_status', signal, in_air_detector,
|
||||
lambda x: 100.0 * np.mean(x > amb_thresh)) - \
|
||||
sensor_metrics['{:s}_percentage_red'.format(result_id)]
|
||||
|
||||
# the peak and mean ratio of samples above / below std dev
|
||||
peak = calculate_stat_from_signal(
|
||||
estimator_status_data, 'estimator_status', signal, in_air_detector, np.amax)
|
||||
if peak > 0.0:
|
||||
sensor_metrics['{:s}_test_max'.format(result_id)] = peak
|
||||
sensor_metrics['{:s}_test_mean'.format(result_id)] = calculate_stat_from_signal(
|
||||
estimator_status_data, 'estimator_status', signal,
|
||||
in_air_detector, np.mean)
|
||||
|
||||
return sensor_metrics
|
||||
|
||||
|
||||
def calculate_innov_fail_metrics(
|
||||
innov_flags: dict, innov_fail_checks: List[str], in_air: InAirDetector,
|
||||
in_air_no_ground_effects: InAirDetector) -> dict:
|
||||
"""
|
||||
:param innov_flags:
|
||||
:param innov_fail_checks:
|
||||
:param in_air:
|
||||
:param in_air_no_ground_effects:
|
||||
:return:
|
||||
"""
|
||||
|
||||
innov_fail_metrics = dict()
|
||||
|
||||
# calculate innovation check fail metrics
|
||||
for signal_id, signal, result in [('posv', 'posv_innov_fail', 'hgt_fail_percentage'),
|
||||
('magx', 'magx_innov_fail', 'magx_fail_percentage'),
|
||||
('magy', 'magy_innov_fail', 'magy_fail_percentage'),
|
||||
('magz', 'magz_innov_fail', 'magz_fail_percentage'),
|
||||
('yaw', 'yaw_innov_fail', 'yaw_fail_percentage'),
|
||||
('vel', 'vel_innov_fail', 'vel_fail_percentage'),
|
||||
('posh', 'posh_innov_fail', 'pos_fail_percentage'),
|
||||
('tas', 'tas_innov_fail', 'tas_fail_percentage'),
|
||||
('hagl', 'hagl_innov_fail', 'hagl_fail_percentage'),
|
||||
('ofx', 'ofx_innov_fail', 'ofx_fail_percentage'),
|
||||
('ofy', 'ofy_innov_fail', 'ofy_fail_percentage')]:
|
||||
|
||||
# only run innov fail checks, if they apply.
|
||||
if signal_id in innov_fail_checks:
|
||||
|
||||
if signal_id.startswith('mag') or signal_id == 'yaw' or signal_id == 'posv' or \
|
||||
signal_id.startswith('of'):
|
||||
in_air_detector = in_air_no_ground_effects
|
||||
else:
|
||||
in_air_detector = in_air
|
||||
|
||||
innov_fail_metrics[result] = calculate_stat_from_signal(
|
||||
innov_flags, 'estimator_status', signal, in_air_detector,
|
||||
lambda x: 100.0 * np.mean(x > 0.5))
|
||||
|
||||
return innov_fail_metrics
|
||||
|
||||
|
||||
def calculate_imu_metrics(
|
||||
ulog: ULog, in_air_no_ground_effects: InAirDetector) -> dict:
|
||||
|
||||
ekf2_innovation_data = ulog.get_dataset('ekf2_innovations').data
|
||||
|
||||
estimator_status_data = ulog.get_dataset('estimator_status').data
|
||||
|
||||
imu_metrics = dict()
|
||||
|
||||
# calculates the median of the output tracking error ekf innovations
|
||||
for signal, result in [('output_tracking_error[0]', 'output_obs_ang_err_median'),
|
||||
('output_tracking_error[1]', 'output_obs_vel_err_median'),
|
||||
('output_tracking_error[2]', 'output_obs_pos_err_median')]:
|
||||
imu_metrics[result] = calculate_stat_from_signal(
|
||||
ekf2_innovation_data, 'ekf2_innovations', signal, in_air_no_ground_effects, np.median)
|
||||
|
||||
# calculates peak and mean for IMU vibration checks
|
||||
for signal, result in [('vibe[0]', 'imu_coning'),
|
||||
('vibe[1]', 'imu_hfdang'),
|
||||
('vibe[2]', 'imu_hfdvel')]:
|
||||
peak = calculate_stat_from_signal(
|
||||
estimator_status_data, 'estimator_status', signal, in_air_no_ground_effects, np.amax)
|
||||
if peak > 0.0:
|
||||
imu_metrics['{:s}_peak'.format(result)] = peak
|
||||
imu_metrics['{:s}_mean'.format(result)] = calculate_stat_from_signal(
|
||||
estimator_status_data, 'estimator_status', signal,
|
||||
in_air_no_ground_effects, np.mean)
|
||||
|
||||
# IMU bias checks
|
||||
imu_metrics['imu_dang_bias_median'] = np.sqrt(np.sum([np.square(calculate_stat_from_signal(
|
||||
estimator_status_data, 'estimator_status', signal, in_air_no_ground_effects, np.median))
|
||||
for signal in ['states[10]', 'states[11]', 'states[12]']]))
|
||||
imu_metrics['imu_dvel_bias_median'] = np.sqrt(np.sum([np.square(calculate_stat_from_signal(
|
||||
estimator_status_data, 'estimator_status', signal, in_air_no_ground_effects, np.median))
|
||||
for signal in ['states[13]', 'states[14]', 'states[15]']]))
|
||||
|
||||
return imu_metrics
|
||||
|
||||
|
||||
def calculate_stat_from_signal(
|
||||
data: Dict[str, np.ndarray], dataset: str, variable: str,
|
||||
in_air_det: InAirDetector, stat_function: Callable) -> float:
|
||||
"""
|
||||
:param data:
|
||||
:param variable:
|
||||
:param in_air_detector:
|
||||
:return:
|
||||
"""
|
||||
|
||||
return stat_function(data[variable][in_air_det.get_airtime(dataset)])
|
||||
@@ -0,0 +1,142 @@
|
||||
#! /usr/bin/env python3
|
||||
"""
|
||||
function collection for post-processing of ulog data.
|
||||
"""
|
||||
|
||||
from typing import Tuple
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
def get_estimator_check_flags(estimator_status: dict) -> Tuple[dict, dict, dict]:
|
||||
"""
|
||||
:param estimator_status:
|
||||
:return:
|
||||
"""
|
||||
control_mode = get_control_mode_flags(estimator_status)
|
||||
innov_flags = get_innovation_check_flags(estimator_status)
|
||||
gps_fail_flags = get_gps_check_fail_flags(estimator_status)
|
||||
return control_mode, innov_flags, gps_fail_flags
|
||||
|
||||
|
||||
def get_control_mode_flags(estimator_status: dict) -> dict:
|
||||
"""
|
||||
:param estimator_status:
|
||||
:return:
|
||||
"""
|
||||
|
||||
control_mode = dict()
|
||||
# extract control mode metadata from estimator_status.control_mode_flags
|
||||
# 0 - true if the filter tilt alignment is complete
|
||||
# 1 - true if the filter yaw alignment is complete
|
||||
# 2 - true if GPS measurements are being fused
|
||||
# 3 - true if optical flow measurements are being fused
|
||||
# 4 - true if a simple magnetic yaw heading is being fused
|
||||
# 5 - true if 3-axis magnetometer measurement are being fused
|
||||
# 6 - true if synthetic magnetic declination measurements are being fused
|
||||
# 7 - true when the vehicle is airborne
|
||||
# 8 - true when wind velocity is being estimated
|
||||
# 9 - true when baro height is being fused as a primary height reference
|
||||
# 10 - true when range finder height is being fused as a primary height reference
|
||||
# 11 - true when range finder height is being fused as a primary height reference
|
||||
# 12 - true when local position data from external vision is being fused
|
||||
# 13 - true when yaw data from external vision measurements is being fused
|
||||
# 14 - true when height data from external vision measurements is being fused
|
||||
control_mode['tilt_aligned'] = ((2 ** 0 & estimator_status['control_mode_flags']) > 0) * 1
|
||||
control_mode['yaw_aligned'] = ((2 ** 1 & estimator_status['control_mode_flags']) > 0) * 1
|
||||
control_mode['using_gps'] = ((2 ** 2 & estimator_status['control_mode_flags']) > 0) * 1
|
||||
control_mode['using_optflow'] = ((2 ** 3 & estimator_status['control_mode_flags']) > 0) * 1
|
||||
control_mode['using_magyaw'] = ((2 ** 4 & estimator_status['control_mode_flags']) > 0) * 1
|
||||
control_mode['using_mag3d'] = ((2 ** 5 & estimator_status['control_mode_flags']) > 0) * 1
|
||||
control_mode['using_magdecl'] = ((2 ** 6 & estimator_status['control_mode_flags']) > 0) * 1
|
||||
control_mode['airborne'] = ((2 ** 7 & estimator_status['control_mode_flags']) > 0) * 1
|
||||
control_mode['estimating_wind'] = ((2 ** 8 & estimator_status['control_mode_flags']) > 0) * 1
|
||||
control_mode['using_barohgt'] = ((2 ** 9 & estimator_status['control_mode_flags']) > 0) * 1
|
||||
control_mode['using_rnghgt'] = ((2 ** 10 & estimator_status['control_mode_flags']) > 0) * 1
|
||||
control_mode['using_gpshgt'] = ((2 ** 11 & estimator_status['control_mode_flags']) > 0) * 1
|
||||
control_mode['using_evpos'] = ((2 ** 12 & estimator_status['control_mode_flags']) > 0) * 1
|
||||
control_mode['using_evyaw'] = ((2 ** 13 & estimator_status['control_mode_flags']) > 0) * 1
|
||||
control_mode['using_evhgt'] = ((2 ** 14 & estimator_status['control_mode_flags']) > 0) * 1
|
||||
return control_mode
|
||||
|
||||
|
||||
def get_innovation_check_flags(estimator_status: dict) -> dict:
|
||||
"""
|
||||
:param estimator_status:
|
||||
:return:
|
||||
"""
|
||||
|
||||
innov_flags = dict()
|
||||
# innovation_check_flags summary
|
||||
# 0 - true if velocity observations have been rejected
|
||||
# 1 - true if horizontal position observations have been rejected
|
||||
# 2 - true if true if vertical position observations have been rejected
|
||||
# 3 - true if the X magnetometer observation has been rejected
|
||||
# 4 - true if the Y magnetometer observation has been rejected
|
||||
# 5 - true if the Z magnetometer observation has been rejected
|
||||
# 6 - true if the yaw observation has been rejected
|
||||
# 7 - true if the airspeed observation has been rejected
|
||||
# 8 - true if synthetic sideslip observation has been rejected
|
||||
# 9 - true if the height above ground observation has been rejected
|
||||
# 10 - true if the X optical flow observation has been rejected
|
||||
# 11 - true if the Y optical flow observation has been rejected
|
||||
innov_flags['vel_innov_fail'] = ((2 ** 0 & estimator_status['innovation_check_flags']) > 0) * 1
|
||||
innov_flags['posh_innov_fail'] = ((2 ** 1 & estimator_status['innovation_check_flags']) > 0) * 1
|
||||
innov_flags['posv_innov_fail'] = ((2 ** 2 & estimator_status['innovation_check_flags']) > 0) * 1
|
||||
innov_flags['magx_innov_fail'] = ((2 ** 3 & estimator_status['innovation_check_flags']) > 0) * 1
|
||||
innov_flags['magy_innov_fail'] = ((2 ** 4 & estimator_status['innovation_check_flags']) > 0) * 1
|
||||
innov_flags['magz_innov_fail'] = ((2 ** 5 & estimator_status['innovation_check_flags']) > 0) * 1
|
||||
innov_flags['yaw_innov_fail'] = ((2 ** 6 & estimator_status['innovation_check_flags']) > 0) * 1
|
||||
innov_flags['tas_innov_fail'] = ((2 ** 7 & estimator_status['innovation_check_flags']) > 0) * 1
|
||||
innov_flags['sli_innov_fail'] = ((2 ** 8 & estimator_status['innovation_check_flags']) > 0) * 1
|
||||
innov_flags['hagl_innov_fail'] = ((2 ** 9 & estimator_status['innovation_check_flags']) > 0) * 1
|
||||
innov_flags['ofx_innov_fail'] = ((2 ** 10 & estimator_status['innovation_check_flags']) > 0) * 1
|
||||
innov_flags['ofy_innov_fail'] = ((2 ** 11 & estimator_status['innovation_check_flags']) > 0) * 1
|
||||
return innov_flags
|
||||
|
||||
|
||||
def get_gps_check_fail_flags(estimator_status: dict) -> dict:
|
||||
"""
|
||||
:param estimator_status:
|
||||
:return:
|
||||
"""
|
||||
gps_fail_flags = dict()
|
||||
|
||||
# 0 : insufficient fix type (no 3D solution)
|
||||
# 1 : minimum required sat count fail
|
||||
# 2 : minimum required GDoP fail
|
||||
# 3 : maximum allowed horizontal position error fail
|
||||
# 4 : maximum allowed vertical position error fail
|
||||
# 5 : maximum allowed speed error fail
|
||||
# 6 : maximum allowed horizontal position drift fail
|
||||
# 7 : maximum allowed vertical position drift fail
|
||||
# 8 : maximum allowed horizontal speed fail
|
||||
# 9 : maximum allowed vertical velocity discrepancy fail
|
||||
gps_fail_flags['gfix_fail'] = ((2 ** 0 & estimator_status['gps_check_fail_flags']) > 0) * 1
|
||||
gps_fail_flags['nsat_fail'] = ((2 ** 1 & estimator_status['gps_check_fail_flags']) > 0) * 1
|
||||
gps_fail_flags['gdop_fail'] = ((2 ** 2 & estimator_status['gps_check_fail_flags']) > 0) * 1
|
||||
gps_fail_flags['herr_fail'] = ((2 ** 3 & estimator_status['gps_check_fail_flags']) > 0) * 1
|
||||
gps_fail_flags['verr_fail'] = ((2 ** 4 & estimator_status['gps_check_fail_flags']) > 0) * 1
|
||||
gps_fail_flags['serr_fail'] = ((2 ** 5 & estimator_status['gps_check_fail_flags']) > 0) * 1
|
||||
gps_fail_flags['hdrift_fail'] = ((2 ** 6 & estimator_status['gps_check_fail_flags']) > 0) * 1
|
||||
gps_fail_flags['vdrift_fail'] = ((2 ** 7 & estimator_status['gps_check_fail_flags']) > 0) * 1
|
||||
gps_fail_flags['hspd_fail'] = ((2 ** 8 & estimator_status['gps_check_fail_flags']) > 0) * 1
|
||||
gps_fail_flags['veld_diff_fail'] = ((2 ** 9 & estimator_status['gps_check_fail_flags']) > 0) * 1
|
||||
return gps_fail_flags
|
||||
|
||||
|
||||
def magnetic_field_estimates_from_status(estimator_status: dict) -> Tuple[float, float, float]:
|
||||
"""
|
||||
|
||||
:param estimator_status:
|
||||
:return:
|
||||
"""
|
||||
rad2deg = 57.2958
|
||||
field_strength = np.sqrt(
|
||||
estimator_status['states[16]'] ** 2 + estimator_status['states[17]'] ** 2 +
|
||||
estimator_status['states[18]'] ** 2)
|
||||
declination = rad2deg * np.arctan2(estimator_status['states[17]'],
|
||||
estimator_status['states[16]'])
|
||||
inclination = rad2deg * np.arcsin(
|
||||
estimator_status['states[18]'] / np.maximum(field_strength, np.finfo(np.float32).eps))
|
||||
return declination, field_strength, inclination
|
||||
Reference in New Issue
Block a user