Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 19:32:36 +08:00
Code Issues Actions 10 Packages Projects Releases Wiki Activity

Multi-EKF support (ekf2)

Browse Source 
- ekf2 can now run in multi-instance mode (currently up to 9 instances)
    - in multi mode all estimates are published to alternate topics (eg estimator_attitude instead of vehicle_attitude)
 - new ekf2 selector runs in multi-instance mode to monitor and compare all instances, selecting a primary (eg N x estimator_attitude => vehicle_attitude)
 - sensors module accel & gyro inconsistency checks are now relative to the mean of all instances, rather than the current primary (when active ekf2 selector is responsible for choosing primary accel & gyro)
 - existing consumers of estimator_status must check estimator_selector_status to select current primary instance status
 - ekf2 single instance mode is still fully supported and the default

Co-authored-by: Paul Riseborough <gncsolns@gmail.com>
This commit is contained in:
Daniel Agar
2020-10-27 10:56:11 -04:00
committed by GitHub
parent d5245a22d3
commit 0f411d6820
56 changed files with 1747 additions and 295 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Tools/ecl_ekf/analysis/metrics.py
+9 -10
View File
@@ -13,7 +13,7 @@ from analysis.detectors import InAirDetector
def calculate_ecl_ekf_metrics(
ulog: ULog, innov_flags: Dict[str, float], innov_fail_checks: List[str],
sensor_checks: List[str], in_air: InAirDetector, in_air_no_ground_effects: InAirDetector,
red_thresh: float = 1.0, amb_thresh: float = 0.5) -> Tuple[dict, dict, dict, dict]:
multi_instance: int = 0, red_thresh: float = 1.0, amb_thresh: float = 0.5) -> Tuple[dict, dict, dict, dict]:
sensor_metrics = calculate_sensor_metrics(
ulog, sensor_checks, in_air, in_air_no_ground_effects,
@@ -22,9 +22,9 @@ def calculate_ecl_ekf_metrics(
innov_fail_metrics = calculate_innov_fail_metrics(
innov_flags, innov_fail_checks, in_air, in_air_no_ground_effects)
imu_metrics = calculate_imu_metrics(ulog, in_air_no_ground_effects)
imu_metrics = calculate_imu_metrics(ulog, multi_instance, in_air_no_ground_effects)
estimator_status_data = ulog.get_dataset('estimator_status').data
estimator_status_data = ulog.get_dataset('estimator_status', multi_instance).data
# Check for internal filter nummerical faults
ekf_metrics = {'filter_faults_max': np.amax(estimator_status_data['filter_fault_flags'])}
@@ -44,10 +44,10 @@ def calculate_ecl_ekf_metrics(
def calculate_sensor_metrics(
ulog: ULog, sensor_checks: List[str], in_air: InAirDetector,
in_air_no_ground_effects: InAirDetector, red_thresh: float = 1.0,
amb_thresh: float = 0.5) -> Dict[str, float]:
in_air_no_ground_effects: InAirDetector, multi_instance: int = 0,
red_thresh: float = 1.0, amb_thresh: float = 0.5) -> Dict[str, float]:
estimator_status_data = ulog.get_dataset('estimator_status').data
estimator_status_data = ulog.get_dataset('estimator_status', multi_instance).data
sensor_metrics = dict()
@@ -131,10 +131,9 @@ def calculate_innov_fail_metrics(
return innov_fail_metrics
def calculate_imu_metrics(
ulog: ULog, in_air_no_ground_effects: InAirDetector) -> dict:
def calculate_imu_metrics(ulog: ULog, multi_instance, in_air_no_ground_effects: InAirDetector) -> dict:
estimator_status_data = ulog.get_dataset('estimator_status').data
estimator_status_data = ulog.get_dataset('estimator_status', multi_instance).data
imu_metrics = dict()
@@ -158,7 +157,7 @@ def calculate_imu_metrics(
in_air_no_ground_effects, np.mean)
# IMU bias checks
estimator_states_data = ulog.get_dataset('estimator_states').data
estimator_states_data = ulog.get_dataset('estimator_states', multi_instance).data
imu_metrics['imu_dang_bias_median'] = np.sqrt(np.sum([np.square(calculate_stat_from_signal(
estimator_states_data, 'estimator_states', signal, in_air_no_ground_effects, np.median))