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

add reference & correct misspelling

Browse Source
This commit is contained in:
Ryohei Sasaki
2019-06-21 18:09:47 +09:00
committed by GitHub
parent fdaf592cde
commit 2fb7c720ef
1 changed files with 4 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
Localization/ensemble_kalman_filter/ensemble_kalman_filter.py
View File

@@ -4,6 +4,9 @@ Ensemble Kalman Filter(EnKF) localization sample
author: Ryohei Sasaki(rsasaki0109)
Ref:
- [Ensemble Kalman filtering](https://rmets.onlinelibrary.wiley.com/doi/10.1256/qj.05.135)
"""
import numpy as np
@@ -124,7 +127,7 @@ def enkf_localization(px, xEst, PEst, z, u):
px_hat=px + K @ (np.tile(z_lm_pos,(NP,1)).T- pz)
xEst=np.average(px_hat, axis=1).reshape(4,1)
pEst=calc_covariance(xEst, px_hat)
PEst=calc_covariance(xEst, px_hat)
return xEst, PEst, px_hat