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

Start fixing randn

Browse Source
This commit is contained in:
Atsushi Sakai
2019-10-12 14:01:54 +09:00
parent 85bf664a91
commit 46e0506cf5
1 changed files with 16 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

34
Localization/extended_kalman_filter/extended_kalman_filter.py
View File

@@ -7,21 +7,22 @@ author: Atsushi Sakai (@Atsushi_twi)
"""
import math
import numpy as np
import matplotlib.pyplot as plt
import numpy as np
# Covariance for EKF simulation
Q = np.diag([
0.1, # variance of location on x-axis
0.1, # variance of location on y-axis
np.deg2rad(1.0), # variance of yaw angle
1.0 # variance of velocity
])**2 # predict state covariance
R = np.diag([1.0, 1.0])**2 # Observation x,y position covariance
0.1, # variance of location on x-axis
0.1, # variance of location on y-axis
np.deg2rad(1.0), # variance of yaw angle
1.0 # variance of velocity
]) ** 2 # predict state covariance
R = np.diag([1.0, 1.0]) ** 2 # Observation x,y position covariance
# Simulation parameter
INPUT_NOISE = np.diag([1.0, np.deg2rad(30.0)])**2
GPS_NOISE = np.diag([0.5, 0.5])**2
INPUT_NOISE = np.diag([1.0, np.deg2rad(30.0)]) ** 2
GPS_NOISE = np.diag([0.5, 0.5]) ** 2
DT = 0.1 # time tick [s]
SIM_TIME = 50.0 # simulation time [s]
@@ -37,7 +38,6 @@ def calc_input():
def observation(xTrue, xd, u):
xTrue = motion_model(xTrue, u)
# add noise to gps x-y
@@ -52,7 +52,6 @@ def observation(xTrue, xd, u):
def motion_model(x, u):
F = np.array([[1.0, 0, 0, 0],
[0, 1.0, 0, 0],
[0, 0, 1.0, 0],
@@ -116,20 +115,19 @@ def jacobH(x):
def ekf_estimation(xEst, PEst, z, u):
# Predict
xPred = motion_model(xEst, u)
jF = jacobF(xPred, u)
PPred = jF@PEst@jF.T + Q
PPred = jF @ PEst @ jF.T + Q
# Update
jH = jacobH(xPred)
zPred = observation_model(xPred)
y = z - zPred
S = jH@PPred@jH.T + R
K = PPred@jH.T@np.linalg.inv(S)
xEst = xPred + K@y
PEst = (np.eye(len(xEst)) - K@jH)@PPred
S = jH @ PPred @ jH.T + R
K = PPred @ jH.T @ np.linalg.inv(S)
xEst = xPred + K @ y
PEst = (np.eye(len(xEst)) - K @ jH) @ PPred
return xEst, PEst
@@ -153,7 +151,7 @@ def plot_covariance_ellipse(xEst, PEst): # pragma: no cover
angle = math.atan2(eigvec[bigind, 1], eigvec[bigind, 0])
R = np.array([[math.cos(angle), math.sin(angle)],
[-math.sin(angle), math.cos(angle)]])
fx = R@(np.array([x, y]))
fx = R @ (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")