AtHeartEngineer/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:41 -04:00
Code Issues Packages Projects Releases Wiki Activity

More intuitive way of plot_covariance_ellipse() (#407)

Browse Source 
* 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
This commit is contained in:
hotsuyuki
2020-10-01 20:21:51 +09:00
committed by GitHub
parent 2b316502e3
commit 2a5bbdc0be
4 changed files with 8 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Localization/ensemble_kalman_filter/ensemble_kalman_filter.py
View File

@@ -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

6
Localization/extended_kalman_filter/extended_kalman_filter.py
View File

@@ -10,6 +10,7 @@ import math
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()

2
Localization/particle_filter/particle_filter.py
View File

@@ -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()

6
Localization/unscented_kalman_filter/unscented_kalman_filter.py
View File

@@ -10,6 +10,7 @@ import math
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()