diff --git a/src/components/state/state.py b/src/components/state/state.py index 7d2d9aa..0f6c17e 100644 --- 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