mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-27 05:13:34 +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])
|
||||
Reference in New Issue
Block a user