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(xi−x¯t+1|t)(yi−y¯t+1|t)T
4: Update estimate using Kalman gain K and scanner measurement yt
xt+1=x¯t+1|t+K(yt−y¯t+1|t)Pt+1=Pxx−KPyyKTK=PxyPyy−1