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 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`	Constant-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.
	`constveljac`	Constant-velocity state update Jacobian
	`cvmeas`	Constant-velocity measurement model
	`cvmeasjac`	Constant-velocity measurement Jacobian
Constant acceleration	`constacc`	Constant-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.
	`constaccjac`	Constant-acceleration state update Jacobian
	`cameas`	Constant-acceleration measurement model
	`cameasjac`	Constant-acceleration measurement Jacobian
Constant turn rate	`constturn`	Constant 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.
	`constturnjac`	Constant turn-rate state update Jacobian
	`ctmeas`	Constant turn-rate measurement model
	`ctmeasjac`	Constant turn-rate measurement Jacobian

Example: Estimate 2-D Target States with Angle and Range Measurements Using `trackingEKF`

Open Live Script

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 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 contains an axes object. The axes object with title True Trajectory, xlabel x (m), ylabel y (m) contains 2 objects of type line. One or more of the lines displays its values using only markers These objects represent Initial Truth, True Trajectory.

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)")

Figure contains 2 axes objects. Axes object 1 with title Angle and Range, xlabel time (s), ylabel angle (deg) contains an object of type line. Axes object 2 with xlabel time (s), ylabel range (m) contains an object of type line.

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")

Figure contains an axes object. The axes object with title True Trajectory vs Estimated Trajectory, xlabel x (m), ylabel y (m) contains 4 objects of type line. One or more of the lines displays its values using only markers These objects represent Initial Truth, True Trajectory, Initial Estimate, 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