Kalman filter

or uncertainty of the estimate.

Uses your system voice; voice and rate in settings.

Snapshot

≈ 48 min read

Names in other languages

No names in other languages are held for this entry yet.

Article bodies are English and are not machine-translated. A name in another language is not a translation of the article.

or uncertainty of the estimate. The estimate is updated using a state transition model and measurements. \hat{x}_{k\mid k-1} denotes the estimate of the system's state at time step k before the k-th measurement y k has been taken into account; P_{k \mid k-1} is the corresponding uncertainty.

In statistics and control theory, Kalman filtering (also known as linear quadratic estimation) is an algorithm that uses a series of measurements observed over time, including statistical noise and other inaccuracies, to produce estimates of unknown variables that tend to be more accurate than those based on a single measurement, by estimating a joint probability distribution over the variables for each time-step. The filter is constructed as a mean squared error minimiser, but also relates to maximum likelihood statistics.

The filter is named after Rudolf E. Kálmán. Kalman filtering has numerous technological applications. A common application is for guidance, navigation, and control of vehicles, particularly aircraft, spacecraft and ships positioned dynamically. Furthermore, Kalman filtering is much applied in time series analysis tasks such as signal processing and econometrics.

Kalman filtering is also important for robotic motion planning and control, and can be used for trajectory optimization.…

or uncertainty of the estimate. The estimate is updated using a state transition model and measurements. \hat{x}_{k\mid k-1} denotes the estimate of the system's state at time step k before the k-th measurement y k has been taken into account; P_{k \mid k-1} is the corresponding uncertainty.

In statistics and control theory, Kalman filtering (also known as linear quadratic estimation) is an algorithm that uses a series of measurements observed over time, including statistical noise and other inaccuracies, to produce estimates of unknown variables that tend to be more accurate than those based on a single measurement, by estimating a joint probability distribution over the variables for each time-step. The filter is constructed as a mean squared error minimiser, but also relates to maximum likelihood statistics.

The filter is named after Rudolf E. Kálmán. Kalman filtering has numerous technological applications. A common application is for guidance, navigation, and control of vehicles, particularly aircraft, spacecraft and ships positioned dynamically. Furthermore, Kalman filtering is much applied in time series analysis tasks such as signal processing and econometrics.

Kalman filtering is also important for robotic motion planning and control, and can be used for trajectory optimization. Kalman filtering also works for modeling the central nervous system's control of movement.

Due to the time delay between issuing motor commands and receiving sensory feedback, the use of Kalman filters provides a realistic model for making estimates of the current state of a motor system and issuing updated commands. The algorithm works via a two-phase process: a prediction phase and an update phase.

In the prediction phase, the Kalman filter produces estimates of the current state variables, including their uncertainties. Once the outcome of the next measurement (necessarily corrupted with some error, including random noise) is observed, these estimates are updated using a weighted average, with more weight given to estimates with greater certainty. The algorithm is recursive.

It can operate in real time, using only the present input measurements and the state calculated previously and its uncertainty matrix; no additional past information is required. Optimality of Kalman filtering assumes that errors have a zero mean normal (Gaussian) distribution.

In the words of Rudolf E. Kálmán, "The following assumptions are made about random processes: Physical random phenomena may be thought of as due to primary random sources exciting dynamic systems. The primary sources are assumed to be independent gaussian random processes with zero mean; the dynamic systems will be linear."

Regardless of Gaussianity, however, if the process and measurement covariances are known and mean is zero, then the Kalman filter is the best possible linear estimator in the minimum mean-square-error sense, although there may be better nonlinear estimators. It is a common misconception (perpetuated in the literature) that the Kalman filter cannot be rigorously applied unless all noise processes are assumed to be Gaussian.

Extensions and generalizations of the method have also been developed, such as the extended Kalman filter and the unscented Kalman filter which work on nonlinear systems. The basis is a hidden Markov model such that the state space of the latent variables is continuous and all latent and observed variables have Gaussian distributions.

Kalman filtering has been used successfully in multi-sensor fusion, and distributed sensor networks to develop distributed or consensus Kalman filtering. \mathbf{x}_k = \mathbf{F}_k \mathbf{x}_{k-1} +\mathbf{B}_k \mathbf{u}_{k} + \mathbf{w}_k where \mathbf{F}_k is the state transition model which is applied to the previous state x k−1 ; \mathbf{B}_k is the control-input model which is applied to the control vector \mathbf{u}_k ; \mathbf{w}_k is the process noise, which is assumed to be drawn from a zero mean multivariate normal distribution, \mathcal{N} , with covariance, \mathbf{Q}_k : \mathbf{w}_k \sim \mathcal{N}\left(0, \mathbf{Q}_k\right) . If \mathbf{Q} is independent of time, one may, following Roweis and Ghahramani, write \mathbf{w}_\bull instead of \mathbf{w}_k to emphasize that the noise has no explicit knowledge of time.

At time k an observation (or measurement) \mathbf{z}_k of the true state \mathbf{x}_k is made according to \mathbf{z}_k = \mathbf{H}_k \mathbf{x}_k + \mathbf{v}_k where \mathbf{H}_k is the observation model, which maps the true state space into the observed space and \mathbf{v}_k is the observation noise, which is assumed to be zero mean Gaussian white noise with covariance \mathbf{R}_k : \mathbf{v}_k \sim \mathcal{N}\left(0, \mathbf{R}_k\right) . Analogously to the situation for \mathbf{w}_k , one may write \mathbf{v}_\bull instead of \mathbf{v}_k if \mathbf{R} is independent of time.

The initial state, and the noise vectors at each step \{ \mathbf{x}_0, \mathbf{w}_1, \dots, \mathbf{w}_k, \mathbf{v}_1, \dots, \mathbf{v}_k \} are all assumed to be mutually independent. Many real-time dynamic systems do not exactly conform to this model.

In fact, unmodeled dynamics can seriously degrade the filter performance, even when it was supposed to work with unknown stochastic signals as inputs. The reason for this is that the effect of unmodeled dynamics depends on the input, and, therefore, can bring the estimation algorithm to instability (it diverges).

On the other hand, independent white noise signals will not make the algorithm diverge. The problem of distinguishing between measurement noise and unmodeled dynamics is a difficult one and is treated as a problem of control theory using robust control. \hat{\mathbf{x}}_{k\mid k} = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \hat{\mathbf{x}}_{k\mid k-1} + \mathbf{K}_k \mathbf{z}_k This expression reminds us of a linear interpolation, x = (1-t)(a) + t(b) for t between [0,1].

