diff --git a/configs/TLI_Oct2019.json b/configs/TLI_Oct2019.json index 70266ff..39d588c 100644 --- a/configs/TLI_Oct2019.json +++ b/configs/TLI_Oct2019.json @@ -7,7 +7,7 @@ "vel_x": -534.084, "vel_y": -3792.878, "vel_z": -867.495, - "ang_vel_x": 4.5, + "h_x": 4.5, "time": 1539102600 }, "models" : ["pos", "gyro"] diff --git a/configs/apollo12SIVB.json b/configs/apollo12SIVB.json index a760159..40976b2 100644 --- a/configs/apollo12SIVB.json +++ b/configs/apollo12SIVB.json @@ -7,7 +7,7 @@ "vel_x": 5106.47906333 , "vel_y": -4728.66201988, "vel_z": 4572.38411289 , - "ang_vel_x": 4.5, + "h_x": 4.5, "time": -4058816 }, "models" : ["pos", "gyro"] diff --git a/configs/attitude_dynamics_test.json b/configs/attitude_dynamics_test.json index 003b574..a2bf7b5 100644 --- a/configs/attitude_dynamics_test.json +++ b/configs/attitude_dynamics_test.json @@ -1,8 +1,9 @@ { "initial_condition": { "x": 100000000.0, - "ang_vel_x": 4.5, - "ang_vel_y": 1.0, + "h_x": 1.5, + "h_y": 0.2, + "h_z": 0.0, "time": 0.0, "quat_v1": 1.0, "quat_v2": 0.0, diff --git a/configs/freefall.json b/configs/freefall.json index ffaafe2..b0a8864 100644 --- a/configs/freefall.json +++ b/configs/freefall.json @@ -4,7 +4,7 @@ "x": 100000000.0, "y": 1000.0, "z": 1000.0, - "ang_vel_x": 4.5, + "h_x": 4.5, "time": 0.0, "quat_v1": 1.0, "quat_v2": 0.0, diff --git a/configs/lro.json b/configs/lro.json index 3e9e30b..b9c0428 100644 --- a/configs/lro.json +++ b/configs/lro.json @@ -7,7 +7,7 @@ "vel_x": 0.2069511992715368, "vel_y": -0.2105451990331415, "vel_z": -1.157255066656339, - "ang_vel_x": 4.5, + "h_x": 4.5, "time": 1651885860 }, "models" : ["pos", "gyro"] diff --git a/configs/roadster.json b/configs/roadster.json index c123109..cde9806 100644 --- a/configs/roadster.json +++ b/configs/roadster.json @@ -7,7 +7,7 @@ "vel_x": -6.837449791362452, "vel_y": -3.855569832386001, "vel_z": -2.658902141013662, - "ang_vel_x": 4.5, + "h_x": 4.5, "time": 1517990400 }, "models" : ["pos", "gyro"] diff --git a/configs/schema.json b/configs/schema.json index b3fac1a..35d65ba 100644 --- a/configs/schema.json +++ b/configs/schema.json @@ -42,13 +42,13 @@ "initial_condition": { "type": "object", "properties": { - "ang_vel_x": { + "h_x": { "type": "number" }, - "ang_vel_y": { + "h_y": { "type": "number" }, - "ang_vel_z": { + "h_z": { "type": "number" }, "quat_v1": { diff --git a/configs/test_angles.json b/configs/test_angles.json index 2c8d176..1ce56f5 100644 --- a/configs/test_angles.json +++ b/configs/test_angles.json @@ -1,9 +1,9 @@ { "initial_condition": { - "ang_vel_x" : 5.0, - "ang_vel_y" : 5.0, - "ang_vel_z" : 5.0, + "h_x" : 5.0, + "h_y" : 5.0, + "h_z" : 5.0, "quat_v1" : 10.0, "quat_v2" : 10.0, diff --git a/configs/test_zeroes.json b/configs/test_zeroes.json index 0d63bce..7e76106 100644 --- a/configs/test_zeroes.json +++ b/configs/test_zeroes.json @@ -10,9 +10,9 @@ }, "initial_condition": { - "ang_vel_x" : 0.0, - "ang_vel_y" : 0.0, - "ang_vel_z" : 0.0, + "h_x" : 0.0, + "h_y" : 0.0, + "h_z" : 0.0, "quat_v1" : 0.0, "quat_v2" : 0.0, diff --git a/configs/tli.json b/configs/tli.json index 70266ff..39d588c 100644 --- a/configs/tli.json +++ b/configs/tli.json @@ -7,7 +7,7 @@ "vel_x": -534.084, "vel_y": -3792.878, "vel_z": -867.495, - "ang_vel_x": 4.5, + "h_x": 4.5, "time": 1539102600 }, "models" : ["pos", "gyro"] diff --git a/src/core/models/derived_models.py b/src/core/models/derived_models.py index 3afec12..00d03b3 100644 --- a/src/core/models/derived_models.py +++ b/src/core/models/derived_models.py @@ -1,11 +1,10 @@ from abc import abstractmethod from core.state.state import State from utils.astropy_util import get_body_position -from typing import Dict import numpy as np from utils.constants import BodyEnum from core.models.model_base import Model -from typing import Union, Tuple, List +from typing import Union, Tuple, List, Dict class DerivedStateModel(Model): @@ -101,4 +100,85 @@ def evaluate(self, t: float, state: State) -> Dict[str, np.ndarray]: } -DERIVED_MODEL_LIST: List[DerivedStateModel] = [DerivedPosition(), DerivedAttitude()] +class InertiaModel(DerivedStateModel): + def evaluate(self, _: float, state: State) -> Dict[str, Union[float, int, bool]]: + """the _b postfix means the value is in the spacecraft's body frame""" + fill_frac = state.fill_frac + + dcm = np.array([[0, 1, 0], [0, 0, -1], [-1, 0, 0]], dtype=np.int32) + dcmT = np.transpose(dcm) + # not sure why we do the above. Did i do that? + # probably to get the (incorrect) solidworks frame into the spacecraft + # body frame + + # Inertia tensor when full. Structure is: + # [[Ixx, Ixy, Ixz], + # [Iyx, Iyy, Iyz], + # [Izx, Izy, Izz]]. + # Units are (kg * m^2). + idf = ( + np.array( + [ + [933513642.20, 260948256.18, 430810000.30], + [260948256.18, 1070855457.07, 387172545.62], + [430810000.30, 387172545.62, 629606813.62], + ], + dtype=np.float64, + ) + * 1e-9 + ) + # inertia tensor in correct body frame + idf_b = np.matmul(np.matmul(dcm, idf), dcmT) + + # Inertia tensor at 125 mL. Structure is: + # [[Ixx, Ixy, Ixz], + # [Iyx, Iyy, Iyz], + # [Izx, Izy, Izz]]. + # Units are (kg * m^2). + idi = ( + np.array( + [ + [855858994.14, 229481961.55, 377087149.13], + [229481961.55, 963124288.81, 353943859.15], + [377087149.13, 353943859.15, 559805590.96], + ], + dtype=np.float64, + ) + * 1e-9 + ) + # inertia tensor in correct body frame + idi_b = np.matmul(np.matmul(dcm, idi), dcmT) + + # Determine inertia tensor for Oxygen via linear interpolation as a function of fill + # fraction. + + # TODO: determine whether linear fit is accurate enough + ineria_matrix_b = (idf_b - idi_b) * fill_frac + idi_b + + # now that we know our moment of inertia (I matrix), we can calculate our angular + # velocities: + # page 14 of the 4060 Attitude Dynamics handout: + # h = I dot omega + # --> omega = inverse(I) matmul h + + angular_momentum = np.array([[state.h_x, state.h_y, state.h_z]]) + + angular_velocity = np.matmul(np.linalg.inv(ineria_matrix_b), angular_momentum.T) + + return { + "Ixx": ineria_matrix_b[0][0], + "Ixy": ineria_matrix_b[0][1], + "Ixz": ineria_matrix_b[0][2], + "Iyx": ineria_matrix_b[1][0], + "Iyy": ineria_matrix_b[1][1], + "Iyz": ineria_matrix_b[1][2], + "Izx": ineria_matrix_b[2][0], + "Izy": ineria_matrix_b[2][1], + "Izz": ineria_matrix_b[2][2], + "ang_vel_x": angular_velocity[0][0], + "ang_vel_y": angular_velocity[1][0], + "ang_vel_z": angular_velocity[2][0], + } + + +DERIVED_MODEL_LIST: List[DerivedStateModel] = [DerivedPosition(), DerivedAttitude(), InertiaModel()] diff --git a/src/core/models/dynamics_model.py b/src/core/models/dynamics_model.py index 11bcd49..b475236 100644 --- a/src/core/models/dynamics_model.py +++ b/src/core/models/dynamics_model.py @@ -7,68 +7,6 @@ from utils.gnc_utils import quaternion_derivative -class InertiaModel(DerivedStateModel): - def evaluate(self, state: State) -> Dict[str, Any]: - fill_frac = state.fill_frac - - dcm = np.array([[0, 1, 0], [0, 0, -1], [-1, 0, 0]], dtype=np.int32) - dcmT = np.transpose(dcm) - - # Inertia tensor when full. Structure is: - # [[Ixx, Ixy, Ixz], - # [Iyx, Iyy, Iyz], - # [Izx, Izy, Izz]]. - # Units are (kg * m^2). - idf = ( - np.array( - [ - [933513642.20, 260948256.18, 430810000.30], - [260948256.18, 1070855457.07, 387172545.62], - [430810000.30, 387172545.62, 629606813.62], - ], - dtype=np.float64, - ) - * 1e-9 - ) - idf_b = np.matmul(np.matmul(dcm, idf), dcmT) - - # Inertia tensor at 125 mL. Structure is: - # [[Ixx, Ixy, Ixz], - # [Iyx, Iyy, Iyz], - # [Izx, Izy, Izz]]. - # Units are (kg * m^2). - idi = ( - np.array( - [ - [855858994.14, 229481961.55, 377087149.13], - [229481961.55, 963124288.81, 353943859.15], - [377087149.13, 353943859.15, 559805590.96], - ], - dtype=np.float64, - ) - * 1e-9 - ) - idi_b = np.matmul(np.matmul(dcm, idi), dcmT) - - # Determine inertia tensor for Oxygen via linear interpolation as a function of fill - # fraction. - - # TODO: determine whether linear fit is accurate enough - ioxy = (idf_b - idi_b) * fill_frac + idi_b - - return { - "Ixx": ioxy[0][0], - "Ixy": ioxy[0][1], - "Ixz": ioxy[0][2], - "Iyx": ioxy[1][0], - "Iyy": ioxy[1][1], - "Iyz": ioxy[1][2], - "Izx": ioxy[2][0], - "Izy": ioxy[2][1], - "Izz": ioxy[2][2], - } - - class KaneModel(DerivedStateModel): """Calculates the Kane damping coefficient from 2016 simulation data by K. Doyle.""" @@ -108,13 +46,15 @@ def d_state(self, state_time: StateTime) -> Dict[str, Any]: """ s = state_time.state - # d = state_time.derived_state + d = state_time.derived_state cur_quat = np.array([s.quat_v1, s.quat_v2, s.quat_v3, s.quat_r]) - angular_vel = np.array([s.ang_vel_x, s.ang_vel_y, s.ang_vel_z]) - + angular_vel = np.array([d.ang_vel_x, d.ang_vel_y, d.ang_vel_z]) d_quat = quaternion_derivative(cur_quat, angular_vel) - # TODO: Use angular momentum as state variable and for most dynamics evaluation + # TODONE: Use angular momentum as state variable and for most dynamics evaluation # then calculate angular rates from momenta b/c inertia matricies change over time + # TODO: track external moments somewhere in state + # external_moments = np.array([[0,0,0]]) + return {"quat_v1": d_quat[0], "quat_v2": d_quat[1], "quat_v3": d_quat[2], "quat_r": d_quat[3]} diff --git a/src/core/models/gyro_model.py b/src/core/models/gyro_model.py index ef280c9..75e1449 100644 --- a/src/core/models/gyro_model.py +++ b/src/core/models/gyro_model.py @@ -27,22 +27,28 @@ def evaluate(self, state_time: StateTime) -> Dict[str, Any]: Dict[str, Any]: The augmented angular velocities """ - # setting up the initial angular velocity using x, y, z components - ang_vel_i = np.array([state_time.state.ang_vel_x, state_time.state.ang_vel_y, state_time.state.ang_vel_z]) - - # adding initial angular velocity to gyro_bias - ang_vel_d = ang_vel_i + self.gyro_bias - ang_vel_d = np.random.normal(loc=ang_vel_d, scale=self.gyro_noise, size=3) - - #filling in the array, applying gyro sensitivity + # smush the x, y, z components from state into a vector + # (because vectorization is cool as hecc) + ang_vel_true = np.array( + [state_time.derived_state.ang_vel_x, + state_time.derived_state.ang_vel_y, + state_time.derived_state.ang_vel_z] + ) + + # add gyro's bias + ang_vel_biased = ang_vel_true + self.gyro_bias + # add gyro's noise + ang_vel_observed = np.random.normal(loc=ang_vel_biased, scale=self.gyro_noise, size=3) + + # filling in the array, applying gyro sensitivity for i in range(3): - ang_vel_d[i] = self._parameters.gyro_sensitivity * int( - self._parameters.gyro_sensitivity / 2 + ang_vel_d[i] / self._parameters.gyro_sensitivity + ang_vel_observed[i] = self._parameters.gyro_sensitivity * int( + self._parameters.gyro_sensitivity / 2 + ang_vel_observed[i] / self._parameters.gyro_sensitivity ) - #return components of the angular velocity + # return components of the angular velocity return { - "ang_vel_x": ang_vel_d[0], - "ang_vel_y": ang_vel_d[1], - "ang_vel_z": ang_vel_d[2], + "ang_vel_x": ang_vel_observed[0], + "ang_vel_y": ang_vel_observed[1], + "ang_vel_z": ang_vel_observed[2], } diff --git a/src/core/parameters.py b/src/core/parameters.py index 82a8b2f..f49df90 100644 --- a/src/core/parameters.py +++ b/src/core/parameters.py @@ -4,7 +4,7 @@ class Parameters: """This is a container class for all parameters as defined in this sheet: - https://cornell.box.com/s/z20wbp66q0pseqievmadf515ucd971g2. + https://cornell.box.com/s/z20wbp66q0pseqievmadf515ucd971g2. """ def __init__(self, param_dict: Dict = {}): @@ -15,10 +15,10 @@ def __init__(self, param_dict: Dict = {}): self.dry_mass = 0 self.com = 0 - + # fuel tank self.tank_volume = 1 - self.electolyzer_rate = 10.0 * (1/1000) + self.electolyzer_rate = 10.0 * (1 / 1000) # prop self.thruster_force = 0 diff --git a/src/core/sim.py b/src/core/sim.py index 806925c..a2d8c15 100644 --- a/src/core/sim.py +++ b/src/core/sim.py @@ -11,20 +11,21 @@ from sys import getsizeof from utils.constants import SHRD_MEM_NAME + class CislunarSim: - """This class consolidates all parts of the sim (config, models, state). It is responsible for + """This class consolidates all parts of the sim (config, models, state). It is responsible for stepping the sim and checking stop conditions. """ def __init__(self, config: Config) -> None: self._config = config - self._models = ModelContainer(self._config) #wouldn't need for event-based + self._models = ModelContainer(self._config) # wouldn't need for event-based self.state_time: StateTime = self._config.init_cond self.observed_state = ObservedState() self.should_run = True self.num_iters = 0 - self.event_queue : "Queue[Event]" = Queue() + self.event_queue: "Queue[Event]" = Queue() def step(self) -> PropagatedOutput: """step() is the combined true and observed state after one step.""" @@ -37,10 +38,10 @@ def step(self) -> PropagatedOutput: # Feed the current observed state of the simulator into the shared memory. observed_array = self.observed_state.to_array() try: - shm = shared_memory.SharedMemory(name=SHRD_MEM_NAME) + shm = shared_memory.SharedMemory(name=SHRD_MEM_NAME) except (FileNotFoundError) as e: - log.warn(e) - log.warn("Shared Memory likely closed by external process. Recreating with current data.") + log.warning(e) + log.warning("Shared Memory likely closed by external process. Recreating with current data.") shm = shared_memory.SharedMemory(create=True, name=SHRD_MEM_NAME, size=getsizeof(observed_array)) state_array = np.ndarray(observed_array.shape, dtype=observed_array.dtype, buffer=shm.buf) state_array[:] = observed_array[:] @@ -74,7 +75,7 @@ def should_stop(self) -> bool: log.error("Stopping sim because two years have passed") log.debug(f"Elapsed time = {int(self.state_time.time - self._config.init_cond.time)}s > 6.312e7") return True - + r_e = (state.x**2 + state.y**2 + state.z**2) ** 0.5 if r_e < R_EARTH: log.error("Stopping sim because craft is inside the Earth") diff --git a/src/core/state/derived_state.py b/src/core/state/derived_state.py index 3b21723..6058f26 100644 --- a/src/core/state/derived_state.py +++ b/src/core/state/derived_state.py @@ -23,6 +23,11 @@ class DerivedState: Izy: float = 0.0 Izz: float = 0.0 + # angular velocity in rad/s + ang_vel_x: float = 0.0 + ang_vel_y: float = 0.0 + ang_vel_z: float = 0.0 + # Kane damping constant kane_c: float = 0.0 @@ -44,13 +49,13 @@ class DerivedState: # earth to the craft r_ec: np.ndarray = np.array((0.0, 0.0, 0.0)) - # attitude (unit) vector of spacecraft in ECI + # attitude (unit) vector of spacecraft's +X vector in ECI attitude_vector: np.ndarray = np.array((0.0, 0.0, 0.0)) - # Azimuth angle of the spacecraft frame in ECI. "Theta" angle in spherical coords + # Azimuth angle of the +X spacecraft frame in ECI. "Theta" angle in spherical coords azimuth: float = 0 # radians - # Elevation angle of the spacecraft frame in ECI. "phi" angle in spherical coords + # Elevation angle of the +X spacecraft frame in ECI. "phi" angle in spherical coords elevation: float = 0 # radians def update(self, derived_state_dict: Dict) -> None: diff --git a/src/core/state/state.py b/src/core/state/state.py index a8f6e71..59ad063 100644 --- a/src/core/state/state.py +++ b/src/core/state/state.py @@ -17,10 +17,10 @@ class State: # primitive state fill_frac: float = 0.0 # TODO, decouple with fuel_mass - # angular momentum (kg*m^2/s) - ang_vel_x: float = 0.0 - ang_vel_y: float = 0.0 - ang_vel_z: float = 0.0 + # angular momentum (kg*m^2/s, or N*m*s) + h_x: float = 0.0 + h_y: float = 0.0 + h_z: float = 0.0 # angular position (quaternion) quat_v1: float = 0.0 @@ -89,7 +89,7 @@ def array_to_state(values: np.ndarray) -> State: @dataclass class ObservedState(State): # This is the true state with some noise applied - # TODO: Implement noise application + # TODONE: Implement noise application (in gyro model) # angular velocity (radians/second) ang_vel_x: float = 0.0 @@ -116,8 +116,6 @@ class ObservedState(State): fuel_mass: float = 0.0 chamber_temp: float = 0.0 - - # derived state (Newtons) force_earth: float = 0.0 force_moon: float = 0.0 diff --git a/src/core/state/statetime.py b/src/core/state/statetime.py index 642fecf..4c2bd50 100644 --- a/src/core/state/statetime.py +++ b/src/core/state/statetime.py @@ -43,7 +43,8 @@ def update(self, state_dict: Dict[str, Union[int, float, bool]]) -> None: self.state.update(state_dict) def update_derived(self, state_dict: Dict) -> None: - """update_derived() is a procedure that updates the fields of the derived state with specified key/value pairs in state_dict. + """update_derived() is a procedure that updates the fields of the derived state with specified key/value pairs + in state_dict. If a key in the `state_dict` is not defined as an attribute in DerivedState.__init__, it will be ignored. """ self.derived_state.update(state_dict) diff --git a/src/utils/matplotlib_util.py b/src/utils/matplotlib_util.py index 69fe282..e0941a1 100644 --- a/src/utils/matplotlib_util.py +++ b/src/utils/matplotlib_util.py @@ -24,7 +24,7 @@ def __init__(self, df): self.vel_zs = df["true_state.state.vel_z"].to_numpy() self.locs = np.array([self.xlocs, self.ylocs, self.zlocs]) - self.ang_x = df["true_state.state.ang_vel_x"].to_numpy() + self.ang_x = df["true_state.derived_state.ang_vel_x"].to_numpy() self.ang_x_obs = df["observed_state.ang_vel_x"].to_numpy() self.fig_2d = plt.figure() diff --git a/src/utils/test_utils.py b/src/utils/test_utils.py index 0a0b85f..fbb5cb7 100644 --- a/src/utils/test_utils.py +++ b/src/utils/test_utils.py @@ -5,9 +5,9 @@ s_0 = { "fill_frac": 0.0, - "ang_vel_x": 0.0, - "ang_vel_y": 0.0, - "ang_vel_z": 0.0, + "h_x": 0.0, + "h_y": 0.0, + "h_z": 0.0, "quat_v1": 0.0, "quat_v2": 0.0, "quat_v3": 0.0, @@ -24,14 +24,14 @@ "force_earth": 0.0, "force_moon": 0.0, "propulsion_on": False, - "solenoid_actuation_on": False + "solenoid_actuation_on": False, } s_1 = { "fill_frac": 1.0, - "ang_vel_x": 2.0, - "ang_vel_y": 3.0, - "ang_vel_z": 4.0, + "h_x": 2.0, + "h_y": 3.0, + "h_z": 4.0, "quat_v1": 5.0, "quat_v2": 6.0, "quat_v3": 7.0, @@ -48,25 +48,24 @@ "force_earth": 18.0, "force_moon": 19.0, "propulsion_on": True, - "solenoid_actuation_on": False + "solenoid_actuation_on": False, } state_1 = State(**s_1) d3456_dict: Dict = { - "gyro_bias": [0.497625, -0.10821875, 0.77490625], - "gyro_noise": [0.1824535, 0.11738579, 0.19192256], - "gyro_sensitivity": 0.015625 * (math.pi / 180), - "dry_mass": 3, - "com": 4, - "tank_volume": 5, - "electolyzer_rate": 10.0 * (1/1000), - "thruster_force": 6, - "combustion_chamber_volume": 7, - "max_iter": 1000000 + "gyro_bias": [0.497625, -0.10821875, 0.77490625], + "gyro_noise": [0.1824535, 0.11738579, 0.19192256], + "gyro_sensitivity": 0.015625 * (math.pi / 180), + "dry_mass": 3, + "com": 4, + "tank_volume": 5, + "electolyzer_rate": 10.0 * (1 / 1000), + "thruster_force": 6, + "combustion_chamber_volume": 7, + "max_iter": 1000000, } d3456: Parameters = Parameters(d3456_dict) - diff --git a/tests/core_tests/config_test.py b/tests/core_tests/config_test.py index 1cfe969..028671c 100644 --- a/tests/core_tests/config_test.py +++ b/tests/core_tests/config_test.py @@ -19,9 +19,9 @@ class ConfigTestCases(unittest.TestCase): } ic_pos = {"x": 3.0, "y": 3.0, "z": 3.0} ic_all = { - "ang_vel_x": 10.0, - "ang_vel_y": 10.0, - "ang_vel_z": 10.0, + "h_x": 10.0, + "h_y": 10.0, + "h_z": 10.0, "quat_v1": 10.0, "quat_v2": 10.0, "quat_v3": 10.0, @@ -52,9 +52,9 @@ class ConfigTestCases(unittest.TestCase): } zeroes_ic = { - "ang_vel_x": 0.0, - "ang_vel_y": 0.0, - "ang_vel_z": 0.0, + "h_x": 0.0, + "h_y": 0.0, + "h_z": 0.0, "quat_v1": 0.0, "quat_v2": 0.0, "quat_v3": 0.0, @@ -75,9 +75,9 @@ class ConfigTestCases(unittest.TestCase): default_model = ["att", "pos"] angles_ic = { - "ang_vel_x": 5.0, - "ang_vel_y": 5.0, - "ang_vel_z": 5.0, + "h_x": 5.0, + "h_y": 5.0, + "h_z": 5.0, "quat_v1": 10.0, "quat_v2": 10.0, "quat_v3": 10.0, diff --git a/tests/core_tests/model_tests/gyro_model_test.py b/tests/core_tests/model_tests/gyro_model_test.py index bbdbc35..7e28a8f 100644 --- a/tests/core_tests/model_tests/gyro_model_test.py +++ b/tests/core_tests/model_tests/gyro_model_test.py @@ -13,7 +13,7 @@ def test_gyro_model(self): clean_vars = { "gyro_bias": [0, 0, 0], "gyro_noise": [0, 0, 0], - "gyro_sensitivity": 1, + "gyro_sensitivity": 1e-18, } param_clean = Parameters(clean_vars) @@ -28,9 +28,9 @@ def test_gyro_model(self): self.assertIsInstance(eval_clean, Dict) # verify the original values have not been augmented by the model - self.assertEqual(eval_clean["ang_vel_x"], state_1.ang_vel_x) - self.assertEqual(eval_clean["ang_vel_y"], state_1.ang_vel_y) - self.assertEqual(eval_clean["ang_vel_z"], state_1.ang_vel_z) + self.assertAlmostEqual(eval_clean["ang_vel_x"], dummy_state.derived_state.ang_vel_x) + self.assertAlmostEqual(eval_clean["ang_vel_y"], dummy_state.derived_state.ang_vel_y) + self.assertAlmostEqual(eval_clean["ang_vel_z"], dummy_state.derived_state.ang_vel_z) # test noisy and biased model param_noisy_biased = Parameters({}) @@ -45,9 +45,9 @@ def test_gyro_model(self): self.assertIsInstance(eval_noisy_biased, Dict) # verify the original values have been augmented by the model - self.assertNotEqual(eval_noisy_biased["ang_vel_x"], state_1.ang_vel_x) - self.assertNotEqual(eval_noisy_biased["ang_vel_y"], state_1.ang_vel_y) - self.assertNotEqual(eval_noisy_biased["ang_vel_z"], state_1.ang_vel_z) + self.assertNotEqual(eval_noisy_biased["ang_vel_x"], dummy_state.derived_state.ang_vel_x) + self.assertNotEqual(eval_noisy_biased["ang_vel_y"], dummy_state.derived_state.ang_vel_y) + self.assertNotEqual(eval_noisy_biased["ang_vel_z"], dummy_state.derived_state.ang_vel_z) if __name__ == "__main__":