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.

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}{dt}\left[\begin{array}{c}{x}_{1}\\ {x}_{2}\end{array}\right]=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]\left[\begin{array}{c}{x}_{1}\\ {x}_{2}\end{array}\right]+\left[\begin{array}{c}0\\ 1\end{array}\right]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}{dt}\left[\begin{array}{c}{x}_{1}\\ {x}_{2}\end{array}\right]=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]\left[\begin{array}{c}{x}_{1}\\ {x}_{2}\end{array}\right]+\left[\begin{array}{c}0\\ 1\end{array}\right]a+\left[\begin{array}{c}0\\ 1\end{array}\right]{v}_{k}$$

*v _{k}* and
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}{dt}\left[\begin{array}{c}{x}_{1}\\ {x}_{2}\\ {y}_{1}\\ {y}_{2}\end{array}\right]=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 1\\ 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}{x}_{1}\\ {x}_{2}\\ {y}_{1}\\ {y}_{2}\end{array}\right]+\left[\begin{array}{c}0\\ {a}_{x}\\ 0\\ {a}_{y}\end{array}\right]+\left[\begin{array}{c}0\\ {v}_{x}\\ 0\\ {v}_{y}\end{array}\right]$$

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

$$\left[\begin{array}{c}{x}_{1,k+1}\\ {x}_{2,k+1}\end{array}\right]=\left[\begin{array}{cc}1& T\\ 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{1,k}\\ {x}_{2,k}\end{array}\right]+\left[\begin{array}{c}0\\ T\end{array}\right]a+\left[\begin{array}{c}0\\ 1\end{array}\right]\tilde{v}$$

The quantity *x _{k+1}* is
the state at discrete time

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

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.

*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

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.

Start with a best estimate of the state, *x _{0/0}*, and
the state covariance,

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*is the Kalman gain. The corrected state is often called the_{k+1}*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}^{\prime}}_{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.

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

$$\left[\begin{array}{c}{x}_{k+1}\\ {v}_{x,k+1}\\ {y}_{k+1}\\ {v}_{y,k+1}\\ {z}_{k+1}\\ {v}_{z,k+1}\end{array}\right]=\left[\begin{array}{cccccc}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{array}\right]\left[\begin{array}{c}{x}_{k}\\ {v}_{x,k}\\ {y}_{k}\\ {v}_{y,k}\\ {z}_{k}\\ {v}_{z,k}\end{array}\right]$$

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.

$$\left[\begin{array}{c}{m}_{x,k}\\ {m}_{y,k}\\ {m}_{z,k}\end{array}\right]=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 1& 0\end{array}\right]\left[\begin{array}{c}{x}_{k}\\ {v}_{x,k}\\ {y}_{k}\\ {v}_{y,k}\\ {z}_{k}\\ {v}_{z,k}\end{array}\right]$$

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

$$\left[\begin{array}{c}{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{array}\right]=\left[\begin{array}{ccccccccc}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{array}\right]\left[\begin{array}{c}{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{array}\right]$$

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

$$\left[\begin{array}{c}{m}_{x,k}\\ {m}_{y,k}\\ {m}_{z,k}\end{array}\right]=\left[\begin{array}{ccccccccc}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{array}\right]\left[\begin{array}{c}{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{array}\right]$$