Main Content

Use an extended Kalman filter when object motion follows a nonlinear state equation or when the measurements are nonlinear functions of the state. A simple example is when the state or measurements of the object are calculated in spherical coordinates, such as azimuth, elevation, and range.

The extended Kalman filter formulation linearizes the state equations. The updated state and covariance matrix remain linear functions of the previous state and covariance matrix. However, the state transition matrix in the linear Kalman filter is replaced by the Jacobian of the state equations. The Jacobian matrix is not constant but can depend on the state itself and time. To use the extended Kalman filter, you must specify both a state transition function and the Jacobian of the state transition function.

Assume there is a closed-form expression for the predicted state as a function of the previous state, controls, noise, and time.

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

The Jacobian of the predicted state with respect to the previous state is

$${F}^{(x)}=\frac{\partial f}{\partial x}.$$

The Jacobian of the predicted state with respect to the noise is

$${F}^{(w)}=\frac{\partial f}{\partial {w}_{i}}.$$

These functions take simpler forms when the noise enters linearly into the state update equation:

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

In this case, *F ^{(w)} =
1_{M}*.

In the extended Kalman filter, the measurement can be a nonlinear function of the state and the measurement noise.

$${z}_{k}=h({x}_{k},{v}_{k},t)$$

The Jacobian of the measurement with respect to the state is

$${H}^{(x)}=\frac{\partial h}{\partial x}.$$

The Jacobian of the measurement with respect to the measurement noise is

$${H}^{(v)}=\frac{\partial h}{\partial v}.$$

These functions take simpler forms when the noise enters linearly into the measurement equation:

$${z}_{k}=h({x}_{k},t)+{v}_{k}$$

In this case, *H ^{(v)} =
1_{N}*.

This extended kalman filter loop is almost identical to the linear Kalman filter loop except that:

The exact nonlinear state update and measurement functions are used whenever possible and the state transition matrix is replaced by the state Jacobian

The measurement matrices are replaced by the appropriate Jacobians.

Automated Driving Toolbox™ provides predefined state update and measurement functions to use in the extended Kalman filter.

Motion Model | Function Name | Function Purpose |
---|---|---|

Constant velocity | `constvel` | Constant-velocity state update model |

`constveljac` | Constant-velocity state update Jacobian | |

`cvmeas` | Constant-velocity measurement model | |

`cvmeasjac` | Constant-velocity measurement Jacobian | |

Constant acceleration | `constacc` | Constant-acceleration state update model |

`constaccjac` | Constant-acceleration state update Jacobian | |

`cameas` | Constant-acceleration measurement model | |

`cameasjac` | Constant-acceleration measurement Jacobian | |

Constant turn rate | `constturn` | Constant turn-rate state update model |

`constturnjac` | Constant turn-rate state update Jacobian | |

`ctmeas` | Constant turn-rate measurement model | |

`ctmeasjac` | Constant-turnrate measurement Jacobian |