Fandom

Computer vision

Kalman filter

457pages on
this wiki
Add New Page
Talk0 Share

The Kalman filter (named after its inventor, Rudolf E. Kalman) is an efficient recursive computational solution for tracking a time-dependent state vector with noisy equations of motion in real time by the least-squares method. It is used to separate signal from noise so as to optimally predict changes in a modeled system with time. Kalman filtering is an important topic in control theory and control systems engineering. It is in a wide range of engineering applications from radar to computer vision. The filter was developed in papers by Swerling (1958), Kalman (1960) and Kalman and Bucy (1961). Edit

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

A wide variety of Kalman filters have now been developed, from Kalman's original formulation, now called the simple Kalman filter, to Schmidt's extended filter, the information filter, and a variety of square-root filters, developed by Bierman, Thornton and many others. Perhaps the most commonly used type of Kalman filter is the phase-locked loop now ubiquitous in radios, computers, and nearly any other type of video or communications equipment. Edit

Kalman filter basics Edit

The Kalman filter is used to estimate the state of a dynamic system from a series of noisy measurements. Edit

In order to accomplish this the Kalman filter employs 3 (sometimes 4) models. The first model is called the state transition model. This model describes how the state is expected to change from one timestep to the next. This is necessary because the system is dynamic, which means its underlying state is always subject to change. In reality this model is an approximation to the true process which is driving the dynamics of system. Because the state transition model is an approximation, a second model called the process noise model, is used. This model is used to mask the errors caused by the approximation. The third model is called the observation model. This model describes how the estimation space maps into the observation space. For example, an estimate of a target being tracked may include the target's velocity even though only the target's position is measured, or perhaps the target's position is measured in polar co-ordinates, but the estimated position in cartesian co-ordinates. The last model describes how the state is expected to change in response to a given control input (the control-input model). Often the latter is not used, for example in target tracking applications, as the control inputs are obviously unknown in this case. As with the state transition model, this is generally an approximation. Edit

The Kalman filter is a recursive estimator. This means that only estimated state from the previous time step and the current measurement are needed to compute the estimate for the current state. In contrast to batch estimation techniques, no history of observations and/or measurements are required. It has two phases: Predict and Update. The predict phase uses the estimate from the previous timestep to produce an estimate of the current state. In the update phase measurement information from the current timestep is used to refine this prediction to arrive at a new, more accurate (hopefully) estimate. Edit

The Kalman filter assumes a linear state space whose state transition is of the following form Edit

 \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{F} Edit

is applied to Edit

\textbf{x}_{k-1} Edit

the previous state. Edit

The process noise Edit

 \textbf{w}_{k} Edit

is the part of the state transition which is not modelled by the state transition model. The presence of process noise requires the use of a process noise model in the filter. The process noise at time k is assumed to be gaussian white noise with covariance Edit

\textbf{Q}_{k} \delta(k-j) = E[\textbf{w}_{k} \textbf{w}_{j}^{T}] Edit

. Edit

The control input, Edit

\textbf{u}_{k} Edit

is mapped into the estimation space by Edit

\textbf{B}_{k} Edit

the control-input model. Edit

The observation, assumed to be of the form Edit

\textbf{z}_{k} = \textbf{H}_{k} \textbf{x}_{k} + \textbf{v}_{k} Edit

is a function of the true state where Edit

\textbf{H}_{k} Edit

is the observation model and Edit

\textbf{v}_{k} Edit

is the observation noise at time k which is assumed to be gaussian white noise with covariance Edit

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

==

==

Kalman filter equations 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) ==

. Edit

Predict Edit

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

==

==

 \hat{\textbf{P}}_{k|k-1} =  \textbf{F}_{k} \hat{\textbf{P}}_{k-1|k-1} \textbf{F}_{k}^{T} + \textbf{Q}_{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

==

==

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

==
Scheme of the Kalman Filter ==

Numerically stable update Edit

The covariance update equation assumes the gain is computed exactly as 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

==

==

Optimality Edit

In the linear, gaussian case where the same linear models are used to drive the true and estimated states and where the noise parameters used in the filter are matched to the true noises perturbing the underlying state and observations, then the estimated covariance is a faithful representation of the true covariance of the state, that is, 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. ==

Example Edit

Tracking a 1D particle which is being perturbed with gaussian accelerations using a piece-wise constant process model. (Note: Time indices for F, G, R and Q have been dropped.) Edit

The position and velocity of a point particle is described by the linear state space Edit

\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} , ==

 N( \textbf{w}_{k}, 0, \textbf{Q} ) Edit

