Linear Kalman Filters

When you use a Kalman filter to track objects, you use a sequence of detections or measurements to construct a model of the object motion. Object motion is defined by the evolution of the state of the object. The Kalman filter is an optimal, recursive algorithm for estimating the track of an object. The filter is recursive because it updates the current state using the previous state, using measurements that may have been made in the interval. A Kalman filter incorporates these new measurements to keep the state estimate as accurate as possible. The filter is optimal because it minimizes the mean-square error of the state. You can use the filter to predict future states or estimate the current state or past state.

State Equations

For most types of objects tracked in Sensor Fusion and Tracking Toolbox™, the state vector consists of one-, two- or three-dimensional positions and velocities.

Start with Newton equations for an object moving in the x-direction at constant acceleration and convert these equations to space-state form.

$\begin{array}{l} m \ddot{x} = f \\ \ddot{x} = \frac{f}{m} = a \end{array}$

If you define the state as

$\begin{array}{l} x_{1} = x \\ x_{2} = \dot{x}, \end{array}$

you can write Newton’s law in state-space form.

$\frac{d}{d t} [\begin{matrix} x_{1} \\ x_{2} \end{matrix}] = [\begin{matrix} 0 & 1 \\ 0 & 0 \end{matrix}] [\begin{matrix} x_{1} \\ x_{2} \end{matrix}] + [\begin{matrix} 0 \\ 1 \end{matrix}] a$

You use a linear dynamic model when you have confidence that the object follows this type of motion. Sometimes the model includes process noise to reflect uncertainty in the motion model. In this case, Newton’s equations have an additional term.

$\frac{d}{d t} [\begin{matrix} x_{1} \\ x_{2} \end{matrix}] = [\begin{matrix} 0 & 1 \\ 0 & 0 \end{matrix}] [\begin{matrix} x_{1} \\ x_{2} \end{matrix}] + [\begin{matrix} 0 \\ 1 \end{matrix}] a + [\begin{matrix} 0 \\ 1 \end{matrix}] v_{k}$

v_k is the unknown noise perturbations of the acceleration. Only the statistics of the noise are known. It is assumed to be zero-mean Gaussian white noise.

You can extend this type of equation to more than one dimension. In two dimensions, the equation has the form

$\frac{d}{d t} [\begin{matrix} x_{1} \\ x_{2} \\ y_{1} \\ y_{2} \end{matrix}] = [\begin{matrix} 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 \end{matrix}] [\begin{matrix} x_{1} \\ x_{2} \\ y_{1} \\ y_{2} \end{matrix}] + [\begin{matrix} 0 \\ a_{x} \\ 0 \\ a_{y} \end{matrix}] + [\begin{matrix} 0 \\ v_{x} \\ 0 \\ v_{y} \end{matrix}]$

The 4-by-4 matrix on the right side is the state transition model matrix. For independent x- and y- motions, this matrix is block diagonal.

When you transition to discrete time, you integrate the equations of motion over the length of the time interval. In discrete form, for a sample interval of T, the state-representation becomes

$[\begin{matrix} x_{1, k + 1} \\ x_{2, k + 1} \end{matrix}] = [\begin{matrix} 1 & T \\ 0 & 1 \end{matrix}] [\begin{matrix} x_{1, k} \\ x_{2, k} \end{matrix}] + [\begin{matrix} 0 \\ T \end{matrix}] a + [\begin{matrix} 0 \\ 1 \end{matrix}] \tilde{v}$

The quantity x_k+1 is the state at discrete time k+1, and x_k is the state at the earlier discrete time, k. If you include noise, the equation becomes more complicated, because the integration of noise is not straightforward.

The state equation can be generalized to

$x_{k + 1} = F_{k} x_{k} + G_{k} u_{k} + v_{k}$

F_k is the state transition matrix and G_k is the control matrix. The control matrix takes into account any known forces acting on the object. Both of these matrices are given. The last term represents noise-like random perturbations of the dynamic model. The noise is assumed to be zero-mean Gaussian white noise.

Continuous-time systems with input noise are described by linear stochastic differential equations. Discrete-time systems with input noise are described by linear stochastic differential equations. A state-space representation is a mathematical model of a physical system where the inputs, outputs, and state variables are related by first-order coupled equations.

Measurement Models

Measurements are what you observe about your system. Measurements depend on the state vector but are not always the same as the state vector. For instance, in a radar system, the measurements can be spherical coordinates such as range, azimuth, and elevation, while the state vector is the Cartesian position and velocity. For the linear Kalman filter, the measurements are always linear functions of the state vector, ruling out spherical coordinates. To use spherical coordinates, use the extended Kalman filter.

The measurement model assumes that the actual measurement at any time is related to the current state by

$z_{k} = H_{k} x_{k} + w_{k}$

w_k represents measurement noise at the current time step. The measurement noise is also zero-mean white Gaussian noise with covariance matrix Q described by Q_k = E[n_kn_k^T].

Linear Kalman Filter Equations

Without noise, the dynamic equations are

$x_{k + 1} = F_{k} x_{k} + G_{k} u_{k} .$

Likewise, the measurement model has no measurement noise contribution. At each instance, the process and measurement noises are not known. Only the noise statistics are known. The

