Extended Kalman filter for object tracking
trackingEKF object is a discrete-time
extended 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 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,
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.
filter = trackingEKF creates an extended Kalman filter object for a
discrete-time system by using default values for the
The process and measurement noises are assumed to be additive.
the state transition function,
filter = trackingEKF(
the measurement function,
the initial state of the system,
configures the properties of the extended Kalman filter object by using one or
filter = trackingEKF(___,
Name,Value pair arguments and any of the previous
syntaxes. Any unspecified properties have default values.
EnableSmoothing— Enable state smoothing
Enable state smoothing, specified as
Setting this property to
true requires the Sensor Fusion and Tracking Toolbox™ license. When specified as
true, you can:
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
Specify the maximum number of smoothing steps using the
MaxNumSmoothingSteps property of the tracking
MaxNumSmoothingSteps— Maximum number of smoothing steps
5(default) | positive integer
Maximum number of backward smoothing steps, specified as a positive integer.
To enable this property, set the
EnableSmoothing property to
|Predict state and state estimation error covariance of tracking filter|
|Correct state and state estimation error covariance using tracking filter|
|Correct state and state estimation error covariance using tracking filter and JPDA|
|Distances between current and predicted measurements of tracking filter|
|Likelihood of measurement from tracking filter|
|Create duplicate tracking filter|
|Measurement residual and residual noise from tracking filter|
|Initialize state and covariance of tracking filter|
Create a two-dimensional
trackingEKF object and use name-value pairs to define the
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
correct functions to propagate the state. You may call
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
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.
|Filter 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 ||Function returns M-element vector|
|h||Measurement function that specifies how the measurements are functions of the state and measurement noise.||Function returns N-element vector|
|xk||Estimate of the object state.||M-element vector|
|Pk||State error covariance matrix representing the uncertainty in the values of the state.||M-by-M matrix|
|Qk||Estimate of the process noise covariance matrix at step || ||M-by-M matrix when
|Rk||Estimate of the measurement noise covariance at step ||N-by-N matrix when
|F||Function determining Jacobian of propagated state with respect to previous state.||M-by-M matrix|
|H||Function determining Jacobians of measurement with respect to the state and measurement noise.||N-by-M for state vector Jacobian and N-by-R for measurement vector Jacobian|
The extended Kalman filter estimates the state of a process governed by this nonlinear stochastic equation:
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,
To use the simplified form, set
In the extended Kalman filter, the measurements are also general functions of the state:
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.
To use the simplified form, set
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.
 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.
 Blackman, Samuel and R. Popoli. Design and Analysis of Modern Tracking Systems. Artech House.1999.
 Blackman, Samuel. Multiple-Target Tracking with Radar Applications. Artech House. 1986.