Extended Kalman Filters
When you use a filter to track objects, you use a sequence of detections or measurements to
estimate the state of an object based on the motion model of the object.
In a motion model, state is a collection of quantities that represent the status of an object,
such as its position, velocity, and acceleration. Use an extended Kalman filter (trackingEKF
) when object motion follows a nonlinear state
equation or when the measurements are nonlinear functions of the state. For example, consider
using an extended Kalman filter when the measurements of the object are expressed in spherical
coordinates, such as azimuth, elevation, and range, but the states of the target are expressed
in Cartesian coordinates.
The formulation of an extended Kalman is based on the linearization of the state equation and measurement equation. Linearization enables you to propagate the state and state covariance in an approximately linear format, and requires Jacobians of the state equation and measurement equation.
Note
If your estimate system is linear, you can use the linear Kalman filter (trackingKF
) or the extended Kalman filter (trackingEKF
) to estimate the target state. If your system is
nonlinear, you should use a nonlinear filter, such as the extended Kalman filter or the
unscented Kalman filter (trackingUKF
).
State Update Model
Assume a closedform expression for the predicted state as a function of the previous state x_{k}, controls u_{k}, noise w_{k}, and time t.
$${x}_{k+1}=f({x}_{k},{u}_{k},{w}_{k},t)$$
The Jacobian of the predicted state with respect to the previous state is obtained by partial derivatives as:
$${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}.$$
These functions take simpler forms when the noise is additive in the state update equation:
$${x}_{k+1}=f({x}_{k},{u}_{k},t)+{w}_{k}$$
In this case, F^{(w)} is an identity matrix.
You can specify the state Jacobian matrix using the
StateTransitionJacobianFcn
property of the trackingEKF
object. If you do not specify this property, the object computes Jacobians using numeric
differencing, which is slightly less accurate and can increase the computation time.
Measurement Model
In an extended Kalman filter, the measurement can also 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 is additive in the measurement equation:
$${z}_{k}=h({x}_{k},t)+{v}_{k}$$
In this case, H^{(v)} is an identity matrix.
In trackingEKF
, you can specify the measurement Jacobian matrix using the
MeasurementJacobianFcn
property. If you do not specify this property,
the object computes the Jacobians using numeric differencing, which is slightly less accurate
and can increase the computation time.
Extended Kalman Filter Loop
The extended Kalman filter loop is almost identical to the loop of Linear Kalman Filters except that:
The filter uses the exact nonlinear state update and measurement functions whenever possible.
The state Jacobian replaces the state transition matrix.
The measurement Jacobian replaces the measurement matrix.
Predefined Extended Kalman Filter Functions
The toolbox provides predefined state update and measurement functions to use in trackingEKF
.
Motion Model  Function Name  Function Purpose  State Representation 

Constant velocity  constvel  Constantvelocity state update model 
where

constveljac  Constantvelocity state update Jacobian  
cvmeas  Constantvelocity measurement model  
cvmeasjac  Constantvelocity measurement Jacobian  
Constant acceleration  constacc  Constantacceleration state update model 
where

constaccjac  Constantacceleration state update Jacobian  
cameas  Constantacceleration measurement model  
cameasjac  Constantacceleration measurement Jacobian  
Constant turn rate  constturn  Constant turnrate state update model 
where 
constturnjac  Constant turnrate state update Jacobian  
ctmeas  Constant turnrate measurement model  
ctmeasjac  Constant turnrate measurement Jacobian 
Example: Estimate 2D Target States with Angle and Range Measurements Using trackingEKF
Initialize Estimation Model
Assume a target moves in 2D with the following initial position and velocity. The simulation lasts 20 seconds with a sample time of 0.2 seconds.
rng(2022); % For repeatable results dt = 0.2; % seconds simTime = 20; % seconds tspan = 0:dt:simTime; trueInitialState = [30; 1; 40; 1]; % [x;vx;y;vy] initialCovariance = diag([100,1e3,100,1e3]); processNoise = diag([0; .01; 0; .01]); % Process noise matrix
Assume the measurements are the azimuth angle relative to the positivex direction and the range to from the origin to the target. The measurement noise covariance matrix is:
measureNoise = diag([2e6;1]); % Measurement noise matrix. Units are m^2 and rad^2.
Preallocate variables in which to save results.
numSteps = length(tspan); trueStates = NaN(4,numSteps); trueStates(:,1) = trueInitialState; estimateStates = NaN(size(trueStates)); measurements = NaN(2,numSteps);
Obtain True States and Measurements
Propagate the constant velocity model and generate the measurements with noise.
for i = 2:length(tspan) if i ~= 1 trueStates(:,i) = stateModel(trueStates(:,i1),dt) + sqrt(processNoise)*randn(4,1); end measurements(:,i) = measureModel(trueStates(:,i)) + sqrt(measureNoise)*randn(2,1); end
Plot the true trajectory and the measurements.
figure(1) plot(trueStates(1,1),trueStates(3,1),"r*",DisplayName="Initial Truth") hold on plot(trueStates(1,:),trueStates(3,:),"r",DisplayName="True Trajectory") xlabel("x (m)") ylabel("y (m)") title("True Trajectory") axis square
figure(2) subplot(2,1,1) plot(tspan,measurements(1,:)*180/pi) xlabel("time (s)") ylabel("angle (deg)") title("Angle and Range") subplot(2,1,2) plot(tspan,measurements(2,:)) xlabel("time (s)") ylabel("range (m)")
Initialize Extended Kalman Filter
Initialize the filter with an initial state estimate at [35; 0; 45; 0]
.
filter = trackingEKF(State=[35; 0; 45; 0],StateCovariance=initialCovariance, ... StateTransitionFcn=@stateModel,ProcessNoise=processNoise, ... MeasurementFcn=@measureModel,MeasurementNoise=measureNoise); estimateStates(:,1) = filter.State;
Run Extended Kalman Filter And Show Results
Run the filter by recursively calling the predict
and correct
object functions.
for i=2:length(tspan) predict(filter,dt); estimateStates(:,i) = correct(filter,measurements(:,i)); end figure(1) plot(estimateStates(1,1),estimateStates(3,1),"g*",DisplayName="Initial Estimate") plot(estimateStates(1,:),estimateStates(3,:),"g",DisplayName="Estimated Trajectory") legend(Location="northwest") title("True Trajectory vs Estimated Trajectory")
Helper Functions
stateModel
modeled constant velocity motion without process noise.
function stateNext = stateModel(state,dt) F = [1 dt 0 0; 0 1 0 0; 0 0 1 dt; 0 0 0 1]; stateNext = F*state; end
meausreModel
models range and azimuth angle measurements without noise.
function z = measureModel(state) angle = atan(state(3)/state(1)); range = norm([state(1) state(3)]); z = [angle;range]; end
See Also
trackingKF
 trackingEKF
 trackingUKF
 Linear Kalman Filters