mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-05-28 10:46:33 +08:00
ekf2_post-processing: use estimator_status_flags instead of bitmasks
This commit is contained in:
bresch
committed by
Mathieu Bresciani
Mathieu Bresciani
parent
016b8aeb35
commit
d04a91a3ae
@@ -123,7 +123,8 @@ def perform_sensor_innov_checks(
|
||||
('magy', 'magy_fail_percentage', 'mag'),
|
||||
('magz', 'magz_fail_percentage', 'mag'),
|
||||
('yaw', 'yaw_fail_percentage', 'yaw'),
|
||||
('vel', 'vel_fail_percentage', 'vel'),
|
||||
('velh', 'vel_fail_percentage', 'vel'),
|
||||
('velv', 'vel_fail_percentage', 'vel'),
|
||||
('posh', 'pos_fail_percentage', 'pos'),
|
||||
('tas', 'tas_fail_percentage', 'tas'),
|
||||
('hagl', 'hagl_fail_percentage', 'hagl'),
|
||||
|
||||
@@ -11,7 +11,7 @@ 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],
|
||||
ulog: ULog, estimator_status_flags: Dict[str, float], innov_fail_checks: List[str],
|
||||
sensor_checks: List[str], in_air: InAirDetector, in_air_no_ground_effects: InAirDetector,
|
||||
multi_instance: int = 0, red_thresh: float = 1.0, amb_thresh: float = 0.5) -> Tuple[dict, dict, dict, dict]:
|
||||
|
||||
@@ -20,7 +20,7 @@ def calculate_ecl_ekf_metrics(
|
||||
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)
|
||||
estimator_status_flags, innov_fail_checks, in_air, in_air_no_ground_effects)
|
||||
|
||||
imu_metrics = calculate_imu_metrics(ulog, multi_instance, in_air_no_ground_effects)
|
||||
|
||||
@@ -90,10 +90,10 @@ def calculate_sensor_metrics(
|
||||
|
||||
|
||||
def calculate_innov_fail_metrics(
|
||||
innov_flags: dict, innov_fail_checks: List[str], in_air: InAirDetector,
|
||||
estimator_status_flags: dict, innov_fail_checks: List[str], in_air: InAirDetector,
|
||||
in_air_no_ground_effects: InAirDetector) -> dict:
|
||||
"""
|
||||
:param innov_flags:
|
||||
:param estimator_status_flags:
|
||||
:param innov_fail_checks:
|
||||
:param in_air:
|
||||
:param in_air_no_ground_effects:
|
||||
@@ -103,17 +103,18 @@ def calculate_innov_fail_metrics(
|
||||
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')]:
|
||||
for signal_id, signal, result in [('posv', 'reject_ver_pos', 'hgt_fail_percentage'),
|
||||
('magx', 'reject_mag_x', 'magx_fail_percentage'),
|
||||
('magy', 'reject_mag_y', 'magy_fail_percentage'),
|
||||
('magz', 'reject_mag_z', 'magz_fail_percentage'),
|
||||
('yaw', 'reject_yaw', 'yaw_fail_percentage'),
|
||||
('velh', 'reject_hor_vel', 'vel_fail_percentage'),
|
||||
('velv', 'reject_ver_vel', 'vel_fail_percentage'),
|
||||
('posh', 'reject_hor_pos', 'pos_fail_percentage'),
|
||||
('tas', 'reject_airspeed', 'tas_fail_percentage'),
|
||||
('hagl', 'reject_hagl', 'hagl_fail_percentage'),
|
||||
('ofx', 'reject_optflow_x', 'ofx_fail_percentage'),
|
||||
('ofy', 'reject_optflow_y', 'ofy_fail_percentage')]:
|
||||
|
||||
# only run innov fail checks, if they apply.
|
||||
if signal_id in innov_fail_checks:
|
||||
@@ -125,7 +126,7 @@ def calculate_innov_fail_metrics(
|
||||
in_air_detector = in_air
|
||||
|
||||
innov_fail_metrics[result] = calculate_stat_from_signal(
|
||||
innov_flags, 'estimator_status', signal, in_air_detector,
|
||||
estimator_status_flags, 'estimator_status_flags', signal, in_air_detector,
|
||||
lambda x: 100.0 * np.mean(x > 0.5))
|
||||
|
||||
return innov_fail_metrics
|
||||
@@ -152,7 +153,7 @@ def calculate_imu_metrics(ulog: ULog, multi_instance, in_air_no_ground_effects:
|
||||
|
||||
if vehicle_imu_status_data['accel_device_id'][0] == estimator_status_data['accel_device_id'][0]:
|
||||
|
||||
for signal, result in [('delta_angle_coning_metric', 'imu_coning'),
|
||||
for signal, result in [('gyro_coning_vibration', 'imu_coning'),
|
||||
('gyro_vibration_metric', 'imu_hfgyro'),
|
||||
('accel_vibration_metric', 'imu_hfaccel')]:
|
||||
|
||||
|
||||
@@ -7,115 +7,6 @@ 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
|
||||
# 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
|
||||
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
|
||||
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:
|
||||
: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:
|
||||
|
||||
Reference in New Issue
Block a user