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

example

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

## Properties

expand all

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`

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

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.

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

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

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.

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

Example: `@cameas`

Data Types: `function_handle`

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`

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.

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

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.

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.

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

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

## Examples

collapse all

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

expand all

## Algorithms

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

`${x}_{k+1}=f\left({x}_{k},{u}_{k},{w}_{k},t\right)$`

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

`${x}_{k+1}=f\left({x}_{k},{u}_{k},t\right)+{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\left({x}_{k},{v}_{k},t\right)$`

where h(xk,vk,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 vk. Again the class offers a simpler formulation

`${z}_{k}=h\left({x}_{k},t\right)+{v}_{k}$`

To use the simplified form, set `HasAdditiveMeasurmentNoise` 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.

 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.

 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.

 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.

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

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

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