update state and covariance matrix

src/components/localization/kalman_filter/extended_kalman_filter_localizer.py

@@ -11,15 +11,18 @@
 
 sys.path.append(str(Path(__file__).absolute().parent) + "/../../state")
 sys.path.append(str(Path(__file__).absolute().parent) + "/../../array")
+sys.path.append(str(Path(__file__).absolute().parent) + "/../../sensors/gnss")
 from state import State
 from xy_array import XYArray
 
+
 class ExtendedKalmanFilterLocalizer:
     """
     Self localization by Extended Kalman Filter class
     """
 
-    def __init__(self, accel_noise=1.0, yaw_rate_noise=30.0, color='r'):
+    def __init__(self, accel_noise=0.5, yaw_rate_noise=10.0,
+                 obsrv_x_noise=1.0, obsrv_y_noise=1.0, color='r'):
         """
         Constructor
         color: Color of drawing error covariance ellipse
@@ -28,6 +31,9 @@ def __init__(self, accel_noise=1.0, yaw_rate_noise=30.0, color='r'):
         self.state = np.zeros((4, 1))
         self.cov_mat = np.eye(4)
         self.INPUT_NOISE_VAR_MAT = np.diag([accel_noise, np.deg2rad(yaw_rate_noise)]) ** 2
+        self.OBSRV_NOISE_VAR_MAT = np.diag([obsrv_x_noise, obsrv_y_noise]) ** 2
+        self.JACOB_H = np.array([[1, 0, 0, 0],
+                                 [0, 1, 0, 0]])
         self.DRAW_COLOR = color
 
     def update(self, state, accel_mps2, yaw_rate_rps, time_s, gnss):
@@ -42,18 +48,32 @@ def update(self, state, accel_mps2, yaw_rate_rps, time_s, gnss):
         input_noise = self.INPUT_NOISE_VAR_MAT @ np.random.randn(2, 1)
         next_input = input_org + input_noise
 
-        # predict
+        # predict state
         pred_state = State.motion_model(last_state, next_input, time_s)
         jF = self._jacobian_F(pred_state, next_input, time_s)
         jG = self._jacobian_G(pred_state, time_s)
         Q = self.INPUT_NOISE_VAR_MAT
         last_cov = self.cov_mat
         pred_cov = jF @ last_cov @jF.T + jG @ Q @ jG.T
 
-        self.state = pred_state
-        self.cov_mat = pred_cov
+        # predict observation
+        pred_obsrv = self._observation_model(pred_state)
+        jH = self.JACOB_H
+        R = self.OBSRV_NOISE_VAR_MAT
+        pred_obsrv_cov = jH @ pred_cov @ jH.T + R
+
+        # kalman gain
+        k = pred_cov @ jH.T @ np.linalg.inv(pred_obsrv_cov)
+
+        # update
+        inov = gnss - pred_obsrv
+        est_state = pred_state + k @ inov
+        est_cov = pred_cov - k @ pred_obsrv_cov @ k.T
+
+        self.state = est_state
+        self.cov_mat = est_cov
 
-        return pred_state
+        return est_state
 
     def draw(self, axes, elems, pose):
         eig_val, eig_vec = np.linalg.eig(self.cov_mat)
@@ -98,3 +118,14 @@ def _jacobian_G(self, state, time_s):
                        [t, 0]])
 
         return jG
+
+    def _observation_model(self, state):
+        H = np.array([[1, 0, 0, 0],
+                      [0, 1, 0, 0]])
+
+        x = np.array([[state[0, 0]],
+                      [state[1, 0]],
+                      [state[2, 0]],
+                      [state[3, 0]]])
+
+        return H @ x

src/simulations/localization/extended_kalman_filter_localization/extended_kalman_filter_localization.py

@@ -47,7 +47,7 @@ def main():
 
     # set simulation parameters
     x_lim, y_lim = MinMax(-5, 55), MinMax(-20, 25)
-    vis = GlobalXYVisualizer(x_lim, y_lim, TimeParameters(span_sec=25))
+    vis = GlobalXYVisualizer(x_lim, y_lim, TimeParameters(span_sec=30))
 
     # create course data instance
     course = SinCurveCourse(0, 50, 0.5, 20, color='k')
@@ -64,7 +64,7 @@ def main():
 
     # create vehicle instance
     # set state, spec, controller, sensors and localizer instances as arguments
-    gnss = Sensors(gnss=Gnss())
+    gnss = Sensors(gnss=Gnss(x_noise_std=1.0, y_noise_std=1.0))
     ekf = ExtendedKalmanFilterLocalizer()
     vehicle = FourWheelsVehicle(state, spec, controller=pure_pursuit, sensors=gnss, localizer=ekf)
     vis.add_object(vehicle)