Angular momentum

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"]

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"]

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,

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,

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"]

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"]

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": {

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, 

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, 

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"]

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()]

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]}

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],
         }

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