From b01e470ff9b72665d157881f8d49141d19eaa4db Mon Sep 17 00:00:00 2001
From: JohannesBrand <johannes@auterion.com>
Date: Mon, 18 Feb 2019 16:52:02 +0100
Subject: [PATCH] refactor ecl ekf analysis (#11412)
* refactor ekf analysis part 1: move plotting to functions
* add plot_check_flags plot function
* put plots in seperate file
* use object-oriented programming for plotting
* move functions for post processing and pdf report creation to new files
* add in_air_detector and description as a csv file
* refactor metrics and checks into separate functions
* refactor metrics into seperate file, seperate plotting
* ecl-ekf tools: re-structure folder and move results table generation
* ecl-ekf-tool: fix imports and test_results_table
* ecl-ekf tools: bugfix output observer tracking error plot
* ecl-ekf-tools: update batch processing to new api, fix exception handling
* ecl-ekf-tools: use correct in_air_detector
* ecl-ekf-tools: rename csv file containing the bare test results table
* ecl-tools: refactor for improving readability
* ecl-ekf tools: small plotting bugfixes
* ecl-ekf tools: small bugfixes in_air time, on_ground_trans, filenames
* ecl-ekf-tools: fix amber metric bug
* ecl-ekf-tools: remove custom function in inairdetector
* ecl-ekf-tools: remove import of pandas
* ecl-ekf-tools: add python interpreter to the script start
* ecl-ekf-tools pdf_report: fix python interpreter line
* px4-dev-ros-kinetic: update container tag to 2019-02-13
* ecl-ekf-tools python interpreter line: call python3 bin directly
* ecl-ekf-tools: change airtime from namedtuple to class for python 3.5
* ecl-ekf-tools: update docker image px4-dev-ros-kinetic
* ecl-ekf-tools: fix memory leak by correctly closing matplotlib figures
---
.ci/Jenkinsfile-SITL_tests | 4 +-
.ci/Jenkinsfile-SITL_tests_coverage | 2 +-
.ci/Jenkinsfile-compile | 2 +-
Tools/ecl_ekf/__init__.py | 0
Tools/ecl_ekf/analyse_logdata_ekf.py | 1494 ++-----------------
Tools/ecl_ekf/analysis/__init__.py | 0
Tools/ecl_ekf/analysis/checks.py | 143 ++
Tools/ecl_ekf/analysis/detectors.py | 182 +++
Tools/ecl_ekf/analysis/metrics.py | 183 +++
Tools/ecl_ekf/analysis/post_processing.py | 142 ++
Tools/ecl_ekf/batch_process_logdata_ekf.py | 120 +-
Tools/ecl_ekf/batch_process_metadata_ekf.py | 2 +-
Tools/ecl_ekf/check_level_dict.csv | 1 +
Tools/ecl_ekf/check_table.csv | 66 +
Tools/ecl_ekf/plotting/__init__.py | 0
Tools/ecl_ekf/plotting/data_plots.py | 408 +++++
Tools/ecl_ekf/plotting/pdf_report.py | 353 +++++
Tools/ecl_ekf/process_logdata_ekf.py | 244 +--
Tools/setup/requirements.txt | 1 +
19 files changed, 1839 insertions(+), 1508 deletions(-)
create mode 100644 Tools/ecl_ekf/__init__.py
create mode 100644 Tools/ecl_ekf/analysis/__init__.py
create mode 100644 Tools/ecl_ekf/analysis/checks.py
create mode 100644 Tools/ecl_ekf/analysis/detectors.py
create mode 100644 Tools/ecl_ekf/analysis/metrics.py
create mode 100644 Tools/ecl_ekf/analysis/post_processing.py
create mode 100644 Tools/ecl_ekf/check_table.csv
create mode 100644 Tools/ecl_ekf/plotting/__init__.py
create mode 100644 Tools/ecl_ekf/plotting/data_plots.py
create mode 100644 Tools/ecl_ekf/plotting/pdf_report.py
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 @@
-import numpy as np
-
-# matplotlib don't use Xwindows backend (must be before pyplot import)
-import matplotlib
-matplotlib.use('Agg')
-
-import matplotlib.pyplot as plt
-from matplotlib.backends.backend_pdf import PdfPages
-
-def analyse_ekf(estimator_status, ekf2_innovations, sensor_preflight, check_levels,
- plot=False, output_plot_filename=None, late_start_early_ending=True):
+#! /usr/bin/env python3
+"""
+the ecl ekf analysis
+"""
- 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)
+from typing import Tuple, List, Dict
- # generate max, min and 1-std metadata
- innov_time = 1e-6 * ekf2_innovations['timestamp']
- status_time = 1e-6 * estimator_status['timestamp']
-
- 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)
-
- # generate metadata for the normalised innovation consistency test levels
+import numpy as np
+from pyulog import ULog
+
+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
+
+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:
+ """
+
+ try:
+ estimator_status = ulog.get_dataset('estimator_status').data
+ print('found estimator_status data')
+ except:
+ raise PreconditionError('could not find estimator_status data')
+
+ try:
+ _ = ulog.get_dataset('ekf2_innovations').data
+ print('found ekf2_innovation data')
+ except:
+ raise PreconditionError('could not find ekf2_innovation data')
+
+ 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 in_air_no_ground_effects.take_off is None:
+ raise PreconditionError('no airtime detected.')
+
+ 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 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)
-
- # 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
-
- # 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(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])
+ if (np.amax(estimator_status['tas_test_ratio']) > 0.0):
+ sensor_checks.append('tas')
+ innov_fail_checks.append('tas')
- # 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
+ # 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')
- # 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
+ # optical flow sensor checks
+ if (np.amax(control_mode['using_optflow']) > 0.5):
+ innov_fail_checks.append('ofx')
+ innov_fail_checks.append('ofy')
- # 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
+ return sensor_checks, innov_fail_checks
- # 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
+
+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()
-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 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))
-
-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))
+ 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")
+
+ 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 <inputfilename>.mdat.csv
Outputs summary plots in a pdf file named <inputfilename>.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 is_valid_directory(parser, arg):
- if os.path.isdir(arg):
- # Directory exists so return the directory
- return arg
+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 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))
-
-args = parser.parse_args()
-
-## load the log and extract the necessary data for the analyses
-ulog = ULog(args.filename, None)
-data = ulog.data_list
-
-# 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)
-
-# 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)
+ 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")
+
+ 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")
+
+ 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)
+
+ # 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
--
GitLab