From: An accuracy comparison of polynomial chaos type methods for the propagation of uncertainties
Model
x k =f( x k − 1 , u k − 1 , w k − 1 ), w k − 1 ∼N(0, Q k − 1 ), y k =h( x k , v k ), v k ∼N(0, R k )
Initialization
x ˆ 0 , P 0 =E[ x ˆ 0 x ˆ 0 T ]
Predictor
x ˆ k − =f( x ˆ k − 1 , u k − 1 ,0), P k − = A k − 1 P k − 1 A k − 1 T + W k − 1 Q k − 1 W k − 1 T
Kalman gain
K k = P k − H k T [ H k P k − H k T + V k R k V k T ] − 1
Corrector
x ˆ k = x ˆ k − + K k ( y k −h( x ˆ k − ,0)), P k =(I− K k H k ) P k −