. Edit

The updated position and velocity Edit

 \textbf{x}_{k} = \textbf{F} \textbf{x}_{k-1} + \textbf{G w}_{k} Edit

==
where ==

 \textbf{F} = \begin{bmatrix} 1 & T \\ 0 & 1 \end{bmatrix} Edit

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

At each time step, a noisy measurement of the true position of the particle is made. Edit

\textbf{z}_{k} = \textbf{H x}_{k} + \textbf{v}_{k} Edit

==
where ==

 N( \textbf{v}_{k}, 0, \textbf{R} ) Edit

==
and ==

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

==
is the observation model. ==

Using these measurements an estimate of the state can be computed. Edit

The predicted state at the kth timestep using the estimated state from the (k − 1)th timestep is Edit

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

The measurement innovation (or residual) Edit

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

\textbf{K}_{k} = \hat{\textbf{P}}_{k|k-1}\textbf{H}^{T}\textbf{S}_{k}^{-1} Edit

is the ratio between the predicted covariance and the residual covariance. Edit

The updated state estimate Edit

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

 \hat{\textbf{P}}_{k|k} = \chi_{k}\hat{\textbf{P}}_{k|k-1}\chi_{k}^{T} + \textbf{K}_{k} \textbf{RK}_{k}^{T} Edit

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

Hidden Markov Model Edit

Because of the Markov assumption, the true state is conditionally dependant only on the previous state and is independent of all previous states. Edit

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

This leads to the predict and update steps of the Kalman filter written probabilistically. The probability distribution associated with the predicted state is product of the probability distribution associated with the transition from the (k - 1)th timestep to the kth and the probability distribution associated with the previous state, with the true state at (k - 1) integrated out. Edit

 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{z}_k|\textbf{Z}_{k-1}) = \int p(\textbf{z}_k|\textbf{x}_k) p(\textbf{x}_k|\textbf{Z}_{k-1}) d\textbf{x}_k Edit

is an unimportant normalisation term. Edit

The remaining probability density functions are PDF: Edit

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

Information filterEdit

In the Information filter, or Inverse Convariance filter, the estimated covariance and estimated state are replaced by the information matrix and information vector respectively. Edit

\hat{\textbf{Y}}_{k|k} \equiv  \hat{\textbf{P}}_{k|k}^{-1} Edit

\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} Edit

\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{H}_{k} Edit

\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

\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

\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

==

==

\textbf{C}_{k} = \textbf{M}_{k} [\textbf{M}_{k}+\textbf{Q}_{k}^{-1}]^{-1} Edit

\textbf{L}_{k} = I - \textbf{C}_{k} 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. ==

Non-linear filtersEdit

The basic Kalman filter is limited to a linear assumption. However most non-trivial systems are non-linear. The non-linearity can be associated either with the process model or with the observation model or with both. Edit

Extended Kalman filter Edit

In the Extended Kalman filter (EKF) the state transition and observation models need not be linear functions of the state but may instead be (differentiable) functions. Edit

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

This results in the following extended Kalman filter equations: Edit

Predict Edit

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

==

==

 \hat{\textbf{P}}_{k|k-1} =  \textbf{F}_{k} \hat{\textbf{P}}_{k-1|k-1} \textbf{F}_{k}^{T} + \textbf{Q}_{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} - 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

==

==

Unscented Kalman filter Edit

The Extended Kalman filter gives particularly poor performance on highly non-linear functions because only the mean is propagated through the non-linearity. The Unscented Kalman filter (UKF) [JU97] uses a deterministic sampling technique to pick a minimal set of sample points (called sigma points) around the mean. These sigma points are then propagated through the non-linear functions and the estimated covariance is then recovered. The result is a filter which more accurately captures the true mean and covariance. (This can be verified using Monte Carlo sampling.) In addition, this technique removes the requirement to calculate Jacobians, which for complex functions can be a difficult task in-itself. Edit

