move dynamics model to state estimation.

uwsbel · Nov 6, 2023 · 651ad1f · 651ad1f
1 parent e9ff77a
commit 651ad1f
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 4 deletions.
diff --git a/workspace/src/localization/localization_py/localization_py/EKF.py b/workspace/src/localization/localization_py/localization_py/EKF.py
@@ -28,7 +28,7 @@ def __init__(self, dt, dynamics, Q, R):
         """
 
         self.dt = dt
-        self.dyn = Dynamics(dt, dynamics)
+        self.dyn = dynamics
 
         self.Q = (
             np.diag(

diff --git a/workspace/src/localization/localization_py/localization_py/particle_filter.py b/workspace/src/localization/localization_py/localization_py/particle_filter.py
@@ -40,7 +40,7 @@ def __init__(self, dt, dynamics):
             dynamics: the parameters for the dynamics
         """
         self.dt = dt
-        self.dyn = Dynamics(dt, dynamics)
+        self.dyn = dynamics
 
         self.show_animation = False
         self.num_particles = 100

diff --git a/workspace/src/localization/localization_py/localization_py/state_estimation.py b/workspace/src/localization/localization_py/localization_py/state_estimation.py
@@ -16,6 +16,7 @@
 from localization_py.EKF import EKF
 from localization_py.particle_filter import ParticleFilter as PF
 from localization_py.chrono_coordinate_transfer import Graph
+from localization_py.dynamics import Dynamics
 
 
 class EstimationAlgorithmOption(Enum):
@@ -116,11 +117,13 @@ def __init__(self):
         # time between imu updates, sec
         self.dt_gps = 1 / self.freq
 
+        # the ROM
+        self.dynamics_model = Dynamics(self.dt_gps, dyn)
         # filter
         if self.estimation_alg == EstimationAlgorithmOption.EXTENDED_KALMAN_FILTER:
-            self.ekf = EKF(self.dt_gps, dyn, Q, R)
+            self.ekf = EKF(self.dt_gps, self.dynamics_model, Q, R)
         elif self.estimation_alg == EstimationAlgorithmOption.PARTICLE_FILTER:
-            self.pf = PF(self.dt_gps, dyn)
+            self.pf = PF(self.dt_gps, self.dynamics_model)
 
         # our graph object, for reference frame
         self.graph = Graph()