# Kalman filter

457pages on
this wiki

## What makes the Kalman filter particularly unique is that it is purely a time domain filter. Most filters (for example, a low-pass filter) are formulated in the frequency domain and then transformed back to the time domain for implementation. Edit

== Peter Swerling actually developed a similar algorithm earlier. Stanley Schmidt is generally credited with developing the first implementation of a Kalman filter. It was during a visit of Kalman to the NASA Ames Research Center that he saw the applicability of his ideas to the problem of trajectory estimation for the Apollo program, leading to its incorporation in the Apollo navigation computer. ==

## $\textbf{x}_{k} = \textbf{F}_{k} \textbf{x}_{k-1} + \textbf{B}_{k}\textbf{u}_{k} + \textbf{w}_{k}$ Edit

==
The state transition model ==

## $\textbf{R}_{k} \delta(k-j) = E[\textbf{v}_{k} \textbf{v}_{j}^{T}]$ Edit

==

==


## The Kalman filter is used to obtain an estimate of the $\hat{\textbf{x}}_{k|k}$ Edit

== of the true state $\textbf{x}_{k}$ using only measurements $\textbf{z}_{i} \; \forall i\in(0 ... k)$ and control inputs $\textbf{u}_{i} \; \forall i\in(0 ... k)$ ==

## $\hat{\textbf{x}}_{k|k-1} = \textbf{F}_{k}\hat{\textbf{x}}_{k-1|k-1} + \textbf{B}_{k} \textbf{u}_{k}$ Edit

==

==


==
Update ==

## $\textbf{K}_{k} = \hat{\textbf{P}}_{k|k-1}\textbf{H}_{k}^{T}(\textbf{H}_{k}\hat{\textbf{P}}_{k|k-1}\textbf{H}_{k}^{T} + \textbf{R}_{k})^{-1}$ Edit

==

==


## $\hat{\textbf{x}}_{k|k} = \hat{\textbf{x}}_{k|k-1} + \textbf{K}_{k}(\textbf{z}_{k} - \textbf{H}_{k}\hat{\textbf{x}}_{k|k-1})$ Edit

==

==


## $\textbf{K}_{k} = \hat{\textbf{P}}_{k|k-1}\textbf{H}_{k}^{T}(\textbf{H}_{k}\hat{\textbf{P}}_{k|k-1}\textbf{H}_{k}^{T} + \textbf{R}_{k})^{-1}$ Edit

==
If due to computational error the gain is inexact the following gives greater stability ==

## $\hat{\textbf{P}}_{k|k} = (I - \textbf{K}_{k} \textbf{H}_{k})\hat{\textbf{P}}_{k|k-1}(I - \textbf{K}_{k} \textbf{H}_{k})^{T} + \textbf{K}_{k} \textbf{R}_{k}\textbf{K}_{k}^{T}$ Edit

==

==


## $\hat{\textbf{P}_{k|k}} = \textbf{P}_{k} = \textbf{E}\{(\textbf{x}_{k} - \hat{\textbf{x}}_{k|k})(\textbf{x}_{k} - \hat{\textbf{x}}_{k|k})^{T}\}$ Edit

==
Similary the following also holds this case ==

## $\hat{\textbf{P}_{k|k-1}} = \textbf{E}\{(\textbf{x}_{k} - \hat{\textbf{x}}_{k|k-1})(\textbf{x}_{k} - \hat{\textbf{x}}_{k|k-1})^{T}\}$ Edit

==
In this case the Kalman filter is an optimal estimator in a least squares sense of the true state. ==

## $\textbf{x}_{k} = \begin{bmatrix} X, \dot{X} \end{bmatrix}^{T}$ Edit

==
where $\dot{X}$ ==

## is the velocity, that is, the derivative of position. Edit

== Between the (k − 1)th and kth timestep the particle undergoes an acceleration $\textbf{w}_{k}$ , ==

==
where ==

==
and ==

## $\textbf{G} = \begin{bmatrix} \begin{matrix} \frac{T^{2}}{2} \end{matrix} , T \end{bmatrix}^{T}$ Edit

==
follows logically from the Newtonian equations of motion. T is the time difference between the (k − 1)th and kth time step. (N.B. This process model requires that T is constant.) ==

