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

add comment

Browse Source
This commit is contained in:
Atsushi Sakai
2018-11-08 20:49:23 +09:00
parent ea5d3fff54
commit 204ea3b7c2
1 changed files with 3 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
Localization/extended_kalman_filter/extended_kalman_filter.py
View File

@@ -11,8 +11,8 @@ import math
import matplotlib.pyplot as plt
# Estimation parameter of EKF
Q = np.diag([1.0, 1.0])**2
R = np.diag([0.1, 0.1, np.deg2rad(1.0), 1.0])**2
Q = np.diag([1.0, 1.0])**2 # Observation x,y position covariance
R = np.diag([0.1, 0.1, np.deg2rad(1.0), 1.0])**2 # predict state covariance
# Simulation parameter
Qsim = np.diag([0.5, 0.5])**2
@@ -152,7 +152,7 @@ def plot_covariance_ellipse(xEst, PEst):
y = [b * math.sin(it) for it in t]
angle = math.atan2(eigvec[bigind, 1], eigvec[bigind, 0])
R = np.array([[math.cos(angle), math.sin(angle)],
[-math.sin(angle), math.cos(angle)]])
[-math.sin(angle), math.cos(angle)]])
fx = R.dot(np.array([[x, y]]))
px = np.array(fx[0, :] + xEst[0, 0]).flatten()
py = np.array(fx[1, :] + xEst[1, 0]).flatten()