trackingCKF

Cubature Kalman filter for object tracking

Description

The trackingCKF object is a cubature Kalman filter used for tracking objects that follow a nonlinear motion model or are measured by a nonlinear measurement model. Use the filter to predict the future location of an object, to reduce noise in a measured location, or to help associate multiple object detections with their tracks.

The cubature Kalman filter estimates the uncertainty of the state and the propagation of that uncertainty through the nonlinear state and measurement equations. There are a fixed number of cubature points chosen based on the spherical-radial transformation to guarantee an exact approximation of a Gaussian distribution up to the third moment. As a result, the corresponding filter is the same as an unscented Kalman filter, trackingUKF, with Alpha = 1, Beta = 0, and Kappa = 0.

Creation

Syntax

ckf = trackingCKF

ckf = trackingCKF(transitionFcn,measuremntFcn,state)

ckf = trackingCKF(___,Name,Value)

Description

ckf = trackingCKF returns a cubature Kalman filter object with default state transition function, measurement function, state, and additive noise model.

ckf = trackingCKF(transitionFcn,measuremntFcn,state) specifies the StateTransitionFcn, MeasurementFcn, and State properties directly.

ckf = trackingCKF(___,Name,Value) specifies the properties of the Kalman filter using one or more Name,Value pair arguments. Any unspecified properties take default values.

Properties

State — Kalman filter state

real-valued M-element vector

Kalman filter state, specified as a real-valued M-element vector.

If you want a filter with single-precision floating-point variables, specify State as a single-precision vector variable. For example,

filter = trackingCKF('State',single([1;2;3;4]))

Example: [200;0.2;150;0.1;0;0.25]

Data Types: single | double

StateCovariance — State estimation error covariance

positive-definite real-valued M-by-M matrix

State error covariance, specified as a positive-definite real-valued M-by-M matrix, where M is the size of the filter state. The covariance matrix represents the uncertainty in the filter state.

Example: eye(6)

StateTransitionFcn — State transition function

function handle

State transition function, specified as a function handle. This function calculates the state vector at time step k from the state vector at time step k – 1. The function can take additional input parameters, such as control inputs or time step size. The function can also include noise values. You can use one of these functions as your state transition function.

Function Name	Function Purpose
constvel	Constant-velocity state update model
constacc	Constant-acceleration state update model
constturn	Constant turn-rate state update model

You can also write your own state transition function. The valid syntaxes for the state transition function depend on whether the filter has additive process noise. The table shows the valid syntaxes based on the value of the HasAdditiveProcessNoise property.

Valid Syntaxes (HasAdditiveProcessNoise = true)

Valid Syntaxes (HasAdditiveProcessNoise = false)

x(k) = statetransitionfcn(x(k-1)) x(k) = statetransitionfcn(x(k-1),parameters) x(k) is the state at time k. parameters stands for all additional arguments required by the state transition function.

x(k) = statetransitionfcn(x(k-1),w(k-1)) x(k) = statetransitionfcn(x(k-1),w(k-1),dt) x(k) = statetransitionfcn(__,parameters) x(k) is the state at time k. w(k) is a value for the process noise at time k. dt is the time step of the trackingCKF filter, filter, specified in the most recent call to the predict function. The dt argument applies when you use the filter within a tracker and call the predict function with the filter to predict the state of the tracker at the next time step. For the nonadditive process noise case, the tracker assumes that you explicitly specify the time step by using this syntax: predict(filter,dt). parameters stands for all additional arguments required by the state transition function.

Example: @constacc

Data Types: function_handle

ProcessNoise — Process noise covariance

1 (default) | positive real-valued scalar | positive-definite real-valued matrix

Process noise covariance:

• When HasAdditiveProcessNoise is true, specify the process noise covariance as a scalar or a positive-definite real-valued M-by-M matrix. M is the dimension of the state vector. When specified as a scalar, the matrix is a multiple of the M-by-M identity matrix.

• When HasAdditiveProcessNoise is false, specify the process noise covariance as a Q-by-Q matrix. Q is the size of the process noise vector.

  Specify ProcessNoise before any call to the predict function. In later calls to predict, you can optionally specify the process noise as a scalar. In this case, the process noise matrix is a multiple of the Q-by-Q identity matrix.

Example: [1.0 0.05 0; 0.05 1.0 2.0; 0 2.0 1.0]

Dependencies

This parameter depends on the HasAdditiveNoise property.

HasAdditiveProcessNoise — Model additive process noise

true (default) | false

Option to model process noise as additive, specified as true or false. When this property is true, process noise is added to the state vector. Otherwise, noise is incorporated into the state transition function.

MeasurementFcn — Measurement model function

function handle

Measurement model function, specified as a function handle. This function can be a nonlinear function that models measurements from the predicted state. Input to the function is the M-element state vector. The output is the N-element measurement vector. The function can take additional input arguments, such as sensor position and orientation.

• If HasAdditiveMeasurementNoise is true, specify the function using one of these syntaxes:

  z(k) = measurementfcn(x(k))
  

  z(k) = measurementfcn(x(k),parameters)

  where x(k) is the state at time k, and z(k) is the predicted measurement at time k. The parameters term stands for all additional arguments required by the measurement function.

• If HasAdditiveMeasurementNoise is false, specify the function using one of these syntaxes:

  z(k) = measurementfcn(x(k),v(k))
  

  z(k) = measurementfcn(x(k),v(k),parameters)

  where x(k) is the state at time k, and v(k) is the measurement noise at time k. The parameters argument stands for all additional arguments required by the measurement function.

