tracking maneuvering targets -凯发k8网页登录

this example shows how to track maneuvering targets using various tracking filters. the example shows the difference between filters that use a single motion model and multiple motion models.

define a scenario

in this example, you define a single target that initially travels at a constant velocity of 200 m/s for 33 seconds, then enters a constant turn of 10 deg/s. the turn lasts for 33 seconds, then the target accelerates in a straight line at 3 m/s^2.

[truestate, time, fig1] = helpergeneratetruthdata;
dt = diff(time(1:2));
numsteps = numel(time);
figure(fig1)

define the measurements to be the position and add normal random noise with a standard deviation of 1 to the measurements.

% set the rng seed for repeatable results
s = rng;
rng(2018);
positionselector = [1 0 0 0 0 0;0 0 1 0 0 0;0 0 0 0 1 0]; % position from state
truepos = positionselector * truestate;
measnoise = randn(size(truepos));
measpos = truepos   measnoise;

track using a constant-velocity filter

you define a trackingekf with a constant-velocity motion model. you use the first measurement to define the initial state and state covariance, and set the process noise to be non-additive, to define the process noise in terms of the unknown acceleration in the x, y, and z components. this definition is similar to how the function initcvekf works.

initialstate = positionselector' * measpos(:,1);
initialcovariance = diag([1,1e4,1,1e4,1,1e4]); % velocity is not measured
cvekf = trackingekf(@constvel, @cvmeas, initialstate, ...
    'statetransitionjacobianfcn', @constveljac, ...
    'measurementjacobianfcn', @cvmeasjac, ...
    'statecovariance', initialcovariance, ...
    'hasadditiveprocessnoise', false, ...
    'processnoise', eye(3));

for each measurement, you predict the filter, calculate the distance of the predicted state from the true position, and correct the filter using the measurement to obtain the filtered estimate of the position.

dist = zeros(1,numsteps);
estpos = zeros(3,numsteps);
for i = 2:size(measpos,2)
    predict(cvekf, dt);
    dist(i) = distance(cvekf,truepos(:,i)); % distance from true position
    estpos(:,i) = positionselector * correct(cvekf, measpos(:,i));
end
figure(fig1);
plot(estpos(1,:),estpos(2,:),'.g','displayname','cv low pn')
title('true and estimated positions')
axis([5000 8000 -500 2500])

as depicted in the figure, the filter is able to track the constant velocity part of the motion very well, but when the target executes the turn, the filter estimated position diverges from the true position. you can see the distance of the estimate from the truth in the following plot. during the turn, at 33-66 seconds, the normalized distance jumps to very high values, which means that the filter is unable to track the maneuvering target.

fig2 = figure;
hold on
plot((1:numsteps)*dt,dist,'g','displayname', 'cv low pn')
title('normalized distance from estimated position to true position')
xlabel('time (s)')
ylabel('normalized distance')
legend

increase the process noise

one possible solution is to increase the process noise. the process noise represents the unmodeled terms in the motion model. for a constant velocity model, these are the unknown acceleration terms. by increasing the process noise, you allow for larger uncertainty in the motion model, which causes the filter to rely on the measurements more than on the model. the following lines create a constant velocity filter, with high process noise values that correspond to about 5-g turn.

cvekf2 = trackingekf(@constvel, @cvmeas, initialstate, ...
    'statetransitionjacobianfcn', @constveljac, ...
    'measurementjacobianfcn', @cvmeasjac, ...
    'statecovariance', initialcovariance, ...
    'hasadditiveprocessnoise', false, ...
    'processnoise', diag([50,50,1])); % large uncertainty in the horizontal acceleration
dist = zeros(1,numsteps);
estpos = zeros(3,numsteps);
for i = 2:size(measpos,2)
    predict(cvekf2, dt);
    dist(i) = distance(cvekf2,truepos(:,i)); % distance from true position
    estpos(:,i) = positionselector * correct(cvekf2, measpos(:,i));
end
figure(fig1)
plot(estpos(1,:),estpos(2,:),'.c','displayname','cv high pn')
axis([5000 8000 -500 2500])

increasing the process noise significantly improves the filter's ability to track the target during the turn. however, there is a cost: the filter is less capable of smoothing out the measurement noise during the constant velocity period of the motion. even though the normalized distance during the turn was significantly reduced, the normalized distance increased in the first 33 seconds, during the constant velocity period of the motion.

figure(fig2)
plot((1:numsteps)*dt,dist,'c','displayname', 'cv high pn')
axis([0 100 0 50])

use an interacting motion-model filter

