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

Merge pull request #306 from ndinhtuan/master

Browse Source 
fix compute jacobian of F in ekf_estimation
This commit is contained in:
Atsushi Sakai
2020-04-09 20:11:25 +09:00
committed by GitHub
parent 9274aacefb e319032812
commit 93e211603a
1 changed files with 1 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
Localization/extended_kalman_filter/extended_kalman_filter.py
View File

@@ -117,7 +117,7 @@ def jacob_h():
def ekf_estimation(xEst, PEst, z, u):
# Predict
xPred = motion_model(xEst, u)
jF = jacob_f(xPred, u)
jF = jacob_f(xEst, u)
PPred = jF @ PEst @ jF.T + Q
# Update
@@ -128,7 +128,6 @@ def ekf_estimation(xEst, PEst, z, u):
K = PPred @ jH.T @ np.linalg.inv(S)
xEst = xPred + K @ y
PEst = (np.eye(len(xEst)) - K @ jH) @ PPred
return xEst, PEst