==
where ==

==
and ==

## $\textbf{H} = \begin{bmatrix} 1, 0 \end{bmatrix}$ Edit

==
is the observation model. ==

## $\hat{\textbf{x}}_{k|k-1} = \textbf{F} \hat{\textbf{x}}_{k-1|k-1}$ Edit

==
and the predicted covariance is ==

## $\hat{\textbf{P}}_{k|k-1} = \textbf{F}\hat{\textbf{P}}_{k-1|k-1}\textbf{F}^{T} + \textbf{GQG}^{T}$ Edit

==
The predicted state and covariance are updated with the measurement and measurement (co)variance. ==

## $\tilde{\textbf{y}}_{k} = \textbf{z}_{k} - \textbf{H}\hat{\textbf{x}}_{k}$ Edit

==
is the difference between the actual and predicted measurements, while the innovation (residual) covariance ==

## $\textbf{S}_{k} = \textbf{H}\hat{\textbf{P}}_{k|k-1}\textbf{H}^{T} + \textbf{R}$ Edit

==
is the sum of the predicted covariance and the measurement covariance. The Kalman gain ==

## $\hat{\textbf{x}}_{k|k} = \hat{\textbf{x}}_{k|k-1} + \textbf{K}_{k} \tilde{\textbf{y}_{k}}$ Edit

==
is the predicted state plus the measurement innovation weighted by the Kalman gain. The updated estimated covariance is ==

==
where ==

## $\chi_{k} = I - \textbf{K}_{k} \textbf{H}.\,$ Edit

==
The Kalman gain will converge to a steady-state position if Q and R are time-invariant. The steady-state Kalman-gain can then be precomputed. This will reduce the Kalman-filter to an ordinary observer; which is computationally simpler. ==

## DerivationEdit

== The Kalman filter can be derived in several ways. The one presented here uses probability theory. The true state is assumed to be an unobserved Markov process, and the measurements are the observed states of a hidden Markov model. ==

## $p(\textbf{x}_k|\textbf{x}_0,...,\textbf{x}_{k-1}) = p(\textbf{x}_k|\textbf{x}_{k-1})$ Edit

==
Similarly the measurement a the k-th timestep is dependent only upon the current state and is independent of all other states. ==

## $p(\textbf{z}_k|\textbf{x}_0,...,\textbf{x}_{k}) = p(\textbf{z}_k|\textbf{x}_{k} )$ Edit

==
Using these assumptions the probability distribution over all states of the HMM can be written simply as: ==

## $p(\textbf{x}_0,...,\textbf{x}_k,\textbf{z}_1,...,\textbf{z}_k) = p(\textbf{x}_0)\prod_{i=1}^k p(\textbf{z}_i|\textbf{x}_i)p(\textbf{x}_i|\textbf{x}_{i-1})$ Edit

==
However, when the Kalman filter to estimate the state x the probability distribution of interest is that associated with the current states conditioned on the measurements upto the current timestep. (This is achieved by marginalising out the previous states and dividing by the probability of the measurement set.) ==

## $p(\textbf{x}_k|\textbf{Z}_{k-1}) = \int p(\textbf{x}_k | \textbf{x}_{k-1}) p(\textbf{x}_{k-1} | \textbf{Z}_{k-1} ) \, d\textbf{x}_{k-1}$ Edit

==
The measurement set upto time t is ==

## $\textbf{Z}_{t} = \left \{ \textbf{z}_{1},...,\textbf{z}_{t} \right \}$ Edit

==
The probability distribution of updated is proportional to the product of the measurement likelihood and the predicted state. ==

## $p(\textbf{x}_k|\textbf{Z}_{k}) = \frac{p(\textbf{z}_k|\textbf{x}_k) p(\textbf{x}_k|\textbf{Z}_{k-1})}{p(\textbf{z}_k|\textbf{Z}_{k-1})}$ Edit

==
The denominator ==

## $p(\textbf{x}_k | \textbf{x}_{k-1}) = N(\textbf{x}_k, \textbf{F}_k\textbf{x}_{k-1}, \textbf{Q}_k)$ Edit

==

==