• If the HasMeasurementWrapping property is true, you must additionally return the measurement wrapping bounds, which the filter uses to wrap the measurement residuals, as the second output argument of the measurement function.

  [z(k),bounds] = measurementfcn(__)
  

  The function must return bounds as an M-by-2 real-valued matrix, where M is the size of z(k). In each row, the first and second elements specify the lower and upper bounds, respectively, for the corresponding measurement variable. You can use −Inf or Inf to represent that the variable does not have a lower or upper bound.

  For example, consider a measurement function that returns the azimuth and range of a platform as [azimuth; range]. If the azimuth angle wraps between -180 and 180 degrees while the range is unbounded and nonnegative, then specify the second output argument of the function as [-180 180; 0 Inf].

• If the HasMeasurementWrapping property is true, you must additionally return the measurement wrapping bounds, which the filter uses to wrap the measurement residuals, as the second output argument of the measurement function.

  [z(k),bounds] = measurementfcn(__)
  

  The function must return bounds as an M-by-2 real-valued matrix, where M is the size of z(k). In each row, the first and second elements specify the lower and upper bounds, respectively, for the corresponding measurement variable. You can use −Inf or Inf to represent that the variable does not have a lower or upper bound.

  For example, consider a measurement function that returns the azimuth and range of a platform as [azimuth; range]. If the azimuth angle wraps between -180 and 180 degrees while the range is unbounded and nonnegative, then specify the second output argument of the function as [-180 180; 0 Inf].

Example: @cameas

Data Types: function_handle

HasMeasurementWrapping — Wrapping of measurement residuals

0 (default) | false or 0 | true or 1

Wrapping of measurement residuals in the filter, specified as a logical 0 (false) or 1 (true). When specified as true, the measurement function specified in the MeasurementFcn property must return two output arguments:

• The first argument is the measurement, returned as an M-element real-valued vector.

• The second argument is the wrapping bounds, returned as an M-by-2 real-valued matrix, where M is the dimension of the measurement. In each row, the first and second elements are the lower and upper bounds for the corresponding measurement variable. You can use −Inf or Inf to represent that the variable does not have a lower or upper bound.

If you enable this property, the filter wraps the measurement residuals according to the measurement bounds, which helps prevent the filter from divergence caused by incorrect measurement residual values.

These measurement functions have predefined wrapping bounds:

• cvmeas

• cameas

• ctmeas

• cvmeasmsc

• singermeas

In these functions, the wrapping bounds are [-180 180] degrees for azimuth angle measurements and [-90 90] degrees for elevation angle measurements. Other measurements are not bounded.

Note

You can specify this property only when constructing the filter.

MeasurementNoise — Measurement noise covariance

1 (default) | positive scalar | positive-definite real-valued matrix

Measurement noise covariance:.

• When HasAdditiveMeasurementNoise is true, specify the measurement noise covariance as a scalar or an N-by-N matrix. N is the size of the measurement vector. When specified as a scalar, the matrix is a multiple of the N-by-N identity matrix.

• When HasAdditiveMeasurementNoise is false, specify the measurement noise covariance as an R-by-R matrix. R is the size of the measurement noise vector.

  Specify MeasurementNoise before any call to the correct function. After the first call to correct, you can optionally specify the measurement noise as a scalar. In this case, the measurement noise matrix is a multiple of the R-by-R identity matrix.

Example: 0.2

HasAdditiveMeasurementNoise — Model additive measurement noise

true (default) | false

Option to enable additive measurement noise, specified as true or false. When this property is true, noise is added to the measurement. Otherwise, noise is incorporated into the measurement function.

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 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.

Object Functions

predict	Predict state and state estimation error covariance of tracking 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
residual	Measurement residual and residual noise from tracking filter
smooth	Backward smooth state estimates of tracking filter
clone	Create duplicate tracking filter
tunableProperties	Get tunable properties of filter
setTunedProperties	Set properties to tuned values
setMeasurementSizes	Sets the sizes of the measurement and measurement noise

Examples

Run trackingCKF Filter

This example shows how to create and run a trackingCKF filter. Call the predict and correct functions to track an object and correct the state estimation based on measurements.

Create the filter. Specify the constant velocity motion model, the measurement model, and the initial state.

state = [0;0;0;0;0;0];
ckf = trackingCKF(@constvel,@cvmeas,state);

Call predict to get the predicted state and covariance of the filter. Use a 0.5 second time step.

[xPred,pPred] = predict(ckf,0.5);

Call correct with a given measurement.

meas = [1;1;0];
[xCorr,pCorr] = correct(ckf,meas);

Continue to predict the filter state. Specify the desired time step in seconds if necessary.

[xPred,pPred] = predict(ckf);         % Predict over 1 second
[xPred,pPred] = predict(ckf,2);       % Predict over 2 seconds

References

[1] Arasaratnam, Ienkaran, and Simon Haykin. "Cubature kalman filters." IEEE Transactions on automatic control 54, no. 6 (2009): 1254-1269.

Extended Capabilities

C/C++ Code Generation

Generate C and C++ code using MATLAB® Coder™.

Usage notes and limitations:

• The filter supports strict single-precision code generation when the specified state transition function and measurement function both support single-precision code generation.

  For details, see Generate Code with Strict Single-Precision and Non-Dynamic Memory Allocation.

• The filter supports non-dynamic memory allocation code generation.

  For details, see Generate Code with Strict Single-Precision and Non-Dynamic Memory Allocation.

Version History

Introduced in R2018b