$z_{k} = H_{k} x_{k}$

You can put these equations into a recursive loop to estimate how the state evolves and also how the uncertainties in the state components evolve.

Filter Loop

Start with a best estimate of the state, x_0/0, and the state covariance, P_0/0. The filter performs these steps in a continual loop.

Propagate the state to the next step using the motion equations.

$x_{k + 1 | k} = F_{k} x_{k | k} + G_{k} u_{k} .$
Propagate the covariance matrix as well.

$P_{k + 1 | k} = F_{k} P_{k | k} F_{k}^{T} + Q_{k} .$
The subscript notation k+1|k indicates that the quantity is the optimum estimate at the k+1 step propagated from step k. This estimate is often called the a priori estimate.
Then predict the measurement at the updated time.

$z_{k + 1 | k} = H_{k + 1} x_{k + 1 | k}$
Use the difference between the actual measurement and predicted measurement to correct the state at the updated time. The correction requires computing the Kalman gain. To do this, first compute the measurement prediction covariance (innovation)

$S_{k + 1} = H_{k + 1} P_{k + 1 | k} H_{k + 1}^{T} + R_{k + 1}$
Then the Kalman gain is

$K_{k + 1} = P_{k + 1 | k} H_{k + 1}^{T} S_{k + 1}^{- 1}$
and is derived from using an optimality condition.
Correct the predicted estimate with the measurement. Assume that the estimate is a linear combination of the predicted state and the measurement. The estimate after correction uses the subscript notation, k+1|k+1. is computed from

$x_{k + 1 | k + 1} = x_{k + 1 | k} + K_{k + 1} (z_{k + 1} - z_{k + 1 | k})$
where K_k+1 is the Kalman gain. The corrected state is often called the a posteriori estimate of the state because it is derived after the measurement is included.
Correct the state covariance matrix

$P_{k + 1 | k + 1} = P_{k + 1 | k} - K_{k + 1} S_{k + 1} {K^{'}}_{k + 1}$
Finally, you can compute a measurement based upon the corrected state. This is not a correction to the measurement but is a best estimate of what the measurement would be based upon the best estimate of the state. Comparing this to the actual measurement gives you an indication of the performance of the filter.

This figure summarizes the Kalman loop operations.

Constant Velocity Model

The linear Kalman filter contains a built-in linear constant-velocity motion model. Alternatively, you can specify the transition matrix for linear motion. The state update at the next time step is a linear function of the state at the present time. In this filter, the measurements are also linear functions of the state described by a measurement matrix. For an object moving in 3-D space, the state is described by position and velocity in the x-, y-, and z-coordinates. The state transition model for the constant-velocity motion is

$[\begin{matrix} x_{k + 1} \\ v_{x, k + 1} \\ y_{k + 1} \\ v_{y, k + 1} \\ z_{k + 1} \\ v_{z, k + 1} \end{matrix}] = [\begin{matrix} 1 & T & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & T & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & T \\ 0 & 0 & 0 & 0 & 0 & 1 \end{matrix}] [\begin{matrix} x_{k} \\ v_{x, k} \\ y_{k} \\ v_{y, k} \\ z_{k} \\ v_{z, k} \end{matrix}]$

The measurement model is a linear function of the state vector. The simplest case is one where the measurements are the position components of the state.

$[\begin{matrix} m_{x, k} \\ m_{y, k} \\ m_{z, k} \end{matrix}] = [\begin{matrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & 0 \end{matrix}] [\begin{matrix} x_{k} \\ v_{x, k} \\ y_{k} \\ v_{y, k} \\ z_{k} \\ v_{z, k} \end{matrix}]$

Constant Acceleration Model

The linear Kalman filter contains a built-in linear constant-acceleration motion model. Alternatively, you can specify the transition matrix for constant-acceleration linear motion. The transition model for linear acceleration is

$[\begin{matrix} x_{k + 1} \\ v_{x, k + 1} \\ a_{x, k + 1} \\ y_{k + 1} \\ v_{y, k + 1} \\ a_{y, k + 1} \\ z_{k + 1} \\ v_{z, k + 1} \\ a_{z, k + 1} \end{matrix}] = [\begin{matrix} 1 & T & \frac{1}{2} T^{2} & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & T & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & T & \frac{1}{2} T^{2} & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 1 & T & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & T & \frac{1}{2} T^{2} \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & T \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 \end{matrix}] [\begin{matrix} x_{k} \\ v_{x, k} \\ a_{x, k} \\ y_{k} \\ v_{y, k} \\ a_{y, k} \\ z_{k} \\ v_{z, k} \\ a_{z, k} \end{matrix}]$

The simplest case is one where the measurements are the position components of the state.

$[\begin{matrix} m_{x, k} \\ m_{y, k} \\ m_{z, k} \end{matrix}] = [\begin{matrix} 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \end{matrix}] [\begin{matrix} x_{k} \\ v_{x, k} \\ a_{x, k} \\ y_{k} \\ v_{y, k} \\ a_{y, k} \\ z_{k} \\ v_{z, k} \\ a_{y, k} \end{matrix}]$

Documentation