github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 18:28:06 -05:00
Code Issues Packages Projects Releases Wiki Activity

fix EKF

Browse Source
This commit is contained in:
Yo Iida
2018-11-07 11:41:21 +09:00
parent abab326b3e
commit bf093a95a3
1 changed files with 5 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

10
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([0.1, 0.1, np.deg2rad(1.0), 1.0])**2
R = np.diag([1.0, np.deg2rad(40.0)])**2
Q = np.diag([1.0, np.deg2rad(40.0)])**2
R = np.diag([0.1, 0.1, np.deg2rad(1.0), 1.0])**2
# Simulation parameter
Qsim = np.diag([0.5, 0.5])**2
@@ -120,13 +120,13 @@ 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.dot(PEst).dot(jF.T) + R
# Update
jH = jacobH(xPred)
zPred = observation_model(xPred)
y = z.T - zPred
S = jH.dot(PPred).dot(jH.T) + R
S = jH.dot(PPred).dot(jH.T) + Q
K = PPred.dot(jH.T).dot(np.linalg.inv(S))
xEst = xPred + K.dot(y)
PEst = (np.eye(len(xEst)) - K.dot(jH)).dot(PPred)
@@ -183,7 +183,7 @@ def main():
xTrue, z, xDR, ud = observation(xTrue, xDR, u)
xEst, PEst = ekf_estimation(xEst, PEst, z, ud)
xEst, PEst = ekf_estimation(xEst, PEst, z, u)
# store data history
hxEst = np.hstack((hxEst, xEst))