trackingKF

Linear Kalman filter for object tracking

Since R2021a

Description

A trackingKF object is a discrete-time linear Kalman filter used to track states, such as positions and velocities of target platforms.

A Kalman filter is a recursive algorithm for estimating the evolving state of a process when measurements are made on the process. The filter assumes the state-space model, including the state model and the measurement model, is linear. When the process noise and measurement noise are Gaussian and the motion model is linear, the Kalman filter is optimal. For a brief description of the linear Kalman filter algorithm, see Linear Kalman Filters.

You can use a trackingKF object in these ways:

Set the MotionModel property to one of predefined state transition models. See the MotionModel property for details on these models.
- "1D Constant Velocity"
- "1D Constant Acceleration"
- "2D Constant Velocity"
- "2D Constant Acceleration"
- "3D Constant Velocity"
- "3D Constant Acceleration"
Explicitly set the motion model. Set the MotionModel property to "Custom", and then use the StateTransitionModel and MeasurementModel properties to specify the state transition matrix and measurement matrix, respectively. Optionally, you can specify control inputs by specifying the ControlModel property.

Creation

Syntax

filter = trackingKF

filter = trackingKF("MotionModel",model)

filter = trackingKF(A,H)

filter = trackingKF(A,H,B)

filter = trackingKF(___,Name,Value)

Description

filter = trackingKF creates a discrete-time linear Kalman filter object for estimating the state of a 2-D, constant-velocity, moving object. The function sets the MotionModel property of the filter to "2D Constant Velocity".

filter = trackingKF("MotionModel",model) sets the MotionModel property to a predefined motion model, model. In this case, the filter initializes the state as a double-precision zero vector based on the dimension of the motion model. The filter also configures the MeasurementModel property so that the measurement model returns position measurements.

filter = trackingKF(A,H) specifies the StateTransitionModel and the MeasurementModel properties to A and H, respectively. The function sets the MotionModel property to "Custom".

filter = trackingKF(A,H,B) sets the ControlModel property to the specified B. The function sets the MotionModel property to "Custom".

example

filter = trackingKF(___,Name,Value) configures the properties of the Kalman filter by using one or more name-value arguments and any of the previous syntaxes. Any unspecified properties take default values. Enclose each property name in quotes.

example

Properties

expand all

`State` — Kalman filter state
`0` (default) | real-valued scalar | real-valued M-element vector

Kalman filter state, specified as a real-valued M-element vector, where M is the size of the state vector. For information on the typical size of the state vector for each motion model, see the MotionModel property. If you specify the initial state as a scalar, the filter extends the state to an M-by-1 vector.

To use the filter with single-precision, floating-point variables, specify the MootionModel property as a predefined model and specify State as a single-precision vector variable. For example:

filter = trackingKF("MotionModel","2D Constant Velocity","State",single([1; 2; 3; 4]))

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

Data Types: single | double

`StateCovariance` — State estimation error covariance
`1` (default) | positive scalar | positive-definite real-valued M-by-M matrix

State estimation 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 vector. If you specify a scalar, the property value is the product of the specified scalar and an M-by-M identity matrix. The matrix represents the uncertainty in the state, and each diagonal element of the matrix represents the variance of the corresponding state component. The off-diagonal elements represent cross-covariance between different state components.

Example: [20 0.1; 0.1 1]

Data Types: double

`MotionModel` — Kalman filter motion model
`"Custom"` | `"1D Constant Velocity"` | `"2D Constant Velocity"` | `"3D Constant Velocity"` | `"1D Constant Acceleration"` | `"2D Constant Acceleration"` | `"3D Constant Acceleration"`

Kalman filter motion model, specified as "Custom" or one of these predefined models:

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

If you specify the property as one of the predefined motion models, the filter uses this state-space model:

$\begin{array}{l} x (k + 1) = A (k) x (k) + G (k) w (k) \\ z (k) = H (k) x (k) + v (k) \end{array}$

