mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-09-28 01:18:55 +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,408 @@
|
||||
#! /usr/bin/env python3
|
||||
"""
|
||||
function collection for plotting
|
||||
"""
|
||||
|
||||
from typing import Optional, List, Tuple, Dict
|
||||
|
||||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
from matplotlib.pyplot import Figure, Axes
|
||||
from matplotlib.backends.backend_pdf import PdfPages
|
||||
|
||||
|
||||
def get_min_arg_time_value(
|
||||
time_series_data: np.ndarray, data_time: np.ndarray) -> Tuple[int, float, float]:
|
||||
"""
|
||||
:param time_series_data:
|
||||
:param data_time:
|
||||
:return:
|
||||
"""
|
||||
min_arg = np.argmin(time_series_data)
|
||||
min_time = data_time[min_arg]
|
||||
min_value = np.amin(time_series_data)
|
||||
return (min_arg, min_value, min_time)
|
||||
|
||||
|
||||
def get_max_arg_time_value(
|
||||
time_series_data: np.ndarray, data_time: np.ndarray) -> Tuple[int, float, float]:
|
||||
"""
|
||||
:param time_series_data:
|
||||
:param data_time:
|
||||
:return:
|
||||
"""
|
||||
max_arg = np.argmax(time_series_data)
|
||||
max_time = data_time[max_arg]
|
||||
max_value = np.amax(time_series_data)
|
||||
return max_arg, max_value, max_time
|
||||
|
||||
|
||||
class DataPlot():
|
||||
"""
|
||||
A plotting class interface. Provides functions such as saving the figure.
|
||||
"""
|
||||
def __init__(
|
||||
self, plot_data: Dict[str, np.ndarray], variable_names: List[List[str]],
|
||||
plot_title: str = '', sub_titles: Optional[List[str]] = None,
|
||||
x_labels: Optional[List[str]] = None, y_labels: Optional[List[str]] = None,
|
||||
y_lim: Optional[Tuple[int, int]] = None, legend: Optional[List[str]] = None,
|
||||
pdf_handle: Optional[PdfPages] = None) -> None:
|
||||
"""
|
||||
Initializes the data plot class interface.
|
||||
:param plot_title:
|
||||
:param pdf_handle:
|
||||
"""
|
||||
self._plot_data = plot_data
|
||||
self._variable_names = variable_names
|
||||
self._plot_title = plot_title
|
||||
self._sub_titles = sub_titles
|
||||
self._x_labels = x_labels
|
||||
self._y_labels = y_labels
|
||||
self._y_lim = y_lim
|
||||
self._legend = legend
|
||||
self._pdf_handle = pdf_handle
|
||||
self._fig = None
|
||||
self._ax = None
|
||||
self._fig_size = (20, 13)
|
||||
|
||||
@property
|
||||
def fig(self) -> Figure:
|
||||
"""
|
||||
:return: the figure handle
|
||||
"""
|
||||
if self._fig is None:
|
||||
self._create_figure()
|
||||
return self._fig
|
||||
|
||||
@property
|
||||
def ax(self) -> Axes:
|
||||
"""
|
||||
:return: the axes handle
|
||||
"""
|
||||
if self._ax is None:
|
||||
self._create_figure()
|
||||
return self._ax
|
||||
|
||||
@property
|
||||
def plot_data(self) -> dict:
|
||||
"""
|
||||
returns the plot data. calls _generate_plot_data if necessary.
|
||||
:return:
|
||||
"""
|
||||
if self._plot_data is None:
|
||||
self._generate_plot_data()
|
||||
return self._plot_data
|
||||
|
||||
def plot(self) -> None:
|
||||
"""
|
||||
placeholder for the plotting function. A child class should implement this function.
|
||||
:return:
|
||||
"""
|
||||
|
||||
def _create_figure(self) -> None:
|
||||
"""
|
||||
creates the figure handle.
|
||||
:return:
|
||||
"""
|
||||
self._fig, self._ax = plt.subplots(frameon=True, figsize=self._fig_size)
|
||||
self._fig.suptitle(self._plot_title)
|
||||
|
||||
|
||||
def _generate_plot_data(self) -> None:
|
||||
"""
|
||||
placeholder for a function that generates a data table necessary for plotting
|
||||
:return:
|
||||
"""
|
||||
|
||||
def show(self) -> None:
|
||||
"""
|
||||
displays the figure on the screen.
|
||||
:return: None
|
||||
"""
|
||||
self.fig.show()
|
||||
|
||||
|
||||
def save(self) -> None:
|
||||
"""
|
||||
saves the figure if a pdf_handle was initialized.
|
||||
:return:
|
||||
"""
|
||||
|
||||
if self._pdf_handle is not None and self.fig is not None:
|
||||
self.plot()
|
||||
self._pdf_handle.savefig(figure=self.fig)
|
||||
else:
|
||||
print('skipping saving to pdf: handle was not initialized.')
|
||||
|
||||
|
||||
def close(self) -> None:
|
||||
"""
|
||||
closes the figure.
|
||||
:return:
|
||||
"""
|
||||
plt.close(self._fig)
|
||||
|
||||
|
||||
class TimeSeriesPlot(DataPlot):
|
||||
"""
|
||||
class for creating multiple time series plot.
|
||||
"""
|
||||
def __init__(
|
||||
self, plot_data: dict, variable_names: List[List[str]], x_labels: List[str],
|
||||
y_labels: List[str], plot_title: str = '', sub_titles: Optional[List[str]] = None,
|
||||
pdf_handle: Optional[PdfPages] = None) -> None:
|
||||
"""
|
||||
initializes a timeseries plot
|
||||
:param plot_data:
|
||||
:param variable_names:
|
||||
:param xlabels:
|
||||
:param ylabels:
|
||||
:param plot_title:
|
||||
:param pdf_handle:
|
||||
"""
|
||||
super().__init__(
|
||||
plot_data, variable_names, plot_title=plot_title, sub_titles=sub_titles,
|
||||
x_labels=x_labels, y_labels=y_labels, pdf_handle=pdf_handle)
|
||||
|
||||
def plot(self):
|
||||
"""
|
||||
plots the time series data.
|
||||
:return:
|
||||
"""
|
||||
if self.fig is None:
|
||||
return
|
||||
|
||||
for i in range(len(self._variable_names)):
|
||||
plt.subplot(len(self._variable_names), 1, i + 1)
|
||||
for v in self._variable_names[i]:
|
||||
plt.plot(self.plot_data[v], 'b')
|
||||
plt.xlabel(self._x_labels[i])
|
||||
plt.ylabel(self._y_labels[i])
|
||||
|
||||
self.fig.tight_layout(rect=[0, 0.03, 1, 0.95])
|
||||
|
||||
|
||||
class InnovationPlot(DataPlot):
|
||||
"""
|
||||
class for creating an innovation plot.
|
||||
"""
|
||||
def __init__(
|
||||
self, plot_data: dict, variable_names: List[Tuple[str, str]], x_labels: List[str],
|
||||
y_labels: List[str], plot_title: str = '', sub_titles: Optional[List[str]] = None,
|
||||
pdf_handle: Optional[PdfPages] = None) -> None:
|
||||
"""
|
||||
initializes a timeseries plot
|
||||
:param plot_data:
|
||||
:param variable_names:
|
||||
:param xlabels:
|
||||
:param ylabels:
|
||||
:param plot_title:
|
||||
:param sub_titles:
|
||||
:param pdf_handle:
|
||||
"""
|
||||
super().__init__(
|
||||
plot_data, variable_names, plot_title=plot_title, sub_titles=sub_titles,
|
||||
x_labels=x_labels, y_labels=y_labels, pdf_handle=pdf_handle)
|
||||
|
||||
|
||||
def plot(self):
|
||||
"""
|
||||
plots the Innovation data.
|
||||
:return:
|
||||
"""
|
||||
|
||||
if self.fig is None:
|
||||
return
|
||||
|
||||
for i in range(len(self._variable_names)):
|
||||
# create a subplot for every variable
|
||||
plt.subplot(len(self._variable_names), 1, i + 1)
|
||||
if self._sub_titles is not None:
|
||||
plt.title(self._sub_titles[i])
|
||||
|
||||
# plot the value and the standard deviation
|
||||
plt.plot(
|
||||
1e-6 * self.plot_data['timestamp'], self.plot_data[self._variable_names[i][0]], 'b')
|
||||
plt.plot(
|
||||
1e-6 * self.plot_data['timestamp'],
|
||||
np.sqrt(self.plot_data[self._variable_names[i][1]]), 'r')
|
||||
plt.plot(
|
||||
1e-6 * self.plot_data['timestamp'],
|
||||
-np.sqrt(self.plot_data[self._variable_names[i][1]]), 'r')
|
||||
|
||||
plt.xlabel(self._x_labels[i])
|
||||
plt.ylabel(self._y_labels[i])
|
||||
plt.grid()
|
||||
|
||||
# add the maximum and minimum value as an annotation
|
||||
_, max_value, max_time = get_max_arg_time_value(
|
||||
self.plot_data[self._variable_names[i][0]], 1e-6 * self.plot_data['timestamp'])
|
||||
_, min_value, min_time = get_min_arg_time_value(
|
||||
self.plot_data[self._variable_names[i][0]], 1e-6 * self.plot_data['timestamp'])
|
||||
|
||||
plt.text(
|
||||
max_time, max_value, 'max={:.2f}'.format(max_value), fontsize=12,
|
||||
horizontalalignment='left',
|
||||
verticalalignment='bottom')
|
||||
plt.text(
|
||||
min_time, min_value, 'min={:.2f}'.format(min_value), fontsize=12,
|
||||
horizontalalignment='left',
|
||||
verticalalignment='top')
|
||||
|
||||
self.fig.tight_layout(rect=[0, 0.03, 1, 0.95])
|
||||
|
||||
|
||||
class ControlModeSummaryPlot(DataPlot):
|
||||
"""
|
||||
class for creating a control mode summary plot.
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self, data_time: np.ndarray, plot_data: dict, variable_names: List[List[str]],
|
||||
x_label: str, y_labels: List[str], annotation_text: List[str],
|
||||
additional_annotation: Optional[List[str]] = None, plot_title: str = '',
|
||||
sub_titles: Optional[List[str]] = None,
|
||||
pdf_handle: Optional[PdfPages] = None) -> None:
|
||||
"""
|
||||
initializes a timeseries plot
|
||||
:param plot_data:
|
||||
:param variable_names:
|
||||
:param xlabels:
|
||||
:param ylabels:
|
||||
:param plot_title:
|
||||
:param sub_titles:
|
||||
:param pdf_handle:
|
||||
"""
|
||||
super().__init__(
|
||||
plot_data, variable_names, plot_title=plot_title, sub_titles=sub_titles,
|
||||
x_labels=[x_label]*len(y_labels), y_labels=y_labels, pdf_handle=pdf_handle)
|
||||
self._data_time = data_time
|
||||
self._annotation_text = annotation_text
|
||||
self._additional_annotation = additional_annotation
|
||||
|
||||
|
||||
def plot(self):
|
||||
"""
|
||||
plots the control mode data.
|
||||
:return:
|
||||
"""
|
||||
|
||||
if self.fig is None:
|
||||
return
|
||||
|
||||
colors = ['b', 'r', 'g', 'c']
|
||||
|
||||
for i in range(len(self._variable_names)):
|
||||
# create a subplot for every variable
|
||||
plt.subplot(len(self._variable_names), 1, i + 1)
|
||||
if self._sub_titles is not None:
|
||||
plt.title(self._sub_titles[i])
|
||||
|
||||
for col, var in zip(colors[:len(self._variable_names[i])], self._variable_names[i]):
|
||||
plt.plot(self._data_time, self.plot_data[var], col)
|
||||
|
||||
plt.xlabel(self._x_labels[i])
|
||||
plt.ylabel(self._y_labels[i])
|
||||
plt.grid()
|
||||
plt.ylim(-0.1, 1.1)
|
||||
|
||||
for t in range(len(self._annotation_text[i])):
|
||||
|
||||
_, _, align_time = get_max_arg_time_value(
|
||||
np.diff(self.plot_data[self._variable_names[i][t]]), self._data_time)
|
||||
v_annot_pos = (t+1.0)/(len(self._variable_names[i])+1) # vert annotation position
|
||||
|
||||
if np.amin(self.plot_data[self._variable_names[i][t]]) > 0:
|
||||
plt.text(
|
||||
align_time, v_annot_pos,
|
||||
'no pre-arm data - cannot calculate {:s} start time'.format(
|
||||
self._annotation_text[i][t]), fontsize=12, horizontalalignment='left',
|
||||
verticalalignment='center', color=colors[t])
|
||||
elif np.amax(self.plot_data[self._variable_names[i][t]]) > 0:
|
||||
plt.text(
|
||||
align_time, v_annot_pos, '{:s} at {:.1f} sec'.format(
|
||||
self._annotation_text[i][t], align_time), fontsize=12,
|
||||
horizontalalignment='left', verticalalignment='center', color=colors[t])
|
||||
|
||||
if self._additional_annotation is not None:
|
||||
for a in range(len(self._additional_annotation[i])):
|
||||
v_annot_pos = (a + 1.0) / (len(self._additional_annotation[i]) + 1)
|
||||
plt.text(
|
||||
self._additional_annotation[i][a][0], v_annot_pos,
|
||||
self._additional_annotation[i][a][1], fontsize=12,
|
||||
horizontalalignment='left', verticalalignment='center', color='b')
|
||||
|
||||
self.fig.tight_layout(rect=[0, 0.03, 1, 0.95])
|
||||
|
||||
|
||||
class CheckFlagsPlot(DataPlot):
|
||||
"""
|
||||
class for creating a control mode summary plot.
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self, data_time: np.ndarray, plot_data: dict, variable_names: List[List[str]],
|
||||
x_label: str, y_labels: List[str], y_lim: Optional[Tuple[int, int]] = None,
|
||||
plot_title: str = '', legend: Optional[List[str]] = None,
|
||||
sub_titles: Optional[List[str]] = None, pdf_handle: Optional[PdfPages] = None,
|
||||
annotate: bool = False) -> None:
|
||||
"""
|
||||
initializes a timeseries plot
|
||||
:param plot_data:
|
||||
:param variable_names:
|
||||
:param xlabels:
|
||||
:param ylabels:
|
||||
:param plot_title:
|
||||
:param sub_titles:
|
||||
:param pdf_handle:
|
||||
"""
|
||||
super().__init__(
|
||||
plot_data, variable_names, plot_title=plot_title, sub_titles=sub_titles,
|
||||
x_labels=[x_label]*len(y_labels), y_labels=y_labels, y_lim=y_lim, legend=legend,
|
||||
pdf_handle=pdf_handle)
|
||||
self._data_time = data_time
|
||||
self._b_annotate = annotate
|
||||
|
||||
|
||||
def plot(self):
|
||||
"""
|
||||
plots the control mode data.
|
||||
:return:
|
||||
"""
|
||||
|
||||
if self.fig is None:
|
||||
return
|
||||
|
||||
colors = ['b', 'r', 'g', 'c', 'k', 'm']
|
||||
|
||||
for i in range(len(self._variable_names)):
|
||||
# create a subplot for every variable
|
||||
plt.subplot(len(self._variable_names), 1, i + 1)
|
||||
if self._sub_titles is not None:
|
||||
plt.title(self._sub_titles[i])
|
||||
|
||||
for col, var in zip(colors[:len(self._variable_names[i])], self._variable_names[i]):
|
||||
plt.plot(self._data_time, self.plot_data[var], col)
|
||||
|
||||
plt.xlabel(self._x_labels[i])
|
||||
plt.ylabel(self._y_labels[i])
|
||||
plt.grid()
|
||||
if self._y_lim is not None:
|
||||
plt.ylim(self._y_lim)
|
||||
|
||||
if self._legend is not None:
|
||||
plt.legend(self._legend[i], loc='upper left')
|
||||
|
||||
if self._b_annotate:
|
||||
for col, var in zip(colors[:len(self._variable_names[i])], self._variable_names[i]):
|
||||
# add the maximum and minimum value as an annotation
|
||||
_, max_value, max_time = get_max_arg_time_value(
|
||||
self.plot_data[var], self._data_time)
|
||||
mean_value = np.mean(self.plot_data[var])
|
||||
|
||||
plt.text(
|
||||
max_time, max_value,
|
||||
'max={:.4f}, mean={:.4f}'.format(max_value, mean_value), color=col,
|
||||
fontsize=12, horizontalalignment='left', verticalalignment='bottom')
|
||||
|
||||
self.fig.tight_layout(rect=[0, 0.03, 1, 0.95])
|
||||
@@ -0,0 +1,353 @@
|
||||
#! /usr/bin/env python3
|
||||
"""
|
||||
function collection for plotting
|
||||
"""
|
||||
|
||||
# matplotlib don't use Xwindows backend (must be before pyplot import)
|
||||
import matplotlib
|
||||
matplotlib.use('Agg')
|
||||
|
||||
import numpy as np
|
||||
from matplotlib.backends.backend_pdf import PdfPages
|
||||
from pyulog import ULog
|
||||
|
||||
from analysis.post_processing import magnetic_field_estimates_from_status, get_estimator_check_flags
|
||||
from plotting.data_plots import TimeSeriesPlot, InnovationPlot, ControlModeSummaryPlot, \
|
||||
CheckFlagsPlot
|
||||
from analysis.detectors import PreconditionError
|
||||
|
||||
def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
|
||||
"""
|
||||
creates a pdf report of the ekf analysis.
|
||||
:param ulog:
|
||||
:param output_plot_filename:
|
||||
:return:
|
||||
"""
|
||||
|
||||
# create summary plots
|
||||
# save the plots to PDF
|
||||
|
||||
try:
|
||||
estimator_status = ulog.get_dataset('estimator_status').data
|
||||
print('found estimator_status data')
|
||||
except:
|
||||
raise PreconditionError('could not find estimator_status data')
|
||||
|
||||
try:
|
||||
ekf2_innovations = ulog.get_dataset('ekf2_innovations').data
|
||||
print('found ekf2_innovation data')
|
||||
except:
|
||||
raise PreconditionError('could not find ekf2_innovation data')
|
||||
|
||||
try:
|
||||
sensor_preflight = ulog.get_dataset('sensor_preflight').data
|
||||
print('found sensor_preflight data')
|
||||
except:
|
||||
raise PreconditionError('could not find 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 test 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, if applicable
|
||||
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()
|
||||
|
||||
|
||||
def detect_airtime(control_mode, status_time):
|
||||
# define flags for starting and finishing in air
|
||||
b_starts_in_air = False
|
||||
b_finishes_in_air = False
|
||||
# calculate in-air transition time
|
||||
if (np.amin(control_mode['airborne']) < 0.5) and (np.amax(control_mode['airborne']) > 0.5):
|
||||
in_air_transtion_time_arg = np.argmax(np.diff(control_mode['airborne']))
|
||||
in_air_transition_time = status_time[in_air_transtion_time_arg]
|
||||
elif (np.amax(control_mode['airborne']) > 0.5):
|
||||
in_air_transition_time = np.amin(status_time)
|
||||
print('log starts while in-air at ' + str(round(in_air_transition_time, 1)) + ' sec')
|
||||
b_starts_in_air = True
|
||||
else:
|
||||
in_air_transition_time = float('NaN')
|
||||
print('always on ground')
|
||||
# calculate on-ground transition time
|
||||
if (np.amin(np.diff(control_mode['airborne'])) < 0.0):
|
||||
on_ground_transition_time_arg = np.argmin(np.diff(control_mode['airborne']))
|
||||
on_ground_transition_time = status_time[on_ground_transition_time_arg]
|
||||
elif (np.amax(control_mode['airborne']) > 0.5):
|
||||
on_ground_transition_time = np.amax(status_time)
|
||||
print('log finishes while in-air at ' + str(round(on_ground_transition_time, 1)) + ' sec')
|
||||
b_finishes_in_air = True
|
||||
else:
|
||||
on_ground_transition_time = float('NaN')
|
||||
print('always on ground')
|
||||
if (np.amax(np.diff(control_mode['airborne'])) > 0.5) and (np.amin(np.diff(control_mode['airborne'])) < -0.5):
|
||||
if ((on_ground_transition_time - in_air_transition_time) > 0.0):
|
||||
in_air_duration = on_ground_transition_time - in_air_transition_time
|
||||
else:
|
||||
in_air_duration = float('NaN')
|
||||
else:
|
||||
in_air_duration = float('NaN')
|
||||
return b_finishes_in_air, b_starts_in_air, in_air_duration, in_air_transition_time, on_ground_transition_time
|
||||
Reference in New Issue
Block a user