trackingUKF

Unscented Kalman filter for object tracking

Description

The trackingUKF object is a discrete-time unscented 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.

An unscented Kalman filter is a recursive algorithm for estimating the evolving state of a process when measurements are made on the process. The unscented Kalman filter can model the evolution of a state that obeys a nonlinear motion model. The measurements can also be nonlinear functions of the state, and the process and measurements can have noise.

Use an unscented Kalman filter when one of both of these conditions apply:

The current state is a nonlinear function of the previous state.
The measurements are nonlinear functions of the state.

The unscented Kalman filter estimates the uncertainty about the state, and its propagation through the nonlinear state and measurement equations, by using a fixed number of sigma points. Sigma points are chosen by using the unscented transformation, as parameterized by the Alpha, Beta, and Kappa properties.

Creation

Syntax

filter = trackingUKF

filter = trackingUKF(transitionfcn,measurementfcn,state)

filter = trackingUKF(___,Name,Value)

Description

filter = trackingUKF creates an unscented Kalman filter object for a discrete-time system by using default values for the StateTransitionFcn, MeasurementFcn, and State properties. The process and measurement noises are assumed to be additive.

filter = trackingUKF(transitionfcn,measurementfcn,state) specifies the state transition function, transitionfcn, the measurement function, measurementfcn, and the initial state of the system, state.

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

example

Properties

expand all

`State` — Kalman filter state
real-valued M-element vector

Kalman filter state, specified as a real-valued M-element vector, where M is the size of the filter state.

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

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

Example: [200; 0.2]

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: [20 0.1; 0.1 1]

`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)

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

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 trackingUKF 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 scalar | positive-definite real-valued matrix

Process noise covariance, specified as a scalar or matrix.

When HasAdditiveProcessNoise is true, specify the process noise covariance as a positive real 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.
You must 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.05 2]

`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. The function accepts the M-element state vector an inputs and outputs the N-element measurement vector. You can use one of these functions as your measurement function.

Function Name	Function Purpose
`cvmeas`	Constant-velocity measurement model
`cameas`	Constant-acceleration measurement model
`ctmeas`	Constant turn-rate measurement model

You can also write your own measurement function.

