diff --git a/.ci/Jenkinsfile-SITL_tests b/.ci/Jenkinsfile-SITL_tests index 8601f603af..30607aba67 100644 --- a/.ci/Jenkinsfile-SITL_tests +++ b/.ci/Jenkinsfile-SITL_tests @@ -8,7 +8,7 @@ pipeline { stage('Build') { agent { docker { - image 'px4io/px4-dev-ros-kinetic:2019-02-09' + image 'px4io/px4-dev-ros-kinetic:2019-02-14' args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw -e HOME=$WORKSPACE' } } @@ -109,7 +109,7 @@ def createTestNode(Map test_def) { return { node { cleanWs() - docker.image("px4io/px4-dev-ros-kinetic:2019-02-09").inside('-e HOME=${WORKSPACE}') { + docker.image("px4io/px4-dev-ros-kinetic:2019-02-14").inside('-e HOME=${WORKSPACE}') { stage(test_def.name) { def test_ok = true sh('export') diff --git a/.ci/Jenkinsfile-SITL_tests_coverage b/.ci/Jenkinsfile-SITL_tests_coverage index 3c498f6418..8dd06da4e2 100644 --- a/.ci/Jenkinsfile-SITL_tests_coverage +++ b/.ci/Jenkinsfile-SITL_tests_coverage @@ -128,7 +128,7 @@ def createTestNode(Map test_def) { return { node { cleanWs() - docker.image("px4io/px4-dev-ros-kinetic:2019-02-09").inside('-e HOME=${WORKSPACE}') { + docker.image("px4io/px4-dev-ros-kinetic:2019-02-14").inside('-e HOME=${WORKSPACE}') { stage(test_def.name) { def test_ok = true sh('export') diff --git a/.ci/Jenkinsfile-compile b/.ci/Jenkinsfile-compile index b9b990b769..ccb9b0178f 100644 --- a/.ci/Jenkinsfile-compile +++ b/.ci/Jenkinsfile-compile @@ -12,7 +12,7 @@ pipeline { armhf: "px4io/px4-dev-armhf:2019-02-09", base: "px4io/px4-dev-base:2019-02-09", nuttx: "px4io/px4-dev-nuttx:2019-02-09", - ros: "px4io/px4-dev-ros-kinetic:2019-02-09", + ros: "px4io/px4-dev-ros-kinetic:2019-02-14", rpi: "px4io/px4-dev-raspi:2019-02-09", snapdragon: "lorenzmeier/px4-dev-snapdragon:2018-09-12" ] diff --git a/Tools/ecl_ekf/__init__.py b/Tools/ecl_ekf/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/Tools/ecl_ekf/analyse_logdata_ekf.py b/Tools/ecl_ekf/analyse_logdata_ekf.py index c1d32cf4ec..df56bb968e 100644 --- a/Tools/ecl_ekf/analyse_logdata_ekf.py +++ b/Tools/ecl_ekf/analyse_logdata_ekf.py @@ -1,1382 +1,138 @@ +#! /usr/bin/env python3 +""" +the ecl ekf analysis +""" + +from typing import Tuple, List, Dict + import numpy as np +from pyulog import ULog -# matplotlib don't use Xwindows backend (must be before pyplot import) -import matplotlib -matplotlib.use('Agg') +from analysis.detectors import InAirDetector, PreconditionError +from analysis.metrics import calculate_ecl_ekf_metrics +from analysis.checks import perform_ecl_ekf_checks +from analysis.post_processing import get_estimator_check_flags -import matplotlib.pyplot as plt -from matplotlib.backends.backend_pdf import PdfPages +def analyse_ekf( + ulog: ULog, check_levels: Dict[str, float], red_thresh: float = 1.0, + amb_thresh: float = 0.5, min_flight_duration_seconds: float = 5.0, + in_air_margin_seconds: float = 5.0, pos_checks_when_sensors_not_fused: bool = False) -> \ + Tuple[str, Dict[str, str], Dict[str, float], Dict[str, float]]: + """ + :param ulog: + :param check_levels: + :param red_thresh: + :param amb_thresh: + :param min_flight_duration_seconds: + :param in_air_margin_seconds: + :param pos_checks_when_sensors_not_fused: + :return: + """ -def analyse_ekf(estimator_status, ekf2_innovations, sensor_preflight, check_levels, - plot=False, output_plot_filename=None, late_start_early_ending=True): + try: + estimator_status = ulog.get_dataset('estimator_status').data + print('found estimator_status data') + except: + raise PreconditionError('could not find estimator_status data') - if plot: - # create summary plots - # save the plots to PDF - pp = PdfPages(output_plot_filename) - # plot IMU consistency data - if ('accel_inconsistency_m_s_s' in sensor_preflight.keys()) and ( - 'gyro_inconsistency_rad_s' in sensor_preflight.keys()): - plt.figure(0, figsize=(20, 13)) - plt.subplot(2, 1, 1) - plt.plot(sensor_preflight['accel_inconsistency_m_s_s'], 'b') - plt.title('IMU Consistency Check Levels') - plt.ylabel('acceleration (m/s/s)') - plt.xlabel('data index') - plt.grid() - plt.subplot(2, 1, 2) - plt.plot(sensor_preflight['gyro_inconsistency_rad_s'], 'b') - plt.ylabel('angular rate (rad/s)') - plt.xlabel('data index') - pp.savefig() - plt.close(0) + try: + _ = ulog.get_dataset('ekf2_innovations').data + print('found ekf2_innovation data') + except: + raise PreconditionError('could not find ekf2_innovation data') - # generate max, min and 1-std metadata - innov_time = 1e-6 * ekf2_innovations['timestamp'] - status_time = 1e-6 * estimator_status['timestamp'] + try: + in_air = InAirDetector( + ulog, min_flight_time_seconds=min_flight_duration_seconds, in_air_margin_seconds=0.0) + in_air_no_ground_effects = InAirDetector( + ulog, min_flight_time_seconds=min_flight_duration_seconds, + in_air_margin_seconds=in_air_margin_seconds) + except Exception as e: + raise PreconditionError(str(e)) - if plot: - # vertical velocity and position innovations - plt.figure(1, figsize=(20, 13)) - # generate metadata for velocity innovations - innov_2_max_arg = np.argmax(ekf2_innovations['vel_pos_innov[2]']) - innov_2_max_time = innov_time[innov_2_max_arg] - innov_2_max = np.amax(ekf2_innovations['vel_pos_innov[2]']) - innov_2_min_arg = np.argmin(ekf2_innovations['vel_pos_innov[2]']) - innov_2_min_time = innov_time[innov_2_min_arg] - innov_2_min = np.amin(ekf2_innovations['vel_pos_innov[2]']) - s_innov_2_max = str(round(innov_2_max, 2)) - s_innov_2_min = str(round(innov_2_min, 2)) - # s_innov_2_std = str(round(np.std(ekf2_innovations['vel_pos_innov[2]']),2)) - # generate metadata for position innovations - innov_5_max_arg = np.argmax(ekf2_innovations['vel_pos_innov[5]']) - innov_5_max_time = innov_time[innov_5_max_arg] - innov_5_max = np.amax(ekf2_innovations['vel_pos_innov[5]']) - innov_5_min_arg = np.argmin(ekf2_innovations['vel_pos_innov[5]']) - innov_5_min_time = innov_time[innov_5_min_arg] - innov_5_min = np.amin(ekf2_innovations['vel_pos_innov[5]']) - s_innov_5_max = str(round(innov_5_max, 2)) - s_innov_5_min = str(round(innov_5_min, 2)) - # s_innov_5_std = str(round(np.std(ekf2_innovations['vel_pos_innov[5]']),2)) - # generate plot for vertical velocity innovations - plt.subplot(2, 1, 1) - plt.plot(1e-6 * ekf2_innovations['timestamp'], ekf2_innovations['vel_pos_innov[2]'], 'b') - plt.plot(1e-6 * ekf2_innovations['timestamp'], np.sqrt(ekf2_innovations['vel_pos_innov_var[2]']), 'r') - plt.plot(1e-6 * ekf2_innovations['timestamp'], -np.sqrt(ekf2_innovations['vel_pos_innov_var[2]']), 'r') - plt.title('Vertical Innovations') - plt.ylabel('Down Vel (m/s)') - plt.xlabel('time (sec)') - plt.grid() - plt.text(innov_2_max_time, innov_2_max, 'max=' + s_innov_2_max, fontsize=12, horizontalalignment='left', - verticalalignment='bottom') - plt.text(innov_2_min_time, innov_2_min, 'min=' + s_innov_2_min, fontsize=12, horizontalalignment='left', - verticalalignment='top') - # plt.legend(['std='+s_innov_2_std],loc='upper left',frameon=False) - # generate plot for vertical position innovations - plt.subplot(2, 1, 2) - plt.plot(1e-6 * ekf2_innovations['timestamp'], ekf2_innovations['vel_pos_innov[5]'], 'b') - plt.plot(1e-6 * ekf2_innovations['timestamp'], np.sqrt(ekf2_innovations['vel_pos_innov_var[5]']), 'r') - plt.plot(1e-6 * ekf2_innovations['timestamp'], -np.sqrt(ekf2_innovations['vel_pos_innov_var[5]']), 'r') - plt.ylabel('Down Pos (m)') - plt.xlabel('time (sec)') - plt.grid() - plt.text(innov_5_max_time, innov_5_max, 'max=' + s_innov_5_max, fontsize=12, horizontalalignment='left', - verticalalignment='bottom') - plt.text(innov_5_min_time, innov_5_min, 'min=' + s_innov_5_min, fontsize=12, horizontalalignment='left', - verticalalignment='top') - # plt.legend(['std='+s_innov_5_std],loc='upper left',frameon=False) - pp.savefig() - plt.close(1) - # horizontal velocity innovations - plt.figure(2, figsize=(20, 13)) - # generate North axis metadata - innov_0_max_arg = np.argmax(ekf2_innovations['vel_pos_innov[0]']) - innov_0_max_time = innov_time[innov_0_max_arg] - innov_0_max = np.amax(ekf2_innovations['vel_pos_innov[0]']) - innov_0_min_arg = np.argmin(ekf2_innovations['vel_pos_innov[0]']) - innov_0_min_time = innov_time[innov_0_min_arg] - innov_0_min = np.amin(ekf2_innovations['vel_pos_innov[0]']) - s_innov_0_max = str(round(innov_0_max, 2)) - s_innov_0_min = str(round(innov_0_min, 2)) - # s_innov_0_std = str(round(np.std(ekf2_innovations['vel_pos_innov[0]']),2)) - # Generate East axis metadata - innov_1_max_arg = np.argmax(ekf2_innovations['vel_pos_innov[1]']) - innov_1_max_time = innov_time[innov_1_max_arg] - innov_1_max = np.amax(ekf2_innovations['vel_pos_innov[1]']) - innov_1_min_arg = np.argmin(ekf2_innovations['vel_pos_innov[1]']) - innov_1_min_time = innov_time[innov_1_min_arg] - innov_1_min = np.amin(ekf2_innovations['vel_pos_innov[1]']) - s_innov_1_max = str(round(innov_1_max, 2)) - s_innov_1_min = str(round(innov_1_min, 2)) - # s_innov_1_std = str(round(np.std(ekf2_innovations['vel_pos_innov[1]']),2)) - # draw plots - plt.subplot(2, 1, 1) - plt.plot(1e-6 * ekf2_innovations['timestamp'], ekf2_innovations['vel_pos_innov[0]'], 'b') - plt.plot(1e-6 * ekf2_innovations['timestamp'], np.sqrt(ekf2_innovations['vel_pos_innov_var[0]']), 'r') - plt.plot(1e-6 * ekf2_innovations['timestamp'], -np.sqrt(ekf2_innovations['vel_pos_innov_var[0]']), 'r') - plt.title('Horizontal Velocity Innovations') - plt.ylabel('North Vel (m/s)') - plt.xlabel('time (sec)') - plt.grid() - plt.text(innov_0_max_time, innov_0_max, 'max=' + s_innov_0_max, fontsize=12, horizontalalignment='left', - verticalalignment='bottom') - plt.text(innov_0_min_time, innov_0_min, 'min=' + s_innov_0_min, fontsize=12, horizontalalignment='left', - verticalalignment='top') - # plt.legend(['std='+s_innov_0_std],loc='upper left',frameon=False) - plt.subplot(2, 1, 2) - plt.plot(1e-6 * ekf2_innovations['timestamp'], ekf2_innovations['vel_pos_innov[1]'], 'b') - plt.plot(1e-6 * ekf2_innovations['timestamp'], np.sqrt(ekf2_innovations['vel_pos_innov_var[1]']), 'r') - plt.plot(1e-6 * ekf2_innovations['timestamp'], -np.sqrt(ekf2_innovations['vel_pos_innov_var[1]']), 'r') - plt.ylabel('East Vel (m/s)') - plt.xlabel('time (sec)') - plt.grid() - plt.text(innov_1_max_time, innov_1_max, 'max=' + s_innov_1_max, fontsize=12, horizontalalignment='left', - verticalalignment='bottom') - plt.text(innov_1_min_time, innov_1_min, 'min=' + s_innov_1_min, fontsize=12, horizontalalignment='left', - verticalalignment='top') - # plt.legend(['std='+s_innov_1_std],loc='upper left',frameon=False) - pp.savefig() - plt.close(2) - # horizontal position innovations - plt.figure(3, figsize=(20, 13)) - # generate North axis metadata - innov_3_max_arg = np.argmax(ekf2_innovations['vel_pos_innov[3]']) - innov_3_max_time = innov_time[innov_3_max_arg] - innov_3_max = np.amax(ekf2_innovations['vel_pos_innov[3]']) - innov_3_min_arg = np.argmin(ekf2_innovations['vel_pos_innov[3]']) - innov_3_min_time = innov_time[innov_3_min_arg] - innov_3_min = np.amin(ekf2_innovations['vel_pos_innov[3]']) - s_innov_3_max = str(round(innov_3_max, 2)) - s_innov_3_min = str(round(innov_3_min, 2)) - # s_innov_3_std = str(round(np.std(ekf2_innovations['vel_pos_innov[3]']),2)) - # generate East axis metadata - innov_4_max_arg = np.argmax(ekf2_innovations['vel_pos_innov[4]']) - innov_4_max_time = innov_time[innov_4_max_arg] - innov_4_max = np.amax(ekf2_innovations['vel_pos_innov[4]']) - innov_4_min_arg = np.argmin(ekf2_innovations['vel_pos_innov[4]']) - innov_4_min_time = innov_time[innov_4_min_arg] - innov_4_min = np.amin(ekf2_innovations['vel_pos_innov[4]']) - s_innov_4_max = str(round(innov_4_max, 2)) - s_innov_4_min = str(round(innov_4_min, 2)) - # s_innov_4_std = str(round(np.std(ekf2_innovations['vel_pos_innov[4]']),2)) - # generate plots - plt.subplot(2, 1, 1) - plt.plot(1e-6 * ekf2_innovations['timestamp'], ekf2_innovations['vel_pos_innov[3]'], 'b') - plt.plot(1e-6 * ekf2_innovations['timestamp'], np.sqrt(ekf2_innovations['vel_pos_innov_var[3]']), 'r') - plt.plot(1e-6 * ekf2_innovations['timestamp'], -np.sqrt(ekf2_innovations['vel_pos_innov_var[3]']), 'r') - plt.title('Horizontal Position Innovations') - plt.ylabel('North Pos (m)') - plt.xlabel('time (sec)') - plt.grid() - plt.text(innov_3_max_time, innov_3_max, 'max=' + s_innov_3_max, fontsize=12, horizontalalignment='left', - verticalalignment='bottom') - plt.text(innov_3_min_time, innov_3_min, 'min=' + s_innov_3_min, fontsize=12, horizontalalignment='left', - verticalalignment='top') - # plt.legend(['std='+s_innov_3_std],loc='upper left',frameon=False) - plt.subplot(2, 1, 2) - plt.plot(1e-6 * ekf2_innovations['timestamp'], ekf2_innovations['vel_pos_innov[4]'], 'b') - plt.plot(1e-6 * ekf2_innovations['timestamp'], np.sqrt(ekf2_innovations['vel_pos_innov_var[4]']), 'r') - plt.plot(1e-6 * ekf2_innovations['timestamp'], -np.sqrt(ekf2_innovations['vel_pos_innov_var[4]']), 'r') - plt.ylabel('East Pos (m)') - plt.xlabel('time (sec)') - plt.grid() - plt.text(innov_4_max_time, innov_4_max, 'max=' + s_innov_4_max, fontsize=12, horizontalalignment='left', - verticalalignment='bottom') - plt.text(innov_4_min_time, innov_4_min, 'min=' + s_innov_4_min, fontsize=12, horizontalalignment='left', - verticalalignment='top') - # plt.legend(['std='+s_innov_4_std],loc='upper left',frameon=False) - pp.savefig() - plt.close(3) - # manetometer innovations - plt.figure(4, figsize=(20, 13)) - # generate X axis metadata - innov_0_max_arg = np.argmax(ekf2_innovations['mag_innov[0]']) - innov_0_max_time = innov_time[innov_0_max_arg] - innov_0_max = np.amax(ekf2_innovations['mag_innov[0]']) - innov_0_min_arg = np.argmin(ekf2_innovations['mag_innov[0]']) - innov_0_min_time = innov_time[innov_0_min_arg] - innov_0_min = np.amin(ekf2_innovations['mag_innov[0]']) - s_innov_0_max = str(round(innov_0_max, 3)) - s_innov_0_min = str(round(innov_0_min, 3)) - # s_innov_0_std = str(round(np.std(ekf2_innovations['mag_innov[0]']),3)) - # generate Y axis metadata - innov_1_max_arg = np.argmax(ekf2_innovations['mag_innov[1]']) - innov_1_max_time = innov_time[innov_1_max_arg] - innov_1_max = np.amax(ekf2_innovations['mag_innov[1]']) - innov_1_min_arg = np.argmin(ekf2_innovations['mag_innov[1]']) - innov_1_min_time = innov_time[innov_1_min_arg] - innov_1_min = np.amin(ekf2_innovations['mag_innov[1]']) - s_innov_1_max = str(round(innov_1_max, 3)) - s_innov_1_min = str(round(innov_1_min, 3)) - # s_innov_1_std = str(round(np.std(ekf2_innovations['mag_innov[1]']),3)) - # generate Z axis metadata - innov_2_max_arg = np.argmax(ekf2_innovations['mag_innov[2]']) - innov_2_max_time = innov_time[innov_2_max_arg] - innov_2_max = np.amax(ekf2_innovations['mag_innov[2]']) - innov_2_min_arg = np.argmin(ekf2_innovations['mag_innov[2]']) - innov_2_min_time = innov_time[innov_2_min_arg] - innov_2_min = np.amin(ekf2_innovations['mag_innov[2]']) - s_innov_2_max = str(round(innov_2_max, 3)) - s_innov_2_min = str(round(innov_2_min, 3)) - # s_innov_2_std = str(round(np.std(ekf2_innovations['mag_innov[0]']),3)) - # draw plots - plt.subplot(3, 1, 1) - plt.plot(1e-6 * ekf2_innovations['timestamp'], ekf2_innovations['mag_innov[0]'], 'b') - plt.plot(1e-6 * ekf2_innovations['timestamp'], np.sqrt(ekf2_innovations['mag_innov_var[0]']), 'r') - plt.plot(1e-6 * ekf2_innovations['timestamp'], -np.sqrt(ekf2_innovations['mag_innov_var[0]']), 'r') - plt.title('Magnetometer Innovations') - plt.ylabel('X (Gauss)') - plt.xlabel('time (sec)') - plt.grid() - plt.text(innov_0_max_time, innov_0_max, 'max=' + s_innov_0_max, fontsize=12, horizontalalignment='left', - verticalalignment='bottom') - plt.text(innov_0_min_time, innov_0_min, 'min=' + s_innov_0_min, fontsize=12, horizontalalignment='left', - verticalalignment='top') - # plt.legend(['std='+s_innov_0_std],loc='upper left',frameon=False) - plt.subplot(3, 1, 2) - plt.plot(1e-6 * ekf2_innovations['timestamp'], ekf2_innovations['mag_innov[1]'], 'b') - plt.plot(1e-6 * ekf2_innovations['timestamp'], np.sqrt(ekf2_innovations['mag_innov_var[1]']), 'r') - plt.plot(1e-6 * ekf2_innovations['timestamp'], -np.sqrt(ekf2_innovations['mag_innov_var[1]']), 'r') - plt.ylabel('Y (Gauss)') - plt.xlabel('time (sec)') - plt.grid() - plt.text(innov_1_max_time, innov_1_max, 'max=' + s_innov_1_max, fontsize=12, horizontalalignment='left', - verticalalignment='bottom') - plt.text(innov_1_min_time, innov_1_min, 'min=' + s_innov_1_min, fontsize=12, horizontalalignment='left', - verticalalignment='top') - # plt.legend(['std='+s_innov_1_std],loc='upper left',frameon=False) - plt.subplot(3, 1, 3) - plt.plot(1e-6 * ekf2_innovations['timestamp'], ekf2_innovations['mag_innov[2]'], 'b') - plt.plot(1e-6 * ekf2_innovations['timestamp'], np.sqrt(ekf2_innovations['mag_innov_var[2]']), 'r') - plt.plot(1e-6 * ekf2_innovations['timestamp'], -np.sqrt(ekf2_innovations['mag_innov_var[2]']), 'r') - plt.ylabel('Z (Gauss)') - plt.xlabel('time (sec)') - plt.grid() - plt.text(innov_2_max_time, innov_2_max, 'max=' + s_innov_2_max, fontsize=12, horizontalalignment='left', - verticalalignment='bottom') - plt.text(innov_2_min_time, innov_2_min, 'min=' + s_innov_2_min, fontsize=12, horizontalalignment='left', - verticalalignment='top') - # plt.legend(['std='+s_innov_2_std],loc='upper left',frameon=False) - pp.savefig() - plt.close(4) - # magnetic heading innovations - plt.figure(5, figsize=(20, 13)) - # generate metadata - innov_0_max_arg = np.argmax(ekf2_innovations['heading_innov']) - innov_0_max_time = innov_time[innov_0_max_arg] - innov_0_max = np.amax(ekf2_innovations['heading_innov']) - innov_0_min_arg = np.argmin(ekf2_innovations['heading_innov']) - innov_0_min_time = innov_time[innov_0_min_arg] - innov_0_min = np.amin(ekf2_innovations['heading_innov']) - s_innov_0_max = str(round(innov_0_max, 3)) - s_innov_0_min = str(round(innov_0_min, 3)) - # s_innov_0_std = str(round(np.std(ekf2_innovations['heading_innov']),3)) - # draw plot - plt.plot(1e-6 * ekf2_innovations['timestamp'], ekf2_innovations['heading_innov'], 'b') - plt.plot(1e-6 * ekf2_innovations['timestamp'], np.sqrt(ekf2_innovations['heading_innov_var']), 'r') - plt.plot(1e-6 * ekf2_innovations['timestamp'], -np.sqrt(ekf2_innovations['heading_innov_var']), 'r') - plt.title('Magnetic Heading Innovations') - plt.ylabel('Heaing (rad)') - plt.xlabel('time (sec)') - plt.grid() - plt.text(innov_0_max_time, innov_0_max, 'max=' + s_innov_0_max, fontsize=12, horizontalalignment='left', - verticalalignment='bottom') - plt.text(innov_0_min_time, innov_0_min, 'min=' + s_innov_0_min, fontsize=12, horizontalalignment='left', - verticalalignment='top') - # plt.legend(['std='+s_innov_0_std],loc='upper left',frameon=False) - pp.savefig() - plt.close(5) - # air data innovations - plt.figure(6, figsize=(20, 13)) - # generate airspeed metadata - airspeed_innov_max_arg = np.argmax(ekf2_innovations['airspeed_innov']) - airspeed_innov_max_time = innov_time[airspeed_innov_max_arg] - airspeed_innov_max = np.amax(ekf2_innovations['airspeed_innov']) - airspeed_innov_min_arg = np.argmin(ekf2_innovations['airspeed_innov']) - airspeed_innov_min_time = innov_time[airspeed_innov_min_arg] - airspeed_innov_min = np.amin(ekf2_innovations['airspeed_innov']) - s_airspeed_innov_max = str(round(airspeed_innov_max, 3)) - s_airspeed_innov_min = str(round(airspeed_innov_min, 3)) - # generate sideslip metadata - beta_innov_max_arg = np.argmax(ekf2_innovations['beta_innov']) - beta_innov_max_time = innov_time[beta_innov_max_arg] - beta_innov_max = np.amax(ekf2_innovations['beta_innov']) - beta_innov_min_arg = np.argmin(ekf2_innovations['beta_innov']) - beta_innov_min_time = innov_time[beta_innov_min_arg] - beta_innov_min = np.amin(ekf2_innovations['beta_innov']) - s_beta_innov_max = str(round(beta_innov_max, 3)) - s_beta_innov_min = str(round(beta_innov_min, 3)) - # draw plots - plt.subplot(2, 1, 1) - plt.plot(1e-6 * ekf2_innovations['timestamp'], ekf2_innovations['airspeed_innov'], 'b') - plt.plot(1e-6 * ekf2_innovations['timestamp'], np.sqrt(ekf2_innovations['airspeed_innov_var']), 'r') - plt.plot(1e-6 * ekf2_innovations['timestamp'], -np.sqrt(ekf2_innovations['airspeed_innov_var']), 'r') - plt.title('True Airspeed Innovations') - plt.ylabel('innovation (m/sec)') - plt.xlabel('time (sec)') - plt.grid() - plt.text(airspeed_innov_max_time, airspeed_innov_max, 'max=' + s_airspeed_innov_max, fontsize=12, - horizontalalignment='left', verticalalignment='bottom') - plt.text(airspeed_innov_min_time, airspeed_innov_min, 'min=' + s_airspeed_innov_min, fontsize=12, - horizontalalignment='left', verticalalignment='top') - plt.subplot(2, 1, 2) - plt.plot(1e-6 * ekf2_innovations['timestamp'], ekf2_innovations['beta_innov'], 'b') - plt.plot(1e-6 * ekf2_innovations['timestamp'], np.sqrt(ekf2_innovations['beta_innov_var']), 'r') - plt.plot(1e-6 * ekf2_innovations['timestamp'], -np.sqrt(ekf2_innovations['beta_innov_var']), 'r') - plt.title('Synthetic Sideslip Innovations') - plt.ylabel('innovation (rad)') - plt.xlabel('time (sec)') - plt.grid() - plt.text(beta_innov_max_time, beta_innov_max, 'max=' + s_beta_innov_max, fontsize=12, horizontalalignment='left', - verticalalignment='bottom') - plt.text(beta_innov_min_time, beta_innov_min, 'min=' + s_beta_innov_min, fontsize=12, horizontalalignment='left', - verticalalignment='top') - pp.savefig() - plt.close(6) - # optical flow innovations - plt.figure(7, figsize=(20, 13)) - # generate X axis metadata - flow_innov_x_max_arg = np.argmax(ekf2_innovations['flow_innov[0]']) - flow_innov_x_max_time = innov_time[flow_innov_x_max_arg] - flow_innov_x_max = np.amax(ekf2_innovations['flow_innov[0]']) - flow_innov_x_min_arg = np.argmin(ekf2_innovations['flow_innov[0]']) - flow_innov_x_min_time = innov_time[flow_innov_x_min_arg] - flow_innov_x_min = np.amin(ekf2_innovations['flow_innov[0]']) - s_flow_innov_x_max = str(round(flow_innov_x_max, 3)) - s_flow_innov_x_min = str(round(flow_innov_x_min, 3)) - # s_flow_innov_x_std = str(round(np.std(ekf2_innovations['flow_innov[0]']),3)) - # generate Y axis metadata - flow_innov_y_max_arg = np.argmax(ekf2_innovations['flow_innov[1]']) - flow_innov_y_max_time = innov_time[flow_innov_y_max_arg] - flow_innov_y_max = np.amax(ekf2_innovations['flow_innov[1]']) - flow_innov_y_min_arg = np.argmin(ekf2_innovations['flow_innov[1]']) - flow_innov_y_min_time = innov_time[flow_innov_y_min_arg] - flow_innov_y_min = np.amin(ekf2_innovations['flow_innov[1]']) - s_flow_innov_y_max = str(round(flow_innov_y_max, 3)) - s_flow_innov_y_min = str(round(flow_innov_y_min, 3)) - # s_flow_innov_y_std = str(round(np.std(ekf2_innovations['flow_innov[1]']),3)) - # draw plots - plt.subplot(2, 1, 1) - plt.plot(1e-6 * ekf2_innovations['timestamp'], ekf2_innovations['flow_innov[0]'], 'b') - plt.plot(1e-6 * ekf2_innovations['timestamp'], np.sqrt(ekf2_innovations['flow_innov_var[0]']), 'r') - plt.plot(1e-6 * ekf2_innovations['timestamp'], -np.sqrt(ekf2_innovations['flow_innov_var[0]']), 'r') - plt.title('Optical Flow Innovations') - plt.ylabel('X (rad/sec)') - plt.xlabel('time (sec)') - plt.grid() - plt.text(flow_innov_x_max_time, flow_innov_x_max, 'max=' + s_flow_innov_x_max, fontsize=12, - horizontalalignment='left', verticalalignment='bottom') - plt.text(flow_innov_x_min_time, flow_innov_x_min, 'min=' + s_flow_innov_x_min, fontsize=12, - horizontalalignment='left', verticalalignment='top') - # plt.legend(['std='+s_flow_innov_x_std],loc='upper left',frameon=False) - plt.subplot(2, 1, 2) - plt.plot(1e-6 * ekf2_innovations['timestamp'], ekf2_innovations['flow_innov[1]'], 'b') - plt.plot(1e-6 * ekf2_innovations['timestamp'], np.sqrt(ekf2_innovations['flow_innov_var[1]']), 'r') - plt.plot(1e-6 * ekf2_innovations['timestamp'], -np.sqrt(ekf2_innovations['flow_innov_var[1]']), 'r') - plt.ylabel('Y (rad/sec)') - plt.xlabel('time (sec)') - plt.grid() - plt.text(flow_innov_y_max_time, flow_innov_y_max, 'max=' + s_flow_innov_y_max, fontsize=12, - horizontalalignment='left', verticalalignment='bottom') - plt.text(flow_innov_y_min_time, flow_innov_y_min, 'min=' + s_flow_innov_y_min, fontsize=12, - horizontalalignment='left', verticalalignment='top') - # plt.legend(['std='+s_flow_innov_y_std],loc='upper left',frameon=False) - pp.savefig() - plt.close(7) + if in_air_no_ground_effects.take_off is None: + raise PreconditionError('no airtime detected.') - # generate metadata for the normalised innovation consistency test levels + airtime_info = { + 'in_air_transition_time': round(in_air.take_off + in_air.log_start, 2), + 'on_ground_transition_time': round(in_air.landing + in_air.log_start, 2)} + + control_mode, innov_flags, gps_fail_flags = get_estimator_check_flags(estimator_status) + + sensor_checks, innov_fail_checks = find_checks_that_apply( + control_mode, estimator_status, + pos_checks_when_sensors_not_fused=pos_checks_when_sensors_not_fused) + + metrics = calculate_ecl_ekf_metrics( + ulog, innov_flags, innov_fail_checks, sensor_checks, in_air, in_air_no_ground_effects, + red_thresh=red_thresh, amb_thresh=amb_thresh) + + check_status, master_status = perform_ecl_ekf_checks( + metrics, sensor_checks, innov_fail_checks, check_levels) + + return master_status, check_status, metrics, airtime_info + + +def find_checks_that_apply( + control_mode: dict, estimator_status: dict, pos_checks_when_sensors_not_fused: bool = False) ->\ + Tuple[List[str], List[str]]: + """ + finds the checks that apply and stores them in lists for the std checks and the innovation + fail checks. + :param control_mode: + :param estimator_status: + :param b_pos_only_when_sensors_fused: + :return: a tuple of two lists that contain strings for the std checks and for the innovation + fail checks. + """ + sensor_checks = list() + innov_fail_checks = list() + + # Height Sensor Checks + sensor_checks.append('hgt') + innov_fail_checks.append('posv') + + # Magnetometer Sensor Checks + if (np.amax(control_mode['yaw_aligned']) > 0.5): + sensor_checks.append('mag') + + innov_fail_checks.append('magx') + innov_fail_checks.append('magy') + innov_fail_checks.append('magz') + innov_fail_checks.append('yaw') + + # Velocity Sensor Checks + if (np.amax(control_mode['using_gps']) > 0.5): + sensor_checks.append('vel') + innov_fail_checks.append('vel') + + # Position Sensor Checks + if (pos_checks_when_sensors_not_fused or (np.amax(control_mode['using_gps']) > 0.5) + or (np.amax(control_mode['using_evpos']) > 0.5)): + sensor_checks.append('pos') + innov_fail_checks.append('posh') + + # Airspeed Sensor Checks # a value > 1.0 means the measurement data for that test has been rejected by the EKF - # magnetometer data - mag_test_max_arg = np.argmax(estimator_status['mag_test_ratio']) - mag_test_max_time = status_time[mag_test_max_arg] - mag_test_max = np.amax(estimator_status['mag_test_ratio']) - mag_test_mean = np.mean(estimator_status['mag_test_ratio']) - # velocity data (GPS) - vel_test_max_arg = np.argmax(estimator_status['vel_test_ratio']) - vel_test_max_time = status_time[vel_test_max_arg] - vel_test_max = np.amax(estimator_status['vel_test_ratio']) - vel_test_mean = np.mean(estimator_status['vel_test_ratio']) - # horizontal position data (GPS or external vision) - pos_test_max_arg = np.argmax(estimator_status['pos_test_ratio']) - pos_test_max_time = status_time[pos_test_max_arg] - pos_test_max = np.amax(estimator_status['pos_test_ratio']) - pos_test_mean = np.mean(estimator_status['pos_test_ratio']) - # height data (Barometer, GPS or rangefinder) - hgt_test_max_arg = np.argmax(estimator_status['hgt_test_ratio']) - hgt_test_max_time = status_time[hgt_test_max_arg] - hgt_test_max = np.amax(estimator_status['hgt_test_ratio']) - hgt_test_mean = np.mean(estimator_status['hgt_test_ratio']) - # airspeed data - tas_test_max_arg = np.argmax(estimator_status['tas_test_ratio']) - tas_test_max_time = status_time[tas_test_max_arg] - tas_test_max = np.amax(estimator_status['tas_test_ratio']) - tas_test_mean = np.mean(estimator_status['tas_test_ratio']) - # height above ground data (rangefinder) - hagl_test_max_arg = np.argmax(estimator_status['hagl_test_ratio']) - hagl_test_max_time = status_time[hagl_test_max_arg] - hagl_test_max = np.amax(estimator_status['hagl_test_ratio']) - hagl_test_mean = np.mean(estimator_status['hagl_test_ratio']) + if (np.amax(estimator_status['tas_test_ratio']) > 0.0): + sensor_checks.append('tas') + innov_fail_checks.append('tas') - if plot: - # plot normalised innovation test levels - plt.figure(8, figsize=(20, 13)) - if tas_test_max == 0.0: - n_plots = 3 - else: - n_plots = 4 - plt.subplot(n_plots, 1, 1) - plt.plot(status_time, estimator_status['mag_test_ratio'], 'b') - plt.title('Normalised Innovation Test Levels') - plt.ylabel('mag') - plt.xlabel('time (sec)') - plt.grid() - plt.text(mag_test_max_time, mag_test_max, - 'max=' + str(round(mag_test_max, 2)) + ' , mean=' + str(round(mag_test_mean, 2)), fontsize=12, - horizontalalignment='left', verticalalignment='bottom', color='b') - plt.subplot(n_plots, 1, 2) - plt.plot(status_time, estimator_status['vel_test_ratio'], 'b') - plt.plot(status_time, estimator_status['pos_test_ratio'], 'r') - plt.ylabel('vel,pos') - plt.xlabel('time (sec)') - plt.grid() - plt.text(vel_test_max_time, vel_test_max, - 'vel max=' + str(round(vel_test_max, 2)) + ' , mean=' + str(round(vel_test_mean, 2)), fontsize=12, - horizontalalignment='left', verticalalignment='bottom', color='b') - plt.text(pos_test_max_time, pos_test_max, - 'pos max=' + str(round(pos_test_max, 2)) + ' , mean=' + str(round(pos_test_mean, 2)), fontsize=12, - horizontalalignment='left', verticalalignment='bottom', color='r') - plt.subplot(n_plots, 1, 3) - plt.plot(status_time, estimator_status['hgt_test_ratio'], 'b') - plt.ylabel('hgt') - plt.xlabel('time (sec)') - plt.grid() - plt.text(hgt_test_max_time, hgt_test_max, - 'hgt max=' + str(round(hgt_test_max, 2)) + ' , mean=' + str(round(hgt_test_mean, 2)), fontsize=12, - horizontalalignment='left', verticalalignment='bottom', color='b') - if hagl_test_max > 0.0: - plt.plot(status_time, estimator_status['hagl_test_ratio'], 'r') - plt.text(hagl_test_max_time, hagl_test_max, - 'hagl max=' + str(round(hagl_test_max, 2)) + ' , mean=' + str(round(hagl_test_mean, 2)), fontsize=12, - horizontalalignment='left', verticalalignment='bottom', color='r') - plt.ylabel('hgt,HAGL') - if n_plots == 4: - plt.subplot(n_plots, 1, 4) - plt.plot(status_time, estimator_status['tas_test_ratio'], 'b') - plt.ylabel('TAS') - plt.xlabel('time (sec)') - plt.grid() - plt.text(tas_test_max_time, tas_test_max, - 'max=' + str(round(tas_test_max, 2)) + ' , mean=' + str(round(tas_test_mean, 2)), fontsize=12, - horizontalalignment='left', verticalalignment='bottom', color='b') - pp.savefig() - plt.close(8) + # Height above ground (rangefinder) sensor checks + if (np.amax(estimator_status['hagl_test_ratio']) > 0.0): + sensor_checks.append('hagl') + innov_fail_checks.append('hagl') - # extract control mode metadata from estimator_status.control_mode_flags - # 0 - true if the filter tilt alignment is complete - # 1 - true if the filter yaw alignment is complete - # 2 - true if GPS measurements are being fused - # 3 - true if optical flow measurements are being fused - # 4 - true if a simple magnetic yaw heading is being fused - # 5 - true if 3-axis magnetometer measurement are being fused - # 6 - true if synthetic magnetic declination measurements are being fused - # 7 - true when the vehicle is airborne - # 8 - true when wind velocity is being estimated - # 9 - true when baro height is being fused as a primary height reference - # 10 - true when range finder height is being fused as a primary height reference - # 11 - true when range finder height is being fused as a primary height reference - # 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 - tilt_aligned = ((2 ** 0 & estimator_status['control_mode_flags']) > 0) * 1 - yaw_aligned = ((2 ** 1 & estimator_status['control_mode_flags']) > 0) * 1 - using_gps = ((2 ** 2 & estimator_status['control_mode_flags']) > 0) * 1 - using_optflow = ((2 ** 3 & estimator_status['control_mode_flags']) > 0) * 1 - using_magyaw = ((2 ** 4 & estimator_status['control_mode_flags']) > 0) * 1 - using_mag3d = ((2 ** 5 & estimator_status['control_mode_flags']) > 0) * 1 - using_magdecl = ((2 ** 6 & estimator_status['control_mode_flags']) > 0) * 1 - airborne = ((2 ** 7 & estimator_status['control_mode_flags']) > 0) * 1 - estimating_wind = ((2 ** 8 & estimator_status['control_mode_flags']) > 0) * 1 - using_barohgt = ((2 ** 9 & estimator_status['control_mode_flags']) > 0) * 1 - using_rnghgt = ((2 ** 10 & estimator_status['control_mode_flags']) > 0) * 1 - using_gpshgt = ((2 ** 11 & estimator_status['control_mode_flags']) > 0) * 1 - using_evpos = ((2 ** 12 & estimator_status['control_mode_flags']) > 0) * 1 - using_evyaw = ((2 ** 13 & estimator_status['control_mode_flags']) > 0) * 1 - using_evhgt = ((2 ** 14 & estimator_status['control_mode_flags']) > 0) * 1 + # optical flow sensor checks + if (np.amax(control_mode['using_optflow']) > 0.5): + innov_fail_checks.append('ofx') + innov_fail_checks.append('ofy') - # define flags for starting and finishing in air - b_starts_in_air = False - b_finishes_in_air = False + return sensor_checks, innov_fail_checks - # calculate in-air transition time - if (np.amin(airborne) < 0.5) and (np.amax(airborne) > 0.5): - in_air_transtion_time_arg = np.argmax(np.diff(airborne)) - in_air_transition_time = status_time[in_air_transtion_time_arg] - elif (np.amax(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(airborne)) < 0.0): - on_ground_transition_time_arg = np.argmin(np.diff(airborne)) - on_ground_transition_time = status_time[on_ground_transition_time_arg] - elif (np.amax(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(airborne)) > 0.5) and (np.amin(np.diff(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') - # calculate alignment completion times - tilt_align_time_arg = np.argmax(np.diff(tilt_aligned)) - tilt_align_time = status_time[tilt_align_time_arg] - yaw_align_time_arg = np.argmax(np.diff(yaw_aligned)) - yaw_align_time = status_time[yaw_align_time_arg] - # calculate position aiding start times - gps_aid_time_arg = np.argmax(np.diff(using_gps)) - gps_aid_time = status_time[gps_aid_time_arg] - optflow_aid_time_arg = np.argmax(np.diff(using_optflow)) - optflow_aid_time = status_time[optflow_aid_time_arg] - evpos_aid_time_arg = np.argmax(np.diff(using_evpos)) - evpos_aid_time = status_time[evpos_aid_time_arg] - # calculate height aiding start times - barohgt_aid_time_arg = np.argmax(np.diff(using_barohgt)) - barohgt_aid_time = status_time[barohgt_aid_time_arg] - gpshgt_aid_time_arg = np.argmax(np.diff(using_gpshgt)) - gpshgt_aid_time = status_time[gpshgt_aid_time_arg] - rnghgt_aid_time_arg = np.argmax(np.diff(using_rnghgt)) - rnghgt_aid_time = status_time[rnghgt_aid_time_arg] - evhgt_aid_time_arg = np.argmax(np.diff(using_evhgt)) - evhgt_aid_time = status_time[evhgt_aid_time_arg] - # calculate magnetometer aiding start times - using_magyaw_time_arg = np.argmax(np.diff(using_magyaw)) - using_magyaw_time = status_time[using_magyaw_time_arg] - using_mag3d_time_arg = np.argmax(np.diff(using_mag3d)) - using_mag3d_time = status_time[using_mag3d_time_arg] - using_magdecl_time_arg = np.argmax(np.diff(using_magdecl)) - using_magdecl_time = status_time[using_magdecl_time_arg] - if plot: - # control mode summary plot A - plt.figure(9, figsize=(20, 13)) - # subplot for alignment completion - plt.subplot(4, 1, 1) - plt.title('EKF Control Status - Figure A') - plt.plot(status_time, tilt_aligned, 'b') - plt.plot(status_time, yaw_aligned, 'r') - plt.ylim(-0.1, 1.1) - plt.ylabel('aligned') - plt.grid() - if np.amin(tilt_aligned) > 0: - plt.text(tilt_align_time, 0.5, 'no pre-arm data - cannot calculate alignment completion times', fontsize=12, - horizontalalignment='left', verticalalignment='center', color='black') - else: - plt.text(tilt_align_time, 0.33, 'tilt alignment at ' + str(round(tilt_align_time, 1)) + ' sec', fontsize=12, - horizontalalignment='left', verticalalignment='center', color='b') - plt.text(yaw_align_time, 0.67, 'yaw alignment at ' + str(round(tilt_align_time, 1)) + ' sec', fontsize=12, - horizontalalignment='left', verticalalignment='center', color='r') - # subplot for position aiding - plt.subplot(4, 1, 2) - plt.plot(status_time, using_gps, 'b') - plt.plot(status_time, using_optflow, 'r') - plt.plot(status_time, using_evpos, 'g') - plt.ylim(-0.1, 1.1) - plt.ylabel('pos aiding') - plt.grid() - if np.amin(using_gps) > 0: - plt.text(gps_aid_time, 0.25, 'no pre-arm data - cannot calculate GPS aiding start time', fontsize=12, - horizontalalignment='left', verticalalignment='center', color='b') - elif np.amax(using_gps) > 0: - plt.text(gps_aid_time, 0.25, 'GPS aiding at ' + str(round(gps_aid_time, 1)) + ' sec', fontsize=12, - horizontalalignment='left', verticalalignment='center', color='b') - if np.amin(using_optflow) > 0: - plt.text(optflow_aid_time, 0.50, 'no pre-arm data - cannot calculate optical flow aiding start time', - fontsize=12, horizontalalignment='left', verticalalignment='center', color='r') - elif np.amax(using_optflow) > 0: - plt.text(optflow_aid_time, 0.50, 'optical flow aiding at ' + str(round(optflow_aid_time, 1)) + ' sec', - fontsize=12, horizontalalignment='left', verticalalignment='center', color='r') - if np.amin(using_evpos) > 0: - plt.text(evpos_aid_time, 0.75, 'no pre-arm data - cannot calculate external vision aiding start time', - fontsize=12, horizontalalignment='left', verticalalignment='center', color='g') - elif np.amax(using_evpos) > 0: - plt.text(evpos_aid_time, 0.75, 'external vision aiding at ' + str(round(evpos_aid_time, 1)) + ' sec', - fontsize=12, horizontalalignment='left', verticalalignment='center', color='g') - # subplot for height aiding - plt.subplot(4, 1, 3) - plt.plot(status_time, using_barohgt, 'b') - plt.plot(status_time, using_gpshgt, 'r') - plt.plot(status_time, using_rnghgt, 'g') - plt.plot(status_time, using_evhgt, 'c') - plt.ylim(-0.1, 1.1) - plt.ylabel('hgt aiding') - plt.grid() - if np.amin(using_barohgt) > 0: - plt.text(barohgt_aid_time, 0.2, 'no pre-arm data - cannot calculate Baro aiding start time', fontsize=12, - horizontalalignment='left', verticalalignment='center', color='b') - elif np.amax(using_barohgt) > 0: - plt.text(barohgt_aid_time, 0.2, 'Baro aiding at ' + str(round(gps_aid_time, 1)) + ' sec', fontsize=12, - horizontalalignment='left', verticalalignment='center', color='b') - if np.amin(using_gpshgt) > 0: - plt.text(gpshgt_aid_time, 0.4, 'no pre-arm data - cannot calculate GPS aiding start time', fontsize=12, - horizontalalignment='left', verticalalignment='center', color='r') - elif np.amax(using_gpshgt) > 0: - plt.text(gpshgt_aid_time, 0.4, 'GPS aiding at ' + str(round(gpshgt_aid_time, 1)) + ' sec', fontsize=12, - horizontalalignment='left', verticalalignment='center', color='r') - if np.amin(using_rnghgt) > 0: - plt.text(rnghgt_aid_time, 0.6, 'no pre-arm data - cannot calculate rangfinder aiding start time', fontsize=12, - horizontalalignment='left', verticalalignment='center', color='g') - elif np.amax(using_rnghgt) > 0: - plt.text(rnghgt_aid_time, 0.6, 'rangefinder aiding at ' + str(round(rnghgt_aid_time, 1)) + ' sec', fontsize=12, - horizontalalignment='left', verticalalignment='center', color='g') - if np.amin(using_evhgt) > 0: - plt.text(evhgt_aid_time, 0.8, 'no pre-arm data - cannot calculate external vision aiding start time', - fontsize=12, horizontalalignment='left', verticalalignment='center', color='c') - elif np.amax(using_evhgt) > 0: - plt.text(evhgt_aid_time, 0.8, 'external vision aiding at ' + str(round(evhgt_aid_time, 1)) + ' sec', - fontsize=12, horizontalalignment='left', verticalalignment='center', color='c') - # subplot for magnetometer aiding - plt.subplot(4, 1, 4) - plt.plot(status_time, using_magyaw, 'b') - plt.plot(status_time, using_mag3d, 'r') - plt.plot(status_time, using_magdecl, 'g') - plt.ylim(-0.1, 1.1) - plt.ylabel('mag aiding') - plt.xlabel('time (sec)') - plt.grid() - if np.amin(using_magyaw) > 0: - plt.text(using_magyaw_time, 0.25, 'no pre-arm data - cannot calculate magnetic yaw aiding start time', - fontsize=12, horizontalalignment='left', verticalalignment='center', color='b') - elif np.amax(using_magyaw) > 0: - plt.text(using_magyaw_time, 0.25, 'magnetic yaw aiding at ' + str(round(using_magyaw_time, 1)) + ' sec', - fontsize=12, horizontalalignment='right', verticalalignment='center', color='b') - if np.amin(using_mag3d) > 0: - plt.text(using_mag3d_time, 0.50, 'no pre-arm data - cannot calculate 3D magnetoemter aiding start time', - fontsize=12, horizontalalignment='left', verticalalignment='center', color='r') - elif np.amax(using_mag3d) > 0: - plt.text(using_mag3d_time, 0.50, 'magnetometer 3D aiding at ' + str(round(using_mag3d_time, 1)) + ' sec', - fontsize=12, horizontalalignment='left', verticalalignment='center', color='r') - if np.amin(using_magdecl) > 0: - plt.text(using_magdecl_time, 0.75, 'no pre-arm data - cannot magnetic declination aiding start time', - fontsize=12, horizontalalignment='left', verticalalignment='center', color='g') - elif np.amax(using_magdecl) > 0: - plt.text(using_magdecl_time, 0.75, - 'magnetic declination aiding at ' + str(round(using_magdecl_time, 1)) + ' sec', fontsize=12, - horizontalalignment='left', verticalalignment='center', color='g') - pp.savefig() - plt.close(9) - # control mode summary plot B - plt.figure(10, figsize=(20, 13)) - # subplot for airborne status - plt.subplot(2, 1, 1) - plt.title('EKF Control Status - Figure B') - plt.plot(status_time, airborne, 'b') - plt.ylim(-0.1, 1.1) - plt.ylabel('airborne') - plt.grid() - if np.amax(np.diff(airborne)) < 0.5: - plt.text(in_air_transition_time, 0.67, 'ground to air transition not detected', fontsize=12, - horizontalalignment='left', verticalalignment='center', color='b') - else: - plt.text(in_air_transition_time, 0.67, 'in-air at ' + str(round(in_air_transition_time, 1)) + ' sec', - fontsize=12, horizontalalignment='left', verticalalignment='center', color='b') - if np.amin(np.diff(airborne)) > -0.5: - plt.text(on_ground_transition_time, 0.33, 'air to ground transition not detected', fontsize=12, - horizontalalignment='left', verticalalignment='center', color='b') - else: - plt.text(on_ground_transition_time, 0.33, 'on-ground at ' + str(round(on_ground_transition_time, 1)) + ' sec', - fontsize=12, horizontalalignment='right', verticalalignment='center', color='b') - if in_air_duration > 0.0: - plt.text((in_air_transition_time + on_ground_transition_time) / 2, 0.5, - 'duration = ' + str(round(in_air_duration, 1)) + ' sec', fontsize=12, horizontalalignment='center', - verticalalignment='center', color='b') - # subplot for wind estimation status - plt.subplot(2, 1, 2) - plt.plot(status_time, estimating_wind, 'b') - plt.ylim(-0.1, 1.1) - plt.ylabel('estimating wind') - plt.xlabel('time (sec)') - plt.grid() - pp.savefig() - plt.close(10) - # innovation_check_flags summary - # 0 - true if velocity observations have been rejected - # 1 - true if horizontal position observations have been rejected - # 2 - true if true if vertical position observations have been rejected - # 3 - true if the X magnetometer observation has been rejected - # 4 - true if the Y magnetometer observation has been rejected - # 5 - true if the Z magnetometer observation has been rejected - # 6 - true if the yaw observation has been rejected - # 7 - true if the airspeed observation has been rejected - # 8 - true if synthetic sideslip observation has been rejected - # 9 - true if the height above ground observation has been rejected - # 10 - true if the X optical flow observation has been rejected - # 11 - true if the Y optical flow observation has been rejected - vel_innov_fail = ((2 ** 0 & estimator_status['innovation_check_flags']) > 0) * 1 - posh_innov_fail = ((2 ** 1 & estimator_status['innovation_check_flags']) > 0) * 1 - posv_innov_fail = ((2 ** 2 & estimator_status['innovation_check_flags']) > 0) * 1 - magx_innov_fail = ((2 ** 3 & estimator_status['innovation_check_flags']) > 0) * 1 - magy_innov_fail = ((2 ** 4 & estimator_status['innovation_check_flags']) > 0) * 1 - magz_innov_fail = ((2 ** 5 & estimator_status['innovation_check_flags']) > 0) * 1 - yaw_innov_fail = ((2 ** 6 & estimator_status['innovation_check_flags']) > 0) * 1 - tas_innov_fail = ((2 ** 7 & estimator_status['innovation_check_flags']) > 0) * 1 - sli_innov_fail = ((2 ** 8 & estimator_status['innovation_check_flags']) > 0) * 1 - hagl_innov_fail = ((2 ** 9 & estimator_status['innovation_check_flags']) > 0) * 1 - ofx_innov_fail = ((2 ** 10 & estimator_status['innovation_check_flags']) > 0) * 1 - ofy_innov_fail = ((2 ** 11 & estimator_status['innovation_check_flags']) > 0) * 1 - if plot: - # plot innovation_check_flags summary - plt.figure(11, figsize=(20, 13)) - plt.subplot(6, 1, 1) - plt.title('EKF Innovation Test Fails') - plt.plot(status_time, vel_innov_fail, 'b', label='vel NED') - plt.plot(status_time, posh_innov_fail, 'r', label='pos NE') - plt.ylim(-0.1, 1.1) - plt.ylabel('failed') - plt.legend(loc='upper left') - plt.grid() - plt.subplot(6, 1, 2) - plt.plot(status_time, posv_innov_fail, 'b', label='hgt absolute') - plt.plot(status_time, hagl_innov_fail, 'r', label='hgt above ground') - plt.ylim(-0.1, 1.1) - plt.ylabel('failed') - plt.legend(loc='upper left') - plt.grid() - plt.subplot(6, 1, 3) - plt.plot(status_time, magx_innov_fail, 'b', label='mag_x') - plt.plot(status_time, magy_innov_fail, 'r', label='mag_y') - plt.plot(status_time, magz_innov_fail, 'g', label='mag_z') - plt.plot(status_time, yaw_innov_fail, 'c', label='yaw') - plt.legend(loc='upper left') - plt.ylim(-0.1, 1.1) - plt.ylabel('failed') - plt.grid() - plt.subplot(6, 1, 4) - plt.plot(status_time, tas_innov_fail, 'b', label='airspeed') - plt.ylim(-0.1, 1.1) - plt.ylabel('failed') - plt.legend(loc='upper left') - plt.grid() - plt.subplot(6, 1, 5) - plt.plot(status_time, sli_innov_fail, 'b', label='sideslip') - plt.ylim(-0.1, 1.1) - plt.ylabel('failed') - plt.legend(loc='upper left') - plt.grid() - plt.subplot(6, 1, 6) - plt.plot(status_time, ofx_innov_fail, 'b', label='flow X') - plt.plot(status_time, ofy_innov_fail, 'r', label='flow Y') - plt.ylim(-0.1, 1.1) - plt.ylabel('failed') - plt.xlabel('time (sec') - plt.legend(loc='upper left') - plt.grid() - pp.savefig() - plt.close(11) - # gps_check_fail_flags summary - plt.figure(12, figsize=(20, 13)) - # 0 : insufficient fix type (no 3D solution) - # 1 : minimum required sat count fail - # 2 : minimum required GDoP fail - # 3 : maximum allowed horizontal position error fail - # 4 : maximum allowed vertical position error fail - # 5 : maximum allowed speed error fail - # 6 : maximum allowed horizontal position drift fail - # 7 : maximum allowed vertical position drift fail - # 8 : maximum allowed horizontal speed fail - # 9 : maximum allowed vertical velocity discrepancy fail - gfix_fail = ((2 ** 0 & estimator_status['gps_check_fail_flags']) > 0) * 1 - nsat_fail = ((2 ** 1 & estimator_status['gps_check_fail_flags']) > 0) * 1 - gdop_fail = ((2 ** 2 & estimator_status['gps_check_fail_flags']) > 0) * 1 - herr_fail = ((2 ** 3 & estimator_status['gps_check_fail_flags']) > 0) * 1 - verr_fail = ((2 ** 4 & estimator_status['gps_check_fail_flags']) > 0) * 1 - serr_fail = ((2 ** 5 & estimator_status['gps_check_fail_flags']) > 0) * 1 - hdrift_fail = ((2 ** 6 & estimator_status['gps_check_fail_flags']) > 0) * 1 - vdrift_fail = ((2 ** 7 & estimator_status['gps_check_fail_flags']) > 0) * 1 - hspd_fail = ((2 ** 8 & estimator_status['gps_check_fail_flags']) > 0) * 1 - veld_diff_fail = ((2 ** 9 & estimator_status['gps_check_fail_flags']) > 0) * 1 - plt.subplot(2, 1, 1) - plt.title('GPS Direct Output Check Failures') - plt.plot(status_time, gfix_fail, 'k', label='fix type') - plt.plot(status_time, nsat_fail, 'b', label='N sats') - plt.plot(status_time, gdop_fail, 'r', label='GDOP') - plt.plot(status_time, herr_fail, 'g', label='horiz pos error') - plt.plot(status_time, verr_fail, 'c', label='vert pos error') - plt.plot(status_time, serr_fail, 'm', label='speed error') - plt.ylim(-0.1, 1.1) - plt.ylabel('failed') - plt.legend(loc='upper right') - plt.grid() - plt.subplot(2, 1, 2) - plt.title('GPS Derived Output Check Failures') - plt.plot(status_time, hdrift_fail, 'b', label='horiz drift') - plt.plot(status_time, vdrift_fail, 'r', label='vert drift') - plt.plot(status_time, hspd_fail, 'g', label='horiz speed') - plt.plot(status_time, veld_diff_fail, 'c', label='vert vel inconsistent') - plt.ylim(-0.1, 1.1) - plt.ylabel('failed') - plt.xlabel('time (sec') - plt.legend(loc='upper right') - plt.grid() - pp.savefig() - plt.close(12) - # filter reported accuracy - plt.figure(13, figsize=(20, 13)) - plt.title('Reported Accuracy') - plt.plot(status_time, estimator_status['pos_horiz_accuracy'], 'b', label='horizontal') - plt.plot(status_time, estimator_status['pos_vert_accuracy'], 'r', label='vertical') - plt.ylabel('accuracy (m)') - plt.xlabel('time (sec') - plt.legend(loc='upper right') - plt.grid() - pp.savefig() - plt.close(13) - # Plot the EKF IMU vibration metrics - plt.figure(14, figsize=(20, 13)) - vibe_coning_max_arg = np.argmax(estimator_status['vibe[0]']) - vibe_coning_max_time = status_time[vibe_coning_max_arg] - vibe_coning_max = np.amax(estimator_status['vibe[0]']) - vibe_hf_dang_max_arg = np.argmax(estimator_status['vibe[1]']) - vibe_hf_dang_max_time = status_time[vibe_hf_dang_max_arg] - vibe_hf_dang_max = np.amax(estimator_status['vibe[1]']) - vibe_hf_dvel_max_arg = np.argmax(estimator_status['vibe[2]']) - vibe_hf_dvel_max_time = status_time[vibe_hf_dvel_max_arg] - vibe_hf_dvel_max = np.amax(estimator_status['vibe[2]']) - plt.subplot(3, 1, 1) - plt.plot(1e-6 * estimator_status['timestamp'], 1000.0 * estimator_status['vibe[0]'], 'b') - plt.title('IMU Vibration Metrics') - plt.ylabel('Del Ang Coning (mrad)') - plt.grid() - plt.text(vibe_coning_max_time, 1000.0 * vibe_coning_max, 'max=' + str(round(1000.0 * vibe_coning_max, 5)), - fontsize=12, horizontalalignment='left', verticalalignment='top') - plt.subplot(3, 1, 2) - plt.plot(1e-6 * estimator_status['timestamp'], 1000.0 * estimator_status['vibe[1]'], 'b') - plt.ylabel('HF Del Ang (mrad)') - plt.grid() - plt.text(vibe_hf_dang_max_time, 1000.0 * vibe_hf_dang_max, 'max=' + str(round(1000.0 * vibe_hf_dang_max, 3)), - fontsize=12, horizontalalignment='left', verticalalignment='top') - plt.subplot(3, 1, 3) - plt.plot(1e-6 * estimator_status['timestamp'], estimator_status['vibe[2]'], 'b') - plt.ylabel('HF Del Vel (m/s)') - plt.xlabel('time (sec)') - plt.grid() - plt.text(vibe_hf_dvel_max_time, vibe_hf_dvel_max, 'max=' + str(round(vibe_hf_dvel_max, 4)), fontsize=12, - horizontalalignment='left', verticalalignment='top') - pp.savefig() - plt.close(14) - # Plot the EKF output observer tracking errors - plt.figure(15, figsize=(20, 13)) - ang_track_err_max_arg = np.argmax(ekf2_innovations['output_tracking_error[0]']) - ang_track_err_max_time = innov_time[ang_track_err_max_arg] - ang_track_err_max = np.amax(ekf2_innovations['output_tracking_error[0]']) - vel_track_err_max_arg = np.argmax(ekf2_innovations['output_tracking_error[1]']) - vel_track_err_max_time = innov_time[vel_track_err_max_arg] - vel_track_err_max = np.amax(ekf2_innovations['output_tracking_error[1]']) - pos_track_err_max_arg = np.argmax(ekf2_innovations['output_tracking_error[2]']) - pos_track_err_max_time = innov_time[pos_track_err_max_arg] - pos_track_err_max = np.amax(ekf2_innovations['output_tracking_error[2]']) - plt.subplot(3, 1, 1) - plt.plot(1e-6 * ekf2_innovations['timestamp'], 1e3 * ekf2_innovations['output_tracking_error[0]'], 'b') - plt.title('Output Observer Tracking Error Magnitudes') - plt.ylabel('angles (mrad)') - plt.grid() - plt.text(ang_track_err_max_time, 1e3 * ang_track_err_max, 'max=' + str(round(1e3 * ang_track_err_max, 2)), - fontsize=12, horizontalalignment='left', verticalalignment='top') - plt.subplot(3, 1, 2) - plt.plot(1e-6 * ekf2_innovations['timestamp'], ekf2_innovations['output_tracking_error[1]'], 'b') - plt.ylabel('velocity (m/s)') - plt.grid() - plt.text(vel_track_err_max_time, vel_track_err_max, 'max=' + str(round(vel_track_err_max, 2)), fontsize=12, - horizontalalignment='left', verticalalignment='top') - plt.subplot(3, 1, 3) - plt.plot(1e-6 * ekf2_innovations['timestamp'], ekf2_innovations['output_tracking_error[2]'], 'b') - plt.ylabel('position (m)') - plt.xlabel('time (sec)') - plt.grid() - plt.text(pos_track_err_max_time, pos_track_err_max, 'max=' + str(round(pos_track_err_max, 2)), fontsize=12, - horizontalalignment='left', verticalalignment='top') - pp.savefig() - plt.close(15) - # Plot the delta angle bias estimates - plt.figure(16, figsize=(20, 13)) - plt.subplot(3, 1, 1) - plt.plot(1e-6 * estimator_status['timestamp'], estimator_status['states[10]'], 'b') - plt.title('Delta Angle Bias Estimates') - plt.ylabel('X (rad)') - plt.xlabel('time (sec)') - plt.grid() - plt.subplot(3, 1, 2) - plt.plot(1e-6 * estimator_status['timestamp'], estimator_status['states[11]'], 'b') - plt.ylabel('Y (rad)') - plt.xlabel('time (sec)') - plt.grid() - plt.subplot(3, 1, 3) - plt.plot(1e-6 * estimator_status['timestamp'], estimator_status['states[12]'], 'b') - plt.ylabel('Z (rad)') - plt.xlabel('time (sec)') - plt.grid() - pp.savefig() - plt.close(16) - # Plot the delta velocity bias estimates - plt.figure(17, figsize=(20, 13)) - plt.subplot(3, 1, 1) - plt.plot(1e-6 * estimator_status['timestamp'], estimator_status['states[13]'], 'b') - plt.title('Delta Velocity Bias Estimates') - plt.ylabel('X (m/s)') - plt.xlabel('time (sec)') - plt.grid() - plt.subplot(3, 1, 2) - plt.plot(1e-6 * estimator_status['timestamp'], estimator_status['states[14]'], 'b') - plt.ylabel('Y (m/s)') - plt.xlabel('time (sec)') - plt.grid() - plt.subplot(3, 1, 3) - plt.plot(1e-6 * estimator_status['timestamp'], estimator_status['states[15]'], 'b') - plt.ylabel('Z (m/s)') - plt.xlabel('time (sec)') - plt.grid() - pp.savefig() - plt.close(17) - # Plot the earth frame magnetic field estimates - plt.figure(18, figsize=(20, 13)) - plt.subplot(3, 1, 3) - strength = (estimator_status['states[16]'] ** 2 + estimator_status['states[17]'] ** 2 + estimator_status[ - 'states[18]'] ** 2) ** 0.5 - plt.plot(1e-6 * estimator_status['timestamp'], strength, 'b') - plt.ylabel('strength (Gauss)') - plt.xlabel('time (sec)') - plt.grid() - plt.subplot(3, 1, 1) - rad2deg = 57.2958 - declination = rad2deg * np.arctan2(estimator_status['states[17]'], estimator_status['states[16]']) - plt.plot(1e-6 * estimator_status['timestamp'], declination, 'b') - plt.title('Earth Magnetic Field Estimates') - plt.ylabel('declination (deg)') - plt.xlabel('time (sec)') - plt.grid() - plt.subplot(3, 1, 2) - inclination = rad2deg * np.arcsin(estimator_status['states[18]'] / np.maximum(strength, np.finfo(np.float32).eps) ) - plt.plot(1e-6 * estimator_status['timestamp'], inclination, 'b') - plt.ylabel('inclination (deg)') - plt.xlabel('time (sec)') - plt.grid() - pp.savefig() - plt.close(18) - # Plot the body frame magnetic field estimates - plt.figure(19, figsize=(20, 13)) - plt.subplot(3, 1, 1) - plt.plot(1e-6 * estimator_status['timestamp'], estimator_status['states[19]'], 'b') - plt.title('Magnetomer Bias Estimates') - plt.ylabel('X (Gauss)') - plt.xlabel('time (sec)') - plt.grid() - plt.subplot(3, 1, 2) - plt.plot(1e-6 * estimator_status['timestamp'], estimator_status['states[20]'], 'b') - plt.ylabel('Y (Gauss)') - plt.xlabel('time (sec)') - plt.grid() - plt.subplot(3, 1, 3) - plt.plot(1e-6 * estimator_status['timestamp'], estimator_status['states[21]'], 'b') - plt.ylabel('Z (Gauss)') - plt.xlabel('time (sec)') - plt.grid() - pp.savefig() - plt.close(19) - # Plot the EKF wind estimates - plt.figure(20, figsize=(20, 13)) - plt.subplot(2, 1, 1) - plt.plot(1e-6 * estimator_status['timestamp'], estimator_status['states[22]'], 'b') - plt.title('Wind Velocity Estimates') - plt.ylabel('North (m/s)') - plt.xlabel('time (sec)') - plt.grid() - plt.subplot(2, 1, 2) - plt.plot(1e-6 * estimator_status['timestamp'], estimator_status['states[23]'], 'b') - plt.ylabel('East (m/s)') - plt.xlabel('time (sec)') - plt.grid() - pp.savefig() - plt.close(20) - # close the pdf file - pp.close() - # don't display to screen - # plt.show() - # clase all figures - plt.close("all") - # Do some automated analysis of the status data - # normal index range is defined by the flight duration - start_index = np.amin(np.where(status_time > in_air_transition_time)) - end_index = np.amax(np.where(status_time <= on_ground_transition_time)) - num_valid_values = (end_index - start_index + 1) - # find a late/early index range from 5 sec after in_air_transtion_time to 5 sec before on-ground transition time for mag and optical flow checks to avoid false positives - # this can be used to prevent false positives for sensors adversely affected by close proximity to the ground - # don't do this if the log starts or finishes in air or if it is shut off by flag - late_start_index = np.amin(np.where(status_time > (in_air_transition_time + 5.0)))\ - if (late_start_early_ending and not b_starts_in_air) else start_index - early_end_index = np.amax(np.where(status_time <= (on_ground_transition_time - 5.0))) \ - if (late_start_early_ending and not b_finishes_in_air) else end_index - num_valid_values_trimmed = (early_end_index - late_start_index + 1) - # also find the start and finish indexes for the innovation data - innov_start_index = np.amin(np.where(innov_time > in_air_transition_time)) - innov_end_index = np.amax(np.where(innov_time <= on_ground_transition_time)) - innov_num_valid_values = (innov_end_index - innov_start_index + 1) - innov_late_start_index = np.amin(np.where(innov_time > (in_air_transition_time + 5.0))) \ - if (late_start_early_ending and not b_starts_in_air) else innov_start_index - innov_early_end_index = np.amax(np.where(innov_time <= (on_ground_transition_time - 5.0))) \ - if (late_start_early_ending and not b_finishes_in_air) else innov_end_index - innov_num_valid_values_trimmed = (innov_early_end_index - innov_late_start_index + 1) - # define dictionary of test results and descriptions - test_results = { - 'master_status': ['Pass', - 'Master check status which can be either Pass Warning or Fail. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required'], - 'mag_sensor_status': ['Pass', - 'Magnetometer sensor check summary. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required'], - 'yaw_sensor_status': ['Pass', - 'Yaw sensor check summary. This sensor data can be sourced from the magnetometer or an external vision system. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required'], - 'vel_sensor_status': ['Pass', - 'Velocity sensor check summary. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required'], - 'pos_sensor_status': ['Pass', - 'Position sensor check summary. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required'], - 'hgt_sensor_status': ['Pass', - 'Height sensor check summary. This sensor data can be sourced from either Baro or GPS or range finder or external vision system. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no anomalies were detected and no further investigation is required'], - 'hagl_sensor_status': ['Pass', - 'Height above ground sensor check summary. This sensor data is normally sourced from a rangefinder sensor. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required'], - 'tas_sensor_status': ['Pass', - 'Airspeed sensor check summary. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required'], - 'imu_sensor_status': ['Pass', - 'IMU sensor check summary. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required'], - 'imu_vibration_check': ['Pass', - 'IMU vibration check summary. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required'], - 'imu_bias_check': ['Pass', - 'IMU bias check summary. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required'], - 'imu_output_predictor_check': ['Pass', - 'IMU output predictor check summary. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required'], - 'flow_sensor_status': ['Pass', - 'Optical Flow sensor check summary. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required'], - 'filter_fault_status': ['Pass', - 'Internal Filter check summary. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required'], - 'mag_percentage_red': [float('NaN'), - 'The percentage of in-flight consolidated magnetic field sensor innovation consistency test values > 1.0.'], - 'mag_percentage_amber': [float('NaN'), - 'The percentage of in-flight consolidated magnetic field sensor innovation consistency test values > 0.5.'], - 'magx_fail_percentage': [float('NaN'), - 'The percentage of in-flight recorded failure events for the X-axis magnetic field sensor innovation consistency test.'], - 'magy_fail_percentage': [float('NaN'), - 'The percentage of in-flight recorded failure events for the Y-axis magnetic field sensor innovation consistency test.'], - 'magz_fail_percentage': [float('NaN'), - 'The percentage of in-flight recorded failure events for the Z-axis magnetic field sensor innovation consistency test.'], - 'yaw_fail_percentage': [float('NaN'), - 'The percentage of in-flight recorded failure events for the yaw sensor innovation consistency test.'], - 'mag_test_max': [float('NaN'), - 'The maximum in-flight value of the magnetic field sensor innovation consistency test ratio.'], - 'mag_test_mean': [float('NaN'), - 'The mean in-flight value of the magnetic field sensor innovation consistency test ratio.'], - 'vel_percentage_red': [float('NaN'), - 'The percentage of in-flight velocity sensor consolidated innovation consistency test values > 1.0.'], - 'vel_percentage_amber': [float('NaN'), - 'The percentage of in-flight velocity sensor consolidated innovation consistency test values > 0.5.'], - 'vel_fail_percentage': [float('NaN'), - 'The percentage of in-flight recorded failure events for the velocity sensor consolidated innovation consistency test.'], - 'vel_test_max': [float('NaN'), - 'The maximum in-flight value of the velocity sensor consolidated innovation consistency test ratio.'], - 'vel_test_mean': [float('NaN'), - 'The mean in-flight value of the velocity sensor consolidated innovation consistency test ratio.'], - 'pos_percentage_red': [float('NaN'), - 'The percentage of in-flight position sensor consolidated innovation consistency test values > 1.0.'], - 'pos_percentage_amber': [float('NaN'), - 'The percentage of in-flight position sensor consolidated innovation consistency test values > 0.5.'], - 'pos_fail_percentage': [float('NaN'), - 'The percentage of in-flight recorded failure events for the velocity sensor consolidated innovation consistency test.'], - 'pos_test_max': [float('NaN'), - 'The maximum in-flight value of the position sensor consolidated innovation consistency test ratio.'], - 'pos_test_mean': [float('NaN'), - 'The mean in-flight value of the position sensor consolidated innovation consistency test ratio.'], - 'hgt_percentage_red': [float('NaN'), - 'The percentage of in-flight height sensor innovation consistency test values > 1.0.'], - 'hgt_percentage_amber': [float('NaN'), - 'The percentage of in-flight height sensor innovation consistency test values > 0.5.'], - 'hgt_fail_percentage': [float('NaN'), - 'The percentage of in-flight recorded failure events for the height sensor innovation consistency test.'], - 'hgt_test_max': [float('NaN'), - 'The maximum in-flight value of the height sensor innovation consistency test ratio.'], - 'hgt_test_mean': [float('NaN'), - 'The mean in-flight value of the height sensor innovation consistency test ratio.'], - 'tas_percentage_red': [float('NaN'), - 'The percentage of in-flight airspeed sensor innovation consistency test values > 1.0.'], - 'tas_percentage_amber': [float('NaN'), - 'The percentage of in-flight airspeed sensor innovation consistency test values > 0.5.'], - 'tas_fail_percentage': [float('NaN'), - 'The percentage of in-flight recorded failure events for the airspeed sensor innovation consistency test.'], - 'tas_test_max': [float('NaN'), - 'The maximum in-flight value of the airspeed sensor innovation consistency test ratio.'], - 'tas_test_mean': [float('NaN'), - 'The mean in-flight value of the airspeed sensor innovation consistency test ratio.'], - 'hagl_percentage_red': [float('NaN'), - 'The percentage of in-flight height above ground sensor innovation consistency test values > 1.0.'], - 'hagl_percentage_amber': [float('NaN'), - 'The percentage of in-flight height above ground sensor innovation consistency test values > 0.5.'], - 'hagl_fail_percentage': [float('NaN'), - 'The percentage of in-flight recorded failure events for the height above ground sensor innovation consistency test.'], - 'hagl_test_max': [float('NaN'), - 'The maximum in-flight value of the height above ground sensor innovation consistency test ratio.'], - 'hagl_test_mean': [float('NaN'), - 'The mean in-flight value of the height above ground sensor innovation consistency test ratio.'], - 'ofx_fail_percentage': [float('NaN'), - 'The percentage of in-flight recorded failure events for the optical flow sensor X-axis innovation consistency test.'], - 'ofy_fail_percentage': [float('NaN'), - 'The percentage of in-flight recorded failure events for the optical flow sensor Y-axis innovation consistency test.'], - 'filter_faults_max': [float('NaN'), - 'Largest recorded value of the filter internal fault bitmask. Should always be zero.'], - 'imu_coning_peak': [float('NaN'), 'Peak in-flight value of the IMU delta angle coning vibration metric (rad)'], - 'imu_coning_mean': [float('NaN'), 'Mean in-flight value of the IMU delta angle coning vibration metric (rad)'], - 'imu_hfdang_peak': [float('NaN'), - 'Peak in-flight value of the IMU delta angle high frequency vibration metric (rad)'], - 'imu_hfdang_mean': [float('NaN'), - 'Mean in-flight value of the IMU delta angle high frequency vibration metric (rad)'], - 'imu_hfdvel_peak': [float('NaN'), - 'Peak in-flight value of the IMU delta velocity high frequency vibration metric (m/s)'], - 'imu_hfdvel_mean': [float('NaN'), - 'Mean in-flight value of the IMU delta velocity high frequency vibration metric (m/s)'], - 'output_obs_ang_err_median': [float('NaN'), - 'Median in-flight value of the output observer angular error (rad)'], - 'output_obs_vel_err_median': [float('NaN'), - 'Median in-flight value of the output observer velocity error (m/s)'], - 'output_obs_pos_err_median': [float('NaN'), 'Median in-flight value of the output observer position error (m)'], - 'imu_dang_bias_median': [float('NaN'), 'Median in-flight value of the delta angle bias vector length (rad)'], - 'imu_dvel_bias_median': [float('NaN'), 'Median in-flight value of the delta velocity bias vector length (m/s)'], - 'tilt_align_time': [float('NaN'), - 'The time in seconds measured from startup that the EKF completed the tilt alignment. A nan value indicates that the alignment had completed before logging started or alignment did not complete.'], - 'yaw_align_time': [float('NaN'), - 'The time in seconds measured from startup that the EKF completed the yaw alignment.'], - 'in_air_transition_time': [round(in_air_transition_time, 1), - 'The time in seconds measured from startup that the EKF transtioned into in-air mode. Set to a nan if a transition event is not detected.'], - 'on_ground_transition_time': [round(on_ground_transition_time, 1), - 'The time in seconds measured from startup that the EKF transitioned out of in-air mode. Set to a nan if a transition event is not detected.'], - } - # generate test metadata - # reduction of innovation message data - if (innov_early_end_index > (innov_late_start_index + 50)): - # Output Observer Tracking Errors - test_results['output_obs_ang_err_median'][0] = np.median( - ekf2_innovations['output_tracking_error[0]'][innov_late_start_index:innov_early_end_index + 1]) - test_results['output_obs_vel_err_median'][0] = np.median( - ekf2_innovations['output_tracking_error[1]'][innov_late_start_index:innov_early_end_index + 1]) - test_results['output_obs_pos_err_median'][0] = np.median( - ekf2_innovations['output_tracking_error[2]'][innov_late_start_index:innov_early_end_index + 1]) - # reduction of status message data - if (early_end_index > (late_start_index + 50)): - # IMU vibration checks - temp = np.amax(estimator_status['vibe[0]'][late_start_index:early_end_index]) - if (temp > 0.0): - test_results['imu_coning_peak'][0] = temp - test_results['imu_coning_mean'][0] = np.mean(estimator_status['vibe[0]'][late_start_index:early_end_index + 1]) - temp = np.amax(estimator_status['vibe[1]'][late_start_index:early_end_index]) - if (temp > 0.0): - test_results['imu_hfdang_peak'][0] = temp - test_results['imu_hfdang_mean'][0] = np.mean(estimator_status['vibe[1]'][late_start_index:early_end_index + 1]) - temp = np.amax(estimator_status['vibe[2]'][late_start_index:early_end_index]) - if (temp > 0.0): - test_results['imu_hfdvel_peak'][0] = temp - test_results['imu_hfdvel_mean'][0] = np.mean(estimator_status['vibe[2]'][late_start_index:early_end_index + 1]) - # Magnetometer Sensor Checks - if (np.amax(yaw_aligned) > 0.5): - mag_num_red = (estimator_status['mag_test_ratio'][start_index:end_index + 1] > 1.0).sum() - mag_num_amber = (estimator_status['mag_test_ratio'][start_index:end_index + 1] > 0.5).sum() - mag_num_red - test_results['mag_percentage_red'][0] = 100.0 * mag_num_red / num_valid_values_trimmed - test_results['mag_percentage_amber'][0] = 100.0 * mag_num_amber / num_valid_values_trimmed - test_results['mag_test_max'][0] = np.amax( - estimator_status['mag_test_ratio'][late_start_index:early_end_index + 1]) - test_results['mag_test_mean'][0] = np.mean(estimator_status['mag_test_ratio'][start_index:end_index]) - test_results['magx_fail_percentage'][0] = 100.0 * ( - magx_innov_fail[late_start_index:early_end_index + 1] > 0.5).sum() / num_valid_values_trimmed - test_results['magy_fail_percentage'][0] = 100.0 * ( - magy_innov_fail[late_start_index:early_end_index + 1] > 0.5).sum() / num_valid_values_trimmed - test_results['magz_fail_percentage'][0] = 100.0 * ( - magz_innov_fail[late_start_index:early_end_index + 1] > 0.5).sum() / num_valid_values_trimmed - test_results['yaw_fail_percentage'][0] = 100.0 * ( - yaw_innov_fail[late_start_index:early_end_index + 1] > 0.5).sum() / num_valid_values_trimmed - # Velocity Sensor Checks - if (np.amax(using_gps) > 0.5): - vel_num_red = (estimator_status['vel_test_ratio'][start_index:end_index + 1] > 1.0).sum() - vel_num_amber = (estimator_status['vel_test_ratio'][start_index:end_index + 1] > 0.5).sum() - vel_num_red - test_results['vel_percentage_red'][0] = 100.0 * vel_num_red / num_valid_values - test_results['vel_percentage_amber'][0] = 100.0 * vel_num_amber / num_valid_values - test_results['vel_test_max'][0] = np.amax(estimator_status['vel_test_ratio'][start_index:end_index + 1]) - test_results['vel_test_mean'][0] = np.mean(estimator_status['vel_test_ratio'][start_index:end_index + 1]) - test_results['vel_fail_percentage'][0] = 100.0 * ( - vel_innov_fail[start_index:end_index + 1] > 0.5).sum() / num_valid_values - - # Position Sensor Checks - if ((np.amax(using_gps) > 0.5) or (np.amax(using_evpos) > 0.5)): - pos_num_red = (estimator_status['pos_test_ratio'][start_index:end_index + 1] > 1.0).sum() - pos_num_amber = (estimator_status['pos_test_ratio'][start_index:end_index + 1] > 0.5).sum() - pos_num_red - test_results['pos_percentage_red'][0] = 100.0 * pos_num_red / num_valid_values - test_results['pos_percentage_amber'][0] = 100.0 * pos_num_amber / num_valid_values - test_results['pos_test_max'][0] = np.amax(estimator_status['pos_test_ratio'][start_index:end_index + 1]) - test_results['pos_test_mean'][0] = np.mean(estimator_status['pos_test_ratio'][start_index:end_index + 1]) - test_results['pos_fail_percentage'][0] = 100.0 * ( - posh_innov_fail[start_index:end_index + 1] > 0.5).sum() / num_valid_values - - # Height Sensor Checks - hgt_num_red = (estimator_status['hgt_test_ratio'][late_start_index:early_end_index + 1] > 1.0).sum() - hgt_num_amber = (estimator_status['hgt_test_ratio'][late_start_index:early_end_index + 1] > 0.5).sum() - hgt_num_red - test_results['hgt_percentage_red'][0] = 100.0 * hgt_num_red / num_valid_values_trimmed - test_results['hgt_percentage_amber'][0] = 100.0 * hgt_num_amber / num_valid_values_trimmed - test_results['hgt_test_max'][0] = np.amax(estimator_status['hgt_test_ratio'][late_start_index:early_end_index + 1]) - test_results['hgt_test_mean'][0] = np.mean(estimator_status['hgt_test_ratio'][late_start_index:early_end_index + 1]) - test_results['hgt_fail_percentage'][0] = 100.0 * ( - posv_innov_fail[late_start_index:early_end_index + 1] > 0.5).sum() / num_valid_values_trimmed - - # Airspeed Sensor Checks - if (tas_test_max > 0.0): - tas_num_red = (estimator_status['tas_test_ratio'][start_index:end_index + 1] > 1.0).sum() - tas_num_amber = (estimator_status['tas_test_ratio'][start_index:end_index + 1] > 0.5).sum() - tas_num_red - test_results['tas_percentage_red'][0] = 100.0 * tas_num_red / num_valid_values - test_results['tas_percentage_amber'][0] = 100.0 * tas_num_amber / num_valid_values - test_results['tas_test_max'][0] = np.amax(estimator_status['tas_test_ratio'][start_index:end_index + 1]) - test_results['tas_test_mean'][0] = np.mean(estimator_status['tas_test_ratio'][start_index:end_index + 1]) - test_results['tas_fail_percentage'][0] = 100.0 * ( - tas_innov_fail[start_index:end_index + 1] > 0.5).sum() / num_valid_values - - # HAGL Sensor Checks - if (hagl_test_max > 0.0): - hagl_num_red = (estimator_status['hagl_test_ratio'][start_index:end_index + 1] > 1.0).sum() - hagl_num_amber = (estimator_status['hagl_test_ratio'][start_index:end_index + 1] > 0.5).sum() - hagl_num_red - test_results['hagl_percentage_red'][0] = 100.0 * hagl_num_red / num_valid_values - test_results['hagl_percentage_amber'][0] = 100.0 * hagl_num_amber / num_valid_values - test_results['hagl_test_max'][0] = np.amax(estimator_status['hagl_test_ratio'][start_index:end_index + 1]) - test_results['hagl_test_mean'][0] = np.mean(estimator_status['hagl_test_ratio'][start_index:end_index + 1]) - test_results['hagl_fail_percentage'][0] = 100.0 * ( - hagl_innov_fail[start_index:end_index + 1] > 0.5).sum() / num_valid_values - - # optical flow sensor checks - if (np.amax(using_optflow) > 0.5): - test_results['ofx_fail_percentage'][0] = 100.0 * ( - ofx_innov_fail[late_start_index:early_end_index + 1] > 0.5).sum() / num_valid_values_trimmed - test_results['ofy_fail_percentage'][0] = 100.0 * ( - ofy_innov_fail[late_start_index:early_end_index + 1] > 0.5).sum() / num_valid_values_trimmed - - # IMU bias checks - test_results['imu_dang_bias_median'][0] = (np.median(estimator_status['states[10]']) ** 2 + np.median( - estimator_status['states[11]']) ** 2 + np.median(estimator_status['states[12]']) ** 2) ** 0.5 - test_results['imu_dvel_bias_median'][0] = (np.median(estimator_status['states[13]']) ** 2 + np.median( - estimator_status['states[14]']) ** 2 + np.median(estimator_status['states[15]']) ** 2) ** 0.5 - # Check for internal filter nummerical faults - test_results['filter_faults_max'][0] = np.amax(estimator_status['filter_fault_flags']) - # TODO - process the following bitmask's when they have been properly documented in the uORB topic - # estimator_status['health_flags'] - # estimator_status['timeout_flags'] - # calculate a master status - Fail, Warning, Pass - - # check test results against levels to provide a master status - # check for warnings - if (test_results.get('mag_percentage_amber')[0] > check_levels.get('mag_amber_warn_pct')): - test_results['master_status'][0] = 'Warning' - test_results['mag_sensor_status'][0] = 'Warning' - if (test_results.get('vel_percentage_amber')[0] > check_levels.get('vel_amber_warn_pct')): - test_results['master_status'][0] = 'Warning' - test_results['vel_sensor_status'][0] = 'Warning' - if (test_results.get('pos_percentage_amber')[0] > check_levels.get('pos_amber_warn_pct')): - test_results['master_status'][0] = 'Warning' - test_results['pos_sensor_status'][0] = 'Warning' - if (test_results.get('hgt_percentage_amber')[0] > check_levels.get('hgt_amber_warn_pct')): - test_results['master_status'][0] = 'Warning' - test_results['hgt_sensor_status'][0] = 'Warning' - if (test_results.get('hagl_percentage_amber')[0] > check_levels.get('hagl_amber_warn_pct')): - test_results['master_status'][0] = 'Warning' - test_results['hagl_sensor_status'][0] = 'Warning' - if (test_results.get('tas_percentage_amber')[0] > check_levels.get('tas_amber_warn_pct')): - test_results['master_status'][0] = 'Warning' - test_results['tas_sensor_status'][0] = 'Warning' - # check for IMU sensor warnings - if ((test_results.get('imu_coning_peak')[0] > check_levels.get('imu_coning_peak_warn')) or - (test_results.get('imu_coning_mean')[0] > check_levels.get('imu_coning_mean_warn'))): - test_results['master_status'][0] = 'Warning' - test_results['imu_sensor_status'][0] = 'Warning' - test_results['imu_vibration_check'][0] = 'Warning' - print('IMU gyro coning check warning.') - if ((test_results.get('imu_hfdang_peak')[0] > check_levels.get('imu_hfdang_peak_warn')) or - (test_results.get('imu_hfdang_mean')[0] > check_levels.get('imu_hfdang_mean_warn'))): - test_results['master_status'][0] = 'Warning' - test_results['imu_sensor_status'][0] = 'Warning' - test_results['imu_vibration_check'][0] = 'Warning' - print('IMU gyro vibration check warning.') - if ((test_results.get('imu_hfdvel_peak')[0] > check_levels.get('imu_hfdvel_peak_warn')) or - (test_results.get('imu_hfdvel_mean')[0] > check_levels.get('imu_hfdvel_mean_warn'))): - test_results['master_status'][0] = 'Warning' - test_results['imu_sensor_status'][0] = 'Warning' - test_results['imu_vibration_check'][0] = 'Warning' - print('IMU accel vibration check warning.') - if ((test_results.get('imu_dang_bias_median')[0] > check_levels.get('imu_dang_bias_median_warn')) or - (test_results.get('imu_dvel_bias_median')[0] > check_levels.get('imu_dvel_bias_median_warn'))): - test_results['master_status'][0] = 'Warning' - test_results['imu_sensor_status'][0] = 'Warning' - test_results['imu_bias_check'][0] = 'Warning' - print('IMU bias check warning.') - if ((test_results.get('output_obs_ang_err_median')[0] > check_levels.get('obs_ang_err_median_warn')) or - (test_results.get('output_obs_vel_err_median')[0] > check_levels.get('obs_vel_err_median_warn')) or - (test_results.get('output_obs_pos_err_median')[0] > check_levels.get('obs_pos_err_median_warn'))): - test_results['master_status'][0] = 'Warning' - test_results['imu_sensor_status'][0] = 'Warning' - test_results['imu_output_predictor_check'][0] = 'Warning' - print('IMU output predictor check warning.') - # check for failures - if ((test_results.get('magx_fail_percentage')[0] > check_levels.get('mag_fail_pct')) or - (test_results.get('magy_fail_percentage')[0] > check_levels.get('mag_fail_pct')) or - (test_results.get('magz_fail_percentage')[0] > check_levels.get('mag_fail_pct')) or - (test_results.get('mag_percentage_amber')[0] > check_levels.get('mag_amber_fail_pct'))): - test_results['master_status'][0] = 'Fail' - test_results['mag_sensor_status'][0] = 'Fail' - print('Magnetometer sensor check failure.') - if (test_results.get('yaw_fail_percentage')[0] > check_levels.get('yaw_fail_pct')): - test_results['master_status'][0] = 'Fail' - test_results['yaw_sensor_status'][0] = 'Fail' - print('Yaw sensor check failure.') - if ((test_results.get('vel_fail_percentage')[0] > check_levels.get('vel_fail_pct')) or - (test_results.get('vel_percentage_amber')[0] > check_levels.get('vel_amber_fail_pct'))): - test_results['master_status'][0] = 'Fail' - test_results['vel_sensor_status'][0] = 'Fail' - print('Velocity sensor check failure.') - if ((test_results.get('pos_fail_percentage')[0] > check_levels.get('pos_fail_pct')) or - (test_results.get('pos_percentage_amber')[0] > check_levels.get('pos_amber_fail_pct'))): - test_results['master_status'][0] = 'Fail' - test_results['pos_sensor_status'][0] = 'Fail' - print('Position sensor check failure.') - if ((test_results.get('hgt_fail_percentage')[0] > check_levels.get('hgt_fail_pct')) or - (test_results.get('hgt_percentage_amber')[0] > check_levels.get('hgt_amber_fail_pct'))): - test_results['master_status'][0] = 'Fail' - test_results['hgt_sensor_status'][0] = 'Fail' - print('Height sensor check failure.') - if ((test_results.get('tas_fail_percentage')[0] > check_levels.get('tas_fail_pct')) or - (test_results.get('tas_percentage_amber')[0] > check_levels.get('tas_amber_fail_pct'))): - test_results['master_status'][0] = 'Fail' - test_results['tas_sensor_status'][0] = 'Fail' - print('Airspeed sensor check failure.') - if ((test_results.get('hagl_fail_percentage')[0] > check_levels.get('hagl_fail_pct')) or - (test_results.get('hagl_percentage_amber')[0] > check_levels.get('hagl_amber_fail_pct'))): - test_results['master_status'][0] = 'Fail' - test_results['hagl_sensor_status'][0] = 'Fail' - print('Height above ground sensor check failure.') - if ((test_results.get('ofx_fail_percentage')[0] > check_levels.get('flow_fail_pct')) or - (test_results.get('ofy_fail_percentage')[0] > check_levels.get('flow_fail_pct'))): - test_results['master_status'][0] = 'Fail' - test_results['flow_sensor_status'][0] = 'Fail' - print('Optical flow sensor check failure.') - if (test_results.get('filter_faults_max')[0] > 0): - test_results['master_status'][0] = 'Fail' - test_results['filter_fault_status'][0] = 'Fail' - - return test_results diff --git a/Tools/ecl_ekf/analysis/__init__.py b/Tools/ecl_ekf/analysis/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/Tools/ecl_ekf/analysis/checks.py b/Tools/ecl_ekf/analysis/checks.py new file mode 100644 index 0000000000..9a1b20b96e --- /dev/null +++ b/Tools/ecl_ekf/analysis/checks.py @@ -0,0 +1,143 @@ +#! /usr/bin/env python3 +""" +function collection for performing ecl ekf checks +""" + +from typing import Tuple, List, Dict + + +def perform_ecl_ekf_checks( + metrics: Dict[str, float], sensor_checks: List[str], innov_fail_checks: List[str], + check_levels: Dict[str, float]) -> Tuple[Dict[str, str], str]: + """ + # performs the imu, sensor, amd ekf checks and calculates a master status. + :param metrics: + :param sensor_checks: + :param innov_fail_checks: + :param check_levels: + :return: + """ + + imu_status = perform_imu_checks(metrics, check_levels) + + sensor_status = perform_sensor_innov_checks( + metrics, sensor_checks, innov_fail_checks, check_levels) + + ekf_status = dict() + ekf_status['filter_fault_status'] = 'Fail' if metrics['filter_faults_max'] > 0 else 'Pass' + + # combine the status from the checks + combined_status = dict() + combined_status.update(imu_status) + combined_status.update(sensor_status) + combined_status.update(ekf_status) + if any(val == 'Fail' for val in combined_status.values()): + master_status = 'Fail' + elif any(val == 'Warning' for val in combined_status.values()): + master_status = 'Warning' + else: + master_status = 'Pass' + + return combined_status, master_status + + +def perform_imu_checks( + imu_metrics: Dict[str, float], check_levels: Dict[str, float]) -> Dict[str, str]: + """ + performs the imu checks. + :param imu_metrics: + :param check_levels: + :return: + """ + + # check for IMU sensor warnings + imu_status = dict() + + # perform the vibration check + imu_status['imu_vibration_check'] = 'Pass' + for imu_vibr_metric in ['imu_coning', 'imu_hfdang', 'imu_hfdvel']: + mean_metric = '{:s}_mean'.format(imu_vibr_metric) + peak_metric = '{:s}_peak'.format(imu_vibr_metric) + if imu_metrics[mean_metric] > check_levels['{:s}_warn'.format(mean_metric)] \ + or imu_metrics[peak_metric] > check_levels['{:s}_warn'.format(peak_metric)]: + imu_status['imu_vibration_check'] = 'Warning' + + if imu_status['imu_vibration_check'] == 'Warning': + print('IMU vibration check warning.') + + # perform the imu bias check + if imu_metrics['imu_dang_bias_median'] > check_levels['imu_dang_bias_median_warn'] \ + or imu_metrics['imu_dvel_bias_median'] > check_levels['imu_dvel_bias_median_warn']: + imu_status['imu_bias_check'] = 'Warning' + print('IMU bias check warning.') + else: + imu_status['imu_bias_check'] = 'Pass' + + # perform output predictor + if imu_metrics['output_obs_ang_err_median'] > check_levels['obs_ang_err_median_warn'] \ + or imu_metrics['output_obs_vel_err_median'] > check_levels['obs_vel_err_median_warn'] \ + or imu_metrics['output_obs_pos_err_median'] > check_levels['obs_pos_err_median_warn']: + imu_status['imu_output_predictor_check'] = 'Warning' + print('IMU output predictor check warning.') + else: + imu_status['imu_output_predictor_check'] = 'Pass' + + imu_status['imu_sensor_status'] = 'Warning' if any( + val == 'Warning' for val in imu_status.values()) else 'Pass' + + return imu_status + + +def perform_sensor_innov_checks( + metrics: Dict[str, float], sensor_checks: List[str], innov_fail_checks: List[str], + check_levels: Dict[str, float]) -> Dict[str, str]: + """ + performs the sensor checks. + :param metrics: + :param sensor_checks: + :param innov_fail_checks: + :param check_levels: + :return: + """ + + sensor_status = dict() + + for result_id in ['hgt', 'mag', 'vel', 'pos', 'tas', 'hagl']: + + # only run sensor checks, if they apply. + if result_id in sensor_checks: + if metrics['{:s}_percentage_amber'.format(result_id)] > check_levels[ + '{:s}_amber_fail_pct'.format(result_id)]: + sensor_status['{:s}_sensor_status'.format(result_id)] = 'Fail' + print('{:s} sensor check failure.'.format(result_id)) + elif metrics['{:s}_percentage_amber'.format(result_id)] > check_levels[ + '{:s}_amber_warn_pct'.format(result_id)]: + sensor_status['{:s}_sensor_status'.format(result_id)] = 'Warning' + print('{:s} sensor check warning.'.format(result_id)) + else: + sensor_status['{:s}_sensor_status'.format(result_id)] = 'Pass' + + # perform innovation checks. + for signal_id, metric_name, result_id in [('posv', 'hgt_fail_percentage', 'hgt'), + ('magx', 'magx_fail_percentage', 'mag'), + ('magy', 'magy_fail_percentage', 'mag'), + ('magz', 'magz_fail_percentage', 'mag'), + ('yaw', 'yaw_fail_percentage', 'yaw'), + ('vel', 'vel_fail_percentage', 'vel'), + ('posh', 'pos_fail_percentage', 'pos'), + ('tas', 'tas_fail_percentage', 'tas'), + ('hagl', 'hagl_fail_percentage', 'hagl'), + ('ofx', 'ofx_fail_percentage', 'flow'), + ('ofy', 'ofy_fail_percentage', 'flow')]: + + # only run innov fail checks, if they apply. + if signal_id in innov_fail_checks: + + if metrics[metric_name] > check_levels['{:s}_fail_pct'.format(result_id)]: + sensor_status['{:s}_sensor_status'.format(result_id)] = 'Fail' + print('{:s} sensor check failure.'.format(result_id)) + else: + if not ('{:s}_sensor_status'.format(result_id) in sensor_status): + sensor_status['{:s}_sensor_status'.format(result_id)] = 'Pass' + + return sensor_status diff --git a/Tools/ecl_ekf/analysis/detectors.py b/Tools/ecl_ekf/analysis/detectors.py new file mode 100644 index 0000000000..948f548c56 --- /dev/null +++ b/Tools/ecl_ekf/analysis/detectors.py @@ -0,0 +1,182 @@ +#! /usr/bin/env python3 +""" +detectors +""" +from typing import Optional +import numpy as np +from pyulog import ULog + + +class PreconditionError(Exception): + """ + a class for a Precondition Error + """ + + +class Airtime(object): + """ + Airtime struct. + """ + def __init__(self, take_off: float, landing: float): + self.take_off = take_off + self.landing = landing + + +class InAirDetector(object): + """ + handles airtime detection. + """ + + 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 _detect_airtime(self) -> Optional[Airtime]: + """ + detects the airtime take_off and landing of a ulog. + :return: a named tuple of ('Airtime', ['take_off', 'landing']) or None. + """ + + # test whether flight was in air at all + if (self._landed > 0).all(): + print('InAirDetector: always on ground.') + return [] + + # find the indices of all take offs and landings + take_offs = np.where(np.diff(self._landed) < 0)[0].tolist() + landings = np.where(np.diff(self._landed) > 0)[0].tolist() + + # check for start in air. + if len(take_offs) == 0 or ((len(landings) > 0) and (landings[0] < take_offs[0])): + + print('Started in air. Take first timestamp value as start point.') + take_offs = [-1] + take_offs + + # correct for offset: add 1 to take_off list + take_offs = [take_off + 1 for take_off in take_offs] + if len(landings) < len(take_offs): + print('No final landing detected. Assume last timestamp is landing.') + landings += [len(self._landed) - 2] + # correct for offset: add 1 to landing list + landings = [landing + 1 for landing in landings] + + assert len(landings) == len(take_offs), 'InAirDetector: different number of take offs' \ + ' and landings.' + + in_air = [] + for take_off, landing in zip(take_offs, landings): + if (self._vehicle_land_detected['timestamp'][landing] / 1e6 - + self._in_air_margin_seconds) - \ + (self._vehicle_land_detected['timestamp'][take_off] / 1e6 + + self._in_air_margin_seconds) >= self._min_flight_time_seconds: + in_air.append(Airtime( + take_off=(self._vehicle_land_detected['timestamp'][take_off] - + self._ulog.start_timestamp) / 1.0e6 + self._in_air_margin_seconds, + landing=(self._vehicle_land_detected['timestamp'][landing] - + self._ulog.start_timestamp) / 1.0e6 - self._in_air_margin_seconds)) + if len(in_air) == 0: + print('InAirDetector: no airtime detected.') + + return in_air + + @property + def airtimes(self): + """ + airtimes + :return: + """ + return self._in_air + + @property + def take_off(self) -> Optional[float]: + """ + first take off + :return: + """ + return self._in_air[0].take_off if self._in_air else None + + @property + def landing(self) -> Optional[float]: + """ + last landing + :return: the last landing of the flight. + """ + return self._in_air[-1].landing if self._in_air else None + + @property + def log_start(self) -> Optional[float]: + """ + log start + :return: the start time of the log. + """ + return self._log_start + + def get_take_off_to_last_landing(self, dataset) -> list: + """ + return all indices of the log file between the first take_off and the + last landing. + :param dataset: + :return: + """ + try: + data = self._ulog.get_dataset(dataset).data + except: + print('InAirDetector: {:s} not found in log.'.format(dataset)) + return [] + + if self._in_air: + airtime = np.where(((data['timestamp'] - self._ulog.start_timestamp) / 1.0e6 >= + self._in_air[0].take_off) & ( + (data['timestamp'] - self._ulog.start_timestamp) / + 1.0e6 < self._in_air[-1].landing))[0] + + else: + airtime = [] + + return airtime + + def get_airtime(self, dataset) -> list: + """ + return all indices of the log file that are in air + :param dataset: + :return: + """ + try: + data = self._ulog.get_dataset(dataset).data + except: + raise PreconditionError('InAirDetector: {:s} not found in log.'.format(dataset)) + + airtime = [] + if self._in_air is not None: + for i in range(len(self._in_air)): + airtime.extend(np.where(((data['timestamp'] - self._ulog.start_timestamp) / 1.0e6 >= + self._in_air[i].take_off) & ( + (data['timestamp'] - self._ulog.start_timestamp) / + 1.0e6 < self._in_air[i].landing))[0]) + + return airtime \ No newline at end of file diff --git a/Tools/ecl_ekf/analysis/metrics.py b/Tools/ecl_ekf/analysis/metrics.py new file mode 100644 index 0000000000..95b5850842 --- /dev/null +++ b/Tools/ecl_ekf/analysis/metrics.py @@ -0,0 +1,183 @@ +#! /usr/bin/env python3 +""" +function collection for calculation ecl ekf metrics. +""" + +from typing import Dict, List, Tuple, Callable + +from pyulog import ULog +import numpy as np + +from analysis.detectors import InAirDetector + +def calculate_ecl_ekf_metrics( + ulog: ULog, innov_flags: Dict[str, float], innov_fail_checks: List[str], + sensor_checks: List[str], in_air: InAirDetector, in_air_no_ground_effects: InAirDetector, + red_thresh: float = 1.0, amb_thresh: float = 0.5) -> Tuple[dict, dict, dict, dict]: + + sensor_metrics = calculate_sensor_metrics( + ulog, sensor_checks, in_air, in_air_no_ground_effects, + red_thresh=red_thresh, amb_thresh=amb_thresh) + + innov_fail_metrics = calculate_innov_fail_metrics( + innov_flags, innov_fail_checks, in_air, in_air_no_ground_effects) + + imu_metrics = calculate_imu_metrics(ulog, in_air_no_ground_effects) + + estimator_status_data = ulog.get_dataset('estimator_status').data + + # Check for internal filter nummerical faults + ekf_metrics = {'filter_faults_max': np.amax(estimator_status_data['filter_fault_flags'])} + # TODO - process these bitmask's when they have been properly documented in the uORB topic + # estimator_status['health_flags'] + # estimator_status['timeout_flags'] + + # combine the metrics + combined_metrics = dict() + combined_metrics.update(imu_metrics) + combined_metrics.update(sensor_metrics) + combined_metrics.update(innov_fail_metrics) + combined_metrics.update(ekf_metrics) + + return combined_metrics + + +def calculate_sensor_metrics( + ulog: ULog, sensor_checks: List[str], in_air: InAirDetector, + in_air_no_ground_effects: InAirDetector, red_thresh: float = 1.0, + amb_thresh: float = 0.5) -> Dict[str, float]: + + estimator_status_data = ulog.get_dataset('estimator_status').data + + sensor_metrics = dict() + + # calculates peak, mean, percentage above 0.5 std, and percentage above std metrics for + # estimator status variables + for signal, result_id in [('hgt_test_ratio', 'hgt'), + ('mag_test_ratio', 'mag'), + ('vel_test_ratio', 'vel'), + ('pos_test_ratio', 'pos'), + ('tas_test_ratio', 'tas'), + ('hagl_test_ratio', 'hagl')]: + + # only run sensor checks, if they apply. + if result_id in sensor_checks: + + if result_id == 'mag' or result_id == 'hgt': + in_air_detector = in_air_no_ground_effects + else: + in_air_detector = in_air + + # the percentage of samples above / below std dev + sensor_metrics['{:s}_percentage_red'.format(result_id)] = calculate_stat_from_signal( + estimator_status_data, 'estimator_status', signal, in_air_detector, + lambda x: 100.0 * np.mean(x > red_thresh)) + sensor_metrics['{:s}_percentage_amber'.format(result_id)] = calculate_stat_from_signal( + estimator_status_data, 'estimator_status', signal, in_air_detector, + lambda x: 100.0 * np.mean(x > amb_thresh)) - \ + sensor_metrics['{:s}_percentage_red'.format(result_id)] + + # the peak and mean ratio of samples above / below std dev + peak = calculate_stat_from_signal( + estimator_status_data, 'estimator_status', signal, in_air_detector, np.amax) + if peak > 0.0: + sensor_metrics['{:s}_test_max'.format(result_id)] = peak + sensor_metrics['{:s}_test_mean'.format(result_id)] = calculate_stat_from_signal( + estimator_status_data, 'estimator_status', signal, + in_air_detector, np.mean) + + return sensor_metrics + + +def calculate_innov_fail_metrics( + innov_flags: dict, innov_fail_checks: List[str], in_air: InAirDetector, + in_air_no_ground_effects: InAirDetector) -> dict: + """ + :param innov_flags: + :param innov_fail_checks: + :param in_air: + :param in_air_no_ground_effects: + :return: + """ + + innov_fail_metrics = dict() + + # calculate innovation check fail metrics + for signal_id, signal, result in [('posv', 'posv_innov_fail', 'hgt_fail_percentage'), + ('magx', 'magx_innov_fail', 'magx_fail_percentage'), + ('magy', 'magy_innov_fail', 'magy_fail_percentage'), + ('magz', 'magz_innov_fail', 'magz_fail_percentage'), + ('yaw', 'yaw_innov_fail', 'yaw_fail_percentage'), + ('vel', 'vel_innov_fail', 'vel_fail_percentage'), + ('posh', 'posh_innov_fail', 'pos_fail_percentage'), + ('tas', 'tas_innov_fail', 'tas_fail_percentage'), + ('hagl', 'hagl_innov_fail', 'hagl_fail_percentage'), + ('ofx', 'ofx_innov_fail', 'ofx_fail_percentage'), + ('ofy', 'ofy_innov_fail', 'ofy_fail_percentage')]: + + # only run innov fail checks, if they apply. + if signal_id in innov_fail_checks: + + if signal_id.startswith('mag') or signal_id == 'yaw' or signal_id == 'posv' or \ + signal_id.startswith('of'): + in_air_detector = in_air_no_ground_effects + else: + in_air_detector = in_air + + innov_fail_metrics[result] = calculate_stat_from_signal( + innov_flags, 'estimator_status', signal, in_air_detector, + lambda x: 100.0 * np.mean(x > 0.5)) + + return 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() + + # calculates the median of the output tracking error ekf innovations + for signal, result in [('output_tracking_error[0]', 'output_obs_ang_err_median'), + ('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) + + # calculates peak and mean for IMU vibration checks + for signal, result in [('vibe[0]', 'imu_coning'), + ('vibe[1]', 'imu_hfdang'), + ('vibe[2]', 'imu_hfdvel')]: + peak = calculate_stat_from_signal( + estimator_status_data, 'estimator_status', signal, in_air_no_ground_effects, np.amax) + if peak > 0.0: + imu_metrics['{:s}_peak'.format(result)] = peak + imu_metrics['{:s}_mean'.format(result)] = calculate_stat_from_signal( + estimator_status_data, 'estimator_status', signal, + in_air_no_ground_effects, np.mean) + + # IMU bias checks + imu_metrics['imu_dang_bias_median'] = np.sqrt(np.sum([np.square(calculate_stat_from_signal( + estimator_status_data, 'estimator_status', signal, in_air_no_ground_effects, np.median)) + for signal in ['states[10]', 'states[11]', 'states[12]']])) + imu_metrics['imu_dvel_bias_median'] = np.sqrt(np.sum([np.square(calculate_stat_from_signal( + estimator_status_data, 'estimator_status', signal, in_air_no_ground_effects, np.median)) + for signal in ['states[13]', 'states[14]', 'states[15]']])) + + return imu_metrics + + +def calculate_stat_from_signal( + data: Dict[str, np.ndarray], dataset: str, variable: str, + in_air_det: InAirDetector, stat_function: Callable) -> float: + """ + :param data: + :param variable: + :param in_air_detector: + :return: + """ + + return stat_function(data[variable][in_air_det.get_airtime(dataset)]) diff --git a/Tools/ecl_ekf/analysis/post_processing.py b/Tools/ecl_ekf/analysis/post_processing.py new file mode 100644 index 0000000000..55499c34cb --- /dev/null +++ b/Tools/ecl_ekf/analysis/post_processing.py @@ -0,0 +1,142 @@ +#! /usr/bin/env python3 +""" +function collection for post-processing of ulog data. +""" + +from typing import Tuple + +import numpy as np + + +def get_estimator_check_flags(estimator_status: dict) -> Tuple[dict, dict, dict]: + """ + :param estimator_status: + :return: + """ + control_mode = get_control_mode_flags(estimator_status) + innov_flags = get_innovation_check_flags(estimator_status) + gps_fail_flags = get_gps_check_fail_flags(estimator_status) + return control_mode, innov_flags, gps_fail_flags + + +def get_control_mode_flags(estimator_status: dict) -> dict: + """ + :param estimator_status: + :return: + """ + + control_mode = dict() + # extract control mode metadata from estimator_status.control_mode_flags + # 0 - true if the filter tilt alignment is complete + # 1 - true if the filter yaw alignment is complete + # 2 - true if GPS measurements are being fused + # 3 - true if optical flow measurements are being fused + # 4 - true if a simple magnetic yaw heading is being fused + # 5 - true if 3-axis magnetometer measurement are being fused + # 6 - true if synthetic magnetic declination measurements are being fused + # 7 - true when the vehicle is airborne + # 8 - true when wind velocity is being estimated + # 9 - true when baro height is being fused as a primary height reference + # 10 - true when range finder height is being fused as a primary height reference + # 11 - true when range finder height is being fused as a primary height reference + # 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 + 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 + control_mode['using_optflow'] = ((2 ** 3 & estimator_status['control_mode_flags']) > 0) * 1 + control_mode['using_magyaw'] = ((2 ** 4 & estimator_status['control_mode_flags']) > 0) * 1 + control_mode['using_mag3d'] = ((2 ** 5 & estimator_status['control_mode_flags']) > 0) * 1 + control_mode['using_magdecl'] = ((2 ** 6 & estimator_status['control_mode_flags']) > 0) * 1 + control_mode['airborne'] = ((2 ** 7 & estimator_status['control_mode_flags']) > 0) * 1 + control_mode['estimating_wind'] = ((2 ** 8 & estimator_status['control_mode_flags']) > 0) * 1 + control_mode['using_barohgt'] = ((2 ** 9 & estimator_status['control_mode_flags']) > 0) * 1 + control_mode['using_rnghgt'] = ((2 ** 10 & estimator_status['control_mode_flags']) > 0) * 1 + control_mode['using_gpshgt'] = ((2 ** 11 & estimator_status['control_mode_flags']) > 0) * 1 + 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 + return control_mode + + +def get_innovation_check_flags(estimator_status: dict) -> dict: + """ + :param estimator_status: + :return: + """ + + innov_flags = dict() + # innovation_check_flags summary + # 0 - true if velocity observations have been rejected + # 1 - true if horizontal position observations have been rejected + # 2 - true if true if vertical position observations have been rejected + # 3 - true if the X magnetometer observation has been rejected + # 4 - true if the Y magnetometer observation has been rejected + # 5 - true if the Z magnetometer observation has been rejected + # 6 - true if the yaw observation has been rejected + # 7 - true if the airspeed observation has been rejected + # 8 - true if synthetic sideslip observation has been rejected + # 9 - true if the height above ground observation has been rejected + # 10 - true if the X optical flow observation has been rejected + # 11 - true if the Y optical flow observation has been rejected + innov_flags['vel_innov_fail'] = ((2 ** 0 & estimator_status['innovation_check_flags']) > 0) * 1 + innov_flags['posh_innov_fail'] = ((2 ** 1 & estimator_status['innovation_check_flags']) > 0) * 1 + innov_flags['posv_innov_fail'] = ((2 ** 2 & estimator_status['innovation_check_flags']) > 0) * 1 + innov_flags['magx_innov_fail'] = ((2 ** 3 & estimator_status['innovation_check_flags']) > 0) * 1 + innov_flags['magy_innov_fail'] = ((2 ** 4 & estimator_status['innovation_check_flags']) > 0) * 1 + innov_flags['magz_innov_fail'] = ((2 ** 5 & estimator_status['innovation_check_flags']) > 0) * 1 + innov_flags['yaw_innov_fail'] = ((2 ** 6 & estimator_status['innovation_check_flags']) > 0) * 1 + innov_flags['tas_innov_fail'] = ((2 ** 7 & estimator_status['innovation_check_flags']) > 0) * 1 + innov_flags['sli_innov_fail'] = ((2 ** 8 & estimator_status['innovation_check_flags']) > 0) * 1 + innov_flags['hagl_innov_fail'] = ((2 ** 9 & estimator_status['innovation_check_flags']) > 0) * 1 + innov_flags['ofx_innov_fail'] = ((2 ** 10 & estimator_status['innovation_check_flags']) > 0) * 1 + innov_flags['ofy_innov_fail'] = ((2 ** 11 & estimator_status['innovation_check_flags']) > 0) * 1 + return innov_flags + + +def get_gps_check_fail_flags(estimator_status: dict) -> dict: + """ + :param estimator_status: + :return: + """ + gps_fail_flags = dict() + + # 0 : insufficient fix type (no 3D solution) + # 1 : minimum required sat count fail + # 2 : minimum required GDoP fail + # 3 : maximum allowed horizontal position error fail + # 4 : maximum allowed vertical position error fail + # 5 : maximum allowed speed error fail + # 6 : maximum allowed horizontal position drift fail + # 7 : maximum allowed vertical position drift fail + # 8 : maximum allowed horizontal speed fail + # 9 : maximum allowed vertical velocity discrepancy fail + gps_fail_flags['gfix_fail'] = ((2 ** 0 & estimator_status['gps_check_fail_flags']) > 0) * 1 + gps_fail_flags['nsat_fail'] = ((2 ** 1 & estimator_status['gps_check_fail_flags']) > 0) * 1 + gps_fail_flags['gdop_fail'] = ((2 ** 2 & estimator_status['gps_check_fail_flags']) > 0) * 1 + gps_fail_flags['herr_fail'] = ((2 ** 3 & estimator_status['gps_check_fail_flags']) > 0) * 1 + gps_fail_flags['verr_fail'] = ((2 ** 4 & estimator_status['gps_check_fail_flags']) > 0) * 1 + gps_fail_flags['serr_fail'] = ((2 ** 5 & estimator_status['gps_check_fail_flags']) > 0) * 1 + gps_fail_flags['hdrift_fail'] = ((2 ** 6 & estimator_status['gps_check_fail_flags']) > 0) * 1 + gps_fail_flags['vdrift_fail'] = ((2 ** 7 & estimator_status['gps_check_fail_flags']) > 0) * 1 + gps_fail_flags['hspd_fail'] = ((2 ** 8 & estimator_status['gps_check_fail_flags']) > 0) * 1 + gps_fail_flags['veld_diff_fail'] = ((2 ** 9 & estimator_status['gps_check_fail_flags']) > 0) * 1 + return gps_fail_flags + + +def magnetic_field_estimates_from_status(estimator_status: dict) -> Tuple[float, float, float]: + """ + + :param estimator_status: + :return: + """ + rad2deg = 57.2958 + field_strength = np.sqrt( + estimator_status['states[16]'] ** 2 + estimator_status['states[17]'] ** 2 + + estimator_status['states[18]'] ** 2) + declination = rad2deg * np.arctan2(estimator_status['states[17]'], + estimator_status['states[16]']) + inclination = rad2deg * np.arcsin( + estimator_status['states[18]'] / np.maximum(field_strength, np.finfo(np.float32).eps)) + return declination, field_strength, inclination diff --git a/Tools/ecl_ekf/batch_process_logdata_ekf.py b/Tools/ecl_ekf/batch_process_logdata_ekf.py index a94c526e7f..9d41431a10 100755 --- a/Tools/ecl_ekf/batch_process_logdata_ekf.py +++ b/Tools/ecl_ekf/batch_process_logdata_ekf.py @@ -1,47 +1,87 @@ -#!/usr/bin/env python +#! /usr/bin/env python3 +""" +Runs process_logdata_ekf.py on the .ulg files in the supplied directory. ulog files are skipped from the analysis, if a + corresponding .pdf file already exists (unless the overwrite flag was set). +""" # -*- coding: utf-8 -*- + import argparse import os, glob -""" -Runs process_logdata_ekf.py on the .ulg files in the supplied directory. ulog files are skipped from the analysis, if a - corresponding .pdf file already exists (unless the overwrite flag was set). -""" +from process_logdata_ekf import process_logdata_ekf -parser = argparse.ArgumentParser(description='Analyse the estimator_status and ekf2_innovation message data for the' - ' .ulg files in the specified directory') -parser.add_argument("directory_path") -parser.add_argument('-o', '--overwrite', action='store_true', - help='Whether to overwrite an already analysed file. If a file with .pdf extension exists for a .ulg' - 'file, the log file will be skipped from analysis unless this flag has been set.') -parser.add_argument('--no-sensor-safety-margin', action='store_true', - help='Whether to not cut-off 5s after take-off and 5s before landing ' - '(for certain sensors that might be influence by proximity to ground).') +def get_arguments(): + parser = argparse.ArgumentParser(description='Analyse the estimator_status and ekf2_innovation message data for the' + ' .ulg files in the specified directory') + parser.add_argument("directory_path") + parser.add_argument('-o', '--overwrite', action='store_true', + help='Whether to overwrite an already analysed file. If a file with .pdf extension exists for a .ulg' + 'file, the log file will be skipped from analysis unless this flag has been set.') + parser.add_argument('--no-plots', action='store_true', + help='Whether to only analyse and not plot the summaries for developers.') + parser.add_argument('--check-level-thresholds', type=str, default=None, + help='The csv file of fail and warning test thresholds for analysis.') + parser.add_argument('--check-table', type=str, default=None, + help='The csv file with descriptions of the checks.') + parser.add_argument('--no-sensor-safety-margin', action='store_true', + help='Whether to not cut-off 5s after take-off and 5s before landing ' + '(for certain sensors that might be influence by proximity to ground).') + return parser.parse_args() -def is_valid_directory(parser, arg): - if os.path.isdir(arg): - # Directory exists so return the directory - return arg + +def main() -> None: + + args = get_arguments() + + if args.check_level_thresholds is not None: + check_level_dict_filename = args.check_level_thresholds else: - parser.error('The directory {} does not exist'.format(arg)) + file_dir = os.path.realpath(os.path.join(os.getcwd(), os.path.dirname(__file__))) + check_level_dict_filename = os.path.join(file_dir, "check_level_dict.csv") -args = parser.parse_args() -ulog_directory = args.directory_path -print("\n"+"analysing the .ulg files in "+ulog_directory) - -# get all the ulog files found in the specified directory and in subdirectories -ulog_files = glob.glob(os.path.join(ulog_directory, '**/*.ulg'), recursive=True) - -# remove the files already analysed unless the overwrite flag was specified. A ulog file is consired to be analysed if -# a corresponding .pdf file exists.' -if not args.overwrite: - print("skipping already analysed ulg files.") - ulog_files = [ulog_file for ulog_file in ulog_files if not os.path.exists('{}.pdf'.format(ulog_file))] - -# analyse all ulog files -for ulog_file in ulog_files: - print("\n"+"loading " + ulog_file + " for analysis") - if args.no_sensor_safety_margin: - os.system("python process_logdata_ekf.py {} --no-sensor-safety-margin".format(ulog_file)) + if args.check_table is not None: + check_table_filename = args.check_table else: - os.system("python process_logdata_ekf.py {}".format(ulog_file)) + file_dir = os.path.realpath(os.path.join(os.getcwd(), os.path.dirname(__file__))) + check_table_filename = os.path.join(file_dir, "check_table.csv") + + ulog_directory = args.directory_path + + # get all the ulog files found in the specified directory and in subdirectories + ulog_files = glob.glob(os.path.join(ulog_directory, '**/*.ulg'), recursive=True) + print("found {:d} .ulg files in {:s}".format(len(ulog_files), ulog_directory)) + + # remove the files already analysed unless the overwrite flag was specified. A ulog file is consired to be analysed if + # a corresponding .pdf file exists.' + if not args.overwrite: + print("skipping already analysed ulg files.") + ulog_files = [ulog_file for ulog_file in ulog_files if + not os.path.exists('{}.pdf'.format(ulog_file))] + + n_files = len(ulog_files) + + print("analysing the {:d} .ulg files".format(n_files)) + + i = 1 + n_skipped = 0 + # analyse all ulog files + for ulog_file in ulog_files: + print('analysing file {:d}/{:d}: {:s}'.format(i, n_files, ulog_file)) + + try: + _ = process_logdata_ekf( + ulog_file, check_level_dict_filename, check_table_filename, + plot=not args.no_plots, sensor_safety_margins=not args.no_sensor_safety_margin) + + except Exception as e: + print(str(e)) + print('an exception occurred, skipping file {:s}'.format(ulog_file)) + n_skipped = n_skipped + 1 + + i = i + 1 + + print('{:d}/{:d} files analysed, {:d} skipped.'.format(n_files-n_skipped, n_files, n_skipped)) + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/Tools/ecl_ekf/batch_process_metadata_ekf.py b/Tools/ecl_ekf/batch_process_metadata_ekf.py index 7618c06cab..f6a5464dc0 100755 --- a/Tools/ecl_ekf/batch_process_metadata_ekf.py +++ b/Tools/ecl_ekf/batch_process_metadata_ekf.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#! /usr/bin/env python3 # -*- coding: utf-8 -*- import argparse import os diff --git a/Tools/ecl_ekf/check_level_dict.csv b/Tools/ecl_ekf/check_level_dict.csv index 956b8cef7d..e1ac34451d 100644 --- a/Tools/ecl_ekf/check_level_dict.csv +++ b/Tools/ecl_ekf/check_level_dict.csv @@ -1,3 +1,4 @@ +check_id,threshold mag_fail_pct,0.0 yaw_fail_pct,0.0 vel_fail_pct,0.0 diff --git a/Tools/ecl_ekf/check_table.csv b/Tools/ecl_ekf/check_table.csv new file mode 100644 index 0000000000..b111e233f0 --- /dev/null +++ b/Tools/ecl_ekf/check_table.csv @@ -0,0 +1,66 @@ +check_id,check_description +master_status, Master check status which can be either Pass Warning or Fail. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required +mag_sensor_status, Magnetometer sensor check summary. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required +yaw_sensor_status, Yaw sensor check summary. This sensor data can be sourced from the magnetometer or an external vision system. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required +vel_sensor_status, Velocity sensor check summary. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required +pos_sensor_status, Position sensor check summary. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required +hgt_sensor_status, Height sensor check summary. This sensor data can be sourced from either Baro or GPS or range finder or external vision system. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no anomalies were detected and no further investigation is required +hagl_sensor_status, Height above ground sensor check summary. This sensor data is normally sourced from a rangefinder sensor. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required +tas_sensor_status, Airspeed sensor check summary. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required +imu_sensor_status, IMU sensor check summary. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required +imu_vibration_check, IMU vibration check summary. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required +imu_bias_check, IMU bias check summary. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required +imu_output_predictor_check, IMU output predictor check summary. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required +flow_sensor_status, Optical Flow sensor check summary. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required +filter_fault_status, Internal Filter check summary. A Fail result indicates a significant error that caused a significant reduction in vehicle navigation performance was detected. A Warning result indicates that error levels higher than normal were detected but these errors did not significantly impact navigation performance. A Pass result indicates that no amonalies were detected and no further investigation is required +mag_percentage_red, The percentage of in-flight consolidated magnetic field sensor innovation consistency test values > 1.0. +mag_percentage_amber, The percentage of in-flight consolidated magnetic field sensor innovation consistency test values > 0.5. +magx_fail_percentage, The percentage of in-flight recorded failure events for the X-axis magnetic field sensor innovation consistency test. +magy_fail_percentage, The percentage of in-flight recorded failure events for the Y-axis magnetic field sensor innovation consistency test. +magz_fail_percentage, The percentage of in-flight recorded failure events for the Z-axis magnetic field sensor innovation consistency test. +yaw_fail_percentage, The percentage of in-flight recorded failure events for the yaw sensor innovation consistency test. +mag_test_max, The maximum in-flight value of the magnetic field sensor innovation consistency test ratio. +mag_test_mean, The mean in-flight value of the magnetic field sensor innovation consistency test ratio. +vel_percentage_red, The percentage of in-flight velocity sensor consolidated innovation consistency test values > 1.0. +vel_percentage_amber, The percentage of in-flight velocity sensor consolidated innovation consistency test values > 0.5. +vel_fail_percentage, The percentage of in-flight recorded failure events for the velocity sensor consolidated innovation consistency test. +vel_test_max, The maximum in-flight value of the velocity sensor consolidated innovation consistency test ratio. +vel_test_mean, The mean in-flight value of the velocity sensor consolidated innovation consistency test ratio. +pos_percentage_red, The percentage of in-flight position sensor consolidated innovation consistency test values > 1.0. +pos_percentage_amber, The percentage of in-flight position sensor consolidated innovation consistency test values > 0.5. +pos_fail_percentage, The percentage of in-flight recorded failure events for the velocity sensor consolidated innovation consistency test. +pos_test_max, The maximum in-flight value of the position sensor consolidated innovation consistency test ratio. +pos_test_mean, The mean in-flight value of the position sensor consolidated innovation consistency test ratio. +hgt_percentage_red, The percentage of in-flight height sensor innovation consistency test values > 1.0. +hgt_percentage_amber, The percentage of in-flight height sensor innovation consistency test values > 0.5. +hgt_fail_percentage, The percentage of in-flight recorded failure events for the height sensor innovation consistency test. +hgt_test_max, The maximum in-flight value of the height sensor innovation consistency test ratio. +hgt_test_mean, The mean in-flight value of the height sensor innovation consistency test ratio. +tas_percentage_red, The percentage of in-flight airspeed sensor innovation consistency test values > 1.0. +tas_percentage_amber, The percentage of in-flight airspeed sensor innovation consistency test values > 0.5. +tas_fail_percentage, The percentage of in-flight recorded failure events for the airspeed sensor innovation consistency test. +tas_test_max, The maximum in-flight value of the airspeed sensor innovation consistency test ratio. +tas_test_mean, The mean in-flight value of the airspeed sensor innovation consistency test ratio. +hagl_percentage_red, The percentage of in-flight height above ground sensor innovation consistency test values > 1.0. +hagl_percentage_amber, The percentage of in-flight height above ground sensor innovation consistency test values > 0.5. +hagl_fail_percentage, The percentage of in-flight recorded failure events for the height above ground sensor innovation consistency test. +hagl_test_max, The maximum in-flight value of the height above ground sensor innovation consistency test ratio. +hagl_test_mean, The mean in-flight value of the height above ground sensor innovation consistency test ratio. +ofx_fail_percentage, The percentage of in-flight recorded failure events for the optical flow sensor X-axis innovation consistency test. +ofy_fail_percentage, The percentage of in-flight recorded failure events for the optical flow sensor Y-axis innovation consistency test. +filter_faults_max, Largest recorded value of the filter internal fault bitmask. Should always be zero. +imu_coning_peak, Peak in-flight value of the IMU delta angle coning vibration metric (rad) +imu_coning_mean, Mean in-flight value of the IMU delta angle coning vibration metric (rad) +imu_hfdang_peak, Peak in-flight value of the IMU delta angle high frequency vibration metric (rad) +imu_hfdang_mean, Mean in-flight value of the IMU delta angle high frequency vibration metric (rad) +imu_hfdvel_peak, Peak in-flight value of the IMU delta velocity high frequency vibration metric (m/s) +imu_hfdvel_mean, Mean in-flight value of the IMU delta velocity high frequency vibration metric (m/s) +output_obs_ang_err_median, Median in-flight value of the output observer angular error (rad) +output_obs_vel_err_median, Median in-flight value of the output observer velocity error (m/s) +output_obs_pos_err_median, Median in-flight value of the output observer position error (m) +imu_dang_bias_median, Median in-flight value of the delta angle bias vector length (rad) +imu_dvel_bias_median, Median in-flight value of the delta velocity bias vector length (m/s) +tilt_align_time, The time in seconds measured from startup that the EKF completed the tilt alignment. A nan value indicates that the alignment had completed before logging started or alignment did not complete. +yaw_align_time, The time in seconds measured from startup that the EKF completed the yaw alignment. +in_air_transition_time, The time in seconds measured from startup that the EKF transtioned into in-air mode. Set to a nan if a transition event is not detected. +on_ground_transition_time, The time in seconds measured from startup that the EKF transitioned out of in-air mode. Set to a nan if a transition event is not detected. diff --git a/Tools/ecl_ekf/plotting/__init__.py b/Tools/ecl_ekf/plotting/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/Tools/ecl_ekf/plotting/data_plots.py b/Tools/ecl_ekf/plotting/data_plots.py new file mode 100644 index 0000000000..0f63f8ca3d --- /dev/null +++ b/Tools/ecl_ekf/plotting/data_plots.py @@ -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]) diff --git a/Tools/ecl_ekf/plotting/pdf_report.py b/Tools/ecl_ekf/plotting/pdf_report.py new file mode 100644 index 0000000000..d57d944f1a --- /dev/null +++ b/Tools/ecl_ekf/plotting/pdf_report.py @@ -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 \ No newline at end of file diff --git a/Tools/ecl_ekf/process_logdata_ekf.py b/Tools/ecl_ekf/process_logdata_ekf.py index 8baeb9460a..14c6df2167 100755 --- a/Tools/ecl_ekf/process_logdata_ekf.py +++ b/Tools/ecl_ekf/process_logdata_ekf.py @@ -1,13 +1,18 @@ -#! /usr/bin/env python +#! /usr/bin/env python3 from __future__ import print_function import argparse -import os, sys +import os +import sys +import csv +from typing import Dict -from pyulog import * +from pyulog import ULog from analyse_logdata_ekf import analyse_ekf +from plotting.pdf_report import create_pdf_report +from analysis.detectors import PreconditionError """ Performs a health assessment on the ecl EKF navigation estimator data contained in a an ULog file @@ -15,95 +20,146 @@ Outputs a health assessment summary in a csv file named .mdat.csv Outputs summary plots in a pdf file named .pdf """ -parser = argparse.ArgumentParser(description='Analyse the estimator_status and ekf2_innovation message data') -parser.add_argument('filename', metavar='file.ulg', help='ULog input file') -parser.add_argument('--no-plots', action='store_true', - help='Whether to only analyse and not plot the summaries for developers.') -parser.add_argument('--check-level-thresholds', type=str, default=None, - help='The csv file of fail and warning test thresholds for analysis.') -parser.add_argument('--no-sensor-safety-margin', action='store_true', - help='Whether to not cut-off 5s after take-off and 5s before landing ' - '(for certain sensors that might be influence by proximity to ground).') +def get_arguments(): + parser = argparse.ArgumentParser(description='Analyse the estimator_status and ekf2_innovation message data') + parser.add_argument('filename', metavar='file.ulg', help='ULog input file') + parser.add_argument('--no-plots', action='store_true', + help='Whether to only analyse and not plot the summaries for developers.') + parser.add_argument('--check-level-thresholds', type=str, default=None, + help='The csv file of fail and warning test thresholds for analysis.') + parser.add_argument('--check-table', type=str, default=None, + help='The csv file with descriptions of the checks.') + parser.add_argument('--no-sensor-safety-margin', action='store_true', + help='Whether to not cut-off 5s after take-off and 5s before landing ' + '(for certain sensors that might be influence by proximity to ground).') + return parser.parse_args() -def is_valid_directory(parser, arg): - if os.path.isdir(arg): - # Directory exists so return the directory - return arg + +def create_results_table( + check_table_filename: str, master_status: str, check_status: Dict[str, str], + metrics: Dict[str, float], airtime_info: Dict[str, float]) -> Dict[str, list]: + """ + creates the output results table + :param check_table_filename: + :param master_status: + :param check_status: + :param metrics: + :param airtime_info: + :return: + """ + + try: + with open(check_table_filename, 'r') as file: + reader = csv.DictReader(file) + test_results_table = { + row['check_id']: [float('NaN'), row['check_description']] for row in reader} + print('Using test description loaded from {:s}'.format(check_table_filename)) + except: + raise PreconditionError('could not find {:s}'.format(check_table_filename)) + + # store metrics + for key, value in metrics.items(): + test_results_table[key][0] = value + + # store check results + for key, value in check_status.items(): + test_results_table[key][0] = value + + # store check results + for key, value in test_results_table.items(): + if key.endswith('_status'): + test_results_table[key][0] = str(value[0]) + + # store master status + test_results_table['master_status'][0] = master_status + + # store take_off and landing information + test_results_table['in_air_transition_time'][0] = airtime_info['in_air_transition_time'] + test_results_table['on_ground_transition_time'][0] = airtime_info['on_ground_transition_time'] + + return test_results_table + + +def process_logdata_ekf( + filename: str, check_level_dict_filename: str, check_table_filename: str, + plot: bool = True, sensor_safety_margins: bool = True): + + ## load the log and extract the necessary data for the analyses + try: + ulog = ULog(filename) + except: + raise PreconditionError('could not open {:s}'.format(filename)) + + try: + # get the dictionary of fail and warning test thresholds from a csv file + with open(check_level_dict_filename, 'r') as file: + reader = csv.DictReader(file) + check_levels = {row['check_id']: float(row['threshold']) for row in reader} + print('Using test criteria loaded from {:s}'.format(check_level_dict_filename)) + except: + raise PreconditionError('could not find {:s}'.format(check_level_dict_filename)) + + in_air_margin = 5.0 if sensor_safety_margins else 0.0 + # perform the ekf analysis + master_status, check_status, metrics, airtime_info = analyse_ekf( + ulog, check_levels, red_thresh=1.0, amb_thresh=0.5, min_flight_duration_seconds=5.0, + in_air_margin_seconds=in_air_margin) + + test_results = create_results_table( + check_table_filename, master_status, check_status, metrics, airtime_info) + + # write metadata to a .csv file + with open('{:s}.mdat.csv'.format(filename), "w") as file: + + file.write("name,value,description\n") + + # loop through the test results dictionary and write each entry on a separate row, with data comma separated + # save data in alphabetical order + key_list = list(test_results.keys()) + key_list.sort() + for key in key_list: + file.write(key + "," + str(test_results[key][0]) + "," + test_results[key][1] + "\n") + print('Test results written to {:s}.mdat.csv'.format(filename)) + + if plot: + create_pdf_report(ulog, '{:s}.pdf'.format(filename)) + print('Plots saved to {:s}.pdf'.format(filename)) + + return test_results + + +def main() -> None: + + args = get_arguments() + + if args.check_level_thresholds is not None: + check_level_dict_filename = args.check_level_thresholds else: - parser.error('The directory {} does not exist'.format(arg)) + file_dir = os.path.realpath(os.path.join(os.getcwd(), os.path.dirname(__file__))) + check_level_dict_filename = os.path.join(file_dir, "check_level_dict.csv") -args = parser.parse_args() + if args.check_table is not None: + check_table_filename = args.check_table + else: + file_dir = os.path.realpath(os.path.join(os.getcwd(), os.path.dirname(__file__))) + check_table_filename = os.path.join(file_dir, "check_table.csv") -## load the log and extract the necessary data for the analyses -ulog = ULog(args.filename, None) -data = ulog.data_list + try: + test_results = process_logdata_ekf( + args.filename, check_level_dict_filename, check_table_filename, + plot=not args.no_plots, sensor_safety_margins=not args.no_sensor_safety_margin) + except Exception as e: + print(str(e)) + sys.exit(-1) -# extract data from EKF status message -estimator_status_data = {} -try: - estimator_status_data = ulog.get_dataset('estimator_status').data; -except (KeyError, IndexError, ValueError) as error: - print(type(error), "(estimator_status):", error) + # print master test status to console + if (test_results['master_status'][0] == 'Pass'): + print('No anomalies detected') + elif (test_results['master_status'][0] == 'Warning'): + print('Minor anomalies detected') + elif (test_results['master_status'][0] == 'Fail'): + print('Major anomalies detected') + sys.exit(-1) -# extract data from EKF innovations message -ekf2_innovations_data = {} -try: - ekf2_innovations_data = ulog.get_dataset('ekf2_innovations').data; -except (KeyError, IndexError, ValueError) as error: - print(type(error), "(ekf2_innovations):", error) - -# extract data from sensor preflight check message -sensor_preflight_data = {} -try: - sensor_preflight_data = ulog.get_dataset('sensor_preflight').data; -except (KeyError, IndexError, ValueError) as error: - print(type(error), "(sensor_preflight):", error) - -if args.check_level_thresholds: - check_level_dict_filename = args.check_level_thresholds -else: - file_dir = os.path.realpath(os.path.join(os.getcwd(), os.path.dirname(__file__))) - check_level_dict_filename = os.path.join(file_dir, "check_level_dict.csv") - -# get the dictionary of fail and warning test thresholds from a csv file -with open(check_level_dict_filename, 'r') as file: - check_levels = {} - for line in file: - x = line.split(",") - a = x[0] - b = x[1] - check_levels[a] = float(b) - -print('Using test criteria loaded from {:s}'.format(check_level_dict_filename)) - -# perform the ekf analysis -test_results = analyse_ekf( - estimator_status_data, ekf2_innovations_data, sensor_preflight_data, - check_levels, plot=not args.no_plots, output_plot_filename=args.filename + ".pdf", - late_start_early_ending=not args.no_sensor_safety_margin) - -# write metadata to a .csv file -with open(args.filename + ".mdat.csv", "w") as file: - - file.write("name,value,description\n") - - # loop through the test results dictionary and write each entry on a separate row, with data comma separated - # save data in alphabetical order - key_list = list(test_results.keys()) - key_list.sort() - for key in key_list: - file.write(key+","+str(test_results[key][0])+","+test_results[key][1]+"\n") - -print('Test results written to {:s}.mdat.csv'.format(args.filename)) - -if not args.no_plots: - print('Plots saved to {:s}.pdf'.format(args.filename)) - -# print master test status to console -if (test_results['master_status'][0] == 'Pass'): - print('No anomalies detected') -elif (test_results['master_status'][0] == 'Warning'): - print('Minor anomalies detected') -elif (test_results['master_status'][0] == 'Fail'): - print('Major anomalies detected') - sys.exit(-1) +if __name__ == '__main__': + main() diff --git a/Tools/setup/requirements.txt b/Tools/setup/requirements.txt index ee5fcc735b..219511c8da 100644 --- a/Tools/setup/requirements.txt +++ b/Tools/setup/requirements.txt @@ -12,3 +12,4 @@ setuptools>=39.2.0 toml>=0.9 tornado wheel>=0.31.1 +matplotlib