Predict Edit

As with the EFK, the UKF prediction can be used independently from the UKF update, in combination with a linear (or indeed EKF) update, or visa versa. Edit

The estimated state and covariance are augmented with the mean and covariance of the process noise. 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-1|k-1}^{0} Edit

= \textbf{x}_{k-1|k-1}^{a} Edit

Edit

\chi_{k-1|k-1}^{i} Edit

=\textbf{x}_{k-1|k-1}^{a} + \left ( \sqrt{ (L + \lambda) \textbf{P}_{k-1|k-1}^{a} } \right )_{i} Edit

i = 1..L \,\! Edit

\chi_{k-1|k-1}^{i} Edit

= \textbf{x}_{k-1|k-1}^{a} - \left ( \sqrt{ (L + \lambda) \textbf{P}_{k-1|k-1}^{a} } \right )_{i-L} Edit

i = L+1..2L \,\! Edit

The sigma points are propagated through the transition function f. Edit

\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: ==

W_{s}^{0} = \frac{\lambda}{L+\lambda} Edit

W_{c}^{0} = \frac{\lambda}{L+\lambda} + (1 - \alpha^2 + \beta) Edit

W_{s}^{i} = W_{c}^{i} = \frac{1}{2(L+\lambda)} Edit

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

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

are 10^{-3} Edit

, 2 and 0 respectively. (These values should suffice for most purposes.) Edit

Update Edit

The predicted state and covariance are augmented as before, except now with the mean and covariance of the measurement noise. Edit

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

\chi_{k|k-1}^{0} Edit

= \textbf{x}_{k|k-1}^{a} Edit

Edit

\chi_{k|k-1}^{i} Edit

=\textbf{x}_{k|k-1}^{a} + \left ( \sqrt{ (L + \lambda) \textbf{P}_{k|k-1}^{a} } \right )_{i} Edit

i = 1..L \,\! Edit

\chi_{k|k-1}^{i} Edit

= \textbf{x}_{k|k-1}^{a} - \left ( \sqrt{ (L + \lambda) \textbf{P}_{k|k-1}^{a} } \right )_{i-L} Edit

i = L+1..2L \,\! Edit

Alternatively if the UKF prediction has been used the sigma points themselves can be augmented along the following lines Edit

 \chi_{k|k-1} := [ \chi_{k|k-1} \quad E[\textbf{v}_{k}^{T}] \ ]^{T} \pm \sqrt{ (L + \lambda) \textbf{R}_{k}^{a} } Edit

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

==

==

ApplicationsEdit

Inertial guidance system Edit

Autopilot Edit

Satellite navigation systems Edit

Simultaneous localization and mapping Edit

ReferencesEdit

Kalman, R. E. A New Approach to Linear Filtering and Prediction Problems, Transactions of the ASME - Journal of Basic Engineering Vol. 82: pp. 35-45 (1960) Edit

Kalman, R. E., Bucy R. S., New Results in Linear Filtering and Prediction Theory, Transactions of the ASME - Journal of Basic Engineering Vol. 83: pp. 95-107 (1961) Edit

[JU97] Julier, Simon J. and Jeffery K. Uhlmann. A New Extension of the Kalman Filter to nonlinear Systems. In The Proceedings of AeroSense: The 11th International Symposium on Aerospace/Defense Sensing,Simulation and Controls, Multi Sensor Fusion, Tracking and Resource Management II, SPIE, 1997. Edit

See also Edit

== Compare with: Wiener filter, and the multimodal Particle filter estimator. ==

External linksEdit

Kalman Filters, thorough introduction to several types, together with applications to Robot Localization Edit

The Kalman Filter Edit

Kalman Filtering Edit

Kalman filters Edit

Fast Kalman Filter (FKF) - the invention of Dr. Antti Lange Edit

Ad blocker interference detected!


Wikia is a free-to-use site that makes money from advertising. We have a modified experience for viewers using ad blockers

Wikia is not accessible if you’ve made further modifications. Remove the custom ad blocker rule(s) and the page will load as expected.