Editing Kalman filter (section)

==== Update ====
Given prediction estimates <math>\hat{\mathbf{x}}_{k \mid k-1}</math> and <math>\mathbf{P}_{k \mid k-1}</math>, a new set of <math>N = 2L+1</math> sigma points <math>\mathbf{s}_0, \dots, \mathbf{s}_{2L}</math> with corresponding first-order weights <math> W_0^a,\dots W_{2L}^a</math> and second-order weights <math>W_0^c,\dots, W_{2L}^c</math> is calculated.<ref>{{cite journal |last1=Sarkka |first1=Simo |title=On Unscented Kalman Filtering for State Estimation of Continuous-Time Nonlinear Systems |journal=IEEE Transactions on Automatic Control |date=September 2007 |volume=52 |issue=9 |pages=1631–1641 |doi=10.1109/TAC.2007.904453}}</ref> These sigma points are transformed through the measurement function <math>h</math>.
:<math> \mathbf{z}_j=h(\mathbf{s}_j), \,\, j=0,1, \dots, 2L </math>.

Then the empirical mean and covariance of the transformed points are calculated.
:<math>\begin{align}
  \hat{\mathbf{z}} &= \sum_{j=0}^{2L} W_j^a \mathbf{z}_j \\[6pt]
  \hat{\mathbf{S}}_k &= \sum_{j=0}^{2L} W_j^c (\mathbf{z}_j-\hat{\mathbf{z}})(\mathbf{z}_j-\hat{\mathbf{z}})^\textsf{T} + \mathbf{R_k}
\end{align}</math>
where <math>\mathbf{R}_k</math> is the covariance matrix of the observation noise, <math>\mathbf{v}_k</math>. Additionally, the cross covariance matrix is also needed
:<math>\begin{align}
  \mathbf{C_{xz}} &= \sum_{j=0}^{2L} W_j^c (\mathbf{x}_j-\hat\mathbf{x}_{k|k-1})(\mathbf{z}_j-\hat\mathbf{z})^\textsf{T}.
\end{align}</math>

The Kalman gain is
: <math>\begin{align}
  \mathbf{K}_k=\mathbf{C_{xz}}\hat{\mathbf{S}}_k^{-1}.
\end{align}</math>

The updated mean and covariance estimates are
:<math>
\begin{align}
\hat\mathbf{x}_{k\mid k}&=\hat\mathbf{x}_{k|k-1}+\mathbf{K}_k(\mathbf{z}_k-\hat\mathbf{z})\\
\mathbf{P}_{k\mid k}&=\mathbf{P}_{k\mid k-1}-\mathbf{K}_k\hat{\mathbf{S}}_k\mathbf{K}_k^\textsf{T}.
\end{align}
</math>