If HasAdditiveMeasurementNoise is true, specify the function using one of these syntaxes:
```
z(k) = measurementfcn(x(k))
```
```
z(k) = measurementfcn(x(k),parameters)
```
x(k) is the state at time k and z(k) is the predicted measurement at time k. The parameters argument 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)
```
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].

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:

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, specified as a positive scalar or positive-definite real-valued matrix.

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

`Alpha` — Sigma point spread around state
`1.0e-3` (default) | positive scalar greater than 0 and less than or equal to 1

Sigma point spread around state, specified as a positive scalar greater than 0 and less than or equal to 1.

`Beta` — Distribution of sigma points
`2` (default) | nonnegative scalar

Distribution of sigma points, specified as a nonnegative scalar. This parameter incorporates knowledge of the noise distribution of states for generating sigma points. For Gaussian distributions, setting Beta to 2 is optimal.

`Kappa` — Secondary scaling factor for generating sigma points
`0` (default) | scalar from 0 to 3

Secondary scaling factor for generation of sigma points, specified as a scalar from 0 to 3. This parameter helps specify the generation of sigma points.

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

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

Constant-Velocity Unscented Kalman Filter

Open Live Script

Create a trackingUKF object using the predefined constant-velocity motion model, constvel, and the associated measurement model, cvmeas. These models assume that the state vector has the form [x;vx;y;vy] and that the position measurement is in Cartesian coordinates, [x;y;z]. Set the sigma point spread property to 1e-2.

filter = trackingUKF(@constvel,@cvmeas,[0;0;0;0],'Alpha',1e-2);

Run the filter. Use the predict and correct functions to propagate the state. You can call predict and correct in any order and as many times as you want.

meas = [1;1;0]; 
[xpred, Ppred] = predict(filter);
[xcorr, Pcorr] = correct(filter,meas);
[xpred, Ppred] = predict(filter);
[xpred, Ppred] = predict(filter)

xpred = 4×1

    1.2500
    0.2500
    1.2500
    0.2500

Ppred = 4×4

   11.7500    4.7500   -0.0000    0.0000
    4.7500    3.7500    0.0000   -0.0000
   -0.0000    0.0000   11.7500    4.7500
    0.0000   -0.0000    4.7500    3.7500

More About

expand all

Filter Parameters

This table relates the filter model parameters to the object properties. M is the size of the state vector. N is the size of the measurement vector.

Model Parameter	Description	Filter Property	Size
f	State transition function that specifies the equations of motion of the object. This function determines the state at time `k+1` as a function of the state and the controls at time `k`. The state transition function depends on the time-increment of the filter.	`StateTransitionFcn`	Function returns M-element vector
h	Measurement function that specifies how the measurements are functions of the state and measurement noise.	`MeasurementFcn`	Function returns N-element vector
x_k	Estimate of the object state.	`State`	M
P_k	State error covariance matrix representing the uncertainty in the values of the state	`StateCovariance`	M-by-M
Q_k	Estimate of the process noise covariance matrix at step `k`. Process noise is measure of the uncertainty in your dynamic model and is assumed to be zero-mean white Gaussian noise	`ProcessNoise`	M-by-M when `HasAdditiveProcessNoise` is `true`. Q-by-Q when `HasAdditiveProcessNoise`is `false`.
R_k	Estimate of the measurement noise covariance at step k. Measurement noise reflects the uncertainty of the measurement and is assumed to be zero-mean white Gaussian noise.	`MeasurementNoise`	N-by-N when `HasAdditiveMeasurementNoise` is `true`. R-by-R when `HasAdditiveMeasurementNoise` is `false`.
α	Determines spread of sigma points.	`Alpha`	scalar
β	A priori knowledge of sigma point distribution.	`Beta`	scalar
κ	Secondary scaling parameter.	`Kappa`	scalar

Algorithms

The unscented Kalman filter estimates the state of a process governed by a nonlinear stochastic equation

$x_{k + 1} = f (x_{k}, u_{k}, w_{k}, t)$

where x_k is the state at step k. f() is the state transition function, u_k are the controls on the process. The motion may be affected by random noise perturbations, w_k. The filter also supports a simplified form,

$x_{k + 1} = f (x_{k}, u_{k}, t) + w_{k}$

To use the simplified form, set HasAdditiveProcessNoise to true.

In the unscented Kalman filter, the measurements are also general functions of the state,

$z_{k} = h (x_{k}, v_{k}, t)$

where h(x_k,v_k,t) is the measurement function that determines the measurements as functions of the state. Typical measurements are position and velocity or some function of these. The measurements can include noise as well, represented by v_k. Again the class offers a simpler formulation

$z_{k} = h (x_{k}, t) + v_{k}$

To use the simplified form, set HasAdditiveMeasurementNoise to true.

These equations represent the actual motion of the object and the actual measurements. However, the noise contribution at each step is unknown and cannot be modeled exactly. Only statistical properties of the noise are known.

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.” Transactions of the ASME–Journal of Basic Engineering. Vol. 82, Series D, March 1960, pp. 35–45.

[3] Wan, Eric A. and R. van der Merwe. “The Unscented Kalman Filter for Nonlinear Estimation”. Adaptive Systems for Signal Processing, Communications, and Control. AS-SPCC, IEEE, 2000, pp.153–158.

[4] Wan, Merle. “The Unscented Kalman Filter.” In Kalman Filtering and Neural Networks. Edited by Simon Haykin. John Wiley & Sons, Inc., 2001.

[5] Sarkka S. “Recursive Bayesian Inference on Stochastic Differential Equations.” Doctoral Dissertation. Helsinki University of Technology, Finland. 2006.

[6] 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:

Generated code uses an algorithm that is different from the algorithm that the trackingUKF object uses. You might see some numerical differences in the results obtained using the two methods.
The filter supports strict single-precision code generation when the specified state transition function and measurement function both support single-precision code generation.
The filter supports non-dynamic memory allocation code generation.

Version History

Introduced in R2017a

trackingUKF

Description

Creation

Syntax

Description

Properties

`State` — Kalman filter state
real-valued M-element vector

`StateCovariance` — State estimation error covariance
positive-definite real-valued M-by-M matrix

`StateTransitionFcn` — State transition function
function handle

`ProcessNoise` — Process noise covariance
1 (default) | positive real scalar | positive-definite real-valued matrix

`HasAdditiveProcessNoise` — Model additive process noise
`true` (default) | `false`

`MeasurementFcn` — Measurement model function
function handle

`HasMeasurementWrapping` — Wrapping of measurement residuals
`0` (default) | `false` or `0` | `true` or `1`

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

`HasAdditiveMeasurementNoise` — Model additive measurement noise
`true` (default) | `false`

`Alpha` — Sigma point spread around state
`1.0e-3` (default) | positive scalar greater than 0 and less than or equal to 1

`Beta` — Distribution of sigma points
`2` (default) | nonnegative scalar

`Kappa` — Secondary scaling factor for generating sigma points
`0` (default) | scalar from 0 to 3

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

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

Dependencies

Object Functions

Examples

Constant-Velocity Unscented Kalman Filter

More About

Filter Parameters

Algorithms

References

Extended Capabilities

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

Version History

See Also

Functions

Objects

trackingUKF

Description

Creation

Syntax

Description

Properties

State — Kalman filter state real-valued M-element vector

StateCovariance — State estimation error covariance positive-definite real-valued M-by-M matrix

StateTransitionFcn — State transition function function handle

ProcessNoise — Process noise covariance 1 (default) | positive real scalar | positive-definite real-valued matrix

HasAdditiveProcessNoise — Model additive process noise true (default) | false

MeasurementFcn — Measurement model function function handle

HasMeasurementWrapping — Wrapping of measurement residuals 0 (default) | false or 0 | true or 1

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

HasAdditiveMeasurementNoise — Model additive measurement noise true (default) | false

Alpha — Sigma point spread around state 1.0e-3 (default) | positive scalar greater than 0 and less than or equal to 1

Beta — Distribution of sigma points 2 (default) | nonnegative scalar

Kappa — Secondary scaling factor for generating sigma points 0 (default) | scalar from 0 to 3

EnableSmoothing — Enable state smoothing false (default) | true

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

Dependencies

Object Functions

Examples

Constant-Velocity Unscented Kalman Filter

More About

Filter Parameters

Algorithms

References

Extended Capabilities

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

Version History

See Also

Functions

Objects

`State` — Kalman filter state
real-valued M-element vector

`StateCovariance` — State estimation error covariance
positive-definite real-valued M-by-M matrix

`StateTransitionFcn` — State transition function
function handle

`ProcessNoise` — Process noise covariance
1 (default) | positive real scalar | positive-definite real-valued matrix

`HasAdditiveProcessNoise` — Model additive process noise
`true` (default) | `false`

`MeasurementFcn` — Measurement model function
function handle

`HasMeasurementWrapping` — Wrapping of measurement residuals
`0` (default) | `false` or `0` | `true` or `1`

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

`HasAdditiveMeasurementNoise` — Model additive measurement noise
`true` (default) | `false`

`Alpha` — Sigma point spread around state
`1.0e-3` (default) | positive scalar greater than 0 and less than or equal to 1

`Beta` — Distribution of sigma points
`2` (default) | nonnegative scalar

`Kappa` — Secondary scaling factor for generating sigma points
`0` (default) | scalar from 0 to 3

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

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

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