# trackingEKF

Extended Kalman filter for object tracking

## Description

A `trackingEKF` object is a discrete-time extended Kalman filter used to track dynamical states, such as 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 extended Kalman filter can model the evolution of a state when the state follows a nonlinear motion model, when the measurements are nonlinear functions of the state, or when both conditions apply. The extended Kalman filter is based on the linearization of the nonlinear equations. This approach leads to a filter formulation similar to the linear Kalman filter, `trackingKF`.

The process and measurements can have Gaussian noise, which you can include in these ways:

• Add noise to both the process and the measurements. In this case, the sizes of the process noise and measurement noise must match the sizes of the state vector and measurement vector, respectively.

• Add noise in the state transition function, the measurement model function, or in both functions. In these cases, the corresponding noise sizes are not restricted.

See Extended Kalman Filters for more details.

## Creation

### Syntax

``filter = trackingEKF``
``filter = trackingEKF(transitionfcn,measurementfcn,state)``
``filter = trackingEKF(___,Name,Value)``

### Description

````filter = trackingEKF` creates an extended 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 = trackingEKF(transitionfcn,measurementfcn,state)` specifies the state transition function, `transitionfcn`, the measurement function, `measurementfcn`, and the initial state of the system, `state`.```

example

````filter = trackingEKF(___,Name,Value)` configures the properties of the extended Kalman filter object by 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. The value of M is determined based on the motion model you use. For example, if you use a 2-D constant velocity model specified by `constvel` (Sensor Fusion and Tracking Toolbox), in which the state is `[x;vx;y;vy]`, M is four.

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

`filter = trackingEKF('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. You can use one of these functions as your state transition function.

Function NameFunction 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 `trackingEKF` 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`

Jacobian of the state transition function, specified as a function handle. This function has the same input arguments as the state transition function.

The valid syntaxes for the Jacobian of 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```)
```Jx(k) = statejacobianfcn(x(k)) Jx(k) = statejacobianfcn(x(k),parameters)```
• `x(k)` is the state at time `k`.

• `Jx(k)` denotes the Jacobian of the predicted state with respect to the previous state. This Jacobian is an M-by-M matrix at time `k`. The Jacobian function can take additional input parameters, such as control inputs or time-step size.

• `parameters` stands for all additional arguments required by the Jacobian function, such as control inputs or time-step size.

```[Jx(k),Jw(k)] = statejacobianfcn(x(k),w(k)) [Jx(k),Jw(k)] = statejacobianfcn(x(k),w(k),dt) [Jx(k),Jw(k)] = statejacobianfcn(__,parameters)```
• `x(k)` is the state at time `k`

• `w(k)` is a sample Q-element vector of the process noise at time `k`. Q is the size of the process noise covariance. The process noise vector in the nonadditive case does not need to have the same dimensions as the state vector.

• `Jx(k)` denotes the Jacobian of the predicted state with respect to the previous state. This Jacobian is an M-by-M matrix at time k. The Jacobian function can take additional input parameters, such as control inputs or time-step size.

• `Jw(k)` denotes the M-by-Q Jacobian of the predicted state with respect to the process noise elements.

• `dt` is the time step of the `trackingEKF` 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 Jacobian function, such as control inputs or time-step size.

If this property is not specified, the Jacobians are computed by numeric differencing at each call of the `predict` function. This computation can increase the processing time and numeric inaccuracy.

Example: `@constaccjac`

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

Jacobian of the measurement function, specified as a function handle. The function has the same input arguments as the measurement function. The function can take additional input parameters, such sensor position and orientation.

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

```Jmx(k) = measjacobianfcn(x(k)) ```
`Jmx(k) = measjacobianfcn(x(k),parameters)`
`x(k)` is the state at time `k`. `Jx(k)` denotes the N-by-M Jacobian of the measurement function with respect to the state. The `parameters` argument stands for all arguments required by the measurement function.

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

```[Jmx(k),Jmv(k)] = measjacobianfcn(x(k),v(k)) ```
`[Jmx(k),Jmv(k)] = measjacobianfcn(x(k),v(k),parameters)`
`x(k)` is the state at time `k` and `v(k)` is an R-dimensional sample noise vector. `Jmx(k)` denotes the N-by-M Jacobian of the measurement function with respect to the state. `Jmv(k)` denotes the Jacobian of the N-by-R measurement function with respect to the measurement noise. The `parameters` argument stands for all arguments required by the measurement function.

If not specified, measurement Jacobians are computed using numerical differencing at each call to the `correct` function. This computation can increase processing time and numerical inaccuracy.

Example: `@cameasjac`

Data Types: `function_handle`

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.

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.

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.

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

#### Dependencies

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

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. With OOSM enabled, the filter object saves the past state and state covariance history. 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 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 two-dimensional `trackingEKF` object and use name-value pairs to define the `StateTransitionJacobianFcn` and `MeasurementJacobianFcn` properties. Use the predefined constant-velocity motion and measurement models and their Jacobians.

```EKF = trackingEKF(@constvel,@cvmeas,[0;0;0;0], ... 'StateTransitionJacobianFcn',@constveljac, ... 'MeasurementJacobianFcn',@cvmeasjac);```

Run the filter. Use the `predict` and `correct` functions to propagate the state. You may call `predict` and `correct` in any order and as many times you want. Specify the measurement in Cartesian coordinates.

```measurement = [1;1;0]; [xpred, Ppred] = predict(EKF); [xcorr, Pcorr] = correct(EKF,measurement); [xpred, Ppred] = predict(EKF); [xpred, Ppred] = predict(EKF)```
```xpred = 4×1 1.2500 0.2500 1.2500 0.2500 ```
```Ppred = 4×4 11.7500 4.7500 0 0 4.7500 3.7500 0 0 0 0 11.7500 4.7500 0 0 4.7500 3.7500 ```

expand all

## Algorithms

The extended Kalman filter estimates the state of a process governed by this nonlinear stochastic equation:

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

xk is the state at step k. f() is the state transition function. Random noise perturbations, wk, can affect the object motion. 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 extended Kalman filter, the measurements are also general functions of the state:

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

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 position and velocity. The measurements can also include noise, represented by vk. Again, the filter offers a simpler formulation.

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

To use the simplified form, set `HasAdditiveMeasurementNoise` to `true`.

These equations represent the actual motion and the actual measurements of the object. However, the noise contribution at each step is unknown and cannot be modeled deterministically. Only the 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] Blackman, Samuel and R. Popoli. Design and Analysis of Modern Tracking Systems. Artech House.1999.

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

## Version History

Introduced in R2017a