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.
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 (
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.
The Jacobian of the predicted state with respect to the previous state is obtained by partial derivatives as:
The Jacobian of the predicted state with respect to the noise is:
These functions take simpler forms when the noise is additive in the state update equation:
In this case, F(w) is an identity matrix.
You can specify the state Jacobian matrix using the
StateTransitionJacobianFcn property of the
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.
In an extended Kalman filter, the measurement can also be a nonlinear function of the state and the measurement noise.
The Jacobian of the measurement with respect to the state is:
The Jacobian of the measurement with respect to the measurement noise is:
These functions take simpler forms when the noise is additive in the measurement equation:
In this case, H(v) is an identity matrix.
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
|Motion Model||Function Name||Function Purpose||State Representation|
|Constant velocity||Constant-velocity state update model||
|Constant-velocity state update Jacobian|
|Constant-velocity measurement model|
|Constant-velocity measurement Jacobian|
|Constant acceleration||Constant-acceleration state update model||
|Constant-acceleration state update Jacobian|
|Constant-acceleration measurement model|
|Constant-acceleration measurement Jacobian|
|Constant turn rate||Constant turn-rate state update model||
|Constant turn-rate state update Jacobian|
|Constant turn-rate measurement model|
|Constant turn-rate measurement Jacobian|
Example: Estimate 2-D Target States with Angle and Range Measurements Using
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
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")
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