## $p(\textbf{z}_k|\textbf{x}_k) = N(\textbf{z}_k,\textbf{H}_{k}\textbf{x}_k, \textbf{R}_k)$ Edit

==

==


## $p(\textbf{x}_{k-1}|\textbf{Z}_{k-1}) = N(\textbf{x}_{k-1},\hat{\textbf{x}}_{k-1},\textbf{P}_{k-1} )$ Edit

==
Note that the PDF at the previous timestep is inductively assumed 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. ==

## $\hat{\textbf{y}}_{k|k} \equiv \hat{\textbf{P}}_{k|k}^{-1}\hat{\textbf{x}}_{k|k}$ Edit

==
Similarly the predicted covariance and state have equivalent information forms, ==

## $\hat{\textbf{y}}_{k|k-1} \equiv \hat{\textbf{P}}_{k|k-1}^{-1}\hat{\textbf{x}}_{k|k-1}$ Edit

==
as have the measurement covariance and measurement vector. ==

## $\textbf{i}_{k} \equiv \textbf{H}_{k}^{T} \textbf{R}_{k}^{-1} \textbf{z}_{k}$ Edit

==
The information update now becomes a trivial sum. ==

## $\hat{\textbf{y}}_{k|k} = \hat{\textbf{y}}_{k|k-1} + \textbf{i}_{k}$ Edit

==
The main advantage of the information filter is that N measurements can be filtered at each timestep simply by summing their information matrices and vectors. ==

## $\hat{\textbf{y}}_{k|k} = \hat{\textbf{y}}_{k|k-1} + \sum_{j=1}^N \textbf{i}_{k,j}$ Edit

==
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. ==

## $\textbf{M}_{k} = [\textbf{F}_{k}^{-1}]^{T} \hat{\textbf{Y}}_{k|k} \textbf{F}_{k}^{-1}$ Edit

==

==


## $\hat{\textbf{Y}}_{k|k-1} = \textbf{L}_{k} \textbf{M}_{k} \textbf{L}_{k}^{T} + \textbf{C}_{k} \textbf{Q}_{k}^{-1} \textbf{C}_{k}^{T}$ Edit

==

==


## $\hat{\textbf{y}}_{k|k-1} = \textbf{L}_{k} [\textbf{F}_{k}^{-1}]^{T} \hat{\textbf{y}}_{k|k}$ Edit

==
Note that if F and Q are time invariant these values can be cached. Note also that F and Q need to be invertible. ==

## $\textbf{x}_{k} = f(\textbf{x}_{k-1}, \textbf{u}_{k}, \textbf{w}_{k})$ Edit

==

==


## $\textbf{z}_{k} = h(\textbf{x}_{k}, \textbf{v}_{k})$ Edit

==
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. ==

## $\textbf{F}_{k} = \left . \frac{\partial f}{\partial \textbf{x} } \right \vert _{\hat{\textbf{x}}_{k|k-1},\textbf{u}_{k}}$ Edit

==

==


## $\textbf{H}_{k} = \left . \frac{\partial h}{\partial \textbf{x} } \right \vert _{\hat{\textbf{x}}_{k|k-1}}$ Edit

==
At each timestep the Jacobian is evaluated with current predicted states. These matrices can be used in the Kalman filter equations. This process essentially linearises the non-linear function around the current estimate. ==

## $\hat{\textbf{x}}_{k|k-1} = f(\textbf{x}_{k-1}, \textbf{u}_{k}, 0)$ Edit

==

==


==
Update ==

## $\textbf{K}_{k} = \hat{\textbf{P}}_{k|k-1}\textbf{H}_{k}^{T}(\textbf{H}_{k}\hat{\textbf{P}}_{k|k-1}\textbf{H}_{k}^{T} + \textbf{R}_{k})^{-1}$ Edit

==

==


## $\hat{\textbf{x}}_{k|k} = \hat{\textbf{x}}_{k|k-1} + \textbf{K}_{k}(\textbf{z}_{k} - h(\textbf{x}_{k}, 0))$ Edit

==

==


## $\hat{\textbf{P}}_{k|k} = (I - \textbf{K}_{k} \textbf{H}_{k})\hat{\textbf{P}}_{k|k-1}$ Edit

==

==