In our case: t is the matrix \mathbf{K}_k \mathbf{H}_k that takes values from 0 (high error in the sensor) to I or a projection (low error). a is the internal state \hat{\mathbf{x}}_{k\mid k-1} estimated from the model. b is the internal state \mathbf{H}_k^{-1} \mathbf{z}_k estimated from the measurement, assuming \mathbf{H}_k is nonsingular (which in many applications is not a reasonable assumption e.g. when the state dimension is greater than the observation dimension). This expression also resembles the alpha beta filter update step. \operatorname{E}[\mathbf{x}_k - \hat{\mathbf{x}}_{k\mid k}] &= \operatorname{E}[\mathbf{x}_k - \hat{\mathbf{x}}_{k\mid k-1}] = 0 \\ \operatorname{E}[\tilde{\mathbf{y}}_k] &= 0 \end{align} where \operatorname{E}[\xi] is the expected value of \xi .

That is, all estimates have a mean error of zero. Also: \begin{align} \mathbf{P}_{k\mid k} &= \operatorname{cov}\left(\mathbf{x}_k - \hat{\mathbf{x}}_{k\mid k}\right) \\ \mathbf{P}_{k\mid k-1} &= \operatorname{cov}\left(\mathbf{x}_k - \hat{\mathbf{x}}_{k\mid k-1}\right) \\ \mathbf{S}_k &= \operatorname{cov}\left(\tilde{\mathbf{y}}_k\right) \end{align} so covariance matrices accurately reflect the covariance of estimates. \mathbf{x}_k = \begin{bmatrix} x \\ \dot{x} \end{bmatrix} where \dot{x} is the velocity, that is, the derivative of position with respect to time.

We assume that between the (k − 1) and k timestep, uncontrolled forces cause a constant acceleration of a k that is normally distributed with mean 0 and standard deviation σ a . From Newton's laws of motion we conclude that \mathbf{x}_k = \mathbf{F} \mathbf{x}_{k-1} + \mathbf{G} a_k (there is no \mathbf{B}u term since there are no known control inputs. Instead, a k is the effect of an unknown input and \mathbf{G} applies that effect to the state vector) where \begin{align} \mathbf{F} &= \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \\[4pt] \mathbf{G} &= \begin{bmatrix} \frac{1}{2}{\Delta t}^2 \\[6pt] \Delta t \end{bmatrix} \end{align} so that \mathbf{x}_k = \mathbf{F} \mathbf{x}_{k-1} + \mathbf{w}_k where \begin{align} \mathbf{w}_k &\sim N(0, \mathbf{Q}) \\ \mathbf{Q} &= \mathbf{G}\mathbf{G}^\textsf{T}\sigma_a^2 = \begin{bmatrix} \frac{1}{4}{\Delta t}^4 & \frac{1}{2}{\Delta t}^3 \\[6pt] \frac{1}{2}{\Delta t}^3 & {\Delta t}^2 \end{bmatrix}\sigma_a^2. \end{align} The matrix \mathbf{Q} is not full rank (it is of rank one if \Delta t \neq 0 ).

Hence, the distribution N(0, \mathbf{Q}) is not absolutely continuous and has no probability density function. Another way to express this, avoiding explicit degenerate distributions is given by \mathbf{w}_k \sim \mathbf{G} \cdot N\left(0, \sigma_a^2\right). At each time phase, a noisy measurement of the true position of the truck is made.

Let us suppose the measurement noise v k is also distributed normally, with mean 0 and standard deviation σ z . \mathbf{z}_k = \mathbf{H x}_k + \mathbf{v}_k where \mathbf{H} = \begin{bmatrix} 1 & 0 \end{bmatrix} and \mathbf{R} = \mathrm{E}\left[\mathbf{v}_k \mathbf{v}_k^\textsf{T}\right] \mathbf{P}_{0 \mid 0} = \begin{bmatrix} 0 & 0 \\ 0 & 0 \end{bmatrix} If the initial position and velocity are not known perfectly, the covariance matrix should be initialized with suitable variances on its diagonal: \mathbf{P}_{0 \mid 0} = \begin{bmatrix} \sigma_x^2 & 0 \\ 0 & \sigma_\dot{x}^2 \end{bmatrix} The filter will then prefer the information from the first measurements over the information already in the model. For simplicity, assume that the control input \mathbf{u}_k=\mathbf{0} .

Then the Kalman filter may be written: \hat{\mathbf{x}}_{k\mid k} = \mathbf{F}_k \hat{\mathbf{x}}_{k-1\mid k-1} + \mathbf{K}_k[\mathbf{z}_k - \mathbf{H}_k \mathbf{F}_k\hat{\mathbf{x}}_{k-1\mid k-1}]. A similar equation holds if we include a non-zero control input.

Gain matrices \mathbf{K}_k and covariance matrices \mathbf{P}_{k\mid k} evolve independently of the measurements \mathbf{z}_k . From above, the four equations needed for updating the matrices are as follows: \begin{align} \mathbf{P}_{k\mid k-1} &= \mathbf{F}_k \mathbf{P}_{k-1\mid k-1} \mathbf{F}_k^\textsf{T} + \mathbf{Q}_k, \\ \mathbf{S}_k &= \mathbf{H}_k \mathbf{P}_{k\mid k-1} \mathbf{H}_k^\textsf{T} + \mathbf{R}_k, \\ \mathbf{K}_k &= \mathbf{P}_{k\mid k-1}\mathbf{H}_k^\textsf{T} \mathbf{S}_k^{-1}, \\ \mathbf{P}_{k|k} &= \left(\mathbf{I} - \mathbf{K}_k \mathbf{H}_k\right) \mathbf{P}_{k|k-1}. \end{align} Since these depend only on the model, and not the measurements, they may be computed offline. Convergence of the gain matrices \mathbf{K}_k to an asymptotic matrix \mathbf{K}_\infty applies for conditions established in Walrand and Dimakis.

If the \mathbf{P}_{k\mid k} series converges, then it converges exponentially to an asymptotic \mathbf{P}_\infty , assuming non-zero plant noise. Recent analysis has shown that the rate and nature of this convergence can involve multiple coequal modes, including oscillatory components, depending on the eigenstructure of the Jacobian of the above Riccati map evaluated at \mathbf{P}_\infty .

For the moving truck example described above, with \Delta t = 1 and \sigma_a^2=\sigma_z^2 =\sigma_x^2= \sigma_\dot{x}^2=1 , simulation shows convergence in 10 iterations. Using the asymptotic gain, and assuming \mathbf{H}_k and \mathbf{F}_k are independent of k , the Kalman filter becomes a linear time-invariant filter: \hat{\mathbf{x}}_{k} = \mathbf{F} \hat{\mathbf{x}}_{k-1} + \mathbf{K}_\infty[\mathbf{z}_k - \mathbf{H}\mathbf{F} \hat{\mathbf{x}}_{k-1}].

