fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 21:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

estimator_status split out estimator_states

Browse Source
This commit is contained in:
Daniel Agar
2020-09-04 10:48:26 -04:00
parent 33a7fed240
commit 9ccc1db649
17 changed files with 150 additions and 108 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Tools/ecl_ekf/analysis/data_version_handler.py
+1 -1
View File
@@ -12,7 +12,7 @@ def get_output_tracking_error_message(ulog: ULog) -> str:
:param ulog:
:return: str
"""
for elem in ulog.data_list:
for elem in ulog.data_list:
if elem.name == "ekf2_innovations":
return "ekf2_innovations"
if elem.name == "estimator_innovations":
Tools/ecl_ekf/analysis/metrics.py
+4 -2
View File
@@ -158,11 +158,13 @@ def calculate_imu_metrics(
in_air_no_ground_effects, np.mean)
# IMU bias checks
estimator_states_data = ulog.get_dataset('estimator_states').data
imu_metrics['imu_dang_bias_median'] = np.sqrt(np.sum([np.square(calculate_stat_from_signal(
estimator_status_data, 'estimator_status', signal, in_air_no_ground_effects, np.median))
estimator_states_data, 'estimator_states', signal, in_air_no_ground_effects, np.median))
for signal in ['states[10]', 'states[11]', 'states[12]']]))
imu_metrics['imu_dvel_bias_median'] = np.sqrt(np.sum([np.square(calculate_stat_from_signal(
estimator_status_data, 'estimator_status', signal, in_air_no_ground_effects, np.median))
estimator_states_data, 'estimator_states', signal, in_air_no_ground_effects, np.median))
for signal in ['states[13]', 'states[14]', 'states[15]']]))
return imu_metrics
Tools/ecl_ekf/analysis/post_processing.py
+7 -7
View File
@@ -146,18 +146,18 @@ def get_gps_check_fail_flags(estimator_status: dict) -> dict:
return gps_fail_flags
def magnetic_field_estimates_from_status(estimator_status: dict) -> Tuple[float, float, float]:
def magnetic_field_estimates_from_status(estimator_states: dict) -> Tuple[float, float, float]:
"""
:param estimator_status:
:param estimator_states:
:return:
"""
rad2deg = 57.2958
field_strength = np.sqrt(
estimator_status['states[16]'] ** 2 + estimator_status['states[17]'] ** 2 +
estimator_status['states[18]'] ** 2)
declination = rad2deg * np.arctan2(estimator_status['states[17]'],
estimator_status['states[16]'])
estimator_states['states[16]'] ** 2 + estimator_states['states[17]'] ** 2 +
estimator_states['states[18]'] ** 2)
declination = rad2deg * np.arctan2(estimator_states['states[17]'],
estimator_states['states[16]'])
inclination = rad2deg * np.arcsin(
estimator_status['states[18]'] / np.maximum(field_strength, np.finfo(np.float32).eps))
estimator_states['states[18]'] / np.maximum(field_strength, np.finfo(np.float32).eps))
return declination, field_strength, inclination