Unscented Kalman filter for object tracking
trackingUKF object is a discrete-time
unscented 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.
An unscented Kalman filter is a recursive algorithm for estimating the evolving state of a process when measurements are made on the process. The unscented Kalman filter can model the evolution of a state that obeys a nonlinear motion model. The measurements can also be nonlinear functions of the state, and the process and measurements can have noise.
Use an unscented Kalman filter when one of both of these conditions apply:
The current state is a nonlinear function of the previous state.
The measurements are nonlinear functions of the state.
The unscented Kalman filter estimates the uncertainty about the state,
and its propagation through the nonlinear state and measurement equations, by using a
fixed number of sigma points. Sigma points are chosen by using the unscented
transformation, as parameterized by the
filter = trackingUKF creates an unscented 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 = trackingUKF(
the measurement function,
the initial state of the system,
configures the properties of the unscented Kalman filter object using one or
filter = trackingUKF(___,
Name,Value pair arguments and any of the previous
syntaxes. Any unspecified properties have default values.
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.
Alpha — Sigma point spread around state
1.0e-3 (default) | positive scalar greater than 0 and less than or equal to 1
Sigma point spread around state, specified as a positive scalar greater than 0 and less than or equal to 1.
Beta — Distribution of sigma points
2 (default) | nonnegative scalar
Distribution of sigma points, specified as a nonnegative scalar. This
parameter incorporates knowledge of the noise distribution of states for
generating sigma points. For Gaussian distributions, setting
Beta to 2 is optimal.
Kappa — Secondary scaling factor for generating sigma points
0 (default) | scalar from 0 to 3
Secondary scaling factor for generation of sigma points, specified as a scalar from 0 to 3. This parameter helps specify the generation of sigma points.
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
|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 Unscented Kalman Filter
trackingUKF object using the predefined constant-velocity motion model,
constvel, and the associated measurement model,
cvmeas. These models assume that the state vector has the form [x;vx;y;vy] and that the position measurement is in Cartesian coordinates, [x;y;z]. Set the sigma point spread property to 1e-2.
filter = trackingUKF(@constvel,@cvmeas,[0;0;0;0],'Alpha',1e-2);
Run the filter. Use the
correct functions to propagate the state. You can call
correct in any order and as many times as you want.
meas = [1;1;0]; [xpred, Ppred] = predict(filter); [xcorr, Pcorr] = correct(filter,meas); [xpred, Ppred] = predict(filter); [xpred, Ppred] = predict(filter)
xpred = 4×1 1.2500 0.2500 1.2500 0.2500
Ppred = 4×4 11.7500 4.7500 -0.0000 0.0000 4.7500 3.7500 0.0000 -0.0000 -0.0000 0.0000 11.7500 4.7500 0.0000 -0.0000 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.
|Model 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|
|Pk||State error covariance matrix representing the uncertainty in the values of the state||M-by-M|
|Qk||Estimate of the process noise covariance matrix at step || ||M-by-M when |
|Rk||Estimate of the measurement noise covariance at step k. Measurement noise reflects the uncertainty of the measurement and is assumed to be zero-mean white Gaussian noise.||N-by-N when |
|α||Determines spread of sigma points.||scalar|
|β||A priori knowledge of sigma point distribution.||scalar|
|κ||Secondary scaling parameter.||scalar|
The unscented Kalman filter estimates the state of a process governed by a nonlinear stochastic equation
where xk is the state at step k. f() is the state transition function, uk are the controls on the process. The motion may be affected by random noise perturbations, wk. The filter also supports a simplified form,
To use the simplified
In the unscented Kalman filter, the measurements are also general functions of the state,
where 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 these. The measurements can include noise as well, represented by vk. Again the class offers a simpler formulation
To use the simplified form, set
These equations represent the actual motion of the object and the actual measurements. However, the noise contribution at each step is unknown and cannot be modeled exactly. Only 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.
 Wan, Eric A. and R. van der Merwe. “The Unscented Kalman Filter for Nonlinear Estimation”. Adaptive Systems for Signal Processing, Communications, and Control. AS-SPCC, IEEE, 2000, pp.153–158.
 Wan, Merle. “The Unscented Kalman Filter.” In Kalman Filtering and Neural Networks. Edited by Simon Haykin. John Wiley & Sons, Inc., 2001.
 Sarkka S. “Recursive Bayesian Inference on Stochastic Differential Equations.” Doctoral Dissertation. Helsinki University of Technology, Finland. 2006.
 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:
Generated code uses an algorithm that is different from the algorithm that the
trackingUKFobject uses. You might see some numerical differences in the results obtained using the two methods.
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.