# trackingKF

Linear Kalman filter for object tracking

## Description

A `trackingKF` object is a discrete-time linear 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.

A Kalman filter is a recursive algorithm for estimating the evolving state of a process when measurements are made on the process. The filter is linear when the evolution of the state follows a linear motion model and the measurements are linear functions of the state. The filter assumes that both the process and measurements have additive noise. When the process noise and measurement noise are Gaussian, the Kalman filter is the optimal minimum mean squared error (MMSE) state estimator for linear processes.

You can use this object in these ways:

• Explicitly set the motion model. Set the motion model property, `MotionModel`, to `Custom`, and then use the `StateTransitionModel` property to set the state transition matrix.

• Set the `MotionModel` property to a predefined state transition model:

Motion Model
`'1D Constant Velocity'`
`'1D Constant Acceleration'`
`'2D Constant Velocity'`
`'2D Constant Acceleration'`
`'3D Constant Velocity'`
`'3D Constant Acceleration'`

## Creation

### Syntax

``filter = trackingKF``
``filter = trackingKF(F,H)``
``filter = trackingKF(F,H,G)``
``filter = trackingKF('MotionModel',model)``
``filter = trackingKF(___,Name,Value)``

### Description

````filter = trackingKF` creates a linear Kalman filter object for a discrete-time, 2-D, constant-velocity moving object. The Kalman filter uses default values for the `StateTransitionModel`, `MeasurementModel`, and `ControlModel` properties. The function also sets the `MotionModel` property to `'2D Constant Velocity'`.```
````filter = trackingKF(F,H)` specifies the state transition model, `F`, and the measurement model, `H`. With this syntax, the function also sets the `MotionModel` property to `'Custom'`.```
````filter = trackingKF(F,H,G)` also specifies the control model, `G`. With this syntax, the function also sets the `MotionModel` property to `'Custom'`.```
````filter = trackingKF('MotionModel',model)` sets the motion model property, `MotionModel`, to `model`.```

example

````filter = trackingKF(___,Name,Value)` configures the properties of the Kalman filter by using one or more `Name`,`Value` pair arguments and any of the previous syntaxes. Any unspecified properties take default values.```

## Properties

expand all

Kalman filter state, specified as a real-valued M-element vector. M is the size of the state vector. Typical state vector sizes are described in the `MotionModel` property. When the initial state is specified as a scalar, the state is expanded into an M-element vector.

You can set the state to a scalar in these cases:

• When the `MotionModel` property is set to `'Custom'`, M is determined by the size of the state transition model.

• When the `MotionModel` property is set to ```'2D Constant Velocity'```, `'3D Constant Velocity'`, `'2D Constant Acceleration'`, or ```'3D Constant Acceleration'```, you must first specify the state as an M-element vector. You can use a scalar for all subsequent specifications of the state vector.

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

Data Types: `double`

State 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. Specifying the value as a scalar creates a multiple of the M-by-M identity matrix. This matrix represents the uncertainty in the state.

Example: `[20 0.1; 0.1 1]`

Data Types: `double`

Kalman filter motion model, specified as `'Custom'` or one of these predefined models. In this case, the state vector and state transition matrix take the form specified in the table.

Motion ModelForm of State VectorForm of State Transition Model
`'1D Constant Velocity'`

`[x;vx]`

`[1 dt; 0 1]`

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

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

`'1D Constant Acceleration'`

`[x;vx;ax]`

```[1 dt 0.5*dt^2; 0 1 dt; 0 0 1]```

`'2D Constant Acceleration'`

`[x;vx;ax;y;vy;ay]`

Block diagonal matrix with ```[1 dt 0.5*dt^2; 0 1 dt; 0 0 1]``` blocks repeated for the x and y spatial dimensions

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

When the `ControlModel` property is defined, every nonzero element of the state transition model is replaced by `dt`.

When `MotionModel` is `'Custom'`, you must specify a state transition model matrix, a measurement model matrix, and optionally, a control model matrix as input arguments to the Kalman filter.

Data Types: `char`

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 relates the state at any time step to the state at the previous step. The state transition model is a function of the filter time step size.

Example: `[1 0; 1 2]`

#### Dependencies

To enable this property, set `MotionModel` to `'Custom'`.

Data Types: `double`

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.

Example: `[.01 0.2]`

Data Types: `double`

Covariance of process noise, specified as a positive scalar or an M-by-M matrix where M is the dimension of the state. If you specify this property as a scalar, the filter uses the value as a multiplier of the M-by-M identity matrix. Process noise expresses the uncertainty in the dynamic model and is assumed to be zero-mean Gaussian white noise.

Example: `[1.0 0.05; 0.05 2`]

Data Types: `double`

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

Example: `[1 0.5 0.01; 1.0 1 0]`

Data Types: `double`

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 filter uses the value as a multiplier of the N-by-N identity matrix. Measurement noise represents the uncertainty of the measurement and is assumed to be zero-mean Gaussian white noise.

Example: `0.2`

Data Types: `double`

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

## Examples

collapse all

Create a linear Kalman filter that uses a `2D Constant Velocity` motion model. Assume that the measurement consists of the object's x-y location.

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 the measured positions from 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')```

expand all

## Algorithms

The Kalman filter describes the motion of an object by estimating its state. The state generally consists of object position and velocity and possibly its acceleration. The state can span one, two, or three spatial dimensions. Most frequently, you use the Kalman filter to model constant-velocity or constant-acceleration motion. A linear Kalman filter assumes that the process obeys the following linear stochastic difference equation:

`${x}_{k+1}={F}_{k}{x}_{k}+{G}_{k}{u}_{k}+{v}_{k}$`

xk is the state at step k. Fk is the state transition model matrix. Gk is the control model matrix. uk represents known generalized controls acting on the object. In addition to the specified equations of motion, the motion may be affected by random noise perturbations, vk. The state, the state transition matrix, and the controls together provide enough information to determine the future motion of the object in the absence of noise.

In the Kalman filter, the measurements are also linear functions of the state,

`${z}_{k}={H}_{k}{x}_{k}+{w}_{k}$`

where Hk is the measurement model matrix. This model expresses the measurements as functions of the state. A measurement can consist of an object position, position and velocity, or its position, velocity, and acceleration, or some function of these quantities. The measurements can also include noise perturbations, wk.

These equations, in the absence of noise, model the actual motion of the object and the actual measurements. The noise contributions at each step are unknown and cannot be modeled. Only the noise covariance matrices are known. The state covariance matrix is updated with knowledge of the noise covariance only.

For a brief description of the linear Kalman filter algorithm, see Linear Kalman Filters .

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