Using utility rot_mat_2d (#683)

Localization/ensemble_kalman_filter/ensemble_kalman_filter.py

@@ -10,11 +10,15 @@ Ensemble Kalman filtering
 
 """
 
-import math
+import sys
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")
 
+import math
 import matplotlib.pyplot as plt
 import numpy as np
-from scipy.spatial.transform import Rotation as Rot
+
+from utils.angle import rot_mat_2d
 
 #  Simulation parameter
 Q_sim = np.diag([0.2, np.deg2rad(1.0)]) ** 2
@@ -168,8 +172,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[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
+    fx = np.stack([x, y]).T @ rot_mat_2d(angle)
 
     px = np.array(fx[:, 0] + xEst[0, 0]).flatten()
     py = np.array(fx[:, 1] + xEst[1, 0]).flatten()

Localization/extended_kalman_filter/extended_kalman_filter.py

@@ -6,11 +6,15 @@ author: Atsushi Sakai (@Atsushi_twi)
 
 """
 
-import math
+import sys
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")
 
+import math
 import matplotlib.pyplot as plt
 import numpy as np
-from scipy.spatial.transform import Rotation as Rot
+
+from utils.angle import rot_mat_2d
 
 # Covariance for EKF simulation
 Q = np.diag([
@@ -149,8 +153,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(eigvec[1, bigind], eigvec[0, bigind])
-    rot = Rot.from_euler('z', angle).as_matrix()[0:2, 0:2]
-    fx = rot @ (np.array([x, y]))
+    fx = rot_mat_2d(angle) @ (np.array([x, y]))
     px = np.array(fx[0, :] + xEst[0, 0]).flatten()
     py = np.array(fx[1, :] + xEst[1, 0]).flatten()
     plt.plot(px, py, "--r")

Localization/particle_filter/particle_filter.py

@@ -5,12 +5,16 @@ Particle Filter localization sample
 author: Atsushi Sakai (@Atsushi_twi)
 
 """
+import sys
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")
 
 import math
 
 import matplotlib.pyplot as plt
 import numpy as np
-from scipy.spatial.transform import Rotation as Rot
+
+from utils.angle import rot_mat_2d
 
 # Estimation parameter of PF
 Q = np.diag([0.2]) ** 2  # range error
@@ -189,10 +193,9 @@ 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[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()
-    py = np.array(fx[1, :] + x_est[1, 0]).flatten()
+    fx = rot_mat_2d(angle) @ np.array([[x, y]])
+    px = np.array(fx[:, 0] + x_est[0, 0]).flatten()
+    py = np.array(fx[:, 1] + x_est[1, 0]).flatten()
     plt.plot(px, py, "--r")
 
 

Localization/unscented_kalman_filter/unscented_kalman_filter.py

@@ -6,13 +6,18 @@ author: Atsushi Sakai (@Atsushi_twi)
 
 """
 
+import sys
+import os
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../utils/")
+
 import math
 
 import matplotlib.pyplot as plt
 import numpy as np
-from scipy.spatial.transform import Rotation as Rot
 import scipy.linalg
 
+from utils.angle import rot_mat_2d
+
 # Covariance for UKF simulation
 Q = np.diag([
     0.1,  # variance of location on x-axis
@@ -182,8 +187,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(eigvec[1, bigind], eigvec[0, bigind])
-    rot = Rot.from_euler('z', angle).as_matrix()[0:2, 0:2]
-    fx = rot @ np.array([x, y])
+    fx = rot_mat_2d(angle) @ np.array([x, y])
     px = np.array(fx[0, :] + xEst[0, 0]).flatten()
     py = np.array(fx[1, :] + xEst[1, 0]).flatten()
     plt.plot(px, py, "--r")