The asymptotic gain \mathbf{K}_\infty , if it exists, can be computed by first solving the following discrete Riccati equation for the asymptotic state covariance \mathbf{P}_\infty : \mathbf{P}_\infty = \mathbf{F}\left(\mathbf{P}_\infty - \mathbf{P}_\infty \mathbf{H}^\textsf{T} \left(\mathbf{H}\mathbf{P}_\infty\mathbf{H}^\textsf{T} + \mathbf{R}\right) ^{-1} \mathbf{H}\mathbf{P}_\infty\right) \mathbf{F}^\textsf{T} + \mathbf{Q}. The asymptotic gain is then computed as before. \mathbf{K}_\infty = \mathbf{P}_\infty \mathbf{H}^\textsf{T} \left( \mathbf{R} + \mathbf{H} \mathbf{P}_\infty \mathbf{H}^\textsf{T} \right) ^{-1}.

Additionally, a form of the asymptotic Kalman filter more commonly used in control theory is given by {\displaystyle {\hat {\mathbf {x} }}_{k+1}=\mathbf {F}{\hat {\mathbf {x} }}_{k}+ \mathbf {B}\mathbf{u}_k + \mathbf \overline{K} _{\infty}[\mathbf {z} _{k}-\mathbf {H}{\hat {\mathbf {x} }}_{k}],} where \overline{\mathbf{K}}_\infty = \mathbf{F}\mathbf{P}_\infty \mathbf{H}^\textsf{T} \left( \mathbf{R} + \mathbf{H} \mathbf{P}_\infty \mathbf{H}^\textsf{T} \right) ^{-1}. This leads to an estimator of the form {\displaystyle {\hat {\mathbf {x} }}_{k+1}=(\mathbf {F}-\overline{\mathbf{K}} _{\infty}\mathbf {H}){\hat {\mathbf {x} }}_{k} + \mathbf {B}\mathbf{u}_k +\mathbf \overline{K} _{\infty}\mathbf {z} _{k}.} \mathbf{P}_{k \mid k} = \operatorname{cov}\left(\mathbf{x}_k - \hat{\mathbf{x}}_{k \mid k}\right) substitute in the definition of \hat{\mathbf{x}}_{k \mid k} \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] and substitute \tilde{\mathbf{y}}_k \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) and \mathbf{z}_k \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) and by collecting the error vectors we get \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] Since the measurement error v k is uncorrelated with the other terms, this becomes \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] by the properties of vector covariance this becomes \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} which, using our invariant on P k | k−1 and the definition of R k becomes \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} This formula (sometimes known as the Joseph form of the covariance update equation) is valid for any value of K k . It turns out that if K k is the optimal Kalman gain, this can be simplified further as shown below. \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} The trace is minimized when its matrix derivative with respect to the gain matrix is zero.

Using the gradient matrix rules and the symmetry of the matrices involved we find that \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. Solving this for K k yields the Kalman gain: \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} This gain, which is known as the optimal Kalman gain, is the one that yields MMSE estimates when used. \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} Referring back to our expanded formula for the a posteriori error covariance, \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} we find the last two terms cancel out, giving \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} 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. \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} no longer provides the actual error covariance. In other words, \mathbf{P}_{k \mid k} \neq E\left[\left(\mathbf{x}_k - \hat{\mathbf{x}}_{k\mid k}\right)\left(\mathbf{x}_k - \hat{\mathbf{x}}_{k \mid k}\right)^\textsf{T}\right] .

In most real-time applications, the covariance matrices that are used in designing the Kalman filter are different from the actual (true) noise covariances matrices. This sensitivity analysis describes the behavior of the estimation error covariance when the noise covariances as well as the system matrices \mathbf{F}_k and \mathbf{H}_k that are fed as inputs to the filter are incorrect.

Thus, the sensitivity analysis describes the robustness (or sensitivity) of the estimator to misspecified statistical and parametric inputs to the estimator. This discussion is limited to the error sensitivity analysis for the case of statistical uncertainties.

Here the actual noise covariances are denoted by \mathbf{Q}^a_k and \mathbf{R}^a_k respectively, whereas the design values used in the estimator are \mathbf{Q}_k and \mathbf{R}_k respectively. The actual error covariance is denoted by \mathbf{P}_{k \mid k}^a and \mathbf{P}_{k \mid k} as computed by the Kalman filter is referred to as the Riccati variable.

When \mathbf{Q}_k \equiv \mathbf{Q}^a_k and \mathbf{R}_k \equiv \mathbf{R}^a_k , this means that \mathbf{P}_{k \mid k} = \mathbf{P}_{k \mid k}^a . While computing the actual error covariance using \mathbf{P}_{k \mid k}^a = E\left[\left(\mathbf{x}_k - \hat{\mathbf{x}}_{k \mid k}\right)\left(\mathbf{x}_k - \hat{\mathbf{x}}_{k \mid k}\right)^\textsf{T}\right] , substituting for \widehat{\mathbf{x}}_{k \mid k} and using the fact that E\left[\mathbf{w}_k\mathbf{w}_k^\textsf{T}\right] = \mathbf{Q}_k^a and E\left[\mathbf{v}_k \mathbf{v}_k^\textsf{T}\right] = \mathbf{R}_k^a , results in the following recursive equations for \mathbf{P}_{k \mid k}^a : \mathbf{P}_{k \mid k-1}^a = \mathbf{F}_k\mathbf{P}_{k-1 \mid k-1}^a \mathbf{F}_k^\textsf{T} + \mathbf{Q}_k^a and \mathbf{P}_{k \mid k}^a = \left(\mathbf{I} - \mathbf{K}_k \mathbf{H}_k\right)\mathbf{P}_{k \mid k-1}^a \left(\mathbf{I} - \mathbf{K}_k \mathbf{H}_k\right)^\textsf{T} + \mathbf{K}_k \mathbf{R}_k^a \mathbf{K}_k^\textsf{T} While computing \mathbf{P}_{k \mid k} , by design the filter implicitly assumes that E\left[\mathbf{w}_k \mathbf{w}_k^\textsf{T}\right] = \mathbf{Q}_k and E\left[\mathbf{v}_k \mathbf{v}_k^\textsf{T}\right] = \mathbf{R}_k . The recursive expressions for \mathbf{P}_{k \mid k}^a and \mathbf{P}_{k \mid k} are identical except for the presence of \mathbf{Q}_k^a and \mathbf{R}_k^a in place of the design values \mathbf{Q}_k and \mathbf{R}_k respectively. Researches have been done to analyze Kalman filter system's robustness.

Positive definite matrices have the property that they have a factorization into the product of a non-singular, lower-triangular matrix S and its transpose : P = S·S T . The factor S can be computed efficiently using the Cholesky factorization algorithm. This product form of the covariance matrix P is guaranteed to be symmetric, and for all 1 kk is equal to the square of the euclidean norm of the k-th row of S, which is necessarily positive.

