Extended Kalman filter for object tracking
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,
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.
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.
State — Kalman filter state
real-valued M-element vector
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
[x;vx;y;vy], M is four.
If you want a filter with single-precision floating-point variables,
State as a single-precision vector variable.
filter = trackingEKF('State',single([1;2;3;4]))
HasMeasurementWrapping — Wrapping of measurement residuals
0 (default) |
Wrapping of measurement residuals in the filter, specified as a logical
true). When specified as
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
Infto 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.
You can specify this property only when constructing the filter.
EnableSmoothing — Enable state smoothing
false (default) |
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 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
MaxNumSmoothingStepsproperty of the tracking filter.
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
MaxNumOOSMSteps — Maximum number of out-of-sequence measurement steps
0 (default) | nonnegative integer
Maximum number of out-of-sequence measurement (OOSM) steps, specified as a nonnegative integer.
Setting this property to
0disables 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.
|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|
Constant-Velocity Extended Kalman 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.
C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.
Usage notes and limitations:
In code generation, after cloning the filter, you cannot change its
In code generation, after calling the filter, you cannot change its
The filter supports strict single-precision code generation when the specified state transition function and measurement function both support single-precision code generation.
The filter supports non-dynamic memory allocation code generation.