mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-03-27 09:33:51 +08:00
ECL: Clean velPos logging, deprecate ekf2_innovations msg
This commit is contained in:
kamilritz
Julian Kent
@@ -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(
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
48
Tools/ecl_ekf/analysis/data_version_handler.py
Normal file
48
Tools/ecl_ekf/analysis/data_version_handler.py
Normal file
@@ -0,0 +1,48 @@
|
||||
#! /usr/bin/env python3
|
||||
"""
|
||||
function collection for handling different versions of log files
|
||||
"""
|
||||
from pyulog import ULog
|
||||
|
||||
from analysis.detectors import PreconditionError
|
||||
|
||||
def get_output_tracking_error_message(ulog: ULog) -> str:
|
||||
"""
|
||||
return the name of the message containing the output_tracking_error
|
||||
:param ulog:
|
||||
:return: str
|
||||
"""
|
||||
for elem in ulog.data_list:
|
||||
if elem.name == "ekf2_innovations":
|
||||
return "ekf2_innovations"
|
||||
if elem.name == "estimator_innovations":
|
||||
return "estimator_status"
|
||||
|
||||
raise PreconditionError("Could not detect the message containing the output tracking error")
|
||||
|
||||
def get_innovation_message(ulog: ULog, topic: str = 'innovation') -> str:
|
||||
"""
|
||||
return the name of the innovation message (old: ekf2_innovations; new: estimator_innovations)
|
||||
:param ulog:
|
||||
:return: str
|
||||
"""
|
||||
if topic == 'innovation':
|
||||
for elem in ulog.data_list:
|
||||
if elem.name == "ekf2_innovations":
|
||||
return "ekf2_innovations"
|
||||
if elem.name == "estimator_innovations":
|
||||
return "estimator_innovations"
|
||||
if topic == 'innovation_variance':
|
||||
for elem in ulog.data_list:
|
||||
if elem.name == "ekf2_innovations":
|
||||
return "ekf2_innovations"
|
||||
if elem.name == "estimator_innovations":
|
||||
return "estimator_innovations"
|
||||
if topic == 'innovation_test_ratio':
|
||||
for elem in ulog.data_list:
|
||||
if elem.name == "ekf2_innovations":
|
||||
return "ekf2_innovations"
|
||||
if elem.name == "estimator_innovations":
|
||||
return "estimator_innovations"
|
||||
|
||||
raise PreconditionError("Could not detect the message")
|
||||
@@ -179,4 +179,4 @@ class InAirDetector(object):
|
||||
(data['timestamp'] - self._ulog.start_timestamp) /
|
||||
1.0e6 < self._in_air[i].landing))[0])
|
||||
|
||||
return airtime
|
||||
return airtime
|
||||
|
||||
@@ -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'),
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -15,6 +15,7 @@ from analysis.post_processing import magnetic_field_estimates_from_status, get_e
|
||||
from plotting.data_plots import TimeSeriesPlot, InnovationPlot, ControlModeSummaryPlot, \
|
||||
CheckFlagsPlot
|
||||
from analysis.detectors import PreconditionError
|
||||
import analysis.data_version_handler as dvh
|
||||
|
||||
def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
|
||||
"""
|
||||
@@ -34,10 +35,15 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
|
||||
raise PreconditionError('could not find estimator_status data')
|
||||
|
||||
try:
|
||||
ekf2_innovations = ulog.get_dataset('ekf2_innovations').data
|
||||
print('found ekf2_innovation data')
|
||||
estimator_innovations = ulog.get_dataset('estimator_innovations').data
|
||||
estimator_innovation_variances = ulog.get_dataset('estimator_innovation_variances').data
|
||||
innovation_data = estimator_innovations
|
||||
for key in estimator_innovation_variances:
|
||||
# append 'var' to the field name such that we can distingush between innov and innov_var
|
||||
innovation_data.update({str('var_'+key): estimator_innovation_variances[key]})
|
||||
print('found innovation data')
|
||||
except:
|
||||
raise PreconditionError('could not find ekf2_innovation data')
|
||||
raise PreconditionError('could not find innovation data')
|
||||
|
||||
try:
|
||||
sensor_preflight = ulog.get_dataset('sensor_preflight').data
|
||||
@@ -67,8 +73,8 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
|
||||
|
||||
# vertical velocity and position innovations
|
||||
data_plot = InnovationPlot(
|
||||
ekf2_innovations, [('vel_pos_innov[2]', 'vel_pos_innov_var[2]'),
|
||||
('vel_pos_innov[5]', 'vel_pos_innov_var[5]')],
|
||||
innovation_data, [('gps_vpos', 'var_gps_vpos'),
|
||||
('gps_vvel', 'var_gps_vvel')],
|
||||
x_labels=['time (sec)', 'time (sec)'],
|
||||
y_labels=['Down Vel (m/s)', 'Down Pos (m)'], plot_title='Vertical Innovations',
|
||||
pdf_handle=pdf_pages)
|
||||
@@ -77,8 +83,8 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
|
||||
|
||||
# horizontal velocity innovations
|
||||
data_plot = InnovationPlot(
|
||||
ekf2_innovations, [('vel_pos_innov[0]', 'vel_pos_innov_var[0]'),
|
||||
('vel_pos_innov[1]','vel_pos_innov_var[1]')],
|
||||
innovation_data, [('gps_hvel[0]', 'var_gps_hvel[0]'),
|
||||
('gps_hvel[1]', 'var_gps_hvel[1]')],
|
||||
x_labels=['time (sec)', 'time (sec)'],
|
||||
y_labels=['North Vel (m/s)', 'East Vel (m/s)'],
|
||||
plot_title='Horizontal Velocity Innovations', pdf_handle=pdf_pages)
|
||||
@@ -87,8 +93,8 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
|
||||
|
||||
# horizontal position innovations
|
||||
data_plot = InnovationPlot(
|
||||
ekf2_innovations, [('vel_pos_innov[3]', 'vel_pos_innov_var[3]'), ('vel_pos_innov[4]',
|
||||
'vel_pos_innov_var[4]')],
|
||||
innovation_data, [('gps_hpos[0]', 'var_gps_hpos[0]'),
|
||||
('gps_hpos[1]', 'var_gps_hpos[1]')],
|
||||
x_labels=['time (sec)', 'time (sec)'],
|
||||
y_labels=['North Pos (m)', 'East Pos (m)'], plot_title='Horizontal Position Innovations',
|
||||
pdf_handle=pdf_pages)
|
||||
@@ -97,8 +103,9 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
|
||||
|
||||
# magnetometer innovations
|
||||
data_plot = InnovationPlot(
|
||||
ekf2_innovations, [('mag_innov[0]', 'mag_innov_var[0]'),
|
||||
('mag_innov[1]', 'mag_innov_var[1]'), ('mag_innov[2]', 'mag_innov_var[2]')],
|
||||
innovation_data, [('mag_field[0]', 'var_mag_field[0]'),
|
||||
('mag_field[1]', 'var_mag_field[1]'),
|
||||
('mag_field[2]', 'var_mag_field[2]')],
|
||||
x_labels=['time (sec)', 'time (sec)', 'time (sec)'],
|
||||
y_labels=['X (Gauss)', 'Y (Gauss)', 'Z (Gauss)'], plot_title='Magnetometer Innovations',
|
||||
pdf_handle=pdf_pages)
|
||||
@@ -107,7 +114,7 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
|
||||
|
||||
# magnetic heading innovations
|
||||
data_plot = InnovationPlot(
|
||||
ekf2_innovations, [('heading_innov', 'heading_innov_var')],
|
||||
innovation_data, [('heading', 'var_heading')],
|
||||
x_labels=['time (sec)'], y_labels=['Heading (rad)'],
|
||||
plot_title='Magnetic Heading Innovations', pdf_handle=pdf_pages)
|
||||
data_plot.save()
|
||||
@@ -115,8 +122,8 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
|
||||
|
||||
# air data innovations
|
||||
data_plot = InnovationPlot(
|
||||
ekf2_innovations,
|
||||
[('airspeed_innov', 'airspeed_innov_var'), ('beta_innov', 'beta_innov_var')],
|
||||
innovation_data, [('airspeed', 'var_airspeed'),
|
||||
('beta', 'var_beta')],
|
||||
x_labels=['time (sec)', 'time (sec)'],
|
||||
y_labels=['innovation (m/sec)', 'innovation (rad)'],
|
||||
sub_titles=['True Airspeed Innovations', 'Synthetic Sideslip Innovations'],
|
||||
@@ -126,8 +133,8 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
|
||||
|
||||
# optical flow innovations
|
||||
data_plot = InnovationPlot(
|
||||
ekf2_innovations, [('flow_innov[0]', 'flow_innov_var[0]'), ('flow_innov[1]',
|
||||
'flow_innov_var[1]')],
|
||||
innovation_data, [('flow[0]', 'var_flow[0]'),
|
||||
('flow[1]', 'var_flow[1]')],
|
||||
x_labels=['time (sec)', 'time (sec)'],
|
||||
y_labels=['X (rad/sec)', 'Y (rad/sec)'],
|
||||
plot_title='Optical Flow Innovations', pdf_handle=pdf_pages)
|
||||
@@ -252,12 +259,12 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
|
||||
|
||||
# Plot the EKF output observer tracking errors
|
||||
scaled_innovations = {
|
||||
'output_tracking_error[0]': 1000. * ekf2_innovations['output_tracking_error[0]'],
|
||||
'output_tracking_error[1]': ekf2_innovations['output_tracking_error[1]'],
|
||||
'output_tracking_error[2]': ekf2_innovations['output_tracking_error[2]']
|
||||
'output_tracking_error[0]': 1000. * estimator_status['output_tracking_error[0]'],
|
||||
'output_tracking_error[1]': estimator_status['output_tracking_error[1]'],
|
||||
'output_tracking_error[2]': estimator_status['output_tracking_error[2]']
|
||||
}
|
||||
data_plot = CheckFlagsPlot(
|
||||
1e-6 * ekf2_innovations['timestamp'], scaled_innovations,
|
||||
1e-6 * estimator_status['timestamp'], scaled_innovations,
|
||||
[['output_tracking_error[0]'], ['output_tracking_error[1]'],
|
||||
['output_tracking_error[2]']], x_label='time (sec)',
|
||||
y_labels=['angles (mrad)', 'velocity (m/s)', 'position (m)'],
|
||||
@@ -350,4 +357,4 @@ def detect_airtime(control_mode, status_time):
|
||||
in_air_duration = float('NaN')
|
||||
else:
|
||||
in_air_duration = float('NaN')
|
||||
return b_finishes_in_air, b_starts_in_air, in_air_duration, in_air_transition_time, on_ground_transition_time
|
||||
return b_finishes_in_air, b_starts_in_air, in_air_duration, in_air_transition_time, on_ground_transition_time
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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)
|
||||
43
msg/estimator_innovations.msg
Normal file
43
msg/estimator_innovations.msg
Normal file
@@ -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
|
||||
@@ -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)
|
||||
|
||||
@@ -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 ##########
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -42,7 +42,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/ekf2_innovations.h>
|
||||
#include <uORB/topics/estimator_innovations.h>
|
||||
|
||||
#include <matrix/matrix/math.hpp>
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -56,7 +56,7 @@
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/airspeed.h>
|
||||
#include <uORB/topics/distance_sensor.h>
|
||||
#include <uORB/topics/ekf2_innovations.h>
|
||||
#include <uORB/topics/estimator_innovations.h>
|
||||
#include <uORB/topics/ekf2_timestamps.h>
|
||||
#include <uORB/topics/ekf_gps_position.h>
|
||||
#include <uORB/topics/estimator_status.h>
|
||||
@@ -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<typename Param>
|
||||
@@ -267,7 +267,9 @@ private:
|
||||
vehicle_land_detected_s _vehicle_land_detected{};
|
||||
vehicle_status_s _vehicle_status{};
|
||||
|
||||
uORB::Publication<ekf2_innovations_s> _estimator_innovations_pub{ORB_ID(ekf2_innovations)};
|
||||
uORB::Publication<estimator_innovations_s> _estimator_innovations_pub{ORB_ID(estimator_innovations)};
|
||||
uORB::Publication<estimator_innovations_s> _estimator_innovation_variances_pub{ORB_ID(estimator_innovation_variances)};
|
||||
uORB::Publication<estimator_innovations_s> _estimator_innovation_test_ratios_pub{ORB_ID(estimator_innovation_test_ratios)};
|
||||
uORB::Publication<ekf2_timestamps_s> _ekf2_timestamps_pub{ORB_ID(ekf2_timestamps)};
|
||||
uORB::Publication<ekf_gps_drift_s> _ekf_gps_drift_pub{ORB_ID(ekf_gps_drift)};
|
||||
uORB::Publication<ekf_gps_position_s> _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];
|
||||
|
||||
@@ -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();
|
||||
|
||||
@@ -34,7 +34,7 @@
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/estimator_status.h>
|
||||
#include <uORB/topics/ekf2_innovations.h>
|
||||
#include <uORB/topics/estimator_innovations.h>
|
||||
|
||||
using namespace matrix;
|
||||
using namespace control;
|
||||
@@ -285,7 +285,8 @@ private:
|
||||
uORB::PublicationData<vehicle_global_position_s> _pub_gpos{ORB_ID(vehicle_global_position)};
|
||||
uORB::PublicationData<vehicle_odometry_s> _pub_odom{ORB_ID(vehicle_odometry)};
|
||||
uORB::PublicationData<estimator_status_s> _pub_est_status{ORB_ID(estimator_status)};
|
||||
uORB::PublicationData<ekf2_innovations_s> _pub_innov{ORB_ID(ekf2_innovations)};
|
||||
uORB::PublicationData<estimator_innovations_s> _pub_innov{ORB_ID(estimator_innovations)};
|
||||
uORB::PublicationData<estimator_innovations_s> _pub_innov_var{ORB_ID(estimator_innovation_variances)};
|
||||
|
||||
// map projection
|
||||
struct map_projection_reference_s _map_ref;
|
||||
|
||||
@@ -172,10 +172,10 @@ void BlockLocalPositionEstimator::flowCorrect()
|
||||
Matrix<float, n_y_flow, n_y_flow> 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<float, n_y_flow, n_y_flow> S_I = inv<float, n_y_flow>(S);
|
||||
|
||||
@@ -189,10 +189,20 @@ void BlockLocalPositionEstimator::gpsCorrect()
|
||||
Matrix<float, n_y_gps, n_y_gps> 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<float, n_y_gps, n_y_gps> S_I = inv<float, n_y_gps>(S);
|
||||
|
||||
@@ -61,8 +61,8 @@ void BlockLocalPositionEstimator::landCorrect()
|
||||
// residual
|
||||
Matrix<float, n_y_land, n_y_land> S_I = inv<float, n_y_land>((C * m_P * C.transpose()) + R);
|
||||
Vector<float, n_y_land> 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);
|
||||
|
||||
@@ -92,8 +92,8 @@ void BlockLocalPositionEstimator::lidarCorrect()
|
||||
Matrix<float, n_y_lidar, n_y_lidar> 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<float, n_y_lidar, n_y_lidar> S_I = inv<float, n_y_lidar>(S);
|
||||
|
||||
@@ -145,15 +145,20 @@ void BlockLocalPositionEstimator::mocapCorrect()
|
||||
Matrix<float, n_y_mocap, n_y_mocap> 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<float, n_y_mocap, n_y_mocap> S_I = inv<float, n_y_mocap>(S);
|
||||
|
||||
@@ -112,8 +112,8 @@ void BlockLocalPositionEstimator::sonarCorrect()
|
||||
Matrix<float, n_y_sonar, n_y_sonar> 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<float, n_y_sonar, n_y_sonar> S_I = inv<float, n_y_sonar>(S);
|
||||
|
||||
@@ -162,15 +162,20 @@ void BlockLocalPositionEstimator::visionCorrect()
|
||||
Matrix<float, n_y_vision, n_y_vision> 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<float, n_y_vision, n_y_vision> S_I = inv<float, n_y_vision>(S);
|
||||
|
||||
@@ -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);
|
||||
|
||||
Reference in New Issue
Block a user