Linear Kalman Filters

Kalman filters track an object using a sequence of detections or measurements to estimate the state of the object based on the motion model of the object. In a motion model, state is a collection of quantities that represent the status of an object, such as its position, velocity, and acceleration. An object motion model is defined by the evolution of the object state.

The linear Kalman filter (trackingKF) is an optimal, recursive algorithm for estimating the state of an object if the estimation system is linear and Gaussian. An estimation system is linear if both the motion model and measurement model are linear. The filter works by recursively predicting the object state using the motion model and correcting the state using measurements.

Motion Model

For most types of objects tracked in the toolbox, the state vector consists of one-, two-, or three-dimensional positions and velocities.

Consider an object moving in the x-direction at a constant acceleration. You can write the equation of motion, using Newtonian equations, as:

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

Furthermore, if you define the state as:

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

you can write the equation of motion in state-space form as:

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

In most cases, a motion model does not fully model the motion of an object, and you need to include the process noise to compensate the uncertainty in the motion model. For the constant velocity model, you can add process noise as an acceleration 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}$

Here, v_k is the unknown noise perturbation of the acceleration. For the filter to be optimal, you must assume the process noise is zero-mean, white Gaussian 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 in this equation is the state transition matrix. For independent x- and y-motions, this matrix is block diagonal.

When you convert a continuous time model to a discrete time model, you integrate the equations of motion over the length of the time interval. In the 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} \frac{1}{2} T^{2} \\ T \end{matrix}] a + [\begin{matrix} \frac{1}{2} T^{2} \\ T \end{matrix}] \tilde{v}$

where 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. For details on how to obtain the discretized process noise from a continuous system, see [1].

You can generalize the state equation to:

$x_{k + 1} = A_{k} x_{k} + B_{k} u_{k} + G_{k} v_{k}$

where A_k is the state transition matrix and B_k is the control matrix. The control matrix accounts for any known forces acting on the object. v_k represents discretized process noise, following a Gaussian distribution of mean 0 and covariance Q_k. G_k is the process noise gain matrix.

Measurement Models

Measurements are what you observe or measure in a system. Measurements depend on the state vector, but are usually not 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. A linear Kalman filter assumes the measurements are a linear function of the state vector. To apply nonlinear measurement models, you can choose to use an extended Kalman filter (trackingEKF) or an unscented Kalman filter (trackingUKF).

You can represent a linear measurement as:

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

Here, H_k is the measurement matrix and w_k represents measurement noise at the current time step. For an optimal filter, the measurement noise must be zero-mean, Gaussian white noise. Assume the covariance matrix of the measurement noise is R_k.

Filter Loop

The filter starts with best estimates of the state x_0|0 and the state covariance P_0|0. The filter performs these steps in a recursive loop.

Propagate the state to the next step using the motion equation:

$x_{k + 1 | k} = A_{k} x_{k | k} + B_{k} u_{k} .$
Propagate the covariance matrix as well:

$P_{k + 1 | k} = A_{k} P_{k | k} A_{k}^{T} + G_{k} Q_{k} G_{k}^{T} .$
The subscript notation k+1|k indicates that the corresponding quantity is the estimate at the k+1 step propagated from step k. This estimate is often called the a priori estimate. The predicted measurement at the k+1 step is

$z_{k + 1 | k} = H_{k + 1} x_{k + 1 | k}$
Use the difference between the actual measurement and the predicted measurement to correct the state at the k+1 step. To correct the state, the filter must compute the Kalman gain. First, the filter computes the measurement prediction covariance (innovation) as:

$S_{k + 1} = H_{k + 1} P_{k + 1 | k} H_{k + 1}^{T} + R_{k + 1}$
Then, the filter computes the Kalman gain as:

$K_{k + 1} = P_{k + 1 | k} H_{k + 1}^{T} S_{k + 1}^{- 1}$
The filter corrects the predicted estimate by using the measurement. The estimate, after correction using the measurement z_k+1, is

$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 including the measurement.
The filter corrects the state covariance matrix as:

$P_{k + 1 | k + 1} = P_{k + 1 | k} - K_{k + 1} S_{k + 1} K_{k + 1}^{T}$

This figure summarizes the Kalman loop operations. Once initialized, a Kalman filter loops between prediction and correction until reaching the end of the simulation.

Built-In Motion Models in `trackingKF`

When you only need to use the standard 1-D, 2-D, or 3-D constant velocity or constant acceleration motion models, you can specify the MotionModel property of trackingKF as one of these:

