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 closed-form expression for the predicted state as a function of the previous state xk, controls uk, noise wk, and time t.

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

The Jacobian of the predicted state with respect to the previous state is obtained by partial derivatives as:

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

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

${F}^{\left(w\right)}=\frac{\partial f}{\partial w}.$

These functions take simpler forms when the noise is additive in the state update equation:

${x}_{k+1}=f\left({x}_{k},{u}_{k},t\right)+{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\left({x}_{k},{v}_{k},t\right)$

The Jacobian of the measurement with respect to the state is:

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

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

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

These functions take simpler forms when the noise is additive in the measurement equation:

${z}_{k}=h\left({x}_{k},t\right)+{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 ModelFunction NameFunction PurposeState Representation
Constant velocityconstvelConstant-velocity state update model

• 1-D — [x;vx]

• 2-D — [x;vx;y;vy]

• 3-D — [x;vx;y;vy;z;vz]

where

• x, y, and z represents the position in the x-, y-, and z-directions, respectively.

• vx, vy, and vz represents the velocity in the x-, y-, and z-directions, respectively.

constveljacConstant-velocity state update Jacobian
cvmeasConstant-velocity measurement model
cvmeasjacConstant-velocity measurement Jacobian
Constant accelerationconstaccConstant-acceleration state update model

• 1-D — [x;vx;ax]

• 2-D — [x;vx;ax;y;vy;ay]

• 3-D — [x;vx;ax;y;vy;ay;z;vz;az]

where

• ax, ay, and az represents the acceleration in the x-, y-, and z-directions, respectively.

constaccjacConstant-acceleration state update Jacobian
cameasConstant-acceleration measurement model
cameasjacConstant-acceleration measurement Jacobian
Constant turn rateconstturnConstant turn-rate state update model

• 2-D — [x;vx;y;vy;omega]

• 3-D — [x;vx;y;vy;omega;z;vz]

where omega represents the turn-rate.

constturnjacConstant turn-rate state update Jacobian
ctmeasConstant turn-rate measurement model
ctmeasjacConstant turn-rate measurement Jacobian

Example: Estimate 2-D 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 positive-x direction and the range to from the origin to the target. The measurement noise covariance matrix is:

measureNoise = diag([2e-6;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(:,i-1),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