From b73c80428ef3b31e4eb0118acecd57dc28103b85 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Mon, 11 Nov 2019 10:27:57 +0100 Subject: [PATCH] ECL: Clean velPos logging, deprecate ekf2_innovations msg --- Tools/ecl_ekf/analyse_logdata_ekf.py | 9 +- .../ecl_ekf/analysis/data_version_handler.py | 48 ++++++++++ Tools/ecl_ekf/analysis/detectors.py | 2 +- Tools/ecl_ekf/analysis/metrics.py | 4 +- Tools/ecl_ekf/analysis/post_processing.py | 23 ++++- Tools/ecl_ekf/plotting/pdf_report.py | 49 +++++----- msg/CMakeLists.txt | 5 +- msg/ekf2_innovations.msg | 19 ---- msg/estimator_innovations.msg | 43 +++++++++ msg/estimator_status.msg | 2 + msg/tools/uorb_rtps_message_ids.yaml | 9 +- src/modules/ekf2/Utility/PreFlightChecker.cpp | 26 +++--- src/modules/ekf2/Utility/PreFlightChecker.hpp | 14 +-- src/modules/ekf2/ekf2_main.cpp | 92 ++++++++++++------- .../BlockLocalPositionEstimator.cpp | 2 + .../BlockLocalPositionEstimator.hpp | 5 +- .../local_position_estimator/sensors/flow.cpp | 8 +- .../local_position_estimator/sensors/gps.cpp | 18 +++- .../local_position_estimator/sensors/land.cpp | 4 +- .../sensors/lidar.cpp | 4 +- .../sensors/mocap.cpp | 21 +++-- .../sensors/sonar.cpp | 4 +- .../sensors/vision.cpp | 21 +++-- src/modules/logger/logger.cpp | 4 +- 24 files changed, 298 insertions(+), 138 deletions(-) create mode 100644 Tools/ecl_ekf/analysis/data_version_handler.py delete mode 100644 msg/ekf2_innovations.msg create mode 100644 msg/estimator_innovations.msg diff --git a/Tools/ecl_ekf/analyse_logdata_ekf.py b/Tools/ecl_ekf/analyse_logdata_ekf.py index df56bb968e..12ad2755b3 100644 --- a/Tools/ecl_ekf/analyse_logdata_ekf.py +++ b/Tools/ecl_ekf/analyse_logdata_ekf.py @@ -36,10 +36,10 @@ def analyse_ekf( raise PreconditionError('could not find estimator_status data') try: - _ = ulog.get_dataset('ekf2_innovations').data - print('found ekf2_innovation data') + _ = ulog.get_dataset('estimator_innovations').data + print('found estimator_innovations data') except: - raise PreconditionError('could not find ekf2_innovation data') + raise PreconditionError('could not find estimator_innovations data') try: in_air = InAirDetector( @@ -133,6 +133,3 @@ def find_checks_that_apply( - - - diff --git a/Tools/ecl_ekf/analysis/data_version_handler.py b/Tools/ecl_ekf/analysis/data_version_handler.py new file mode 100644 index 0000000000..c2392a6bcd --- /dev/null +++ b/Tools/ecl_ekf/analysis/data_version_handler.py @@ -0,0 +1,48 @@ +#! /usr/bin/env python3 +""" +function collection for handling different versions of log files +""" +from pyulog import ULog + +from analysis.detectors import PreconditionError + +def get_output_tracking_error_message(ulog: ULog) -> str: + """ + return the name of the message containing the output_tracking_error + :param ulog: + :return: str + """ + for elem in ulog.data_list: + if elem.name == "ekf2_innovations": + return "ekf2_innovations" + if elem.name == "estimator_innovations": + return "estimator_status" + + raise PreconditionError("Could not detect the message containing the output tracking error") + +def get_innovation_message(ulog: ULog, topic: str = 'innovation') -> str: + """ + return the name of the innovation message (old: ekf2_innovations; new: estimator_innovations) + :param ulog: + :return: str + """ + if topic == 'innovation': + for elem in ulog.data_list: + if elem.name == "ekf2_innovations": + return "ekf2_innovations" + if elem.name == "estimator_innovations": + return "estimator_innovations" + if topic == 'innovation_variance': + for elem in ulog.data_list: + if elem.name == "ekf2_innovations": + return "ekf2_innovations" + if elem.name == "estimator_innovations": + return "estimator_innovations" + if topic == 'innovation_test_ratio': + for elem in ulog.data_list: + if elem.name == "ekf2_innovations": + return "ekf2_innovations" + if elem.name == "estimator_innovations": + return "estimator_innovations" + + raise PreconditionError("Could not detect the message") diff --git a/Tools/ecl_ekf/analysis/detectors.py b/Tools/ecl_ekf/analysis/detectors.py index 948f548c56..d0ab83275b 100644 --- a/Tools/ecl_ekf/analysis/detectors.py +++ b/Tools/ecl_ekf/analysis/detectors.py @@ -179,4 +179,4 @@ class InAirDetector(object): (data['timestamp'] - self._ulog.start_timestamp) / 1.0e6 < self._in_air[i].landing))[0]) - return airtime \ No newline at end of file + return airtime diff --git a/Tools/ecl_ekf/analysis/metrics.py b/Tools/ecl_ekf/analysis/metrics.py index 95b5850842..6ebe8c6211 100644 --- a/Tools/ecl_ekf/analysis/metrics.py +++ b/Tools/ecl_ekf/analysis/metrics.py @@ -134,8 +134,6 @@ def calculate_innov_fail_metrics( def calculate_imu_metrics( ulog: ULog, in_air_no_ground_effects: InAirDetector) -> dict: - ekf2_innovation_data = ulog.get_dataset('ekf2_innovations').data - estimator_status_data = ulog.get_dataset('estimator_status').data imu_metrics = dict() @@ -145,7 +143,7 @@ def calculate_imu_metrics( ('output_tracking_error[1]', 'output_obs_vel_err_median'), ('output_tracking_error[2]', 'output_obs_pos_err_median')]: imu_metrics[result] = calculate_stat_from_signal( - ekf2_innovation_data, 'ekf2_innovations', signal, in_air_no_ground_effects, np.median) + estimator_status_data, 'estimator_status', signal, in_air_no_ground_effects, np.median) # calculates peak and mean for IMU vibration checks for signal, result in [('vibe[0]', 'imu_coning'), diff --git a/Tools/ecl_ekf/analysis/post_processing.py b/Tools/ecl_ekf/analysis/post_processing.py index 3a367c4bd6..ccb64b9522 100644 --- a/Tools/ecl_ekf/analysis/post_processing.py +++ b/Tools/ecl_ekf/analysis/post_processing.py @@ -42,6 +42,17 @@ def get_control_mode_flags(estimator_status: dict) -> dict: # 12 - true when local position data from external vision is being fused # 13 - true when yaw data from external vision measurements is being fused # 14 - true when height data from external vision measurements is being fused + # 15 - true when synthetic sideslip measurements are being fused + # 16 - true true when the mag field does not match the expected strength + # 17 - true true when the vehicle is operating as a fixed wing vehicle + # 18 - true when the magnetometer has been declared faulty and is no longer being used + # 19 - true true when airspeed measurements are being fused + # 20 - true true when protection from ground effect induced static pressure rise is active + # 21 - true when rng data wasn't ready for more than 10s and new rng values haven't changed enough + # 22 - true when yaw (not ground course) data from a GPS receiver is being fused + # 23 - true when the in-flight mag field alignment has been completed + # 24 - true when local earth frame velocity data from external vision measurements are being fused + # 25 - true when we are using a synthesized measurement for the magnetometer Z component control_mode['tilt_aligned'] = ((2 ** 0 & estimator_status['control_mode_flags']) > 0) * 1 control_mode['yaw_aligned'] = ((2 ** 1 & estimator_status['control_mode_flags']) > 0) * 1 control_mode['using_gps'] = ((2 ** 2 & estimator_status['control_mode_flags']) > 0) * 1 @@ -57,9 +68,19 @@ def get_control_mode_flags(estimator_status: dict) -> dict: control_mode['using_evpos'] = ((2 ** 12 & estimator_status['control_mode_flags']) > 0) * 1 control_mode['using_evyaw'] = ((2 ** 13 & estimator_status['control_mode_flags']) > 0) * 1 control_mode['using_evhgt'] = ((2 ** 14 & estimator_status['control_mode_flags']) > 0) * 1 + control_mode['fuse_beta'] = ((2 ** 15 & estimator_status['control_mode_flags']) > 0) * 1 + control_mode['mag_field_disturbed'] = ((2 ** 16 & estimator_status['control_mode_flags']) > 0) * 1 + control_mode['fixed_wing'] = ((2 ** 17 & estimator_status['control_mode_flags']) > 0) * 1 + control_mode['mag_fault'] = ((2 ** 18 & estimator_status['control_mode_flags']) > 0) * 1 + control_mode['fuse_aspd'] = ((2 ** 19 & estimator_status['control_mode_flags']) > 0) * 1 + control_mode['gnd_effect'] = ((2 ** 20 & estimator_status['control_mode_flags']) > 0) * 1 + control_mode['rng_stuck'] = ((2 ** 21 & estimator_status['control_mode_flags']) > 0) * 1 + control_mode['gps_yaw'] = ((2 ** 22 & estimator_status['control_mode_flags']) > 0) * 1 + control_mode['mag_aligned_in_flight'] = ((2 ** 23 & estimator_status['control_mode_flags']) > 0) * 1 + control_mode['ev_vel'] = ((2 ** 24 & estimator_status['control_mode_flags']) > 0) * 1 + control_mode['synthetic_mag_z'] = ((2 ** 25 & estimator_status['control_mode_flags']) > 0) * 1 return control_mode - def get_innovation_check_flags(estimator_status: dict) -> dict: """ :param estimator_status: diff --git a/Tools/ecl_ekf/plotting/pdf_report.py b/Tools/ecl_ekf/plotting/pdf_report.py index 41da608b05..762580caf9 100644 --- a/Tools/ecl_ekf/plotting/pdf_report.py +++ b/Tools/ecl_ekf/plotting/pdf_report.py @@ -15,6 +15,7 @@ from analysis.post_processing import magnetic_field_estimates_from_status, get_e from plotting.data_plots import TimeSeriesPlot, InnovationPlot, ControlModeSummaryPlot, \ CheckFlagsPlot from analysis.detectors import PreconditionError +import analysis.data_version_handler as dvh def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None: """ @@ -34,10 +35,15 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None: raise PreconditionError('could not find estimator_status data') try: - ekf2_innovations = ulog.get_dataset('ekf2_innovations').data - print('found ekf2_innovation data') + estimator_innovations = ulog.get_dataset('estimator_innovations').data + estimator_innovation_variances = ulog.get_dataset('estimator_innovation_variances').data + innovation_data = estimator_innovations + for key in estimator_innovation_variances: + # append 'var' to the field name such that we can distingush between innov and innov_var + innovation_data.update({str('var_'+key): estimator_innovation_variances[key]}) + print('found innovation data') except: - raise PreconditionError('could not find ekf2_innovation data') + raise PreconditionError('could not find innovation data') try: sensor_preflight = ulog.get_dataset('sensor_preflight').data @@ -67,8 +73,8 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None: # vertical velocity and position innovations data_plot = InnovationPlot( - ekf2_innovations, [('vel_pos_innov[2]', 'vel_pos_innov_var[2]'), - ('vel_pos_innov[5]', 'vel_pos_innov_var[5]')], + innovation_data, [('gps_vpos', 'var_gps_vpos'), + ('gps_vvel', 'var_gps_vvel')], x_labels=['time (sec)', 'time (sec)'], y_labels=['Down Vel (m/s)', 'Down Pos (m)'], plot_title='Vertical Innovations', pdf_handle=pdf_pages) @@ -77,8 +83,8 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None: # horizontal velocity innovations data_plot = InnovationPlot( - ekf2_innovations, [('vel_pos_innov[0]', 'vel_pos_innov_var[0]'), - ('vel_pos_innov[1]','vel_pos_innov_var[1]')], + innovation_data, [('gps_hvel[0]', 'var_gps_hvel[0]'), + ('gps_hvel[1]', 'var_gps_hvel[1]')], x_labels=['time (sec)', 'time (sec)'], y_labels=['North Vel (m/s)', 'East Vel (m/s)'], plot_title='Horizontal Velocity Innovations', pdf_handle=pdf_pages) @@ -87,8 +93,8 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None: # horizontal position innovations data_plot = InnovationPlot( - ekf2_innovations, [('vel_pos_innov[3]', 'vel_pos_innov_var[3]'), ('vel_pos_innov[4]', - 'vel_pos_innov_var[4]')], + innovation_data, [('gps_hpos[0]', 'var_gps_hpos[0]'), + ('gps_hpos[1]', 'var_gps_hpos[1]')], x_labels=['time (sec)', 'time (sec)'], y_labels=['North Pos (m)', 'East Pos (m)'], plot_title='Horizontal Position Innovations', pdf_handle=pdf_pages) @@ -97,8 +103,9 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None: # magnetometer innovations data_plot = InnovationPlot( - ekf2_innovations, [('mag_innov[0]', 'mag_innov_var[0]'), - ('mag_innov[1]', 'mag_innov_var[1]'), ('mag_innov[2]', 'mag_innov_var[2]')], + innovation_data, [('mag_field[0]', 'var_mag_field[0]'), + ('mag_field[1]', 'var_mag_field[1]'), + ('mag_field[2]', 'var_mag_field[2]')], x_labels=['time (sec)', 'time (sec)', 'time (sec)'], y_labels=['X (Gauss)', 'Y (Gauss)', 'Z (Gauss)'], plot_title='Magnetometer Innovations', pdf_handle=pdf_pages) @@ -107,7 +114,7 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None: # magnetic heading innovations data_plot = InnovationPlot( - ekf2_innovations, [('heading_innov', 'heading_innov_var')], + innovation_data, [('heading', 'var_heading')], x_labels=['time (sec)'], y_labels=['Heading (rad)'], plot_title='Magnetic Heading Innovations', pdf_handle=pdf_pages) data_plot.save() @@ -115,8 +122,8 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None: # air data innovations data_plot = InnovationPlot( - ekf2_innovations, - [('airspeed_innov', 'airspeed_innov_var'), ('beta_innov', 'beta_innov_var')], + innovation_data, [('airspeed', 'var_airspeed'), + ('beta', 'var_beta')], x_labels=['time (sec)', 'time (sec)'], y_labels=['innovation (m/sec)', 'innovation (rad)'], sub_titles=['True Airspeed Innovations', 'Synthetic Sideslip Innovations'], @@ -126,8 +133,8 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None: # optical flow innovations data_plot = InnovationPlot( - ekf2_innovations, [('flow_innov[0]', 'flow_innov_var[0]'), ('flow_innov[1]', - 'flow_innov_var[1]')], + innovation_data, [('flow[0]', 'var_flow[0]'), + ('flow[1]', 'var_flow[1]')], x_labels=['time (sec)', 'time (sec)'], y_labels=['X (rad/sec)', 'Y (rad/sec)'], plot_title='Optical Flow Innovations', pdf_handle=pdf_pages) @@ -252,12 +259,12 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None: # Plot the EKF output observer tracking errors scaled_innovations = { - 'output_tracking_error[0]': 1000. * ekf2_innovations['output_tracking_error[0]'], - 'output_tracking_error[1]': ekf2_innovations['output_tracking_error[1]'], - 'output_tracking_error[2]': ekf2_innovations['output_tracking_error[2]'] + 'output_tracking_error[0]': 1000. * estimator_status['output_tracking_error[0]'], + 'output_tracking_error[1]': estimator_status['output_tracking_error[1]'], + 'output_tracking_error[2]': estimator_status['output_tracking_error[2]'] } data_plot = CheckFlagsPlot( - 1e-6 * ekf2_innovations['timestamp'], scaled_innovations, + 1e-6 * estimator_status['timestamp'], scaled_innovations, [['output_tracking_error[0]'], ['output_tracking_error[1]'], ['output_tracking_error[2]']], x_label='time (sec)', y_labels=['angles (mrad)', 'velocity (m/s)', 'position (m)'], @@ -350,4 +357,4 @@ def detect_airtime(control_mode, status_time): in_air_duration = float('NaN') else: in_air_duration = float('NaN') - return b_finishes_in_air, b_starts_in_air, in_air_duration, in_air_transition_time, on_ground_transition_time \ No newline at end of file + return b_finishes_in_air, b_starts_in_air, in_air_duration, in_air_transition_time, on_ground_transition_time diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index ede08861f1..db1d15b54f 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -55,13 +55,13 @@ set(msg_files debug_vect.msg differential_pressure.msg distance_sensor.msg - ekf2_innovations.msg # TODO: remove as soon as https://github.com/PX4/Firmware/pull/13127 is rebased over this ekf2_timestamps.msg ekf_gps_drift.msg ekf_gps_position.msg esc_report.msg esc_status.msg estimator_status.msg + estimator_innovations.msg follow_target.msg geofence_result.msg gps_dump.msg @@ -159,8 +159,7 @@ set(msg_files ) set(deprecated_msgs - # TODO: uncomment as soon as https://github.com/PX4/Firmware/pull/13127 is rebased over this - # ekf2_innovations.msg # 2019-11-22, Updated estimator interface and logging; replaced by 'estimator_innovations'. + ekf2_innovations.msg # 2019-11-22, Updated estimator interface and logging; replaced by 'estimator_innovations'. ) foreach(msg IN LISTS deprecated_msgs) diff --git a/msg/ekf2_innovations.msg b/msg/ekf2_innovations.msg deleted file mode 100644 index 397d712109..0000000000 --- a/msg/ekf2_innovations.msg +++ /dev/null @@ -1,19 +0,0 @@ -uint64 timestamp # time since system start (microseconds) -float32[6] vel_pos_innov # velocity and position innovations -float32[3] mag_innov # earth magnetic field innovations -float32 heading_innov # heading innovation -float32 airspeed_innov # airspeed innovation -float32 beta_innov # synthetic sideslip innovation -float32[2] flow_innov # flow innovation -float32 hagl_innov # innovation from the terrain estimator for the height above ground level measurement (m) -float32[6] vel_pos_innov_var # velocity and position innovation variances -float32[3] mag_innov_var # earth magnetic field innovation variance -float32 heading_innov_var # heading innovation variance -float32 airspeed_innov_var # Airspeed innovation variance -float32 beta_innov_var # synthetic sideslip innovation variance -float32[2] flow_innov_var # flow innovation variance -float32 hagl_innov_var # innovation variance from the terrain estimator for the height above ground level measurement (m^2) -float32[3] output_tracking_error # return a vector containing the output predictor angular, velocity and position tracking error magnitudes (rad), (m/s), (m) -float32[2] drag_innov # drag specific force innovation (m/s/s) -float32[2] drag_innov_var # drag specific force innovation variance (m/s/s)**2 -float32[2] aux_vel_innov # auxiliary NE velocity innovations from landing target measurement (m/s) diff --git a/msg/estimator_innovations.msg b/msg/estimator_innovations.msg new file mode 100644 index 0000000000..a49ab9c5c4 --- /dev/null +++ b/msg/estimator_innovations.msg @@ -0,0 +1,43 @@ +uint64 timestamp # time since system start (microseconds) + +# GPS +float32[2] gps_hvel # horizontal GPS velocity innovation (m/sec) and innovation variance ((m/sec)**2) +float32 gps_vvel # vertical GPS velocity innovation (m/sec) and innovation variance ((m/sec)**2) +float32[2] gps_hpos # horizontal GPS position innovation (m) and innovation variance (m**2) +float32 gps_vpos # vertical GPS position innovation (m) and innovation variance (m**2) + +# External Vision +float32[2] ev_hvel # horizontal external vision velocity innovation (m/sec) and innovation variance ((m/sec)**2) +float32 ev_vvel # vertical external vision velocity innovation (m/sec) and innovation variance ((m/sec)**2) +float32[2] ev_hpos # horizontal external vision position innovation (m) and innovation variance (m**2) +float32 ev_vpos # vertical external vision position innovation (m) and innovation variance (m**2) + +# Fake Position and Velocity +float32[2] fake_hvel # fake horizontal velocity innovation (m/s) and innovation variance ((m/s)**2) +float32 fake_vvel # fake vertical velocity innovation (m/s) and innovation variance ((m/s)**2) +float32[2] fake_hpos # fake horizontal position innovation (m) and innovation variance (m**2) +float32 fake_vpos # fake vertical position innovation (m) and innovation variance (m**2) + +# Height sensors +float32 rng_vpos # range sensor height innovation (m) and innovation variance (m**2) +float32 baro_vpos # barometer height innovation (m) and innovation variance (m**2) + +# Auxiliary velocity +float32[2] aux_hvel # horizontal auxiliar velocity innovation from landing target measurement (m/sec) and innovation variance ((m/sec)**2) +float32 aux_vvel # vertical auxiliar velocity innovation from landing target measurement (m/sec) and innovation variance ((m/sec)**2) + +# Optical flow +float32[2] flow # flow innvoation (rad/sec) and innovation variance ((rad/sec)**2) + +# Various +float32 heading # heading innovation (rad) and innovation variance (rad**2) +float32[3] mag_field # earth magnetic field innovation (Gauss) and innovation variance (Gauss**2) +float32[2] drag # drag specific force innovation (m/sec**2) and innovation variance ((m/sec)**2) +float32 airspeed # airspeed innovation (m/sec) and innovation variance ((m/sec)**2) +float32 beta # synthetic sideslip innovation (rad) and innovation variance (rad**2) +float32 hagl # height of ground innovation (m) and innovation variance (m**2) + +# The innovation test ratios are scalar values. In case the field is a vector, +# the test ratio will be put in the first component of the vector. + +# TOPICS estimator_innovations estimator_innovation_variances estimator_innovation_test_ratios diff --git a/msg/estimator_status.msg b/msg/estimator_status.msg index 3f4c6d3741..391595c7a4 100644 --- a/msg/estimator_status.msg +++ b/msg/estimator_status.msg @@ -9,6 +9,8 @@ float32[3] vibe # IMU vibration metrics in the following array locations float32[24] covariances # Diagonal Elements of Covariance Matrix +float32[3] output_tracking_error # return a vector containing the output predictor angular, velocity and position tracking error magnitudes (rad), (m/s), (m) + uint16 gps_check_fail_flags # Bitmask to indicate status of GPS checks - see definition below # bits are true when corresponding test has failed uint8 GPS_CHECK_FAIL_GPS_FIX = 0 # 0 : insufficient fix type (no 3D solution) diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index 5f8a78203a..5de9c941ad 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -45,8 +45,7 @@ rtps: - msg: distance_sensor id: 17 send: true - # TODO: replace with 'estimator_innovations' as https://github.com/PX4/Firmware/pull/13127 is rebased over this - - msg: ekf2_innovations + - msg: estimator_innovations id: 18 - msg: ekf2_timestamps id: 19 @@ -336,4 +335,10 @@ rtps: - msg: vehicle_visual_odometry_aligned id: 169 alias: vehicle_odometry + - msg: estimator_innovation_variances + id: 170 + alias: estimator_innovations + - msg: estimator_innovation_test_ratios + id: 171 + alias: estimator_innovations ########## multi topics: end ########## diff --git a/src/modules/ekf2/Utility/PreFlightChecker.cpp b/src/modules/ekf2/Utility/PreFlightChecker.cpp index 732870ea8f..9d2aab6544 100644 --- a/src/modules/ekf2/Utility/PreFlightChecker.cpp +++ b/src/modules/ekf2/Utility/PreFlightChecker.cpp @@ -38,7 +38,7 @@ #include "PreFlightChecker.hpp" -void PreFlightChecker::update(const float dt, const ekf2_innovations_s &innov) +void PreFlightChecker::update(const float dt, const estimator_innovations_s &innov) { const float alpha = InnovationLpf::computeAlphaFromDtAndTauInv(dt, _innov_lpf_tau_inv); @@ -48,14 +48,14 @@ void PreFlightChecker::update(const float dt, const ekf2_innovations_s &innov) _has_height_failed = preFlightCheckHeightFailed(innov, alpha); } -bool PreFlightChecker::preFlightCheckHeadingFailed(const ekf2_innovations_s &innov, const float alpha) +bool PreFlightChecker::preFlightCheckHeadingFailed(const estimator_innovations_s &innov, const float alpha) { const float heading_test_limit = selectHeadingTestLimit(); const float heading_innov_spike_lim = 2.0f * heading_test_limit; - const float heading_innov_lpf = _filter_heading_innov.update(innov.heading_innov, alpha, heading_innov_spike_lim); + const float heading_innov_lpf = _filter_heading_innov.update(innov.heading, alpha, heading_innov_spike_lim); - return checkInnovFailed(innov.heading_innov, heading_innov_lpf, heading_test_limit); + return checkInnovFailed(innov.heading, heading_innov_lpf, heading_test_limit); } float PreFlightChecker::selectHeadingTestLimit() @@ -69,12 +69,13 @@ float PreFlightChecker::selectHeadingTestLimit() : _heading_innov_test_lim; // less restrictive test limit } -bool PreFlightChecker::preFlightCheckHorizVelFailed(const ekf2_innovations_s &innov, const float alpha) +bool PreFlightChecker::preFlightCheckHorizVelFailed(const estimator_innovations_s &innov, const float alpha) { bool has_failed = false; - if (_is_using_gps_aiding || _is_using_ev_pos_aiding) { - const Vector2f vel_ne_innov = Vector2f(innov.vel_pos_innov); + if (_is_using_gps_aiding || _is_using_ev_vel_aiding) { + const Vector2f vel_ne_innov = Vector2f(fmaxf(fabsf(innov.gps_hvel[0]), fabsf(innov.ev_hvel[0])), + fmaxf(fabsf(innov.gps_hvel[1]), fabsf(innov.ev_hvel[1]))); Vector2f vel_ne_innov_lpf; vel_ne_innov_lpf(0) = _filter_vel_n_innov.update(vel_ne_innov(0), alpha, _vel_innov_spike_lim); vel_ne_innov_lpf(1) = _filter_vel_n_innov.update(vel_ne_innov(1), alpha, _vel_innov_spike_lim); @@ -82,7 +83,7 @@ bool PreFlightChecker::preFlightCheckHorizVelFailed(const ekf2_innovations_s &in } if (_is_using_flow_aiding) { - const Vector2f flow_innov = Vector2f(innov.flow_innov); + const Vector2f flow_innov = Vector2f(innov.flow); Vector2f flow_innov_lpf; flow_innov_lpf(0) = _filter_flow_x_innov.update(flow_innov(0), alpha, _flow_innov_spike_lim); flow_innov_lpf(1) = _filter_flow_x_innov.update(flow_innov(1), alpha, _flow_innov_spike_lim); @@ -92,16 +93,16 @@ bool PreFlightChecker::preFlightCheckHorizVelFailed(const ekf2_innovations_s &in return has_failed; } -bool PreFlightChecker::preFlightCheckVertVelFailed(const ekf2_innovations_s &innov, const float alpha) +bool PreFlightChecker::preFlightCheckVertVelFailed(const estimator_innovations_s &innov, const float alpha) { - const float vel_d_innov = innov.vel_pos_innov[2]; + const float vel_d_innov = fmaxf(fabsf(innov.gps_vvel), fabs(innov.ev_vvel)); const float vel_d_innov_lpf = _filter_vel_d_innov.update(vel_d_innov, alpha, _vel_innov_spike_lim); return checkInnovFailed(vel_d_innov, vel_d_innov_lpf, _vel_innov_test_lim); } -bool PreFlightChecker::preFlightCheckHeightFailed(const ekf2_innovations_s &innov, const float alpha) +bool PreFlightChecker::preFlightCheckHeightFailed(const estimator_innovations_s &innov, const float alpha) { - const float hgt_innov = innov.vel_pos_innov[5]; + const float hgt_innov = innov.gps_vpos; // HACK: height innovation independent of sensor is published on gps_vpos const float hgt_innov_lpf = _filter_hgt_innov.update(hgt_innov, alpha, _hgt_innov_spike_lim); return checkInnovFailed(hgt_innov, hgt_innov_lpf, _hgt_innov_test_lim); } @@ -122,6 +123,7 @@ void PreFlightChecker::reset() _is_using_gps_aiding = false; _is_using_flow_aiding = false; _is_using_ev_pos_aiding = false; + _is_using_ev_vel_aiding = false; _has_heading_failed = false; _has_horiz_vel_failed = false; _has_vert_vel_failed = false; diff --git a/src/modules/ekf2/Utility/PreFlightChecker.hpp b/src/modules/ekf2/Utility/PreFlightChecker.hpp index 6d6da18c3a..57a4433314 100644 --- a/src/modules/ekf2/Utility/PreFlightChecker.hpp +++ b/src/modules/ekf2/Utility/PreFlightChecker.hpp @@ -42,7 +42,7 @@ #pragma once #include -#include +#include #include @@ -66,7 +66,7 @@ public: * @param dt the sampling time * @param innov the ekf2_innovation_s struct containing the current innovations */ - void update(float dt, const ekf2_innovations_s &innov); + void update(float dt, const estimator_innovations_s &innov); /* * If set to true, the checker will use a less conservative heading innovation check @@ -76,6 +76,7 @@ public: void setUsingGpsAiding(bool val) { _is_using_gps_aiding = val; } void setUsingFlowAiding(bool val) { _is_using_flow_aiding = val; } void setUsingEvPosAiding(bool val) { _is_using_ev_pos_aiding = val; } + void setUsingEvVelAiding(bool val) { _is_using_ev_vel_aiding = val; } bool hasHeadingFailed() const { return _has_heading_failed; } bool hasHorizVelFailed() const { return _has_horiz_vel_failed; } @@ -127,12 +128,12 @@ public: static constexpr float sq(float var) { return var * var; } private: - bool preFlightCheckHeadingFailed(const ekf2_innovations_s &innov, float alpha); + bool preFlightCheckHeadingFailed(const estimator_innovations_s &innov, float alpha); float selectHeadingTestLimit(); - bool preFlightCheckHorizVelFailed(const ekf2_innovations_s &innov, float alpha); - bool preFlightCheckVertVelFailed(const ekf2_innovations_s &innov, float alpha); - bool preFlightCheckHeightFailed(const ekf2_innovations_s &innov, float alpha); + bool preFlightCheckHorizVelFailed(const estimator_innovations_s &innov, float alpha); + bool preFlightCheckVertVelFailed(const estimator_innovations_s &innov, float alpha); + bool preFlightCheckHeightFailed(const estimator_innovations_s &innov, float alpha); void resetPreFlightChecks(); @@ -145,6 +146,7 @@ private: bool _is_using_gps_aiding{}; bool _is_using_flow_aiding{}; bool _is_using_ev_pos_aiding{}; + bool _is_using_ev_vel_aiding{}; // Low-pass filters for innovation pre-flight checks InnovationLpf _filter_vel_n_innov; ///< Preflight low pass filter N axis velocity innovations (m/sec) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index c0421116a6..70efd09908 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -56,7 +56,7 @@ #include #include #include -#include +#include #include #include #include @@ -120,7 +120,7 @@ private: PreFlightChecker _preflt_checker; void runPreFlightChecks(float dt, const filter_control_status_u &control_status, const vehicle_status_s &vehicle_status, - const ekf2_innovations_s &innov); + const estimator_innovations_s &innov); void resetPreFlightChecks(); template @@ -267,7 +267,9 @@ private: vehicle_land_detected_s _vehicle_land_detected{}; vehicle_status_s _vehicle_status{}; - uORB::Publication _estimator_innovations_pub{ORB_ID(ekf2_innovations)}; + uORB::Publication _estimator_innovations_pub{ORB_ID(estimator_innovations)}; + uORB::Publication _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)}; + uORB::Publication _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)}; uORB::Publication _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)}; uORB::Publication _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)}; uORB::Publication _blended_gps_pub{ORB_ID(ekf_gps_position)}; @@ -1484,16 +1486,17 @@ void Ekf2::Run() _ekf.get_state_delayed(status.states); status.n_states = 24; _ekf.covariances_diagonal().copyTo(status.covariances); + _ekf.get_output_tracking_error(&status.output_tracking_error[0]); _ekf.get_gps_check_status(&status.gps_check_fail_flags); // only report enabled GPS check failures (the param indexes are shifted by 1 bit, because they don't include // the GPS Fix bit, which is always checked) status.gps_check_fail_flags &= ((uint16_t)_params->gps_check_mask << 1) | 1; status.control_mode_flags = control_status.value; _ekf.get_filter_fault_status(&status.filter_fault_flags); - _ekf.get_innovation_test_status(&status.innovation_check_flags, &status.mag_test_ratio, - &status.vel_test_ratio, &status.pos_test_ratio, - &status.hgt_test_ratio, &status.tas_test_ratio, - &status.hagl_test_ratio, &status.beta_test_ratio); + _ekf.get_innovation_test_status(status.innovation_check_flags, status.mag_test_ratio, + status.vel_test_ratio, status.pos_test_ratio, + status.hgt_test_ratio, status.tas_test_ratio, + status.hagl_test_ratio, status.beta_test_ratio); status.pos_horiz_accuracy = _vehicle_local_position_pub.get().eph; status.pos_vert_accuracy = _vehicle_local_position_pub.get().epv; @@ -1602,28 +1605,51 @@ void Ekf2::Run() { // publish estimator innovation data - ekf2_innovations_s innovations; + estimator_innovations_s innovations; innovations.timestamp = now; - _ekf.get_vel_pos_innov(&innovations.vel_pos_innov[0]); - _ekf.get_aux_vel_innov(&innovations.aux_vel_innov[0]); - _ekf.get_mag_innov(&innovations.mag_innov[0]); - _ekf.get_heading_innov(&innovations.heading_innov); - _ekf.get_airspeed_innov(&innovations.airspeed_innov); - _ekf.get_beta_innov(&innovations.beta_innov); - _ekf.get_flow_innov(&innovations.flow_innov[0]); - _ekf.get_hagl_innov(&innovations.hagl_innov); - _ekf.get_drag_innov(&innovations.drag_innov[0]); + _ekf.getGpsVelPosInnov(&innovations.gps_hvel[0], innovations.gps_vvel, &innovations.gps_hpos[0], + innovations.gps_vpos); + _ekf.getEvVelPosInnov(&innovations.ev_hvel[0], innovations.ev_vvel, &innovations.ev_hpos[0], innovations.ev_vpos); + _ekf.getAuxVelInnov(&innovations.aux_hvel[0]); + _ekf.getFlowInnov(&innovations.flow[0]); + _ekf.getHeadingInnov(innovations.heading); + _ekf.getMagInnov(innovations.mag_field); + _ekf.getDragInnov(&innovations.drag[0]); + _ekf.getAirspeedInnov(innovations.airspeed); + _ekf.getBetaInnov(innovations.beta); + _ekf.getHaglInnov(innovations.hagl); - _ekf.get_vel_pos_innov_var(&innovations.vel_pos_innov_var[0]); - _ekf.get_mag_innov_var(&innovations.mag_innov_var[0]); - _ekf.get_heading_innov_var(&innovations.heading_innov_var); - _ekf.get_airspeed_innov_var(&innovations.airspeed_innov_var); - _ekf.get_beta_innov_var(&innovations.beta_innov_var); - _ekf.get_flow_innov_var(&innovations.flow_innov_var[0]); - _ekf.get_hagl_innov_var(&innovations.hagl_innov_var); - _ekf.get_drag_innov_var(&innovations.drag_innov_var[0]); + // publish estimator innovation variance data + estimator_innovations_s innovation_var; + innovation_var.timestamp = now; + _ekf.getGpsVelPosInnovVar(&innovation_var.gps_hvel[0], innovation_var.gps_vvel, &innovation_var.gps_hpos[0], + innovation_var.gps_vpos); + _ekf.getEvVelPosInnovVar(&innovation_var.ev_hvel[0], innovation_var.ev_vvel, &innovation_var.ev_hpos[0], + innovation_var.ev_vpos); + _ekf.getAuxVelInnovVar(&innovation_var.aux_hvel[0]); + _ekf.getFlowInnovVar(&innovation_var.flow[0]); + _ekf.getHeadingInnovVar(innovation_var.heading); + _ekf.getMagInnovVar(&innovation_var.mag_field[0]); + _ekf.getDragInnovVar(&innovation_var.drag[0]); + _ekf.getAirspeedInnovVar(innovation_var.airspeed); + _ekf.getBetaInnovVar(innovation_var.beta); + _ekf.getHaglInnovVar(innovation_var.hagl); - _ekf.get_output_tracking_error(&innovations.output_tracking_error[0]); + // publish estimator innovation test ratio data + estimator_innovations_s test_ratios; + test_ratios.timestamp = now; + _ekf.getGpsVelPosInnovRatio(test_ratios.gps_hvel[0], test_ratios.gps_vvel, test_ratios.gps_hpos[0], + test_ratios.gps_vpos); + _ekf.getEvVelPosInnovRatio(test_ratios.ev_hvel[0], test_ratios.ev_vvel, test_ratios.ev_hpos[0], + test_ratios.ev_vpos); + _ekf.getAuxVelInnovRatio(test_ratios.aux_hvel[0]); + _ekf.getFlowInnovRatio(test_ratios.flow[0]); + _ekf.getHeadingInnovRatio(test_ratios.heading); + _ekf.getMagInnovRatio(test_ratios.mag_field[0]); + _ekf.getDragInnovRatio(&test_ratios.drag[0]); + _ekf.getAirspeedInnovRatio(test_ratios.airspeed); + _ekf.getBetaInnovRatio(test_ratios.beta); + _ekf.getHaglInnovRatio(test_ratios.hagl); // calculate noise filtered velocity innovations which are used for pre-flight checking if (_vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) { @@ -1635,6 +1661,9 @@ void Ekf2::Run() } _estimator_innovations_pub.publish(innovations); + _estimator_innovation_variances_pub.publish(innovation_var); + _estimator_innovation_test_ratios_pub.publish(test_ratios); + } } @@ -1667,7 +1696,7 @@ void Ekf2::fillGpsMsgWithVehicleGpsPosData(gps_message &msg, const vehicle_gps_p void Ekf2::runPreFlightChecks(const float dt, const filter_control_status_u &control_status, const vehicle_status_s &vehicle_status, - const ekf2_innovations_s &innov) + const estimator_innovations_s &innov) { const bool can_observe_heading_in_flight = (vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING); @@ -1675,6 +1704,7 @@ void Ekf2::runPreFlightChecks(const float dt, _preflt_checker.setUsingGpsAiding(control_status.flags.gps); _preflt_checker.setUsingFlowAiding(control_status.flags.opt_flow); _preflt_checker.setUsingEvPosAiding(control_status.flags.ev_pos); + _preflt_checker.setUsingEvVelAiding(control_status.flags.ev_vel); _preflt_checker.update(dt, innov); } @@ -1736,10 +1766,10 @@ bool Ekf2::publish_wind_estimate(const hrt_abstime ×tamp) float wind_var[2]; _ekf.get_wind_velocity(velNE_wind); _ekf.get_wind_velocity_var(wind_var); - _ekf.get_airspeed_innov(&wind_estimate.tas_innov); - _ekf.get_airspeed_innov_var(&wind_estimate.tas_innov_var); - _ekf.get_beta_innov(&wind_estimate.beta_innov); - _ekf.get_beta_innov_var(&wind_estimate.beta_innov_var); + _ekf.getAirspeedInnov(wind_estimate.tas_innov); + _ekf.getAirspeedInnovVar(wind_estimate.tas_innov_var); + _ekf.getBetaInnov(wind_estimate.beta_innov); + _ekf.getBetaInnovVar(wind_estimate.beta_innov_var); wind_estimate.timestamp = timestamp; wind_estimate.windspeed_north = velNE_wind[0]; wind_estimate.windspeed_east = velNE_wind[1]; diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 2dfb544c80..7dc729aa74 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -493,6 +493,8 @@ void BlockLocalPositionEstimator::Run() publishEstimatorStatus(); _pub_innov.get().timestamp = _timeStamp; _pub_innov.update(); + _pub_innov_var.get().timestamp = _timeStamp; + _pub_innov_var.update(); if ((_estimatorInitialized & EST_XY) && (_map_ref.init_done || _param_lpe_fake_origin.get())) { publishGlobalPos(); diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index 9c0dfc9c25..28976e98b9 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -34,7 +34,7 @@ #include #include #include -#include +#include using namespace matrix; using namespace control; @@ -285,7 +285,8 @@ private: uORB::PublicationData _pub_gpos{ORB_ID(vehicle_global_position)}; uORB::PublicationData _pub_odom{ORB_ID(vehicle_odometry)}; uORB::PublicationData _pub_est_status{ORB_ID(estimator_status)}; - uORB::PublicationData _pub_innov{ORB_ID(ekf2_innovations)}; + uORB::PublicationData _pub_innov{ORB_ID(estimator_innovations)}; + uORB::PublicationData _pub_innov_var{ORB_ID(estimator_innovation_variances)}; // map projection struct map_projection_reference_s _map_ref; diff --git a/src/modules/local_position_estimator/sensors/flow.cpp b/src/modules/local_position_estimator/sensors/flow.cpp index 6ba01d3645..8cfd66e233 100644 --- a/src/modules/local_position_estimator/sensors/flow.cpp +++ b/src/modules/local_position_estimator/sensors/flow.cpp @@ -172,10 +172,10 @@ void BlockLocalPositionEstimator::flowCorrect() Matrix S = C * m_P * C.transpose() + R; // publish innovations - _pub_innov.get().flow_innov[0] = r(0); - _pub_innov.get().flow_innov[1] = r(1); - _pub_innov.get().flow_innov_var[0] = S(0, 0); - _pub_innov.get().flow_innov_var[1] = S(1, 1); + _pub_innov.get().flow[0] = r(0); + _pub_innov.get().flow[1] = r(1); + _pub_innov_var.get().flow[0] = S(0, 0); + _pub_innov_var.get().flow[1] = S(1, 1); // residual covariance, (inverse) Matrix S_I = inv(S); diff --git a/src/modules/local_position_estimator/sensors/gps.cpp b/src/modules/local_position_estimator/sensors/gps.cpp index a5afa8d6a3..0a512e99a7 100644 --- a/src/modules/local_position_estimator/sensors/gps.cpp +++ b/src/modules/local_position_estimator/sensors/gps.cpp @@ -189,10 +189,20 @@ void BlockLocalPositionEstimator::gpsCorrect() Matrix S = C * m_P * C.transpose() + R; // publish innovations - for (size_t i = 0; i < 6; i++) { - _pub_innov.get().vel_pos_innov[i] = r(i); - _pub_innov.get().vel_pos_innov_var[i] = S(i, i); - } + _pub_innov.get().gps_hvel[0] = r(3); + _pub_innov.get().gps_hvel[1] = r(4); + _pub_innov.get().gps_vvel = r(5); + _pub_innov.get().gps_hpos[0] = r(0); + _pub_innov.get().gps_hpos[1] = r(1); + _pub_innov.get().gps_vpos = r(2); + + // publish innovation variances + _pub_innov_var.get().gps_hvel[0] = S(3, 3); + _pub_innov_var.get().gps_hvel[1] = S(4, 4); + _pub_innov_var.get().gps_vvel = S(5, 5); + _pub_innov_var.get().gps_hpos[0] = S(0, 0); + _pub_innov_var.get().gps_hpos[1] = S(1, 1); + _pub_innov_var.get().gps_vpos = S(2, 2); // residual covariance, (inverse) Matrix S_I = inv(S); diff --git a/src/modules/local_position_estimator/sensors/land.cpp b/src/modules/local_position_estimator/sensors/land.cpp index e307408509..59a6499498 100644 --- a/src/modules/local_position_estimator/sensors/land.cpp +++ b/src/modules/local_position_estimator/sensors/land.cpp @@ -61,8 +61,8 @@ void BlockLocalPositionEstimator::landCorrect() // residual Matrix S_I = inv((C * m_P * C.transpose()) + R); Vector r = y - C * _x; - _pub_innov.get().hagl_innov = r(Y_land_agl); - _pub_innov.get().hagl_innov_var = R(Y_land_agl, Y_land_agl); + _pub_innov.get().hagl = r(Y_land_agl); + _pub_innov_var.get().hagl = R(Y_land_agl, Y_land_agl); // fault detection float beta = (r.transpose() * (S_I * r))(0, 0); diff --git a/src/modules/local_position_estimator/sensors/lidar.cpp b/src/modules/local_position_estimator/sensors/lidar.cpp index e0d9240851..a621f02361 100644 --- a/src/modules/local_position_estimator/sensors/lidar.cpp +++ b/src/modules/local_position_estimator/sensors/lidar.cpp @@ -92,8 +92,8 @@ void BlockLocalPositionEstimator::lidarCorrect() Matrix S = C * m_P * C.transpose() + R; // publish innovations - _pub_innov.get().hagl_innov = r(0); - _pub_innov.get().hagl_innov_var = S(0, 0); + _pub_innov.get().hagl = r(0); + _pub_innov_var.get().hagl = S(0, 0); // residual covariance, (inverse) Matrix S_I = inv(S); diff --git a/src/modules/local_position_estimator/sensors/mocap.cpp b/src/modules/local_position_estimator/sensors/mocap.cpp index cb445bbf9f..2349c8f3d4 100644 --- a/src/modules/local_position_estimator/sensors/mocap.cpp +++ b/src/modules/local_position_estimator/sensors/mocap.cpp @@ -145,15 +145,20 @@ void BlockLocalPositionEstimator::mocapCorrect() Matrix S = C * m_P * C.transpose() + R; // publish innovations - for (size_t i = 0; i < 3; i++) { - _pub_innov.get().vel_pos_innov[i] = r(i); - _pub_innov.get().vel_pos_innov_var[i] = S(i, i); - } + _pub_innov.get().ev_hvel[0] = NAN; + _pub_innov.get().ev_hvel[1] = NAN; + _pub_innov.get().ev_vvel = NAN; + _pub_innov.get().ev_hpos[0] = r(0); + _pub_innov.get().ev_hpos[1] = r(1); + _pub_innov.get().ev_vpos = r(2); - for (size_t i = 3; i < 6; i++) { - _pub_innov.get().vel_pos_innov[i] = 0; - _pub_innov.get().vel_pos_innov_var[i] = 1; - } + // publish innovation variances + _pub_innov_var.get().ev_hvel[0] = NAN; + _pub_innov_var.get().ev_hvel[1] = NAN; + _pub_innov_var.get().ev_vvel = NAN; + _pub_innov_var.get().ev_hpos[0] = S(0, 0); + _pub_innov_var.get().ev_hpos[1] = S(1, 1); + _pub_innov_var.get().ev_vpos = S(2, 2); // residual covariance, (inverse) Matrix S_I = inv(S); diff --git a/src/modules/local_position_estimator/sensors/sonar.cpp b/src/modules/local_position_estimator/sensors/sonar.cpp index 47489a6fa2..3caaf32825 100644 --- a/src/modules/local_position_estimator/sensors/sonar.cpp +++ b/src/modules/local_position_estimator/sensors/sonar.cpp @@ -112,8 +112,8 @@ void BlockLocalPositionEstimator::sonarCorrect() Matrix S = C * m_P * C.transpose() + R; // publish innovations - _pub_innov.get().hagl_innov = r(0); - _pub_innov.get().hagl_innov_var = S(0, 0); + _pub_innov.get().hagl = r(0); + _pub_innov_var.get().hagl = S(0, 0); // residual covariance, (inverse) Matrix S_I = inv(S); diff --git a/src/modules/local_position_estimator/sensors/vision.cpp b/src/modules/local_position_estimator/sensors/vision.cpp index 9ba4e76a20..8172caee96 100644 --- a/src/modules/local_position_estimator/sensors/vision.cpp +++ b/src/modules/local_position_estimator/sensors/vision.cpp @@ -162,15 +162,20 @@ void BlockLocalPositionEstimator::visionCorrect() Matrix S = C * m_P * C.transpose() + R; // publish innovations - for (size_t i = 0; i < 3; i++) { - _pub_innov.get().vel_pos_innov[i] = r(i, 0); - _pub_innov.get().vel_pos_innov_var[i] = S(i, i); - } + _pub_innov.get().ev_hvel[0] = NAN; + _pub_innov.get().ev_hvel[1] = NAN; + _pub_innov.get().ev_vvel = NAN; + _pub_innov.get().ev_hpos[0] = r(0, 0); + _pub_innov.get().ev_hpos[1] = r(1, 0); + _pub_innov.get().ev_vpos = r(2, 0); - for (size_t i = 3; i < 6; i++) { - _pub_innov.get().vel_pos_innov[i] = 0; - _pub_innov.get().vel_pos_innov_var[i] = 1; - } + // publish innovation variances + _pub_innov_var.get().ev_hvel[0] = NAN; + _pub_innov_var.get().ev_hvel[1] = NAN; + _pub_innov_var.get().ev_vvel = NAN; + _pub_innov_var.get().ev_hpos[0] = S(0, 0); + _pub_innov_var.get().ev_hpos[1] = S(1, 1); + _pub_innov_var.get().ev_vpos = S(2, 2); // residual covariance, (inverse) Matrix S_I = inv(S); diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 55103beeba..810b336dd4 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -516,7 +516,9 @@ void Logger::add_default_topics() add_topic("camera_trigger"); add_topic("camera_trigger_secondary"); add_topic("cpuload"); - add_topic("ekf2_innovations", 200); + add_topic("estimator_innovations", 200); + add_topic("estimator_innovation_variances", 200); + add_topic("estimator_innovation_test_ratios", 200); add_topic("ekf_gps_drift"); add_topic("esc_status", 250); add_topic("estimator_status", 200);