where k is the discrete time step, x is the state, A is the state transition matrix, w is the process noise, G is the process noise gain matrix, H is the measurement matrix, v is the measurement noise, and z is the measurement. Note that the size of the gain matrix G is M-by-M/2, and the size of the process noise w is M/2, where M is the size of the state x.

Motion Model	State Vector x	State Transition Matrix (A)	Gain Matrix (G)
`"1D Constant Velocity"`	`[x;vx]`	`[1 dt; 0 1]`	`[dt^2/2; dt]`
`"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	Kronecker product of `kron(eye(2),[dt^2/2; dt])`
`"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.	Kronecker product of `kron(eye(3),[dt^2/2; dt])`
`"1D Constant Acceleration"`	`[x;vx;ax]`	`[1 dt dt^2/2; 0 1 dt; 0 0 1]`	`[dt^2/2; dt;1]`
`"2D Constant Acceleration"`	`[x;vx;ax;y;vy;ay]`	Block diagonal matrix with `[1 dt dt^2/2; 0 1 dt; 0 0 1]` blocks repeated for the x and y spatial dimensions	Kronecker product of `kron(eye(2),[dt^2/2; dt; 1])`
`"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	Kronecker product of `kron(eye(3),[dt^2/2; dt; 1])`

In the table, dt is the time step specified in the predict object function. If you want process noise and measurement noise values different from the default values for the motion model, specify them in the ProcessNoise and MeasurementNoise properties, respectively.

If you specify MotionModel as "Custom", you must specify a state transition model matrix A and a measurement model matrix H as input arguments to the Kalman filter. You can optionally specify a control model matrix, B, as well. When you specify a custom motion model, the filter uses this state-space model:

$\begin{array}{l} x (k + 1) = A (k) x (k) + B (k) u (k) + w (k) \\ z (k) = H (k) x (k) + v (k) \end{array}$

where u is the control input. In this case, the size of the process noise w is M, where M is the size of the state x. You can specify the covariance of w using the ProcessNoise property, and specify the covariance of v using the MeasurementNoise property.

Data Types: char | string

`StateTransitionModel` — State transition model between time steps
`[1 1 0 0; 0 1 0 0; 0 0 1 1; 0 0 0 1]` (default) | real-valued M-by-M matrix

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 predicts the state at a time step to the next time step.

Example: [1 1; 0 1]

Dependencies

To enable this property, set the MotionModel property to "Custom".

Data Types: single | double

`ControlModel` — Control model
M-by-L real-valued matrix

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.

Note

To use a control model, you must specify this property when constructing the filter object. You cannot change the size of the control model matrix after creating the filter.

Example: [.01 0.2]

Data Types: single | double

`ProcessNoise` — Covariance of process noise
`1` (default) | nonnegative scalar | positive-semidefinite D-by-D matrix | positive-semidefinite M-by-M matrix

Covariance of process noise, specified as a nonnegative scalar, a positive-semidefinite D-by-D matrix, or a positive-semidefinite M-by-M matrix. Process noise represents the uncertainty of state propagation, and the filter assumes the process noise to be zero-mean Gaussian white noise.

When the MotionModel property is specified as one of the predefined motion models, specify the ProcessNoise property as a positive-semidefinite D-by-D matrix, where D is the number of dimensions of the target motion. For example, D = 2 for the "2D Constant Velocity" or the "2D Constant Acceleration" motion model.
In this case, if you specify the ProcessNoise property as a nonnegative scalar, then the scalar extends to the diagonal elements of a diagonal covariance matrix, of size D-by-D.
When the MotionModel property is specified as "Custom", specify the ProcessNoise property as a positive-semidefinite M-by-M matrix, where M is the size of the filter state. For example, M = 6 if you customize a 3-D motion model in which the state is (x, v_x, y, v_y, z, v_z).
In this case, if you specify the ProcessNoise property as a nonnegative scalar, then the scalar extends to the diagonal elements of a diagonal covariance matrix, of size M-by-M.

Data Types: single | double

`MeasurementModel` — Measurement model from state vector
`[1 0 0 0; 0 0 1 0]` (default) | real-valued N-by-M matrix

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 measurements from the filter state.

Note

You cannot change the size of the measurement model matrix after creating the filter.

Example: [1 0.5 0.01; 1.0 1 0]

Data Types: single | double

`MeasurementNoise` — Measurement noise covariance
`1` (default) | positive scalar | positive-definite real-valued N-by-N matrix

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 property value is the product of the specified scalar and the N-by-N identity matrix. Measurement noise represents the uncertainty of the measurement and the filter assumes measurement noise to be zero-mean Gaussian white noise.

Example: 0.2

Data Types: single | double

`EnableSmoothing` — Enable state smoothing
`false` (default) | `true`

Enable state smoothing, specified as false or true. Setting this property to true requires the Sensor Fusion and Tracking Toolbox™ license. When specified as true, you can:

Use the smooth (Sensor Fusion and Tracking Toolbox) function, provided in Sensor Fusion and Tracking Toolbox, to smooth state estimates of the previous steps. Internally, the filter stores the results from previous steps to allow backward smoothing.
Specify the maximum number of smoothing steps using the MaxNumSmoothingSteps property of the tracking filter.

`MaxNumSmoothingSteps` — Maximum number of smoothing steps
`5` (default) | positive integer

Maximum number of backward smoothing steps, specified as a positive integer.

Dependencies

To enable this property, set the EnableSmoothing property to true.

`MaxNumOOSMSteps` — Maximum number of out-of-sequence measurement steps
`0` (default) | nonnegative integer

Maximum number of out-of-sequence measurement (OOSM) steps, specified as a nonnegative integer.

Setting this property to 0 disables the OOSM retrodiction capability of the filter object.
Setting this property to a positive integer enables the OOSM retrodiction capability of the filter object. This option requires a Sensor Fusion and Tracking Toolbox license. Also, you cannot set the MotionModel property to "Custom". When you set this property as N>1, the filter object saves the past state and state covariance history up to the last N+1 corrections. You can use the OOSM and the retrodict (Sensor Fusion and Tracking Toolbox) and retroCorrect (Sensor Fusion and Tracking Toolbox) (or retroCorrectJPDA (Sensor Fusion and Tracking Toolbox) for multiple OOSMs) object functions to reduce the uncertainty of the estimated state.

Increasing the value of this property increases the amount of memory that must be allocated for the state history, but enables you to process OOSMs that arrive after longer delays. Note that the effect of the uncertainty reduction using an OOSM decreases as the delay becomes longer.

Object Functions

`predict`	Predict state and state estimation error covariance of linear Kalman filter
`correct`	Correct state and state estimation error covariance using tracking filter
`correctjpda`	Correct state and state estimation error covariance using tracking filter and JPDA
`distance`	Distances between current and predicted measurements of tracking filter
`likelihood`	Likelihood of measurement from tracking filter
`clone`	Create duplicate tracking filter
`residual`	Measurement residual and residual noise from tracking filter
`initialize`	Initialize state and covariance of tracking filter
`tunableProperties` (Sensor Fusion and Tracking Toolbox)	Get tunable properties of filter
`setTunedProperties` (Sensor Fusion and Tracking Toolbox)	Set properties to tuned values

Examples

collapse all

Create Constant-Velocity Linear Kalman Filter

Open Live Script

Create a linear Kalman filter that uses a 2D constant velocity motion model. Assume that the measurement consists of the xy-location of the object.

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 measured positions for the object on 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")

Figure contains an axes object. The axes object with xlabel x [m], ylabel y [m] contains 6 objects of type line, text. One or more of the lines displays its values using only markers These objects represent Object position, Predicted position, Corrected position.

Use Custom `trackingKF` with Control Inputs

Open Live Script

Specify a simulation time of 10 seconds with a time step of 1 second.

rng(2021) % For repeatable results
simulationTime = 20;
dt = 1;
tspan = 0:dt:simulationTime;
steps = length(tspan);

Specify the motion model as a 2-D constant velocity model with a state of [x; vx; y; vy]. The measurement is [x; y].

A1D = [1 dt; 0 1]; 
A = kron(eye(2),A1D) % State transiton model

A = 4×4

     1     1     0     0
     0     1     0     0
     0     0     1     1
     0     0     0     1

H1D = [1 0];
H = kron(eye(2),H1D) % Measurement model

H = 2×4

     1     0     0     0
     0     0     1     0

sigma = 0.2;
R = sigma^2*eye(2); % Measurement noise covariance

Specify a control model matrix.

B1D = [0; 1];
B = kron(eye(2),B1D) % Control model matrix

Assume the control inputs are sinusoidal on the velocity components, vx and vy.

gain = 5;
Ux = gain*sin(tspan(2:end));
Uy = gain*cos(tspan(2:end));
U =[Ux; Uy]; % Control inputs

Assuming the true initial state is [1 1 1 -1], simulate the system to obtain true states and measurements.

initialState = [1 1 1 -1]'; % [m m/s m m/s]
trueStates = NaN(4,steps);
trueStates(:,1) = initialState;

for i=2:steps
    trueStates(:,i) = A*trueStates(:,i-1) + B*U(:,i-1);
end

measurements = H*trueStates + chol(R)*randn(2,steps);

Visualize the true trajectory and the measurements.

figure
plot(trueStates(1,:),trueStates(3,:),"DisplayName","Truth")
hold on
plot(measurements(1,:),measurements(2,:),"x","DisplayName","Measurements")
xlabel("x (m)")
ylabel("y (m)")
legend

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

Create a trackingKF filter with a custom motion model. Enable the control input by specifying the control model. Specify the initial state in the filter based on the first measurement.

initialFilterState = [measurements(1,1); 0; measurements(2,1); 0];
filter = trackingKF("MotionModel","Custom", ...
    "StateTransitionModel",A, ...
    "MeasurementModel",H, ...
    "ControlModel",B, ...
    "State",initialFilterState);

Estimate states by using the predict and correct object functions.

estimateStates = NaN(4,steps);
estimateStates(:,1) = initialFilterState;
for i = 2:steps
    predict(filter,U(:,i-1));
    estimateStates(:,i) = correct(filter,measurements(:,i));
end

Visualize the state estimates.

plot(estimateStates(1,:),estimateStates(3,:),"g","DisplayName","Estimates");

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 Truth, Measurements, Estimates.

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

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Usage notes and limitations:

If you create a trackingKF object, and you specify the MotionModel property as any value other than "Custom", then you must specify the state vector explicitly at construction time using the State property. The choice of motion model determines the size of the state vector, but motion models do not specify the data type such as double precision or single precision. Code generation requires both the size and data type.
In code generation, after cloning the filter, you cannot change its EnableSmoothing property.
In code generation, after calling the filter, you cannot change its MaxNumOOSMSteps property.
The filter supports strict single-precision code generation.
The filter supports non-dynamic memory allocation code generation.

Version History

Introduced in R2021a

trackingKF

Description

Creation

Syntax

Description

Properties

`State` — Kalman filter state
`0` (default) | real-valued scalar | real-valued M-element vector

`StateCovariance` — State estimation error covariance
`1` (default) | positive scalar | positive-definite real-valued M-by-M matrix

`MotionModel` — Kalman filter motion model
`"Custom"` | `"1D Constant Velocity"` | `"2D Constant Velocity"` | `"3D Constant Velocity"` | `"1D Constant Acceleration"` | `"2D Constant Acceleration"` | `"3D Constant Acceleration"`

`StateTransitionModel` — State transition model between time steps
`[1 1 0 0; 0 1 0 0; 0 0 1 1; 0 0 0 1]` (default) | real-valued M-by-M matrix

Dependencies

`ControlModel` — Control model
M-by-L real-valued matrix

`ProcessNoise` — Covariance of process noise
`1` (default) | nonnegative scalar | positive-semidefinite D-by-D matrix | positive-semidefinite M-by-M matrix

`MeasurementModel` — Measurement model from state vector
`[1 0 0 0; 0 0 1 0]` (default) | real-valued N-by-M matrix

`MeasurementNoise` — Measurement noise covariance
`1` (default) | positive scalar | positive-definite real-valued N-by-N matrix

`EnableSmoothing` — Enable state smoothing
`false` (default) | `true`

`MaxNumSmoothingSteps` — Maximum number of smoothing steps
`5` (default) | positive integer

Dependencies

`MaxNumOOSMSteps` — Maximum number of out-of-sequence measurement steps
`0` (default) | nonnegative integer

Object Functions

Examples

Create Constant-Velocity Linear Kalman Filter

Use Custom `trackingKF` with Control Inputs

References

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Version History

See Also

Functions

Objects

Topics

trackingKF

Description

Creation

Syntax

Description

Properties

State — Kalman filter state 0 (default) | real-valued scalar | real-valued M-element vector

StateCovariance — State estimation error covariance 1 (default) | positive scalar | positive-definite real-valued M-by-M matrix

MotionModel — Kalman filter motion model "Custom" | "1D Constant Velocity" | "2D Constant Velocity" | "3D Constant Velocity" | "1D Constant Acceleration" | "2D Constant Acceleration" | "3D Constant Acceleration"

StateTransitionModel — State transition model between time steps [1 1 0 0; 0 1 0 0; 0 0 1 1; 0 0 0 1] (default) | real-valued M-by-M matrix

Dependencies

ControlModel — Control model M-by-L real-valued matrix

ProcessNoise — Covariance of process noise 1 (default) | nonnegative scalar | positive-semidefinite D-by-D matrix | positive-semidefinite M-by-M matrix

MeasurementModel — Measurement model from state vector [1 0 0 0; 0 0 1 0] (default) | real-valued N-by-M matrix

MeasurementNoise — Measurement noise covariance 1 (default) | positive scalar | positive-definite real-valued N-by-N matrix

EnableSmoothing — Enable state smoothing false (default) | true

MaxNumSmoothingSteps — Maximum number of smoothing steps 5 (default) | positive integer

Dependencies

MaxNumOOSMSteps — Maximum number of out-of-sequence measurement steps 0 (default) | nonnegative integer

Object Functions

Examples

Create Constant-Velocity Linear Kalman Filter

Use Custom trackingKF with Control Inputs

References

Extended Capabilities

C/C++ Code Generation Generate C and C++ code using MATLAB® Coder™.

Version History

See Also

Functions

Objects

Topics

`State` — Kalman filter state
`0` (default) | real-valued scalar | real-valued M-element vector

`StateCovariance` — State estimation error covariance
`1` (default) | positive scalar | positive-definite real-valued M-by-M matrix

`MotionModel` — Kalman filter motion model
`"Custom"` | `"1D Constant Velocity"` | `"2D Constant Velocity"` | `"3D Constant Velocity"` | `"1D Constant Acceleration"` | `"2D Constant Acceleration"` | `"3D Constant Acceleration"`

`StateTransitionModel` — State transition model between time steps
`[1 1 0 0; 0 1 0 0; 0 0 1 1; 0 0 0 1]` (default) | real-valued M-by-M matrix

`ControlModel` — Control model
M-by-L real-valued matrix

`ProcessNoise` — Covariance of process noise
`1` (default) | nonnegative scalar | positive-semidefinite D-by-D matrix | positive-semidefinite M-by-M matrix

`MeasurementModel` — Measurement model from state vector
`[1 0 0 0; 0 0 1 0]` (default) | real-valued N-by-M matrix

`MeasurementNoise` — Measurement noise covariance
`1` (default) | positive scalar | positive-definite real-valued N-by-N matrix

`EnableSmoothing` — Enable state smoothing
`false` (default) | `true`

`MaxNumSmoothingSteps` — Maximum number of smoothing steps
`5` (default) | positive integer

`MaxNumOOSMSteps` — Maximum number of out-of-sequence measurement steps
`0` (default) | nonnegative integer

Use Custom `trackingKF` with Control Inputs

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.