analyse_logdata_ekf.py
4.84 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
#! /usr/bin/env python3
"""
the ecl ekf analysis
"""
from typing import Tuple, List, Dict
import numpy as np
from pyulog import ULog
from analysis.detectors import InAirDetector, PreconditionError
from analysis.metrics import calculate_ecl_ekf_metrics
from analysis.checks import perform_ecl_ekf_checks
from analysis.post_processing import get_estimator_check_flags
def analyse_ekf(
ulog: ULog, check_levels: Dict[str, float], multi_instance: int = 0,
red_thresh: float = 1.0, amb_thresh: float = 0.5, min_flight_duration_seconds: float = 5.0,
in_air_margin_seconds: float = 5.0, pos_checks_when_sensors_not_fused: bool = False) -> \
Tuple[str, Dict[str, str], Dict[str, float], Dict[str, float]]:
"""
:param ulog:
:param check_levels:
:param multi_instance:
:param red_thresh:
:param amb_thresh:
:param min_flight_duration_seconds:
:param in_air_margin_seconds:
:param pos_checks_when_sensors_not_fused:
:return:
"""
try:
estimator_states = ulog.get_dataset('estimator_states', multi_instance).data
except:
raise PreconditionError('could not find estimator_states instance', multi_instance)
try:
estimator_status = ulog.get_dataset('estimator_status', multi_instance).data
except:
raise PreconditionError('could not find estimator_status instance', multi_instance)
try:
_ = ulog.get_dataset('estimator_innovations', multi_instance).data
except:
raise PreconditionError('could not find estimator_innovations instance', multi_instance)
try:
in_air = InAirDetector(
ulog, min_flight_time_seconds=min_flight_duration_seconds, in_air_margin_seconds=0.0)
in_air_no_ground_effects = InAirDetector(
ulog, min_flight_time_seconds=min_flight_duration_seconds,
in_air_margin_seconds=in_air_margin_seconds)
except Exception as e:
raise PreconditionError(str(e))
if in_air_no_ground_effects.take_off is None:
raise PreconditionError('no airtime detected.')
airtime_info = {
'in_air_transition_time': round(in_air.take_off + in_air.log_start, 2),
'on_ground_transition_time': round(in_air.landing + in_air.log_start, 2)}
control_mode, innov_flags, gps_fail_flags = get_estimator_check_flags(estimator_status)
sensor_checks, innov_fail_checks = find_checks_that_apply(
control_mode, estimator_status,
pos_checks_when_sensors_not_fused=pos_checks_when_sensors_not_fused)
metrics = calculate_ecl_ekf_metrics(
ulog, innov_flags, innov_fail_checks, sensor_checks, in_air, in_air_no_ground_effects,
multi_instance, red_thresh=red_thresh, amb_thresh=amb_thresh)
check_status, master_status = perform_ecl_ekf_checks(
metrics, sensor_checks, innov_fail_checks, check_levels)
return master_status, check_status, metrics, airtime_info
def find_checks_that_apply(
control_mode: dict, estimator_status: dict, pos_checks_when_sensors_not_fused: bool = False) ->\
Tuple[List[str], List[str]]:
"""
finds the checks that apply and stores them in lists for the std checks and the innovation
fail checks.
:param control_mode:
:param estimator_status:
:param b_pos_only_when_sensors_fused:
:return: a tuple of two lists that contain strings for the std checks and for the innovation
fail checks.
"""
sensor_checks = list()
innov_fail_checks = list()
# Height Sensor Checks
sensor_checks.append('hgt')
innov_fail_checks.append('posv')
# Magnetometer Sensor Checks
if (np.amax(control_mode['yaw_aligned']) > 0.5):
sensor_checks.append('mag')
innov_fail_checks.append('magx')
innov_fail_checks.append('magy')
innov_fail_checks.append('magz')
innov_fail_checks.append('yaw')
# Velocity Sensor Checks
if (np.amax(control_mode['using_gps']) > 0.5):
sensor_checks.append('vel')
innov_fail_checks.append('vel')
# Position Sensor Checks
if (pos_checks_when_sensors_not_fused or (np.amax(control_mode['using_gps']) > 0.5)
or (np.amax(control_mode['using_evpos']) > 0.5)):
sensor_checks.append('pos')
innov_fail_checks.append('posh')
# Airspeed Sensor Checks
# a value > 1.0 means the measurement data for that test has been rejected by the EKF
if (np.amax(estimator_status['tas_test_ratio']) > 0.0):
sensor_checks.append('tas')
innov_fail_checks.append('tas')
# Height above ground (rangefinder) sensor checks
if (np.amax(estimator_status['hagl_test_ratio']) > 0.0):
sensor_checks.append('hagl')
innov_fail_checks.append('hagl')
# optical flow sensor checks
if (np.amax(control_mode['using_optflow']) > 0.5):
innov_fail_checks.append('ofx')
innov_fail_checks.append('ofy')
return sensor_checks, innov_fail_checks