calculate covariance matrix eigen value, vector

src/components/localization/kalman_filter/extended_kalman_filter_localizer.py

@@ -7,7 +7,7 @@
 import sys
 import numpy as np
 from pathlib import Path
-from math import cos, sin
+from math import cos, sin, sqrt, atan2
 
 sys.path.append(str(Path(__file__).absolute().parent) + "/../../state")
 from state import State
@@ -17,15 +17,13 @@ class ExtendedKalmanFilterLocalizer:
     Self localization by Extended Kalman Filter class
     """
 
-    def __init__(self, state, accel_noise=1.0, yaw_rate_noise=30.0, 
-                 color='r'):
+    def __init__(self, accel_noise=1.0, yaw_rate_noise=30.0, color='r'):
         """
         Constructor
-        state: Vehicle's state object
         color: Color of drawing error covariance ellipse
         """
 
-        self.state = state
+        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.DRAW_COLOR = color
@@ -50,8 +48,18 @@ def update(self, state, accel_mps2, yaw_rate_rps, time_s, gnss):
         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
+
         return pred_state
 
+    def draw(self, axes, elems, pose):
+        eig_val, eig_vec = np.linalg.eig(self.cov_mat)
+        if eig_val[0] >= eig_val[1]: big_idx, small_idx = 0, 1
+        else: big_idx, small_idx = 1, 0
+        a, b = sqrt(3.0 * eig_val[big_idx]), sqrt(3.0 * eig_val[small_idx])
+        angle = atan2(eig_vec[1, big_idx], eig_vec[0, big_idx])
+
     def _jacobian_F(self, state, input, time_s):
         yaw = state[2, 0]
         spd = state[3, 0]

src/components/vehicle/four_wheels_vehicle.py

@@ -125,6 +125,9 @@ def _draw_control_data(self, axes, elems):
         else:
             return 0.0
 
+    def _draw_localization_data(self, axes, elems, state):
+        if self.localizer: self.localizer.draw(axes, elems, state.x_y_yaw())
+
     def update(self, time_s):
         """
         Function to update each member objects
@@ -160,6 +163,8 @@ def draw(self, axes, elems):
 
         steer_rad = self._draw_control_data(axes, elems)
 
+        self._draw_localization_data(axes, elems, self.state)
+
         self.state.draw(axes, elems)
         x_y_yaw_array = self.state.x_y_yaw()
         x_m = self.state.get_x_m()

src/simulations/localization/extended_kalman_filter_localization/extended_kalman_filter_localization.py

@@ -65,7 +65,7 @@ def main():
     # create vehicle instance
     # set state, spec, controller, sensors and localizer instances as arguments
     gnss = Sensors(gnss=Gnss())
-    ekf = ExtendedKalmanFilterLocalizer(state)
+    ekf = ExtendedKalmanFilterLocalizer()
     vehicle = FourWheelsVehicle(state, spec, controller=pure_pursuit, sensors=gnss, localizer=ekf)
     vis.add_object(vehicle)