An equivalent form, which avoids many of the square root operations involved in the Cholesky factorization algorithm, yet preserves the desirable numerical properties, is the U-D decomposition form, P = U·D·U T , where U is a unit triangular matrix (with unit diagonal), and D is a diagonal matrix. Between the two, the U-D factorization uses the same amount of storage, and somewhat less computation, and is the most commonly used triangular factorization.

(Early literature on the relative efficiency is somewhat misleading, as it assumed that square roots were much more time-consuming than divisions, while on 21st-century computers they are only slightly more expensive.) Efficient algorithms for the Kalman prediction and update steps in the factored form were developed by G. J. Bierman and C. L. Thornton.

The L·D·L T decomposition of the innovation covariance matrix S k is the basis for another type of numerically efficient and robust square root filter. The algorithm starts with the LU decomposition as implemented in the Linear Algebra PACKage (LAPACK).

These results are further factored into the L·D·L T structure with methods given by Golub and Van Loan (algorithm 4.1.2) for a symmetric nonsingular matrix. Any singular covariance matrix is pivoted so that the first diagonal partition is nonsingular and well-conditioned.

The pivoting algorithm must retain any portion of the innovation covariance matrix directly corresponding to observed state-variables H k ·x k|k-1 that are associated with auxiliary observations in y k . The l·d·l t square-root filter requires orthogonalization of the observation vector. This may be done with the inverse square-root of the covariance matrix for the auxiliary variables using Method 2 in Higham (2002, p. 263). p(\mathbf{x}_k\mid \mathbf{x}_0,\dots,\mathbf{x}_{k-1}) = p(\mathbf{x}_k\mid \mathbf{x}_{k-1}) Similarly, the measurement at the k-th timestep is dependent only upon the current state and is conditionally independent of all other states given the current state. p(\mathbf{z}_k\mid\mathbf{x}_0,\dots,\mathbf{x}_k) = p(\mathbf{z}_k\mid \mathbf{x}_k ) Using these assumptions the probability distribution over all states of the hidden Markov model can be written simply as: p\left(\mathbf{x}_0, \dots, \mathbf{x}_k, \mathbf{z}_1, \dots, \mathbf{z}_k\right) = p\left(\mathbf{x}_0\right)\prod_{i=1}^k p\left(\mathbf{z}_i \mid \mathbf{x}_i\right)p\left(\mathbf{x}_i \mid \mathbf{x}_{i-1}\right) However, when a Kalman filter is used to estimate the state x, the probability distribution of interest is that associated with the current states conditioned on the measurements up to the current timestep.

This is achieved by marginalizing out the previous states and dividing by the probability of the measurement set. This results in the predict and update phases of the Kalman filter written probabilistically.

The probability distribution associated with the predicted state is the sum (integral) of the products of the probability distribution associated with the transition from the (k − 1)-th timestep to the k-th and the probability distribution associated with the previous state, over all possible x_{k-1} . p\left(\mathbf{x}_k \mid \mathbf{Z}_{k-1}\right) = \int p\left(\mathbf{x}_k \mid \mathbf{x}_{k-1}\right) p\left(\mathbf{x}_{k-1} \mid \mathbf{Z}_{k-1}\right)\, d\mathbf{x}_{k-1} The measurement set up to time t is \mathbf{Z}_t = \left\{\mathbf{z}_1, \dots, \mathbf{z}_t\right\} The probability distribution of the update is proportional to the product of the measurement likelihood and the predicted state. p\left(\mathbf{x}_k \mid \mathbf{Z}_k\right) = \frac{p\left(\mathbf{z}_k \mid \mathbf{x}_k\right) p\left(\mathbf{x}_k \mid \mathbf{Z}_{k-1}\right)}{p\left(\mathbf{z}_k \mid \mathbf{Z}_{k-1}\right)} The denominator p\left(\mathbf{z}_k \mid \mathbf{Z}_{k-1}\right) = \int p\left(\mathbf{z}_k \mid \mathbf{x}_k\right) p\left(\mathbf{x}_k \mid \mathbf{Z}_{k-1}\right)\, d\mathbf{x}_k is a normalization term. The remaining probability density functions are \begin{align} p\left(\mathbf{x}_k \mid \mathbf{x}_{k-1}\right) &= \mathcal{N}\left(\mathbf{F}_k\mathbf{x}_{k-1}+\mathbf{B}_k\mathbf{u}_k, \mathbf{Q}_k\right) \\ p\left(\mathbf{z}_k \mid \mathbf{x}_k\right) &= \mathcal{N}\left(\mathbf{H}_k\mathbf{x}_k, \mathbf{R}_k\right) \\ p\left(\mathbf{x}_{k-1} \mid \mathbf{Z}_{k-1}\right) &= \mathcal{N}\left(\hat{\mathbf{x}}_{k-1}, \mathbf{P}_{k-1}\right) \end{align} The PDF at the previous timestep is assumed inductively to be the estimated state and covariance.

This is justified because, as an optimal estimator, the Kalman filter makes best use of the measurements, therefore the PDF for \mathbf{x}_k given the measurements \mathbf{Z}_k is the Kalman filter estimate. Related to the recursive Bayesian interpretation described above, the Kalman filter can be viewed as a generative model, i.e., a process for generating a stream of random observations z = (z 0 , z 1 , z 2 , ...).

Specifically, the process is Sample a hidden state \mathbf{x}_0 from the Gaussian prior distribution p\left(\mathbf{x}_0\right) = \mathcal{N}\left(\hat{\mathbf{x}}_{0 \mid 0}, \mathbf{P}_{0 \mid 0}\right) . Sample an observation \mathbf{z}_0 from the observation model p\left(\mathbf{z}_0 \mid \mathbf{x}_0\right) = \mathcal{N}\left(\mathbf{H}_0\mathbf{x}_0, \mathbf{R}_0\right) .

For k = 1, 2, 3, \ldots , do Sample the next hidden state \mathbf{x}_k from the transition model p\left(\mathbf{x}_k \mid \mathbf{x}_{k-1}\right) = \mathcal{N}\left(\mathbf{F}_k \mathbf{x}_{k-1} + \mathbf{B}_k\mathbf{u}_k, \mathbf{Q}_k\right). Sample an observation \mathbf{z}_k from the observation model p\left(\mathbf{z}_k \mid \mathbf{x}_k\right) = \mathcal{N}\left(\mathbf{H}_k\mathbf{x}_k, \mathbf{R}_k\right).