"1D Constant Velocity"
"1D Constant Acceleration"
"2D Constant Velocity"
"2D Constant Acceleration"
"3D Constant Velocity"
"3D Constant Acceleration"

To customize your own motion model, specify the MotionModel property as "Custom", and then specify the state transition matrix in the StateTransitionModel property of the filter.

For the 3-D constant velocity model, the state equation 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}]$

For the 3-D constant acceleration model, the state equation 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}]$

Example: Estimate 2-D Target States Using `trackingKF`

Open Live Script

Initialize Estimation Model

Specify an initial position and velocity for a target that you assume moving in 2-D. The simulation lasts 20 seconds with a sample time of 0.2 seconds.

rng(2021);    % For repeatable results
dt = 0.2;     % seconds
simTime = 20; % seconds
tspan = 0:dt:simTime;
trueInitialState = [30; 2; 40; 2]; % [x;vx;y;vy]
processNoise = diag([0; 1; 0; 1]); % Process noise matrix

Create a measurement noise covariance matrix, assuming that the target measurements consist of its position states.

measureNoise = diag([4 4]); % Measurement noise matrix

The matrix specifies a standard deviation of 2 meters in both the x- and y-directions.

Preallocate variables in which to save estimation results.

numSteps = length(tspan);
trueStates = NaN(4,numSteps);
trueStates(:,1) = trueInitialState;
estimateStates = NaN(size(trueStates));

Obtain True States and Measurements

Propagate the constant velocity model, and generate the measurements with noise.

F = [1 dt 0  0;
     0  1 0  0; 
     0  0 1 dt;
     0  0 0  1];
H = [1 0 0 0;
     0 0 1 0];
for i = 2:length(tspan)
    trueStates(:,i) = F*trueStates(:,i-1) + sqrt(processNoise)*randn(4,1);  
end
measurements = H*trueStates + sqrt(measureNoise)*randn(2,numSteps);

Plot the true trajectory and the measurements.

figure
plot(trueStates(1,1),trueStates(3,1),"r*",DisplayName="True Initial")
hold on
plot(trueStates(1,:),trueStates(3,:),"r",DisplayName="Truth")
plot(measurements(1,:),measurements(2,:),"kx",DisplayName="Measurements")
xlabel("x (m)")
ylabel("y (m)")
axis image

Figure contains an axes object. The axes object with xlabel x (m), ylabel y (m) contains 3 objects of type line. One or more of the lines displays its values using only markers These objects represent True Initial, Truth, Measurements.

Initialize Linear Kalman Filter

Initialize the filter with a state of [40; 0; 160; 0], which is far from the true initial state. Normally, you can use the initial measurement to construct an initial state as [measurements(1,1);0 ; measurements(2,1); 0]. Here, you use an erroneous initial state, which enables you to test if the filter can quickly converge on the truth.

filter = trackingKF(MotionModel="2D Constant Velocity",State=[40; 0; 160;0], ...
    MeasurementModel=H,MeasurementNoise=measureNoise)

filter = 
  trackingKF with properties:

               State: [4x1 double]
     StateCovariance: [4x4 double]

         MotionModel: '2D Constant Velocity'
        ProcessNoise: [2x2 double]

    MeasurementModel: [2x4 double]
    MeasurementNoise: [2x2 double]

     MaxNumOOSMSteps: 0

     EnableSmoothing: 0

 estimateStates(:,1) = filter.State;

Run Linear Kalman Filter and Show Results

Run the filter by recursively calling the predict and correct object functions. From the results, the estimates converge on the truth quickly. In fact, the linear Kalman filter has an exponential convergence speed.

for i=2:length(tspan)
    predict(filter,dt)
    estimateStates(:,i) = correct(filter,measurements(:,i));
end
plot(estimateStates(1,1),estimateStates(3,1),"g*",DisplayName="Initial Estimates")
plot(estimateStates(1,:),estimateStates(3,:),"g",DisplayName="Estimates")
legend(Location="southeast")

Figure contains an axes object. The axes object with xlabel x (m), ylabel y (m) contains 5 objects of type line. One or more of the lines displays its values using only markers These objects represent True Initial, Truth, Measurements, Initial Estimates, Estimates.

References

[1] Li, X. Rong, and Vesselin P. Jilkov. "Survey of Maneuvering Target Tracking: Dynamic Models". Edited by Oliver E. Drummond, 2000, pp. 212–35. DOI.org (Crossref), https://doi.org/10.1117/12.391979.