Skip to main content
. Author manuscript; available in PMC: 2010 Jun 28.
Published in final edited form as: Med Image Comput Comput Assist Interv. 2009;12(Pt 1):894–902. doi: 10.1007/978-3-642-04268-3_110
Algorithm 1. Unscented Kalman Filter

1: Form weighted sigma points Xt={wi,xi}i=02n around current mean xt and covariance Pt with scaling factor ζ
x0=xtxi=xt+[ζPt]ixi+n=xt[ζPt]i
2: Predict the new sigma points and observations
Xt+1|t=f[Xt]Yt+1|t=h[Xt+1|t]
3: Compute weighted means and covariances, e.g.
x¯t+1|t=iwixiPxy=iwi(xix¯t+1|t)(yiy¯t+1|t)T
4: Update estimate using Kalman gain K and scanner measurement yt
xt+1=x¯t+1|t+K(yty¯t+1|t)Pt+1=PxxKPyyKTK=PxyPyy1