This process has identical structure to the hidden Markov model, except that the discrete state and observations are replaced with continuous variables sampled from Gaussian distributions. In some applications, it is useful to compute the probability that a Kalman filter with a given set of parameters (prior distribution, transition and observation models, and control inputs) would generate a particular observed signal.

This probability is known as the marginal likelihood because it integrates over ("marginalizes out") the values of the hidden state variables, so it can be computed using only the observed signal. The marginal likelihood can be useful to evaluate different parameter choices, or to compare the Kalman filter against other models using Bayesian model comparison.

It is straightforward to compute the marginal likelihood as a side effect of the recursive filtering computation. By the chain rule, the likelihood can be factored as the product of the probability of each observation given previous observations, p(\mathbf{z}) = \prod_{k=0}^T p\left(\mathbf{z}_k \mid \mathbf{z}_{k-1}, \ldots, \mathbf{z}_0\right) , and because the Kalman filter describes a Markov process, all relevant information from previous observations is contained in the current state estimate \hat{\mathbf{x}}_{k \mid k-1}, \mathbf{P}_{k \mid k-1}.

Thus the marginal likelihood is given by \begin{align} p(\mathbf{z}) &= \prod_{k=0}^T \int p\left(\mathbf{z}_k \mid \mathbf{x}_k\right) p\left(\mathbf{x}_k \mid \mathbf{z}_{k-1}, \ldots,\mathbf{z}_0\right) d\mathbf{x}_k\\ &= \prod_{k=0}^T \int \mathcal{N}\left(\mathbf{z}_k; \mathbf{H}_k\mathbf{x}_k, \mathbf{R}_k\right) \mathcal{N}\left(\mathbf{x}_k; \hat{\mathbf{x}}_{k \mid k-1}, \mathbf{P}_{k \mid k-1}\right) d\mathbf{x}_k\\ &= \prod_{k=0}^T \mathcal{N}\left(\mathbf{z}_k; \mathbf{H}_k\hat{\mathbf{x}}_{k \mid k-1}, \mathbf{R}_k + \mathbf{H}_k \mathbf{P}_{k \mid k-1} \mathbf{H}_k^\textsf{T}\right)\\ &= \prod_{k=0}^T \mathcal{N}\left(\mathbf{z}_k; \mathbf{H}_k\hat{\mathbf{x}}_{k \mid k-1}, \mathbf{S}_k\right), \end{align} i.e., a product of Gaussian densities, each corresponding to the density of one observation z k under the current filtering distribution \mathbf{H}_k\hat{\mathbf{x}}_{k \mid k-1}, \mathbf{S}_k . This can easily be computed as a simple recursive update; however, to avoid numeric underflow, in a practical implementation it is usually desirable to compute the log marginal likelihood \ell = \log p(\mathbf{z}) instead. Adopting the convention \ell^{(-1)} = 0 , this can be done via the recursive update rule \ell^{(k)} = \ell^{(k-1)} - \frac{1}{2} \left(\tilde{\mathbf{y}}_k^\textsf{T} \mathbf{S}^{-1}_k \tilde{\mathbf{y}}_k + \log \left|\mathbf{S}_k\right| + d_y\log 2\pi \right), where d_y is the dimension of the measurement vector.

An important application where such a (log) likelihood of the observations (given the filter parameters) is used is multi-target tracking. For example, consider an object tracking scenario where a stream of observations is the input, however, it is unknown how many objects are in the scene (or, the number of objects is known but is greater than one).

For such a scenario, it can be unknown apriori which observations/measurements were generated by which object. A multiple hypothesis tracker (MHT) typically will form different track association hypotheses, where each hypothesis can be considered as a Kalman filter (for the linear Gaussian case) with a specific set of parameters associated with the hypothesized object.

Thus, it is important to compute the likelihood of the observations for the different hypotheses under consideration, such that the most-likely one can be found. In cases where the dimension of the observation vector y is bigger than the dimension of the state space vector x, the information filter can avoid the inversion of a bigger matrix in the Kalman gain calculation at the price of inverting a smaller matrix in the prediction step, thus saving computing time.

Additionally, the information filter allows for system information initialization according to {I_{1|0} = P^{-1}_{1|0} = 0} , which would not be possible for the regular Kalman filter. In the information filter, or inverse covariance filter, the estimated covariance and estimated state are replaced by the information matrix and information vector respectively.

