fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 07:43:33 +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/analyse_logdata_ekf.py
+3 -6
View File
@@ -36,10 +36,10 @@ def analyse_ekf(
raise PreconditionError('could not find estimator_status data')
try:
_ = ulog.get_dataset('ekf2_innovations').data
print('found ekf2_innovation data')
_ = ulog.get_dataset('estimator_innovations').data
print('found estimator_innovations data')
except:
raise PreconditionError('could not find ekf2_innovation data')
raise PreconditionError('could not find estimator_innovations data')
try:
in_air = InAirDetector(
@@ -133,6 +133,3 @@ def find_checks_that_apply(
Tools/ecl_ekf/analysis/data_version_handler.py
+48
View File
@@ -0,0 +1,48 @@
#! /usr/bin/env python3
"""
function collection for handling different versions of log files
"""
from pyulog import ULog
from analysis.detectors import PreconditionError
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_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_innovations"
if topic == 'innovation_test_ratio':
for elem in ulog.data_list:
if elem.name == "ekf2_innovations":
return "ekf2_innovations"
if elem.name == "estimator_innovations":
return "estimator_innovations"
raise PreconditionError("Could not detect the message")
Tools/ecl_ekf/analysis/detectors.py
+1 -1
View File
@@ -179,4 +179,4 @@ class InAirDetector(object):
(data['timestamp'] - self._ulog.start_timestamp) /
1.0e6 < self._in_air[i].landing))[0])
return airtime
return airtime
Tools/ecl_ekf/analysis/metrics.py
+1 -3
View File
@@ -134,8 +134,6 @@ def calculate_innov_fail_metrics(
def calculate_imu_metrics(
ulog: ULog, in_air_no_ground_effects: InAirDetector) -> dict:
ekf2_innovation_data = ulog.get_dataset('ekf2_innovations').data
estimator_status_data = ulog.get_dataset('estimator_status').data
imu_metrics = dict()
@@ -145,7 +143,7 @@ def calculate_imu_metrics(
('output_tracking_error[1]', 'output_obs_vel_err_median'),
('output_tracking_error[2]', 'output_obs_pos_err_median')]:
imu_metrics[result] = calculate_stat_from_signal(
ekf2_innovation_data, 'ekf2_innovations', signal, in_air_no_ground_effects, np.median)
estimator_status_data, 'estimator_status', signal, in_air_no_ground_effects, np.median)
# calculates peak and mean for IMU vibration checks
for signal, result in [('vibe[0]', 'imu_coning'),
Tools/ecl_ekf/analysis/post_processing.py
+22 -1
View File
@@ -42,6 +42,17 @@ def get_control_mode_flags(estimator_status: dict) -> dict:
# 12 - true when local position data from external vision is being fused
# 13 - true when yaw data from external vision measurements is being fused
# 14 - true when height data from external vision measurements is being fused
# 15 - true when synthetic sideslip measurements are being fused
# 16 - true true when the mag field does not match the expected strength
# 17 - true true when the vehicle is operating as a fixed wing vehicle
# 18 - true when the magnetometer has been declared faulty and is no longer being used
# 19 - true true when airspeed measurements are being fused
# 20 - true true when protection from ground effect induced static pressure rise is active
# 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough
# 22 - true when yaw (not ground course) data from a GPS receiver is being fused
# 23 - true when the in-flight mag field alignment has been completed
# 24 - true when local earth frame velocity data from external vision measurements are being fused
# 25 - true when we are using a synthesized measurement for the magnetometer Z component
control_mode['tilt_aligned'] = ((2 ** 0 & estimator_status['control_mode_flags']) > 0) * 1
control_mode['yaw_aligned'] = ((2 ** 1 & estimator_status['control_mode_flags']) > 0) * 1
control_mode['using_gps'] = ((2 ** 2 & estimator_status['control_mode_flags']) > 0) * 1
@@ -57,9 +68,19 @@ def get_control_mode_flags(estimator_status: dict) -> dict:
control_mode['using_evpos'] = ((2 ** 12 & estimator_status['control_mode_flags']) > 0) * 1
control_mode['using_evyaw'] = ((2 ** 13 & estimator_status['control_mode_flags']) > 0) * 1
control_mode['using_evhgt'] = ((2 ** 14 & estimator_status['control_mode_flags']) > 0) * 1
control_mode['fuse_beta'] = ((2 ** 15 & estimator_status['control_mode_flags']) > 0) * 1
control_mode['mag_field_disturbed'] = ((2 ** 16 & estimator_status['control_mode_flags']) > 0) * 1
control_mode['fixed_wing'] = ((2 ** 17 & estimator_status['control_mode_flags']) > 0) * 1
control_mode['mag_fault'] = ((2 ** 18 & estimator_status['control_mode_flags']) > 0) * 1
control_mode['fuse_aspd'] = ((2 ** 19 & estimator_status['control_mode_flags']) > 0) * 1
control_mode['gnd_effect'] = ((2 ** 20 & estimator_status['control_mode_flags']) > 0) * 1
control_mode['rng_stuck'] = ((2 ** 21 & estimator_status['control_mode_flags']) > 0) * 1
control_mode['gps_yaw'] = ((2 ** 22 & estimator_status['control_mode_flags']) > 0) * 1
control_mode['mag_aligned_in_flight'] = ((2 ** 23 & estimator_status['control_mode_flags']) > 0) * 1
control_mode['ev_vel'] = ((2 ** 24 & estimator_status['control_mode_flags']) > 0) * 1
control_mode['synthetic_mag_z'] = ((2 ** 25 & estimator_status['control_mode_flags']) > 0) * 1
return control_mode
def get_innovation_check_flags(estimator_status: dict) -> dict:
"""
:param estimator_status:
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