trackingKF

Linear Kalman filter for object tracking

Description

A trackingKF object is a discrete-time linear Kalman filter used to track the positions and velocities of objects that can be encountered in an automated driving scenario. Such objects include automobiles, pedestrians, bicycles, and stationary structures or obstacles. A Kalman filter is a recursive algorithm for estimating the evolving state of a process when measurements are made on the process. The filter is linear when the evolution of the state follows a linear motion model and the measurements are linear functions of the state. The filter assumes that both the process and measurements have additive noise. When the process noise and measurement noise are Gaussian, the Kalman filter is the optimal minimum mean squared error (MMSE) state estimator for linear processes.

You can use this object in these ways:

  • Explicitly set the motion model. Set the motion model property, MotionModel, to Custom, and then use the StateTransitionModel property to set the state transition matrix.

  • Set the MotionModel property to a predefined state transition model:

    Motion Model
    '1D Constant Velocity'
    '1D Constant Acceleration'
    '2D Constant Velocity'
    '2D Constant Acceleration'
    '3D Constant Velocity'
    '3D Constant Acceleration'

Creation

Description

filter = trackingKF creates a linear Kalman filter object for a discrete-time, 2-D, constant-velocity moving object. The Kalman filter uses default values for the StateTransitionModel, MeasurementModel, and ControlModel properties. The function also sets the MotionModel property to '2D Constant Velocity'.

filter = trackingKF(F,H) specifies the state transition model, F, and the measurement model, H. With this syntax, the function also sets the MotionModel property to 'Custom'.

filter = trackingKF(F,H,G) also specifies the control model, G. With this syntax, the function also sets the MotionModel property to 'Custom'.

filter = trackingKF('MotionModel',model) sets the motion model property, MotionModel, to model.

example

filter = trackingKF(___,Name,Value) configures the properties of the Kalman filter by using one or more Name,Value pair arguments and any of the previous syntaxes. Any unspecified properties take default values.

Properties

expand all

Kalman filter state, specified as a real-valued M-element vector. M is the size of the state vector. Typical state vector sizes are described in the MotionModel property. When the initial state is specified as a scalar, the state is expanded into an M-element vector.

You can set the state to a scalar in these cases:

  • When the MotionModel property is set to 'Custom', M is determined by the size of the state transition model.

  • When the MotionModel property is set to '2D Constant Velocity', '3D Constant Velocity', '2D Constant Acceleration', or '3D Constant Acceleration', you must first specify the state as an M-element vector. You can use a scalar for all subsequent specifications of the state vector.

Example: [200;0.2;-40;-0.01]

Data Types: double

State error covariance, specified as a positive scalar or a positive-definite real-valued M-by-M matrix, where M is the size of the state. Specifying the value as a scalar creates a multiple of the M-by-M identity matrix. This matrix represents the uncertainty in the state.

Example: [20 0.1; 0.1 1]

Data Types: double

Kalman filter motion model, specified as 'Custom' or one of these predefined models. In this case, the state vector and state transition matrix take the form specified in the table.

Motion ModelForm of State VectorForm of State Transition Model
'1D Constant Velocity'

[x;vx]

[1 dt; 0 1]

'2D Constant Velocity'

[x;vx;y;vy]

Block diagonal matrix with the [1 dt; 0 1] block repeated for the x and y spatial dimensions

'3D Constant Velocity'

[x;vx;y;vy;z;vz]

Block diagonal matrix with the [1 dt; 0 1] block repeated for the x, y, and z spatial dimensions.

'1D Constant Acceleration'

[x;vx;ax]

[1 dt 0.5*dt^2; 0 1 dt; 0 0 1]

'2D Constant Acceleration'

[x;vx;ax;y;vy;ay]

Block diagonal matrix with [1 dt 0.5*dt^2; 0 1 dt; 0 0 1] blocks repeated for the x and y spatial dimensions

'3D Constant Acceleration'

[x;vx,ax;y;vy;ay;z;vz;az]

Block diagonal matrix with the [1 dt 0.5*dt^2; 0 1 dt; 0 0 1] block repeated for the x, y, and z spatial dimensions

When the ControlModel property is defined, every nonzero element of the state transition model is replaced by dt.

When MotionModel is 'Custom', you must specify a state transition model matrix, a measurement model matrix, and optionally, a control model matrix as input arguments to the Kalman filter.

Data Types: char

State transition model between time steps, specified as a real-valued M-by-M matrix. M is the size of the state vector. In the absence of controls and noise, the state transition model relates the state at any time step to the state at the previous step. The state transition model is a function of the filter time step size.

Example: [1 0; 1 2]

Dependencies

To enable this property, set MotionModel to 'Custom'.

Data Types: double

