def get_innovation_message(ulog: ULog, topic: str = 'innovation') -> str:
"""
return the name of the innovation message (old: ekf2_innovations; new: estimator_innovations)
:param ulog:
:return: str
"""
ulog_messages = {elem.name for elem in ulog.data_list}
message = ''
if topic == 'innovation':
if 'estimator_innovations' in ulog_messages:
message = 'estimator_innovations'
elif 'ekf2_innovations' in ulog_messages:
message = 'ekf2_innovations'
else:
raise PreconditionError('innovation message not found.')
elif topic == 'innovation_variance':
if 'estimator_innovation_variances' in ulog_messages:
message = 'estimator_innovation_variances'
elif 'ekf2_innovations' in ulog_messages:
message = 'ekf2_innovations'
else:
raise PreconditionError('innovation variances message not found.')
elif topic == 'innovation_test_ratio':
if 'estimator_innovation_test_ratios' in ulog_messages:
message = 'estimator_innovation_test_ratios'
elif 'estimator_status' in ulog_messages:
message = 'estimator_status'
else:
raise PreconditionError(
'estimator innovation test ratios message not found.')
else:
raise NotImplementedError('topic {:s} not supported'.format(topic))
return message
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 get_position_intervals(self,
dataset: str,
multi_instance: int = 0) -> List[list]:
"""
return all intervals of the log file that are valid in position
:param dataset:
:param multi_instance:
:return:
"""
try:
data = self._ulog.get_dataset(dataset,
multi_instance=multi_instance).data
except Exception as e:
raise PreconditionError(
'PositionAnalyzer: {:s} not found in log.'.format(
dataset)) from e
position_intervals = []
for interval in self._position_intervals:
position_intervals.append(
np.where(((data['timestamp'] - self._ulog.start_timestamp) /
1.0e6 >= interval.lower)
& ((data['timestamp'] - self._ulog.start_timestamp) /
1.0e6 < interval.upper))[0])
return position_intervals
def get_innovation_message(ulog: ULog, topic: str = 'innovation') -> str:
"""
return the name of the innovation message (old: ekf2_innovations; new: estimator_innovations)
:param ulog:
:return: str
"""
if topic == 'innovation':
for elem in ulog.data_list:
if elem.name == "ekf2_innovations":
return "ekf2_innovations"
if elem.name == "estimator_innovations":
return "estimator_innovations"
if topic == 'innovation_variance':
for elem in ulog.data_list:
if elem.name == "ekf2_innovations":
return "ekf2_innovations"
if elem.name == "estimator_innovations":
return "estimator_innovation_variances"
if topic == 'innovation_test_ratio':
for elem in ulog.data_list:
if elem.name == "ekf2_innovations":
return None
if elem.name == "estimator_innovations":
return "estimator_innovation_test_ratios"
raise PreconditionError("Could not detect the message")
def get_airtime(self, dataset: str, multi_instance: int = 0) -> list:
"""
return all indices of the log file that are in air
:param dataset:
:return:
"""
try:
data = self._ulog.get_dataset(dataset, multi_instance=multi_instance).data
except:
raise PreconditionError('InAirDetector: {:s} not found in log.'.format(dataset))
return self.get_total_airtime_for_timestamp(
data['timestamp'], start_time=self._ulog.start_timestamp, conversion_factor=1.0e-6)
def get_output_tracking_error_message(ulog: ULog) -> str:
"""
return the name of the message containing the output_tracking_error
:param ulog:
:return: str
"""
for elem in ulog.data_list:
if elem.name == "ekf2_innovations":
return "ekf2_innovations"
if elem.name == "estimator_innovations":
return "estimator_status"
raise PreconditionError(
"Could not detect the message containing the output tracking error")
def get_output_tracking_error_message(ulog: ULog) -> str:
"""
return the name of the message containing the output_tracking_error
:param ulog:
:return: str
"""
output_tracking_error_message = ''
ulog_messages = {elem.name for elem in ulog.data_list}
if 'estimator_innovations' in ulog_messages:
output_tracking_error_message = 'estimator_status'
elif 'ekf2_innovations' in ulog_messages:
output_tracking_error_message = 'ekf2_innovations'
else:
raise PreconditionError(
"Output tracking error message not found in log.")
return output_tracking_error_message
def _above_min_ground_distance_intervals(
self,
ground_distance_meters: float,
phase_change_margin_seconds: float = 0.0,
min_interval_duration_seconds: float = 0.0) -> intervals.Interval:
"""
:param ground_distance_meters:
:return:
"""
intervals_above_min_ground_distance = intervals.empty()
if 'dist_bottom' not in self._vehicle_local_position:
raise PreconditionError(
'Could not find dist_bottom in vehicle_local_position data.')
timestamp = self._vehicle_local_position['timestamp']
dist_bottom = self._vehicle_local_position['dist_bottom']
if 'dist_bottom_valid' in self._vehicle_local_position:
timestamp = timestamp[np.where(
self._vehicle_local_position['dist_bottom_valid'])]
dist_bottom = dist_bottom[np.where(
self._vehicle_local_position['dist_bottom_valid'])]
above_min_ground_distance = dist_bottom > ground_distance_meters
interval_starts, interval_ends = detect_flag_value_changes(
above_min_ground_distance.astype(int))
for interval_start, interval_end in zip(interval_starts,
interval_ends):
if (timestamp[interval_end] / 1.0e6 - phase_change_margin_seconds) - \
(timestamp[interval_start] / 1.0e6 +
phase_change_margin_seconds) >= min_interval_duration_seconds:
intervals_above_min_ground_distance = intervals_above_min_ground_distance | \
intervals.closed(
(timestamp[interval_start] - self._ulog.start_timestamp) / \
1.0e6 + phase_change_margin_seconds,
(timestamp[interval_end] - self._ulog.start_timestamp) / \
1.0e6 - phase_change_margin_seconds)
if intervals_above_min_ground_distance.is_empty():
print('PositionAnalyzer: flag was never activated.')
return intervals_above_min_ground_distance
def __init__(self, ulog: ULog) -> None:
"""
initializes a PositionAnalyzer instance.
:param ulog:
"""
self._ulog = ulog
self._position_intervals = intervals.empty()
self._vehicle_local_position = None
try:
self._vehicle_local_position = self._ulog.get_dataset(
'vehicle_local_position').data
except Exception as e:
raise PreconditionError(
'PositionAnalyzer: Could not find vehicle local position message.'
) from e
self._position_intervals = intervals.closed(
0.0,
(self._ulog.last_timestamp - self._ulog.start_timestamp) / 1.0e6)
def process_logdata_ekf(filename: str, plot: bool = False) -> List[dict]:
"""
main function for processing the logdata for ekf analysis.
:param filename:
:param plot:
:return:
"""
try:
ulog = ULog(filename)
except Exception as e:
raise PreconditionError('could not open {:s}'.format(filename)) from e
test_results = analyse_logdata_ekf(ulog)
with open('{:s}.json'.format(os.path.splitext(filename)[0]), 'w') as file:
json.dump(test_results, file, indent=2)
if plot:
create_pdf_report(ulog, '{:s}.pdf'.format(filename))
print('Plots saved to {:s}.pdf'.format(filename))
return test_results
def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
"""
create a pdf analysis report.
:param ulog:
:param output_plot_filename:
:return:
"""
# create summary plots
# save the plots to PDF
messages = {elem.name for elem in ulog.data_list}
if 'estimator_innovations' in messages:
raise NotImplementedError(
'pdf report not implemented for new log file format')
if 'estimator_status' not in messages:
raise PreconditionError('could not find estimator_status data')
if 'ekf2_innovations' not in messages:
raise PreconditionError('could not find ekf2_innovation data')
if 'sensor_preflight' not in messages:
raise PreconditionError('could not find sensor_preflight data')
estimator_status = ulog.get_dataset('estimator_status').data
ekf2_innovations = ulog.get_dataset('ekf2_innovations').data
sensor_preflight = ulog.get_dataset('sensor_preflight').data
control_mode, innov_flags, gps_fail_flags = get_estimator_check_flags(
estimator_status)
status_time = 1e-6 * estimator_status['timestamp']
b_finishes_in_air, b_starts_in_air, in_air_duration, in_air_transition_time, \
on_ground_transition_time = detect_airtime(control_mode, status_time)
with PdfPages(output_plot_filename) as pdf_pages:
# plot IMU consistency data
if ('accel_inconsistency_m_s_s'
in sensor_preflight.keys()) and ('gyro_inconsistency_rad_s'
in sensor_preflight.keys()):
data_plot = TimeSeriesPlot(
sensor_preflight,
[['accel_inconsistency_m_s_s'], ['gyro_inconsistency_rad_s']],
x_labels=['data index', 'data index'],
y_labels=['acceleration (m/s/s)', 'angular rate (rad/s)'],
plot_title='IMU Consistency Check Levels',
pdf_handle=pdf_pages)
data_plot.save()
data_plot.close()
# vertical velocity and position innovations
data_plot = InnovationPlot(
ekf2_innovations, [('vel_pos_innov[2]', 'vel_pos_innov_var[2]'),
('vel_pos_innov[5]', 'vel_pos_innov_var[5]')],
x_labels=['time (sec)', 'time (sec)'],
y_labels=['Down Vel (m/s)', 'Down Pos (m)'],
plot_title='Vertical Innovations',
pdf_handle=pdf_pages)
data_plot.save()
data_plot.close()
# horizontal velocity innovations
data_plot = InnovationPlot(
ekf2_innovations, [('vel_pos_innov[0]', 'vel_pos_innov_var[0]'),
('vel_pos_innov[1]', 'vel_pos_innov_var[1]')],
x_labels=['time (sec)', 'time (sec)'],
y_labels=['North Vel (m/s)', 'East Vel (m/s)'],
plot_title='Horizontal Velocity Innovations',
pdf_handle=pdf_pages)
data_plot.save()
data_plot.close()
# horizontal position innovations
data_plot = InnovationPlot(
ekf2_innovations, [('vel_pos_innov[3]', 'vel_pos_innov_var[3]'),
('vel_pos_innov[4]', 'vel_pos_innov_var[4]')],
x_labels=['time (sec)', 'time (sec)'],
y_labels=['North Pos (m)', 'East Pos (m)'],
plot_title='Horizontal Position Innovations',
pdf_handle=pdf_pages)
data_plot.save()
data_plot.close()
# magnetometer innovations
data_plot = InnovationPlot(
ekf2_innovations, [('mag_innov[0]', 'mag_innov_var[0]'),
('mag_innov[1]', 'mag_innov_var[1]'),
('mag_innov[2]', 'mag_innov_var[2]')],
x_labels=['time (sec)', 'time (sec)', 'time (sec)'],
y_labels=['X (Gauss)', 'Y (Gauss)', 'Z (Gauss)'],
plot_title='Magnetometer Innovations',
pdf_handle=pdf_pages)
data_plot.save()
data_plot.close()
# magnetic heading innovations
data_plot = InnovationPlot(ekf2_innovations,
[('heading_innov', 'heading_innov_var')],
x_labels=['time (sec)'],
y_labels=['Heading (rad)'],
plot_title='Magnetic Heading Innovations',
pdf_handle=pdf_pages)
data_plot.save()
data_plot.close()
# air data innovations
data_plot = InnovationPlot(
ekf2_innovations, [('airspeed_innov', 'airspeed_innov_var'),
('beta_innov', 'beta_innov_var')],
x_labels=['time (sec)', 'time (sec)'],
y_labels=['innovation (m/sec)', 'innovation (rad)'],
sub_titles=[
'True Airspeed Innovations', 'Synthetic Sideslip Innovations'
],
pdf_handle=pdf_pages)
data_plot.save()
data_plot.close()
# optical flow innovations
data_plot = InnovationPlot(ekf2_innovations,
[('flow_innov[0]', 'flow_innov_var[0]'),
('flow_innov[1]', 'flow_innov_var[1]')],
x_labels=['time (sec)', 'time (sec)'],
y_labels=['X (rad/sec)', 'Y (rad/sec)'],
plot_title='Optical Flow Innovations',
pdf_handle=pdf_pages)
data_plot.save()
data_plot.close()
# plot normalised innovation test levels
# define variables to plot
variables = [['mag_test_ratio'], ['vel_test_ratio', 'pos_test_ratio'],
['hgt_test_ratio']]
y_labels = ['mag', 'vel, pos', 'hgt']
legend = [['mag'], ['vel', 'pos'], ['hgt']]
if np.amax(estimator_status['hagl_test_ratio']
) > 0.0: # plot hagl ratio, if applicable
variables[-1].append('hagl_test_ratio')
y_labels[-1] += ', hagl'
legend[-1].append('hagl')
if np.amax(estimator_status['tas_test_ratio']
) > 0.0: # plot airspeed sensor test ratio
variables.append(['tas_test_ratio'])
y_labels.append('TAS')
legend.append(['airspeed'])
data_plot = CheckFlagsPlot(
status_time,
estimator_status,
variables,
x_label='time (sec)',
y_labels=y_labels,
plot_title='Normalised Innovation Test Levels',
pdf_handle=pdf_pages,
annotate=True,
legend=legend)
data_plot.save()
data_plot.close()
# plot control mode summary A
data_plot = ControlModeSummaryPlot(
status_time,
control_mode,
[['tilt_aligned', 'yaw_aligned'],
['using_gps', 'using_optflow', 'using_evpos'],
['using_barohgt', 'using_gpshgt', 'using_rnghgt', 'using_evhgt'],
['using_magyaw', 'using_mag3d', 'using_magdecl']],
x_label='time (sec)',
y_labels=['aligned', 'pos aiding', 'hgt aiding', 'mag aiding'],
annotation_text=[['tilt alignment', 'yaw alignment'],
[
'GPS aiding', 'optical flow aiding',
'external vision aiding'
],
[
'Baro aiding', 'GPS aiding',
'rangefinder aiding', 'external vision aiding'
],
[
'magnetic yaw aiding',
'3D magnetoemter aiding',
'magnetic declination aiding'
]],
plot_title='EKF Control Status - Figure A',
pdf_handle=pdf_pages)
data_plot.save()
data_plot.close()
# plot control mode summary B
# construct additional annotations for the airborne plot
airborne_annotations = list()
if np.amin(np.diff(control_mode['airborne'])) > -0.5:
airborne_annotations.append(
(on_ground_transition_time,
'air to ground transition not detected'))
else:
airborne_annotations.append(
(on_ground_transition_time,
'on-ground at {:.1f} sec'.format(on_ground_transition_time)))
if in_air_duration > 0.0:
airborne_annotations.append(
((in_air_transition_time + on_ground_transition_time) / 2,
'duration = {:.1f} sec'.format(in_air_duration)))
if np.amax(np.diff(control_mode['airborne'])) < 0.5:
airborne_annotations.append(
(in_air_transition_time,
'ground to air transition not detected'))
else:
airborne_annotations.append(
(in_air_transition_time,
'in-air at {:.1f} sec'.format(in_air_transition_time)))
data_plot = ControlModeSummaryPlot(
status_time,
control_mode, [['airborne'], ['estimating_wind']],
x_label='time (sec)',
y_labels=['airborne', 'estimating wind'],
annotation_text=[[], []],
additional_annotation=[airborne_annotations, []],
plot_title='EKF Control Status - Figure B',
pdf_handle=pdf_pages)
data_plot.save()
data_plot.close()
# plot innovation_check_flags summary
data_plot = CheckFlagsPlot(
status_time,
innov_flags, [['vel_innov_fail', 'posh_innov_fail'],
['posv_innov_fail', 'hagl_innov_fail'],
[
'magx_innov_fail', 'magy_innov_fail',
'magz_innov_fail', 'yaw_innov_fail'
], ['tas_innov_fail'], ['sli_innov_fail'],
['ofx_innov_fail', 'ofy_innov_fail']],
x_label='time (sec)',
y_labels=[
'failed', 'failed', 'failed', 'failed', 'failed', 'failed'
],
y_lim=(-0.1, 1.1),
legend=[['vel NED', 'pos NE'],
['hgt absolute', 'hgt above ground'],
['mag_x', 'mag_y', 'mag_z', 'yaw'], ['airspeed'],
['sideslip'], ['flow X', 'flow Y']],
plot_title='EKF Innovation Test Fails',
annotate=False,
pdf_handle=pdf_pages)
data_plot.save()
data_plot.close()
# gps_check_fail_flags summary
data_plot = CheckFlagsPlot(
status_time,
gps_fail_flags, [[
'nsat_fail', 'gdop_fail', 'herr_fail', 'verr_fail',
'gfix_fail', 'serr_fail'
], ['hdrift_fail', 'vdrift_fail', 'hspd_fail', 'veld_diff_fail']],
x_label='time (sec)',
y_lim=(-0.1, 1.1),
y_labels=['failed', 'failed'],
sub_titles=[
'GPS Direct Output Check Failures',
'GPS Derived Output Check Failures'
],
legend=[[
'N sats', 'GDOP', 'horiz pos error', 'vert pos error',
'fix type', 'speed error'
],
[
'horiz drift', 'vert drift', 'horiz speed',
'vert vel inconsistent'
]],
annotate=False,
pdf_handle=pdf_pages)
data_plot.save()
data_plot.close()
# filter reported accuracy
data_plot = CheckFlagsPlot(
status_time,
estimator_status, [['pos_horiz_accuracy', 'pos_vert_accuracy']],
x_label='time (sec)',
y_labels=['accuracy (m)'],
plot_title='Reported Accuracy',
legend=[['horizontal', 'vertical']],
annotate=False,
pdf_handle=pdf_pages)
data_plot.save()
data_plot.close()
# Plot the EKF IMU vibration metrics
scaled_estimator_status = {
'vibe[0]': 1000. * estimator_status['vibe[0]'],
'vibe[1]': 1000. * estimator_status['vibe[1]'],
'vibe[2]': estimator_status['vibe[2]']
}
data_plot = CheckFlagsPlot(status_time,
scaled_estimator_status,
[['vibe[0]'], ['vibe[1]'], ['vibe[2]']],
x_label='time (sec)',
y_labels=[
'Del Ang Coning (mrad)',
'HF Del Ang (mrad)', 'HF Del Vel (m/s)'
],
plot_title='IMU Vibration Metrics',
pdf_handle=pdf_pages,
annotate=True)
data_plot.save()
data_plot.close()
# Plot the EKF output observer tracking errors
scaled_innovations = {
'output_tracking_error[0]':
1000. * ekf2_innovations['output_tracking_error[0]'],
'output_tracking_error[1]':
ekf2_innovations['output_tracking_error[1]'],
'output_tracking_error[2]':
ekf2_innovations['output_tracking_error[2]']
}
data_plot = CheckFlagsPlot(
1e-6 * ekf2_innovations['timestamp'],
scaled_innovations,
[['output_tracking_error[0]'], ['output_tracking_error[1]'],
['output_tracking_error[2]']],
x_label='time (sec)',
y_labels=['angles (mrad)', 'velocity (m/s)', 'position (m)'],
plot_title='Output Observer Tracking Error Magnitudes',
pdf_handle=pdf_pages,
annotate=True)
data_plot.save()
data_plot.close()
# Plot the delta angle bias estimates
data_plot = CheckFlagsPlot(
1e-6 * estimator_status['timestamp'],
estimator_status, [['states[10]'], ['states[11]'], ['states[12]']],
x_label='time (sec)',
y_labels=['X (rad)', 'Y (rad)', 'Z (rad)'],
plot_title='Delta Angle Bias Estimates',
annotate=False,
pdf_handle=pdf_pages)
data_plot.save()
data_plot.close()
# Plot the delta velocity bias estimates
data_plot = CheckFlagsPlot(
1e-6 * estimator_status['timestamp'],
estimator_status, [['states[13]'], ['states[14]'], ['states[15]']],
x_label='time (sec)',
y_labels=['X (m/s)', 'Y (m/s)', 'Z (m/s)'],
plot_title='Delta Velocity Bias Estimates',
annotate=False,
pdf_handle=pdf_pages)
data_plot.save()
data_plot.close()
# Plot the earth frame magnetic field estimates
declination, field_strength, inclination = magnetic_field_estimates_from_status(
estimator_status)
data_plot = CheckFlagsPlot(1e-6 * estimator_status['timestamp'], {
'strength': field_strength,
'declination': declination,
'inclination': inclination
}, [['declination'], ['inclination'], ['strength']],
x_label='time (sec)',
y_labels=[
'declination (deg)',
'inclination (deg)', 'strength (Gauss)'
],
plot_title='Earth Magnetic Field Estimates',
annotate=False,
pdf_handle=pdf_pages)
data_plot.save()
data_plot.close()
# Plot the body frame magnetic field estimates
data_plot = CheckFlagsPlot(
1e-6 * estimator_status['timestamp'],
estimator_status, [['states[19]'], ['states[20]'], ['states[21]']],
x_label='time (sec)',
y_labels=['X (Gauss)', 'Y (Gauss)', 'Z (Gauss)'],
plot_title='Magnetometer Bias Estimates',
annotate=False,
pdf_handle=pdf_pages)
data_plot.save()
data_plot.close()
# Plot the EKF wind estimates
data_plot = CheckFlagsPlot(1e-6 * estimator_status['timestamp'],
estimator_status,
[['states[22]'], ['states[23]']],
x_label='time (sec)',
y_labels=['North (m/s)', 'East (m/s)'],
plot_title='Wind Velocity Estimates',
annotate=False,
pdf_handle=pdf_pages)
data_plot.save()
data_plot.close()
