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

Using utility rot_mat_2d (#683)

Browse Source
This commit is contained in:
Atsushi Sakai
2022-05-22 16:53:14 +09:00
committed by GitHub
parent 31178c8e37
commit dc879da7b2
10 changed files with 70 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
Localization/particle_filter/particle_filter.py
View File

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