More intuitive way of plot_covariance_ellipse() (AtsushiSakai#407)

* More intuitive way of plot_covariance_ellipse() in EKF and UKF

* Add the same changes in UKF as well

* Modified rotation matrix to scipy.spatial.transform.Rotation

* Modified angle of covariance matrix

* Fixed typos

Author: hotsuyuki

Localization/ensemble_kalman_filter/ensemble_kalman_filter.py

@@ -167,7 +167,7 @@ def plot_covariance_ellipse(xEst, PEst):  # pragma: no cover
 
     x = [a * math.cos(it) for it in t]
     y = [b * math.sin(it) for it in t]
-    angle = math.atan2(eig_vec[big_ind, 1], eig_vec[big_ind, 0])
+    angle = math.atan2(eig_vec[1, big_ind], eig_vec[0, big_ind])
     rot = Rot.from_euler('z', angle).as_matrix()[0:2, 0:2]
     fx = np.stack([x, y]).T @ rot
 

Localization/extended_kalman_filter/extended_kalman_filter.py

@@ -10,6 +10,7 @@
 
 import matplotlib.pyplot as plt
 import numpy as np
+from scipy.spatial.transform import Rotation as Rot
 
 # Covariance for EKF simulation
 Q = np.diag([
@@ -147,9 +148,8 @@ def plot_covariance_ellipse(xEst, PEst):  # pragma: no cover
     b = math.sqrt(eigval[smallind])
     x = [a * math.cos(it) for it in t]
     y = [b * math.sin(it) for it in t]
-    angle = math.atan2(eigvec[bigind, 1], eigvec[bigind, 0])
-    rot = np.array([[math.cos(angle), math.sin(angle)],
-                    [-math.sin(angle), math.cos(angle)]])
+    angle = math.atan2(eigvec[1, bigind], eigvec[0, bigind])
+    rot = Rot.from_euler('z', angle).as_matrix()[0:2, 0:2]
     fx = rot @ (np.array([x, y]))
     px = np.array(fx[0, :] + xEst[0, 0]).flatten()
     py = np.array(fx[1, :] + xEst[1, 0]).flatten()

Localization/particle_filter/particle_filter.py

@@ -188,7 +188,7 @@ def plot_covariance_ellipse(x_est, p_est):  # pragma: no cover
 
     x = [a * math.cos(it) for it in t]
     y = [b * math.sin(it) for it in t]
-    angle = math.atan2(eig_vec[big_ind, 1], eig_vec[big_ind, 0])
+    angle = math.atan2(eig_vec[1, big_ind], eig_vec[0, big_ind])
     rot = Rot.from_euler('z', angle).as_matrix()[0:2, 0:2]
     fx = rot.dot(np.array([[x, y]]))
     px = np.array(fx[0, :] + x_est[0, 0]).flatten()

Localization/unscented_kalman_filter/unscented_kalman_filter.py

@@ -10,6 +10,7 @@
 
 import matplotlib.pyplot as plt
 import numpy as np
+from scipy.spatial.transform import Rotation as Rot
 import scipy.linalg
 
 # Covariance for UKF simulation
@@ -180,9 +181,8 @@ def plot_covariance_ellipse(xEst, PEst):  # pragma: no cover
     b = math.sqrt(eigval[smallind])
     x = [a * math.cos(it) for it in t]
     y = [b * math.sin(it) for it in t]
-    angle = math.atan2(eigvec[bigind, 1], eigvec[bigind, 0])
-    rot = np.array([[math.cos(angle), math.sin(angle)],
-                    [-math.sin(angle), math.cos(angle)]])
+    angle = math.atan2(eigvec[1, bigind], eigvec[0, bigind])
+    rot = Rot.from_euler('z', angle).as_matrix()[0:2, 0:2]
     fx = rot @ np.array([x, y])
     px = np.array(fx[0, :] + xEst[0, 0]).flatten()
     py = np.array(fx[1, :] + xEst[1, 0]).flatten()