These are defined as: \begin{align} \mathbf{Y}_{k \mid k} &= \mathbf{P}_{k \mid k}^{-1} \\ \hat{\mathbf{y}}_{k \mid k} &= \mathbf{P}_{k \mid k}^{-1}\hat{\mathbf{x}}_{k \mid k} \end{align} Similarly the predicted covariance and state have equivalent information forms, defined as: \begin{align} \mathbf{Y}_{k \mid k-1} &= \mathbf{P}_{k \mid k-1}^{-1} \\ \hat{\mathbf{y}}_{k \mid k-1} &= \mathbf{P}_{k \mid k-1}^{-1}\hat{\mathbf{x}}_{k \mid k-1} \end{align} and the measurement covariance and measurement vector, which are defined as: \begin{align} \mathbf{I}_k &= \mathbf{H}_k^\textsf{T} \mathbf{R}_k^{-1} \mathbf{H}_k \\ \mathbf{i}_k &= \mathbf{H}_k^\textsf{T} \mathbf{R}_k^{-1} \mathbf{z}_k \end{align} The information update now becomes a trivial sum. \begin{align} \mathbf{Y}_{k \mid k} &= \mathbf{Y}_{k \mid k-1} + \mathbf{I}_k \\ \hat{\mathbf{y}}_{k \mid k} &= \hat{\mathbf{y}}_{k \mid k-1} + \mathbf{i}_k \end{align} The main advantage of the information filter is that N measurements can be filtered at each time step simply by summing their information matrices and vectors. \begin{align} \mathbf{Y}_{k \mid k} &= \mathbf{Y}_{k \mid k-1} + \sum_{j=1}^N \mathbf{I}_{k,j} \\ \hat{\mathbf{y}}_{k \mid k} &= \hat{\mathbf{y}}_{k \mid k-1} + \sum_{j=1}^N \mathbf{i}_{k,j} \end{align} To predict the information filter the information matrix and vector can be converted back to their state space equivalents, or alternatively the information space prediction can be used. \begin{align} \mathbf{M}_k &= \left[\mathbf{F}_k^{-1}\right]^\textsf{T} \mathbf{Y}_{k-1 \mid k-1} \mathbf{F}_k^{-1} \\ \mathbf{C}_k &= \mathbf{M}_k \left[\mathbf{M}_k + \mathbf{Q}_k^{-1}\right]^{-1} \\ \mathbf{L}_k &= \mathbf{I} - \mathbf{C}_k \\ \mathbf{Y}_{k \mid k-1} &= \mathbf{L}_k \mathbf{M}_k + \mathbf{C}_k \mathbf{Q}_k^{-1} \mathbf{C}_k^\textsf{T} \\ \hat{\mathbf{y}}_{k \mid k-1} &= \mathbf{L}_k \left[\mathbf{F}_k^{-1}\right]^\textsf{T} \hat{\mathbf{y}}_{k-1 \mid k-1} \end{align} the various \hat{\mathbf{x}}_{t-i \mid t} with i = 1, \ldots, N-1 are new variables; i.e., they do not appear in the standard Kalman filter; the gains are computed via the following scheme: \mathbf{K}^{(i+1)} = \mathbf{P}^{(i)} \mathbf{H}^\textsf{T} \left[ \mathbf{H} \mathbf{P} \mathbf{H}^\textsf{T} + \mathbf{R} \right]^{-1} and \mathbf{P}^{(i)} = \mathbf{P} \left[ \left( \mathbf{F} - \mathbf{K} \mathbf{H} \right)^\textsf{T} \right]^i where \mathbf{P} and \mathbf{K} are the prediction error covariance and the gains of the standard Kalman filter (i.e., \mathbf{P}_{t \mid t-1} ). If the estimation error covariance is defined so that \mathbf{P}_i := E \left[ \left( \mathbf{x}_{t-i} - \hat{\mathbf{x}}_{t-i \mid t} \right)^{*} \left( \mathbf{x}_{t-i} - \hat{\mathbf{x}}_{t-i \mid t} \right) \mid z_1 \ldots z_t \right], then we have that the improvement on the estimation of \mathbf{x}_{t-i} is given by: \mathbf{P} - \mathbf{P}_i = \sum_{j = 0}^i \left[ \mathbf{P}^{(j)} \mathbf{H}^\textsf{T} \left( \mathbf{H} \mathbf{P} \mathbf{H}^\textsf{T} + \mathbf{R} \right)^{-1} \mathbf{H} \left( \mathbf{P}^{(i)} \right)^\textsf{T} \right] \hat{\mathbf{x}}_{k \mid n} &= \hat{\mathbf{x}}_{k \mid k} + \mathbf{C}_k \left(\hat{\mathbf{x}}_{k+1 \mid n} - \hat{\mathbf{x}}_{k+1 \mid k}\right) \\ \mathbf{P}_{k \mid n} &= \mathbf{P}_{k \mid k} + \mathbf{C}_k \left(\mathbf{P}_{k+1 \mid n} - \mathbf{P}_{k+1 \mid k}\right) \mathbf{C}_k^\textsf{T} \end{align} where \mathbf{C}_k = \mathbf{P}_{k \mid k} \mathbf{F}_{k+1}^\textsf{T} \mathbf{P}_{k+1 \mid k}^{-1}. \mathbf{x}_{k \mid k} is the a-posteriori state estimate of timestep k and \mathbf{x}_{k+1 \mid k} is the a-priori state estimate of timestep k + 1 .

The same notation applies to the covariance. \tilde{\Lambda}_k &= \mathbf{H}_k^\textsf{T} \mathbf{S}_k^{-1} \mathbf{H}_k + \hat{\mathbf{C}}_k^\textsf{T} \hat{\Lambda}_k \hat{\mathbf{C}}_k \\ \hat{\Lambda}_{k-1} &= \mathbf{F}_k^\textsf{T}\tilde{\Lambda}_k\mathbf{F}_k \\ \hat{\Lambda}_n &= 0 \\ \tilde{\lambda}_k &= -\mathbf{H}_k^\textsf{T} \mathbf{S}_k^{-1} \mathbf{y}_k + \hat{\mathbf{C}}_k^\textsf{T} \hat{\lambda}_k \\ \hat{\lambda}_{k-1} &= \mathbf{F}_k^\textsf{T}\tilde{\lambda}_k \\ \hat{\lambda}_n &= 0 \end{align} where \mathbf{S}_k is the residual covariance and \hat{\mathbf{C}}_k = \mathbf{I} - \mathbf{K}_k \mathbf{H}_k . The smoothed state and covariance can then be found by substitution in the equations \begin{align} \mathbf{P}_{k \mid n} &= \mathbf{P}_{k \mid k} - \mathbf{P}_{k \mid k}\hat{\Lambda}_k\mathbf{P}_{k \mid k} \\ \mathbf{x}_{k \mid n} &= \mathbf{x}_{k \mid k} - \mathbf{P}_{k \mid k}\hat{\lambda}_k \end{align} or \begin{align} \mathbf{P}_{k \mid n} &= \mathbf{P}_{k \mid k-1} - \mathbf{P}_{k \mid k-1}\tilde{\Lambda}_k\mathbf{P}_{k \mid k-1} \\ \mathbf{x}_{k \mid n} &= \mathbf{x}_{k \mid k-1} - \mathbf{P}_{k \mid k-1}\tilde{\lambda}_k. \end{align} An important advantage of the MBF is that it does not require finding the inverse of the covariance matrix. Bierman's derivation is based on the RTS smoother, which assumes that the underlying distributions are Gaussian.

However, a derivation of the MBF based on the concept of the fixed point smoother, which does not require the Gaussian assumption, is given by Gibbs. The MBF can also be used to perform consistency checks on the filter residuals and the difference between the value of a filter state after an update and the smoothed value of the state, that is \mathbf{x}_{k \mid k} - \mathbf{x}_{k \mid n} . \hat{\mathbf{x}}_{k+1 \mid k} &= (\mathbf{F}_k - \mathbf{K}_k\mathbf{H}_k)\hat{\mathbf{x}}_{k \mid k-1} + \mathbf{K}_k\mathbf{z}_k \\ \alpha_k &= -\mathbf{S}_k^{-\frac{1}{2}}\mathbf{H}_k\hat{\mathbf{x}}_{k \mid k-1} + \mathbf{S}_k^{-\frac{1}{2}}\mathbf{z}_k \end{align} The above system is known as the inverse Wiener-Hopf factor.

The backward recursion is the adjoint of the above forward system. The result of the backward pass \beta_k may be calculated by operating the forward equations on the time-reversed \alpha_k and time reversing the result.

In the case of output estimation, the smoothed estimate is given by \hat{\mathbf{y}}_{k \mid N} = \mathbf{z}_k - \mathbf{R}_k\beta_k Taking the causal part of this minimum-variance smoother yields \hat{\mathbf{y}}_{k \mid k} = \mathbf{z}_k - \mathbf{R}_k \mathbf{S}_k^{-\frac{1}{2}} \alpha_k which is identical to the minimum-variance Kalman filter. The above solutions minimize the variance of the output estimation error.

