implementing motion model

ShisatoYano · Nov 3, 2023 · fd265c0 · fd265c0
1 parent 9531567
commit fd265c0
Showing 1 changed file with 7 additions and 0 deletions.
diff --git a/src/components/state/state.py b/src/components/state/state.py
@@ -35,6 +35,13 @@ def __init__(self, x_m=0.0, y_m=0.0, yaw_rad=0.0, speed_mps=0.0, color='k'):
         self.x_history = [self.x_m]
         self.y_history = [self.y_m]
 
+    @staticmethod
+    def motion_model(accel_mps2, yaw_rate_rps, time_s):
+        A = np.array([[1, 0, 0, 0],
+                      [0, 1, 0, 0],
+                      [0, 0, 1, 0],
+                      [0, 0, 0, 1]])
+
     def update(self, accel_mps2, yaw_rate_rps, time_s):
         """
         Function to update state