Editing Kalman filter (section)

== Derivations ==
{{More citations needed section|date=December 2010}}
The Kalman filter can be derived as a [[generalized least squares]] method operating on previous data.<ref>Sant, Donald T. "Generalized least squares applied to time varying parameter models." Annals of Economic and Social Measurement, Volume 6, number 3. NBER, 1977. 301-314. [https://www.nber.org/system/files/chapters/c10557/c10557.pdf Online Pdf]</ref>

=== Deriving the ''posteriori'' estimate covariance matrix ===
Starting with our invariant on the error covariance '''P'''<sub>''k''&nbsp;|&nbsp;''k''</sub> as above
:<math>\mathbf{P}_{k \mid k} = \operatorname{cov}\left(\mathbf{x}_k - \hat{\mathbf{x}}_{k \mid k}\right)</math>

substitute in the definition of <math>\hat{\mathbf{x}}_{k \mid k}</math>
:<math>\mathbf{P}_{k \mid k} = \operatorname{cov}\left[\mathbf{x}_k - \left(\hat{\mathbf{x}}_{k \mid k - 1} + \mathbf{K}_k\tilde{\mathbf{y}}_k\right)\right]</math>

and substitute  <math>\tilde{\mathbf{y}}_k</math>
:<math>\mathbf{P}_{k \mid k} = \operatorname{cov}\left(\mathbf{x}_k - \left[\hat{\mathbf{x}}_{k \mid k - 1} + \mathbf{K}_k\left(\mathbf{z}_k - \mathbf{H}_k\hat{\mathbf{x}}_{k \mid k - 1}\right)\right]\right)</math>

and <math>\mathbf{z}_k</math>
:<math>\mathbf{P}_{k \mid k} = \operatorname{cov}\left(\mathbf{x}_k - \left[\hat{\mathbf{x}}_{k \mid k - 1} + \mathbf{K}_k\left(\mathbf{H}_k\mathbf{x}_k + \mathbf{v}_k - \mathbf{H}_k\hat{\mathbf{x}}_{k \mid k - 1}\right)\right]\right)</math>

and by collecting the error vectors we get
:<math>\mathbf{P}_{k \mid k} = \operatorname{cov}\left[\left(\mathbf{I} - \mathbf{K}_k \mathbf{H}_k\right)\left(\mathbf{x}_k - \hat{\mathbf{x}}_{k \mid k - 1}\right) - \mathbf{K}_k \mathbf{v}_k\right]</math>

Since the measurement error '''v'''<sub>''k''</sub> is uncorrelated with the other terms, this becomes
:<math>\mathbf{P}_{k \mid k} = \operatorname{cov}\left[\left(\mathbf{I} - \mathbf{K}_k \mathbf{H}_k\right)\left(\mathbf{x}_k - \hat{\mathbf{x}}_{k \mid k - 1}\right)\right] + \operatorname{cov}\left[\mathbf{K}_k \mathbf{v}_k\right]</math>

by the properties of [[covariance matrix|vector covariance]] this becomes
:<math>\mathbf{P}_{k \mid k} = \left(\mathbf{I} - \mathbf{K}_k \mathbf{H}_k\right)\operatorname{cov}\left(\mathbf{x}_k - \hat{\mathbf{x}}_{k \mid k - 1}\right)\left(\mathbf{I} - \mathbf{K}_k \mathbf{H}_k\right)^\textsf{T} + \mathbf{K}_k\operatorname{cov}\left(\mathbf{v}_k\right)\mathbf{K}_k^\textsf{T}</math>

which, using our invariant on '''P'''<sub>''k''&nbsp;|&nbsp;''k''−1</sub> and the definition of '''R'''<sub>''k''</sub> becomes
:<math>\mathbf{P}_{k \mid k} = \left(\mathbf{I} - \mathbf{K}_k \mathbf{H}_k\right) \mathbf{P}_{k \mid k - 1} \left(\mathbf{I} - \mathbf{K}_k \mathbf{H}_k\right)^\textsf{T} + \mathbf{K}_k \mathbf{R}_k \mathbf{K}_k^\textsf{T}</math>

This formula (sometimes known as the '''Joseph form''' of the covariance update equation) is valid for any value of '''K'''<sub>''k''</sub>. It turns out that if '''K'''<sub>''k''</sub> is the optimal Kalman gain, this can be simplified further as shown below.

=== Kalman gain derivation ===
The Kalman filter is a [[minimum mean-square error| minimum mean-square error (MMSE)]] estimator. The error in the ''a posteriori'' state estimation is
:<math>\mathbf{x}_k - \hat{\mathbf{x}}_{k \mid k}</math>

We seek to minimize the expected value of the square of the magnitude of this vector, <math>\operatorname{E}\left[\left\|\mathbf{x}_{k} - \hat{\mathbf{x}}_{k|k}\right\|^2\right]</math>. This is equivalent to minimizing the [[trace (matrix)|trace]] of the ''a posteriori'' estimate [[covariance matrix]] <math> \mathbf{P}_{k|k} </math>. By expanding out the terms in the equation above and collecting, we get:
:<math>\begin{align}
  \mathbf{P}_{k\mid k} & = \mathbf{P}_{k\mid k-1} - \mathbf{K}_k \mathbf{H}_k \mathbf{P}_{k\mid k-1} - \mathbf{P}_{k\mid k-1} \mathbf{H}_k^\textsf{T} \mathbf{K}_k^\textsf{T} + \mathbf{K}_k \left(\mathbf{H}_k \mathbf{P}_{k\mid k-1} \mathbf{H}_k^\textsf{T} + \mathbf{R}_k\right) \mathbf{K}_k^\textsf{T} \\[6pt]
                       &= \mathbf{P}_{k\mid k-1} - \mathbf{K}_k \mathbf{H}_k \mathbf{P}_{k\mid k-1} - \mathbf{P}_{k\mid k-1} \mathbf{H}_k^\textsf{T} \mathbf{K}_k^\textsf{T} + \mathbf{K}_k \mathbf{S}_k\mathbf{K}_k^\textsf{T}
\end{align}</math>

The trace is minimized when its [[matrix calculus|matrix derivative]] with respect to the gain matrix is zero. Using the [[matrix calculus#Identities|gradient matrix rules]] and the symmetry of the matrices involved we find that
:<math>\frac{\partial \; \operatorname{tr}(\mathbf{P}_{k\mid k})}{\partial \;\mathbf{K}_k} = -2 \left(\mathbf{H}_k \mathbf{P}_{k\mid k-1}\right)^\textsf{T} + 2 \mathbf{K}_k \mathbf{S}_k = 0.</math>

Solving this for '''K'''<sub>''k''</sub> yields the Kalman gain:
:<math>\begin{align}
  \mathbf{K}_k \mathbf{S}_k &= \left(\mathbf{H}_k \mathbf{P}_{k\mid k-1}\right)^\textsf{T} = \mathbf{P}_{k\mid k-1} \mathbf{H}_k^\textsf{T} \\
   \Rightarrow \mathbf{K}_k &= \mathbf{P}_{k\mid k-1} \mathbf{H}_k^\textsf{T} \mathbf{S}_k^{-1}
\end{align}</math>

This gain, which is known as the ''optimal Kalman gain'', is the one that yields MMSE estimates when used.

=== Simplification of the ''posteriori'' error covariance formula ===
The formula used to calculate the ''a posteriori'' error covariance can be simplified when the Kalman gain equals the optimal value derived above. Multiplying both sides of our Kalman gain formula on the right by '''S'''<sub>''k''</sub>'''K'''<sub>''k''</sub><sup>T</sup>, it follows that
:<math>\mathbf{K}_k \mathbf{S}_k \mathbf{K}_k^\textsf{T} = \mathbf{P}_{k\mid k-1} \mathbf{H}_k^\textsf{T} \mathbf{K}_k^\textsf{T}</math>

Referring back to our expanded formula for the ''a posteriori'' error covariance,
:<math> \mathbf{P}_{k\mid k} = \mathbf{P}_{k\mid k-1} - \mathbf{K}_k \mathbf{H}_k \mathbf{P}_{k\mid k-1}  - \mathbf{P}_{k\mid k-1} \mathbf{H}_k^\textsf{T} \mathbf{K}_k^\textsf{T} + \mathbf{K}_k \mathbf{S}_k \mathbf{K}_k^\textsf{T}</math>

we find the last two terms cancel out, giving
:<math> \mathbf{P}_{k\mid k} = \mathbf{P}_{k\mid k-1} - \mathbf{K}_k \mathbf{H}_k \mathbf{P}_{k\mid k-1} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_{k\mid k-1}</math>

This formula is computationally cheaper and thus nearly always used in practice, but is only correct for the optimal gain. If arithmetic precision is unusually low causing problems with [[numerical stability]], or if a non-optimal Kalman gain is deliberately used, this simplification cannot be applied; the ''a posteriori'' error covariance formula as derived above (Joseph form) must be used.