Control model, specified as an M-by-L matrix. M is the dimension of the state vector and L is the number of controls or forces. The control model adds the effect of controls on the evolution of the state.

Example: [.01 0.2]

Data Types: double

Covariance of process noise, specified as a positive scalar or an M-by-M matrix where M is the dimension of the state. If you specify this property as a scalar, the filter uses the value as a multiplier of the M-by-M identity matrix. Process noise expresses the uncertainty in the dynamic model and is assumed to be zero-mean Gaussian white noise.

Example: [1.0 0.05; 0.05 2]

Data Types: double

Measurement model from the state vector, specified as a real-valued N-by-M matrix, where N is the size of the measurement vector and M is the size of the state vector. The measurement model is a linear matrix that determines predicted measurements from the predicted state.

Example: [1 0.5 0.01; 1.0 1 0]

Data Types: double

Covariance of the measurement noise, specified as a positive scalar or a positive-definite, real-valued N-by-N matrix, where N is the size of the measurement vector. If you specify this property as a scalar, the filter uses the value as a multiplier of the N-by-N identity matrix. Measurement noise represents the uncertainty of the measurement and is assumed to be zero-mean Gaussian white noise.

Example: 0.2

Data Types: double

Object Functions

predictPredict state and state estimation error covariance of linear Kalman filter
correctCorrect state and state estimation error covariance using tracking filter
correctjpdaCorrect state and state estimation error covariance using tracking filter and JPDA
distanceDistances between current and predicted measurements of tracking filter
likelihoodLikelihood of measurement from tracking filter
cloneCreate duplicate tracking filter
residualMeasurement residual and residual noise from tracking filter
initializeInitialize state and covariance of tracking filter

Examples

collapse all

Create a linear Kalman filter that uses a 2D Constant Velocity motion model. Assume that the measurement consists of the object's x-y location.

Specify the initial state estimate to have zero velocity.

x = 5.3;
y = 3.6;
initialState = [x;0;y;0];
KF = trackingKF('MotionModel','2D Constant Velocity','State',initialState);

Create the measured positions from a constant-velocity trajectory.

vx = 0.2;
vy = 0.1;
T  = 0.5;
pos = [0:vx*T:2;5:vy*T:6]';

Predict and correct the state of the object.

for k = 1:size(pos,1)
    pstates(k,:) = predict(KF,T);
    cstates(k,:) = correct(KF,pos(k,:));
end

Plot the tracks.

plot(pos(:,1),pos(:,2),'k.', pstates(:,1),pstates(:,3),'+', ...
    cstates(:,1),cstates(:,3),'o')
xlabel('x [m]')
ylabel('y [m]')
grid
xt  = [x-2 pos(1,1)+0.1 pos(end,1)+0.1];
yt = [y pos(1,2) pos(end,2)];
text(xt,yt,{'First measurement','First position','Last position'})
legend('Object position', 'Predicted position', 'Corrected position')

More About

expand all

Algorithms

The Kalman filter describes the motion of an object by estimating its state. The state generally consists of object position and velocity and possibly its acceleration. The state can span one, two, or three spatial dimensions. Most frequently, you use the Kalman filter to model constant-velocity or constant-acceleration motion. A linear Kalman filter assumes that the process obeys the following linear stochastic difference equation:

xk+1=Fkxk+Gkuk+vk

xk is the state at step k. Fk is the state transition model matrix. Gk is the control model matrix. uk represents known generalized controls acting on the object. In addition to the specified equations of motion, the motion may be affected by random noise perturbations, vk. The state, the state transition matrix, and the controls together provide enough information to determine the future motion of the object in the absence of noise.

In the Kalman filter, the measurements are also linear functions of the state,

zk=Hkxk+wk

where Hk is the measurement model matrix. This model expresses the measurements as functions of the state. A measurement can consist of an object position, position and velocity, or its position, velocity, and acceleration, or some function of these quantities. The measurements can also include noise perturbations, wk.

These equations, in the absence of noise, model the actual motion of the object and the actual measurements. The noise contributions at each step are unknown and cannot be modeled. Only the noise covariance matrices are known. The state covariance matrix is updated with knowledge of the noise covariance only.

For a brief description of the linear Kalman filter algorithm, see Linear Kalman Filters .

References

[1] Brown, R.G. and P.Y.C. Wang. Introduction to Random Signal Analysis and Applied Kalman Filtering. 3rd Edition. New York: John Wiley & Sons, 1997.

[2] Kalman, R. E. "A New Approach to Linear Filtering and Prediction Problems." Transaction of the ASME–Journal of Basic Engineering, Vol. 82, Series D, March 1960, pp. 35–45.

[3] Blackman, Samuel. Multiple-Target Tracking with Radar Applications. Artech House. 1986.

Extended Capabilities

Introduced in R2017a