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 Automated Driving 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}* 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]$$