Note that the Rauch–Tung–Striebel smoother derivation assumes that the underlying distributions are Gaussian, whereas the minimum-variance solutions do not. Optimal smoothers for state estimation and input estimation can be constructed similarly. A continuous-time version of the above smoother is described in.

Expectation–maximization algorithms may be employed to calculate approximate maximum likelihood estimates of unknown state-space parameters within minimum-variance filters and smoothers. Often uncertainties remain within problem assumptions. A smoother that accommodates uncertainties can be designed by adding a positive definite term to the Riccati equation.

In cases where the models are nonlinear, step-wise linearizations may be within the minimum-variance filter and smoother recursions (extended Kalman filtering). \mathbf{x}_k &= f(\mathbf{x}_{k-1}, \mathbf{u}_k) + \mathbf{w}_k \\ \mathbf{z}_k &= h(\mathbf{x}_k) + \mathbf{v}_k \end{align} The function f can be used to compute the predicted state from the previous estimate and similarly the function h can be used to compute the predicted measurement from the predicted state. However, f and h cannot be applied to the covariance directly.

Instead a matrix of partial derivatives (the Jacobian) is computed. At each timestep the Jacobian is evaluated with current predicted states. These matrices can be used in the Kalman filter equations.

This process essentially linearizes the nonlinear function around the current estimate. For a random vector \mathbf{x}=(x_1, \dots, x_L) , sigma points are any set of vectors \{\mathbf{s}_0,\dots, \mathbf{s}_N \}=\bigl\{\begin{pmatrix} s_{0,1}& s_{0,2}&\ldots& s_{0,L} \end{pmatrix}, \dots, \begin{pmatrix} s_{N,1}& s_{N,2}&\ldots& s_{N,L} \end{pmatrix}\bigr\} attributed with first-order weights W_0^a, \dots, W_N^a that fulfill \sum_{j=0}^N W_j^a=1 for all i=1, \dots, L : E[x_i]=\sum_{j=0}^N W_j^a s_{j,i} second-order weights W_0^c, \dots, W_N^c that fulfill \sum_{j=0}^N W_j^c=1 for all pairs (i,l) \in \{1,\dots, L\}^2: E[x_ix_l]=\sum_{j=0}^N W_j^c s_{j,i}s_{j,l} .

A simple choice of sigma points and weights for \mathbf{x}_{k-1\mid k-1} in the UKF algorithm is \begin{align} \mathbf{s}_0&=\hat \mathbf{x}_{k-1\mid k-1}\\ -1& where \hat \mathbf{x}_{k-1\mid k-1} is the mean estimate of \mathbf{x}_{k-1\mid k-1} . The vector \mathbf{A}_j is the jth column of \mathbf{A} where \mathbf{P}_{k-1\mid k-1}=\mathbf{AA}^\textsf{T} .

Typically, \mathbf{A} is obtained via Cholesky decomposition of \mathbf{P}_{k-1\mid k-1} . With some care the filter equations can be expressed in such a way that \mathbf{A} is evaluated directly without intermediate calculations of \mathbf{P}_{k-1\mid k-1} . This is referred to as the square-root unscented Kalman filter.

The weight of the mean value, W_0 , can be chosen arbitrarily. Another popular parameterization (which generalizes the above) is \begin{align} \mathbf{s}_0&=\hat \mathbf{x}_{k-1\mid k-1}\\ W_0^a&= \frac{\alpha^2\kappa-L}{\alpha^2\kappa}\\ W_0^c&= W_0^a + 1-\alpha^2+\beta \\ \mathbf{s}_j&=\hat \mathbf{x}_{k-1\mid k-1} + \alpha\sqrt{\kappa} \mathbf{A}_j, \quad j=1, \dots, L\\ \mathbf{s}_{L+j}&=\hat \mathbf{x}_{k-1\mid k-1} - \alpha\sqrt{\kappa} \mathbf{A}_j, \quad j=1, \dots, L\\ W_j^a&=W_j^c=\frac{1}{2\alpha^2\kappa}, \quad j=1, \dots, 2L. \end{align} \alpha and \kappa control the spread of the sigma points. \beta is related to the distribution of x . Note that this is an overparameterization in the sense that any one of \alpha , \beta and \kappa can be chosen arbitrarily.

Appropriate values depend on the problem at hand, but a typical recommendation is \alpha = 1 , \beta = 0 , and \kappa \approx 3L/2 . If the true distribution of x is Gaussian, \beta = 2 is optimal. Given estimates of the mean and covariance, \hat\mathbf{x}_{k-1\mid k-1} and \mathbf{P}_{k-1\mid k-1} , one obtains N = 2L+1 sigma points as described in the section above.

The sigma points are propagated through the transition function f. \mathbf{x}_{j} = f\left(\mathbf{s}_{j}\right) \quad j = 0, \dots, 2L . The propagated sigma points are weighed to produce the predicted mean and covariance. \begin{align} \hat{\mathbf{x}}_{k \mid k-1} &= \sum_{j=0}^{2L} W_j^a \mathbf{x}_j \\ \mathbf{P}_{k \mid k-1} &= \sum_{j=0}^{2L} W_j^c \left(\mathbf{x}_j - \hat{\mathbf{x}}_{k \mid k-1}\right)\left(\mathbf{x}_j - \hat{\mathbf{x}}_{k \mid k-1}\right)^\textsf{T}+\mathbf{Q}_k \end{align} where W_j^a are the first-order weights of the original sigma points, and W_j^c are the second-order weights. The matrix \mathbf{Q}_k is the covariance of the transition noise, \mathbf{w}_k . Given prediction estimates \hat{\mathbf{x}}_{k \mid k-1} and \mathbf{P}_{k \mid k-1} , a new set of N = 2L+1 sigma points \mathbf{s}_0, \dots, \mathbf{s}_{2L} with corresponding first-order weights W_0^a,\dots W_{2L}^a and second-order weights W_0^c,\dots, W_{2L}^c is calculated.