another solution is to use a filter that can consider all motion models at the same time, called an interacting multiple-model (imm) filter. the imm filter can maintain as many motion models as you want, but typically is used with 2-5 motion models. for this example, three models are sufficient: a constant velocity model, a constant turn model, and a constant acceleration model.

imm = trackingimm('transitionprobabilities', 0.99); % the default imm has all three models
% initialize the state and state covariance in terms of the first model
initialize(imm, initialstate, initialcovariance);

you use the imm filter in the same way that the ekf was used.

dist = zeros(1,numsteps);
estpos = zeros(3,numsteps);
modelprobs = zeros(3,numsteps);
modelprobs(:,1) = imm.modelprobabilities;
for i = 2:size(measpos,2)
    predict(imm, dt);
    dist(i) = distance(imm,truepos(:,i)); % distance from true position
    estpos(:,i) = positionselector * correct(imm, measpos(:,i));
    modelprobs(:,i) = imm.modelprobabilities;
end
figure(fig1)
plot(estpos(1,:),estpos(2,:),'.m','displayname','imm')

the trackingimm filter is able to track the maneuvering target in all three parts of the motion.

examining the distance between the filter's predicted state and the true position, you see that the imm filter is able to reduce the distance for all portions of the motion. in fact, the imm filter is better at tracking the motion than both other constant velocity models used earlier.

figure(fig2)
hold on
plot((1:numsteps)*dt,dist,'m','displayname', 'imm')
axis([0 100 0 50])

to better understand how the imm filter works, plot the model probabilities as a function of time. the figure shows that filter is initialized with the three models having the same probability. as the filter is updated, it quickly converges to a very high probability that the model is a constant velocity model. after 33 seconds of motion, the constant velocity model is no longer true, and the probability of the constant turn model becomes very high for the duration of the turn. in the last section of the motion, during the constant acceleration maneuver, the imm filter assigns a high probability that the motion is constant acceleration, but the filter is less certain about the correct motion model, and there is about 0.3 probability of a constant velocity motion.

figure
plot((1:numsteps)*dt, modelprobs)
title('model probabilities vs. time')
xlabel('time (s)')
ylabel('model probabilities')
legend('imm-cv','imm-ca','imm-ct')
% return the rng to its previous state
rng(s)

summary

this example showed you how to track a target maneuvering with constant turn and constant acceleration motion. the example showed how you can increase the process noise to capture the unknown maneuver with a constant velocity model. you also saw how to improve the tracking of a maneuvering target by using an imm filter.

supporting functions

helpergeneratetruthdata

this function generates the ground truth trajectory.

function [xgt, tt, figs] = helpergeneratetruthdata
% generate ground truth
vx = 200; % m/s
omega = 10; % deg/s
acc = 3; % m/s/s
dt = 0.1;
tt = (0:dt:floor(1000*dt));
figs = [];
xgt = nan(9,numel(tt));
xgt(:,1) = 0;
% constant velocity
seg1 = floor(numel(tt)/3);
xgt(2,1) = vx;
slct = eye(9);
slct(3:3:end,:) = [];
for m = 2:seg1
    x0 = slct*xgt(:,m-1);
    x1 = constvel(x0, dt);
    x1 = slct'*x1;
    xgt(:,m) = x1;
end
% constant turn
seg2 = floor(2*numel(tt)/3);
slct = eye(9);
slct(3:3:end,:) = [];
for m = seg1 1:seg2
    x0 = slct*xgt(:,m-1);
    x0 = [x0(1:4);omega];
    x1 = constturn(x0, dt);
    x1 = x1(1:4);
    x1 = [x1(1:2);0;x1(3:4);0;zeros(3,1)];
    xgt(:,m) = x1;
end
% constant acceleration
first = true;
for m = seg2 1:numel(tt)
    x0 = xgt(:,m-1);
    if first
        vel = x0(2:3:end);
        ua = vel/norm(vel);
        va = acc*ua;
        x0(3:3:end) = va;
        first = false;
    end
    x1 = constacc(x0, dt);
    xgt(:,m) = x1;
end
% drop acceleration dimension
slct = eye(9);
slct(3:3:end,:) = [];
xgt = slct*xgt;
figs = [figs figure];
plot(xgt(1,1:seg1),xgt(3,1:seg1),'.-');
hold on;
plot(xgt(1,seg1 1:seg2),xgt(3,seg1 1:seg2),'.-');
plot(xgt(1,seg2 1:end),xgt(3,seg2 1:end),'.-');
grid on;
xlabel('x position (m)');
ylabel('y position (m)');
title('true position')
axis equal;
legend('constant velocity', 'constant turn', 'constant acceleration')
end