From 5be4d1fefb7e40e7d93d8f1743c7cb02394de077 Mon Sep 17 00:00:00 2001 From: Leah Kiner Date: Mon, 30 Sep 2024 15:26:50 -0600 Subject: [PATCH] Update inertialAttitudeUkf unit test --- .../tests/test_inertialAttitudeUkf.py | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/src/fswAlgorithms/attDetermination/inertialAttitudeUkf/tests/test_inertialAttitudeUkf.py b/src/fswAlgorithms/attDetermination/inertialAttitudeUkf/tests/test_inertialAttitudeUkf.py index ead5a8be9a..184ca1364c 100644 --- a/src/fswAlgorithms/attDetermination/inertialAttitudeUkf/tests/test_inertialAttitudeUkf.py +++ b/src/fswAlgorithms/attDetermination/inertialAttitudeUkf/tests/test_inertialAttitudeUkf.py @@ -20,6 +20,7 @@ import pytest from Basilisk.architecture import messaging from Basilisk.fswAlgorithms import inertialAttitudeUkf +from Basilisk.fswAlgorithms import miruLowPassFilterConverter from Basilisk.utilities import SimulationBaseClass, macros from Basilisk.utilities import RigidBodyKinematics as rbk @@ -107,6 +108,11 @@ def test_propagation_kf(show_plots): test_process = unit_test_sim.CreateNewProcess(unit_process_name) test_process.addTask(unit_test_sim.CreateNewTask(unit_task_name, test_process_rate)) + # Create miruLowPassFilterConverter module + miru_low_pass_filter_converter = miruLowPassFilterConverter.MiruLowPassFilterConverter() + miru_low_pass_filter_converter.setLowPassFilter(0.5, 15/(2*np.pi)) + unit_test_sim.AddModelToTask(unit_task_name, miru_low_pass_filter_converter) + # Construct algorithm and associated C++ container allMeasurements = inertialAttitudeUkf.AttitudeFilterMethod_AllMeasurements intertialAttitudeFilter = inertialAttitudeUkf.InertialAttitudeUkf(allMeasurements) @@ -173,7 +179,7 @@ def test_propagation_kf(show_plots): accel_data = messaging.AccDataMsgPayload() accel_measurement = messaging.AccDataMsg().write(accel_data) - intertialAttitudeFilter.accelDataMsg.subscribeTo(accel_measurement) + miru_low_pass_filter_converter.imuAccelDataInMsg.subscribeTo(accel_measurement) attitude_data_log = intertialAttitudeFilter.inertialFilterOutputMsg.recorder() unit_test_sim.AddModelToTask(unit_task_name, attitude_data_log) @@ -226,6 +232,11 @@ def test_measurements_kf(show_plots, initial_error, method): test_process = unit_test_sim.CreateNewProcess(unit_process_name) test_process.addTask(unit_test_sim.CreateNewTask(unit_task_name, test_process_rate)) + # Create miruLowPassFilterConverter module + miru_low_pass_filter_converter = miruLowPassFilterConverter.MiruLowPassFilterConverter() + miru_low_pass_filter_converter.setLowPassFilter(0.5, 15/(2*np.pi)) + unit_test_sim.AddModelToTask(unit_task_name, miru_low_pass_filter_converter) + intertialAttitudeFilter = inertialAttitudeUkf.InertialAttitudeUkf(method) unit_test_sim.AddModelToTask(unit_task_name, intertialAttitudeFilter) @@ -292,9 +303,13 @@ def test_measurements_kf(show_plots, initial_error, method): star_tracker2.measurementNoise = [[1e-4, 0, 0], [0,1e-4,0], [0,0,1e-4]] intertialAttitudeFilter.addStarTrackerInput(star_tracker2) + imu_sensor_msg = messaging.IMUSensorMsgPayload() + imu_sensor_message = messaging.IMUSensorMsg().write(imu_sensor_msg) + intertialAttitudeFilter.imuSensorDataInMsg.subscribeTo(imu_sensor_message) + accel_data = messaging.AccDataMsgPayload() accel_measurement = messaging.AccDataMsg().write(accel_data) - intertialAttitudeFilter.accelDataMsg.subscribeTo(accel_measurement) + miru_low_pass_filter_converter.imuAccelDataInMsg.subscribeTo(accel_measurement) filter_data_log = intertialAttitudeFilter.inertialFilterOutputMsg.recorder() unit_test_sim.AddModelToTask(unit_task_name, filter_data_log)