## $\textbf{x}_{k-1|k-1}^{a} = [ \hat{\textbf{x}}_{k-1|k-1}^{T} \quad E[\textbf{w}_{k}^{T}] \ ]^{T}$ Edit

==

==


## $\textbf{P}_{k-1|k-1}^{a} = \begin{bmatrix} & \hat{\textbf{P}}_{k-1|k-1} & & 0 & \\ & 0 & &\textbf{Q}_{k} & \end{bmatrix}$ Edit

==
A a set of 2L+1 sigma points is derived from the augmented state and covariance where L is the dimension of the augmented state. ==

## $\chi_{k|k-1}^{i} = f(\chi_{k-1|k-1}^{i}) \quad i = 0..2L$ Edit

==
The weighted sigma points are recombined to produce the predicted state and covariance. ==

## $\hat{\textbf{x}}_{k|k-1} = \sum_{i=1}^N W_{s}^{i} \chi_{k|k-1}^{i}$ Edit

==

==


## $\hat{\textbf{P}}_{k|k-1} = \sum_{i=1}^N W_{c}^{i}\ [\chi_{k|k-1}^{i} - \hat{\textbf{x}}_{k|k-1}] [\chi_{k|k-1}^{i} - \hat{\textbf{x}}_{k|k-1}]^{T}$ Edit

==
Where the weights for the state and covariance are given are: ==

## $\lambda = \alpha^2 / (L+\kappa) - L \,\!$ Edit

==
Typical values for $\alpha$ , $\beta$ , and $\kappa$ ==

## $\textbf{x}_{k|k-1}^{a} = [ \hat{\textbf{x}}_{k|k-1}^{T} \quad E[\textbf{v}_{k}^{T}] \ ]^{T}$ Edit

==

==


## $\textbf{P}_{k|k-1}^{a} = \begin{bmatrix} & \hat{\textbf{P}}_{k|k-1} & & 0 & \\ & 0 & &\textbf{R}_{k} & \end{bmatrix}$ Edit

==
As before, a set of 2L+1 sigma points is derived from the augmented state and covariance where L is the dimension of the augmented state. ==

==
where ==

## $\textbf{R}_{k}^{a} = \begin{bmatrix} & 0 & & 0 & \\ & 0 & &\textbf{R}_{k} & \end{bmatrix}$ Edit

==
The sigma points are projected through the observation function h. ==

## $\gamma_{k}^{i} = h(\chi_{k|k-1}^{i}) \quad i = 0..2L$ Edit

==
The weighted sigma points are recombined to produce the predicted measurement and predicted measurement covariance. ==

## $\hat{\textbf{z}}_{k} = \sum_{i=1}^N W_{s}^{i} \gamma_{k}^{i}$ Edit

==

==


## $\textbf{P}_{z_{k}z_{k}} = \sum_{i=1}^N W_{c}^{i}\ [\gamma_{k}^{i} - \hat{\textbf{z}}_{k}] [\gamma_{k}^{i} - \hat{\textbf{z}}_{k}]^{T}$ Edit

==
The state-measurement cross-correlation matrix, ==

## $\textbf{P}_{x_{k}z_{k}} = \sum_{i=1}^N W_{c}^{i}\ [\chi_{k|k-1}^{i} - \hat{\textbf{x}}_{k|k-1}] [\gamma_{k}^{i} - \hat{\textbf{z}}_{k}]^{T}$ Edit

==
is used to compute the UKF Kalman gain. ==

## $K_{k} = \textbf{P}_{x_{k}z_{k}} \textbf{P}_{z_{k}z_{k}}^{-1}$ Edit

==
As with the Kalman filter, the updated state is the predicted state plus the innovation weighted by the Kalman gain, ==

## $\hat{\textbf{x}}_{k|k} = \hat{\textbf{x}}_{k|k-1} + K_{k}( \textbf{z}_{k} - \hat{\textbf{z}}_{k} )$ Edit

==
And the updated covariance is the predicted covariance, minus the predicted measurement covariance, weighted by the Kalman gain. ==

## $\hat{\textbf{P}}_{k|k} = \hat{\textbf{P}}_{k|k} - K_{k} \textbf{P}_{z_{k}z_{k}} K_{k}^{T}$ Edit

==

==