These sigma points are transformed through the measurement function h . \mathbf{z}_j=h(\mathbf{s}_j), \,\, j=0,1, \dots, 2L . Then the empirical mean and covariance of the transformed points are calculated. \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} where \mathbf{R}_k is the covariance matrix of the observation noise, \mathbf{v}_k . Additionally, the cross covariance matrix is also needed \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} The Kalman gain is \begin{align} \mathbf{K}_k=\mathbf{C_{xz}}\hat{\mathbf{S}}_k^{-1}. \end{align} The updated mean and covariance estimates are \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} p(\mathbf{x}_1) &= \mathcal{N}(0, \mathbf{T}), \\ p(\mathbf{x}_k\mid\mathbf{x}_{k-1}) &= \mathcal{N}(\mathbf{F}\mathbf{x}_{k-1}, \mathbf{C}), \end{align} where \mathbf{T}= \mathbf{F}\mathbf{T}\mathbf{F}^\intercal + \mathbf{C} , if p(\mathbf{x}_k\mid\mathbf{z}_{1:k}) \approx \mathcal{N}(\hat{\mathbf{x}}_{k|k-1}, \mathbf{P}_{k|k-1}), then given a new observation \mathbf{z}_k , it follows that p(\mathbf{x}_{k+1}\mid\mathbf{z}_{1:k+1}) \approx \mathcal{N}(\hat{ \mathbf{x}}_{k+1|k}, \mathbf{P}_{k+1|k}) where \begin{align} \mathbf{M}_{k+1} &= \mathbf{F}\mathbf{P}_{k|k-1}\mathbf{F}^\intercal + \mathbf{C}, \\ \mathbf{P}_{k+1|k} &= (\mathbf{M}_{k+1}^{-1} + Q(\mathbf{z}_k)^{-1} - \mathbf{T}^{-1})^{-1}, \\ \hat{\mathbf{x}}_{k+1|k} &= \mathbf{P}_{k+1|k} (\mathbf{M}_{k+1}^{-1}\mathbf{F}\hat{\mathbf{x}}_{k|k-1} + \mathbf{P}_{k+1|k}^{-1}g(\mathbf{z}_k) ). \end{align} Note that this approximation requires Q(\mathbf{z}_k)^{-1} - \mathbf{T}^{-1} to be positive-definite; in the case that it is not, \mathbf{P}_{k+1|k} = (\mathbf{M}_{k+1}^{-1} + Q(\mathbf{z}_k)^{-1})^{-1} is used instead. Such an approach proves particularly useful when the dimensionality of the observations is much greater than that of the latent states and can be used to build filters that are particularly robust to nonstationarities in the observation model. \frac{d}{dt}\mathbf{x}(t) &= \mathbf{F}(t)\mathbf{x}(t) + \mathbf{B}(t)\mathbf{u}(t) + \mathbf{w}(t) \\ \mathbf{z}(t) &= \mathbf{H}(t) \mathbf{x}(t) + \mathbf{v}(t) \end{align} where \mathbf{Q}(t) and \mathbf{R}(t) represent the intensities of the two white noise terms \mathbf{w}(t) and \mathbf{v}(t) , respectively.

The filter consists of two differential equations, one for the state estimate and one for the covariance: \begin{align} \frac{d}{dt}\hat{\mathbf{x}}(t) &= \mathbf{F}(t)\hat{\mathbf{x}}(t) + \mathbf{B}(t)\mathbf{u}(t) + \mathbf{K}(t) \left(\mathbf{z}(t) - \mathbf{H}(t)\hat{\mathbf{x}}(t)\right) \\ \frac{d}{dt}\mathbf{P}(t) &= \mathbf{F}(t)\mathbf{P}(t) + \mathbf{P}(t)\mathbf{F}^\textsf{T}(t) + \mathbf{Q}(t) - \mathbf{K}(t)\mathbf{R}(t)\mathbf{K}^\textsf{T}(t) \end{align} where the Kalman gain is given by \mathbf{K}(t) = \mathbf{P}(t)\mathbf{H}^\textsf{T}(t)\mathbf{R}^{-1}(t) Note that in this expression for \mathbf{K}(t) the covariance of the observation noise \mathbf{R}(t) represents at the same time the covariance of the prediction error (or innovation) \tilde{\mathbf{y}}(t) = \mathbf{z}(t) - \mathbf{H}(t)\hat{\mathbf{x}}(t) ; these covariances are equal only in the case of continuous time. The distinction between the prediction and update steps of discrete-time Kalman filtering does not exist in continuous time.

The second differential equation, for the covariance, is an example of a Riccati equation. Nonlinear generalizations to Kalman–Bucy filters include continuous time extended Kalman filter. \dot{\mathbf{x}}(t) &= \mathbf{F}(t)\mathbf{x}(t) + \mathbf{B}(t)\mathbf{u}(t) + \mathbf{w}(t), &\mathbf{w}(t) &\sim N\left(\mathbf{0}, \mathbf{Q}(t)\right) \\ \mathbf{z}_k &= \mathbf{H}_k\mathbf{x}_k + \mathbf{v}_k, &\mathbf{v}_k &\sim N(\mathbf{0},\mathbf{R}_k) \end{align} where \mathbf{x}_k = \mathbf{x}(t_k) . \hat{\mathbf{x}}_{0 \mid 0} = E\left[\mathbf{x}(t_0)\right], \mathbf{P}_{0 \mid 0} = \operatorname{Var}\left[\mathbf{x}\left(t_0\right)\right] \dot{\hat{\mathbf{x}}}(t) &= \mathbf{F}(t) \hat{\mathbf{x}}(t) + \mathbf{B}(t) \mathbf{u}(t) \text{, with } \hat{\mathbf{x}}\left(t_{k-1}\right) = \hat{\mathbf{x}}_{k-1 \mid k-1} \\ \Rightarrow \hat{\mathbf{x}}_{k \mid k-1} &= \hat{\mathbf{x}}\left(t_k\right) \\ \dot{\mathbf{P}}(t) &= \mathbf{F}(t)\mathbf{P}(t) + \mathbf{P}(t)\mathbf{F}(t)^\textsf{T} + \mathbf{Q}(t) \text{, with } \mathbf{P}\left(t_{k-1}\right) = \mathbf{P}_{k-1 \mid k-1} \\ \Rightarrow \mathbf{P}_{k \mid k-1} &= \mathbf{P}\left(t_k\right) \end{align} The prediction equations are derived from those of continuous-time Kalman filter without update from measurements, i.e., \mathbf{K}(t) = 0 .

The predicted state and covariance are calculated respectively by solving a set of differential equations with the initial value equal to the estimate at the previous step. For the case of linear time invariant systems, the continuous time dynamics can be exactly discretized into a discrete time system using matrix exponentials. \mathbf{K}_k &= \mathbf{P}_{k \mid k-1}\mathbf{H}_k^\textsf{T} \left(\mathbf{H}_k\mathbf{P}_{k \mid k-1}\mathbf{H}_k^\textsf{T} + \mathbf{R}_k\right)^{-1} \\ \hat{\mathbf{x}}_{k \mid k} &= \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) \\ \mathbf{P}_{k \mid k} &= \left(\mathbf{I} - \mathbf{K}_k\mathbf{H}_k\right)\mathbf{P}_{k \mid k-1} \end{align} The update equations are identical to those of the discrete-time Kalman filter.

Text derived from https://en.wikipedia.org/w/index.php?oldid=1376993718 under CC BY-SA 4.0; modifications have been made.

Sources cited in this revision

31 named source(s) extracted from the cited revision. A source with no author is shown by its publication; nothing is invented.