yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00: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-01 14:25:58 -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

6
Tools/ecl_ekf/analyse_logdata_ekf.py
View File

@@ -29,6 +29,12 @@ def analyse_ekf(
:return:
"""
try:
estimator_states = ulog.get_dataset('estimator_states').data
print('found estimator_states data')
except:
raise PreconditionError('could not find estimator_states data')
try:
estimator_status = ulog.get_dataset('estimator_status').data
print('found estimator_status data')

2
Tools/ecl_ekf/analysis/data_version_handler.py
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":

6
Tools/ecl_ekf/analysis/metrics.py
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

14
Tools/ecl_ekf/analysis/post_processing.py
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

4
Tools/ecl_ekf/plotting/pdf_report.py
View File

@@ -275,7 +275,7 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
# Plot the delta angle bias estimates
data_plot = CheckFlagsPlot(
1e-6 * estimator_status['timestamp'], estimator_status,
1e-6 * estimator_states['timestamp'], estimator_states,
[['states[10]'], ['states[11]'], ['states[12]']],
x_label='time (sec)', y_labels=['X (rad)', 'Y (rad)', 'Z (rad)'],
plot_title='Delta Angle Bias Estimates', annotate=False, pdf_handle=pdf_pages)
@@ -284,7 +284,7 @@ def create_pdf_report(ulog: ULog, output_plot_filename: str) -> None:
# Plot the delta velocity bias estimates
data_plot = CheckFlagsPlot(
1e-6 * estimator_status['timestamp'], estimator_status,
1e-6 * estimator_states['timestamp'], estimator_states,
[['states[13]'], ['states[14]'], ['states[15]']],
x_label='time (sec)', y_labels=['X (m/s)', 'Y (m/s)', 'Z (m/s)'],
plot_title='Delta Velocity Bias Estimates', annotate=False, pdf_handle=pdf_pages)