fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 08:38:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

ECL: Clean velPos logging, deprecate ekf2_innovations msg

Browse Source
This commit is contained in:
kamilritz
2019-12-05 11:29:29 +01:00
committed by Julian Kent
parent 22e4e85eeb
commit b73c80428e
24 changed files with 298 additions and 138 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Tools/ecl_ekf/plotting/pdf_report.py
+28 -21
View File
@@ -15,6 +15,7 @@ from analysis.post_processing import magnetic_field_estimates_from_status, get_e
from plotting.data_plots import TimeSeriesPlot, InnovationPlot, ControlModeSummaryPlot, \
CheckFlagsPlot
from analysis.detectors import PreconditionError
import analysis.data_version_handler as dvh
def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
"""
@@ -34,10 +35,15 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
raise PreconditionError('could not find estimator_status data')
try:
ekf2_innovations = ulog.get_dataset('ekf2_innovations').data
print('found ekf2_innovation data')
estimator_innovations = ulog.get_dataset('estimator_innovations').data
estimator_innovation_variances = ulog.get_dataset('estimator_innovation_variances').data
innovation_data = estimator_innovations
for key in estimator_innovation_variances:
# append 'var' to the field name such that we can distingush between innov and innov_var
innovation_data.update({str('var_'+key): estimator_innovation_variances[key]})
print('found innovation data')
except:
raise PreconditionError('could not find ekf2_innovation data')
raise PreconditionError('could not find innovation data')
try:
sensor_preflight = ulog.get_dataset('sensor_preflight').data
@@ -67,8 +73,8 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
# 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]')],
innovation_data, [('gps_vpos', 'var_gps_vpos'),
('gps_vvel', 'var_gps_vvel')],
x_labels=['time (sec)', 'time (sec)'],
y_labels=['Down Vel (m/s)', 'Down Pos (m)'], plot_title='Vertical Innovations',
pdf_handle=pdf_pages)
@@ -77,8 +83,8 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
# 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]')],
innovation_data, [('gps_hvel[0]', 'var_gps_hvel[0]'),
('gps_hvel[1]', 'var_gps_hvel[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)
@@ -87,8 +93,8 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
# 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]')],
innovation_data, [('gps_hpos[0]', 'var_gps_hpos[0]'),
('gps_hpos[1]', 'var_gps_hpos[1]')],
x_labels=['time (sec)', 'time (sec)'],
y_labels=['North Pos (m)', 'East Pos (m)'], plot_title='Horizontal Position Innovations',
pdf_handle=pdf_pages)
@@ -97,8 +103,9 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
# 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]')],
innovation_data, [('mag_field[0]', 'var_mag_field[0]'),
('mag_field[1]', 'var_mag_field[1]'),
('mag_field[2]', 'var_mag_field[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)
@@ -107,7 +114,7 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
# magnetic heading innovations
data_plot = InnovationPlot(
ekf2_innovations, [('heading_innov', 'heading_innov_var')],
innovation_data, [('heading', 'var_heading')],
x_labels=['time (sec)'], y_labels=['Heading (rad)'],
plot_title='Magnetic Heading Innovations', pdf_handle=pdf_pages)
data_plot.save()
@@ -115,8 +122,8 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
# air data innovations
data_plot = InnovationPlot(
ekf2_innovations,
[('airspeed_innov', 'airspeed_innov_var'), ('beta_innov', 'beta_innov_var')],
innovation_data, [('airspeed', 'var_airspeed'),
('beta', 'var_beta')],
x_labels=['time (sec)', 'time (sec)'],
y_labels=['innovation (m/sec)', 'innovation (rad)'],
sub_titles=['True Airspeed Innovations', 'Synthetic Sideslip Innovations'],
@@ -126,8 +133,8 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
# optical flow innovations
data_plot = InnovationPlot(
ekf2_innovations, [('flow_innov[0]', 'flow_innov_var[0]'), ('flow_innov[1]',
'flow_innov_var[1]')],
innovation_data, [('flow[0]', 'var_flow[0]'),
('flow[1]', 'var_flow[1]')],
x_labels=['time (sec)', 'time (sec)'],
y_labels=['X (rad/sec)', 'Y (rad/sec)'],
plot_title='Optical Flow Innovations', pdf_handle=pdf_pages)
@@ -252,12 +259,12 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
# 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]']
'output_tracking_error[0]': 1000. * estimator_status['output_tracking_error[0]'],
'output_tracking_error[1]': estimator_status['output_tracking_error[1]'],
'output_tracking_error[2]': estimator_status['output_tracking_error[2]']
}
data_plot = CheckFlagsPlot(
1e-6 * ekf2_innovations['timestamp'], scaled_innovations,
1e-6 * estimator_status['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)'],
@@ -350,4 +357,4 @@ def detect_airtime(control_mode, status_time):
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
return b_finishes_in_air, b_starts_in_air, in_air_duration, in_air_transition_time, on_ground_transition_time