www.pudn.com > trackingdemos.zip > plot_3d_PDA1.m
% plot_3d_pda1.m
%
% Kalman filter with 3-D PDA using LP for tracking multiple targets with possibly unresolved measurements,
% assuming the resolvability indicator is known.
% See also: gene_2d_scn.m, gene_2d_meas.m
function fig = plot_3d_pda1(target, measurement)
fig = figure;
hold on;
plot(target(1).state(1,:), target(1).state(3,:), '.');
plot(target(2).state(1,:), target(2).state(3,:), '.');
axis([100e3,130e3,147e3,151e3]);
% define SNR
SNRdb = 10;
SNR = 10^(SNRdb/10);
% detection threshold
threshold = 2.55;
% calc. the probability of detection and false alarm per cell
% according to certain model (currently Swerling-I)
Pfa = exp(-threshold*threshold/2);
Pd = exp(-threshold*threshold/2/(1+SNR));
% measurement std, cell size
C_r = 50;
C_b = 2e-3;
% track initiation
track(1).time(1) = 1;
track(2).time(1) = 1;
track(1).est(:,1) = target(1).state(:,1);
track(2).est(:,1) = target(2).state(:,1);
track(1).P(:,:,1) = 1e4.* [9, 0, -6, 0;
0, .2, 0, 0;
-6 0, 5, 0;
0, 0, 0, .2];
track(2).P(:,:,1) = 1e4.* [9, 0, -6, 0;
0, .2, 0, 0;
-6 0, 5, 0;
0, 0, 0, .2];
% sufficient stat. at the rear end of the frozen window
track(1).X = track(1).est(:,1);
track(1).Cov = track(1).P(:,:,1);
track(2).X = track(2).est(:,1);
track(2).Cov = track(2).P(:,:,1);
frozen_time = track(1).time(1);
q = .01; % process noise
H = [1, 0, 0, 0; 0, 0, 1, 0];
vm = zeros(2,1);
wm = zeros(2,1);
Ik = eye(2);
Qk = q.*Ik;
lambda = 1e-7;
numScan = length(target(1).time);
for i=1:numScan-1
T = measurement(i).time - frozen_time;
T2 = measurement(i+1).time - measurement(i).time;
F = [1, T, 0, 0;
0, 1, 0, 0;
0, 0, 1, T;
0, 0, 0, 1];
G = [T*T/2, 0;
T, 0;
0, T*T/2;
0, T];
F2 = [1, T2, 0, 0;
0, 1, 0, 0;
0, 0, 1, T2;
0, 0, 0, 1];
G2 = [T2*T2/2, 0;
T2, 0;
0, T2*T2/2;
0, T2];
z = []; z1 = []; z2 = [];
R = []; R1 = []; R2 = [];
cost = [];
assign = [];
ind00 = length(measurement(i).flag);
ind01 = length(measurement(i+1).flag);
if ind00 == 0
[track(1).X, track(1).Cov] = prekalman(track(1).X, track(1).Cov,vm,Qk,F,G);
[track(2).X, track(2).Cov] = prekalman(track(2).X, track(2).Cov,vm,Qk,F,G);
if ind01 == 0
[track(1).est(:,i+1), track(1).P(:,:,i+1)] = prekalman(track(1).X, track(1).Cov,vm,Qk,F2,G2);
[track(2).est(:,i+1), track(2).P(:,:,i+1)] = prekalman(track(2).X, track(2).Cov,vm,Qk,F2,G2);
else
k1 = 1;
for j=1:ind01
if measurement(i+1).flag(j) == 0
[z(1,k1), z(2,k1), R(:,:,k1)] = CalcMeasCov2(measurement(i+1).range(j), measurement(i+1).bearing(j), ...
measurement(i+1).pos(1), measurement(i+1).pos(2), C_r, C_b);
plot(z(1,k1), z(2,k1), 'k*');
k1 = k1 + 1;
[z(1,k1), z(2,k1), R(:,:,k1)] = CalcMeasCov2(measurement(i+1).range(j), measurement(i+1).bearing(j), ...
measurement(i+1).pos(1), measurement(i+1).pos(2), C_r, C_b);
k1 = k1 + 1;
else
[z(1,k1), z(2,k1), R(:,:,k1)] = CalcMeasCov2(measurement(i+1).range(j), measurement(i+1).bearing(j), ...
measurement(i+1).pos(1), measurement(i+1).pos(2), C_r/sqrt(12), C_b/sqrt(12));
plot(z(1,k1), z(2,k1), 'c*');
k1 = k1 + 1;
end
end
cost = zeros(2, k1);
for j=1:k1-1
% form the cost function for 2-D assignment
cost(1,j+1) = lr_kalman(track(1).X, track(1).Cov, z(:,j), Qk, R(:,:,j), vm, wm, F2, G2, H, Ik, Pd, lambda);
cost(2,j+1) = lr_kalman(track(2).X, track(2).Cov, z(:,j), Qk, R(:,:,j), vm, wm, F2, G2, H, Ik, Pd, lambda);
end
associate_prob = myLinProg(-cost);
if isempty(associate_prob)
[track(1).est(:,i+1), track(1).P(:,:,i+1)] = prekalman(track(1).X, track(1).Cov,vm,Qk,F2,G2);
[track(2).est(:,i+1), track(2).P(:,:,i+1)] = prekalman(track(2).X, track(2).Cov,vm,Qk,F2,G2);
else
[track(1).est(:,i+1), track(1).P(:,:,i+1)] = pdakalman(track(1).X, track(1).Cov,...
z, R, associate_prob(1,:), Qk, vm, wm, F2, G2, H, Ik);
[track(2).est(:,i+1), track(2).P(:,:,i+1)] = pdakalman(track(2).X, track(2).Cov,...
z, R, associate_prob(2,:), Qk, vm, wm, F2, G2, H, Ik);
end
end
else % ind00~=0
if ind01 == 0
k1 = 1;
for j=1:ind00
if measurement(i).flag(j) == 0
[z(1,k1), z(2,k1), R(:,:,k1)] = CalcMeasCov2(measurement(i).range(j), measurement(i).bearing(j), ...
measurement(i).pos(1), measurement(i).pos(2), C_r, C_b);
plot(z(1,k1), z(2,k1), 'k*');
k1 = k1 + 1;
[z(1,k1), z(2,k1), R(:,:,k1)] = CalcMeasCov2(measurement(i).range(j), measurement(i).bearing(j), ...
measurement(i).pos(1), measurement(i).pos(2), C_r, C_b);
k1 = k1 + 1;
else
[z(1,k1), z(2,k1), R(:,:,k1)] = CalcMeasCov2(measurement(i).range(j), measurement(i).bearing(j), ...
measurement(i).pos(1), measurement(i).pos(2), C_r/sqrt(12), C_b/sqrt(12));
plot(z(1,k1), z(2,k1), 'c*');
k1 = k1 + 1;
end
end
cost = zeros(2, k1);
for j=1:k1-1
% form the cost function for 2-D assignment
cost(1,j+1) = lr_kalman(track(1).X, track(1).Cov, z(:,j), Qk, R(:,:,j), vm, wm, F, G, H, Ik, Pd, lambda);
cost(2,j+1) = lr_kalman(track(2).X, track(2).Cov, z(:,j), Qk, R(:,:,j), vm, wm, F, G, H, Ik, Pd, lambda);
end
associate_prob = myLinProg(-cost);
if isempty(associate_prob)
[track(1).X, track(1).Cov] = prekalman(track(1).X, track(1).Cov,vm,Qk,F,G);
[track(2).X, track(2).Cov] = prekalman(track(2).X, track(2).Cov,vm,Qk,F,G);
else
[track(1).X, track(1).Cov] = pdakalman(track(1).X, track(1).Cov,...
z, R, associate_prob(1,:), Qk, vm, wm, F, G, H, Ik);
[track(2).X, track(2).Cov] = pdakalman(track(2).X, track(2).Cov,...
z, R, associate_prob(2,:), Qk, vm, wm, F, G, H, Ik);
end
[track(1).est(:,i+1), track(1).P(:,:,i+1)] = prekalman(track(1).X, track(1).Cov,vm,Qk,F2,G2);
[track(2).est(:,i+1), track(2).P(:,:,i+1)] = prekalman(track(2).X, track(2).Cov,vm,Qk,F2,G2);
else % ind01~=0
% form the cost for 3-D assignment
k1 = 1;
for j=1:ind00
if measurement(i).flag(j) == 0
[z1(1,k1), z1(2,k1), R1(:,:,k1)] = CalcMeasCov2(measurement(i).range(j), measurement(i).bearing(j), ...
measurement(i).pos(1), measurement(i).pos(2), C_r, C_b);
plot(z1(1,k1), z1(2,k1), 'k*');
k1 = k1 + 1;
[z1(1,k1), z1(2,k1), R1(:,:,k1)] = CalcMeasCov2(measurement(i).range(j), measurement(i).bearing(j), ...
measurement(i).pos(1), measurement(i).pos(2), C_r, C_b);
k1 = k1 + 1;
else
[z1(1,k1), z1(2,k1), R1(:,:,k1)] = CalcMeasCov2(measurement(i).range(j), measurement(i).bearing(j), ...
measurement(i).pos(1), measurement(i).pos(2), C_r/sqrt(12), C_b/sqrt(12));
plot(z1(1,k1), z1(2,k1), 'c*');
k1 = k1 + 1;
end
end
k2 = 1;
for j=1:ind01
if measurement(i+1).flag(j) == 0
[z2(1,k2), z2(2,k2), R2(:,:,k2)] = CalcMeasCov2(measurement(i+1).range(j), measurement(i+1).bearing(j), ...
measurement(i+1).pos(1), measurement(i+1).pos(2), C_r, C_b);
plot(z2(1,k2), z2(2,k2), 'k*');
k2 = k2 + 1;
[z2(1,k2), z2(2,k2), R2(:,:,k2)] = CalcMeasCov2(measurement(i+1).range(j), measurement(i+1).bearing(j), ...
measurement(i+1).pos(1), measurement(i+1).pos(2), C_r, C_b);
k2 = k2 + 1;
else
[z2(1,k2), z2(2,k2), R2(:,:,k2)] = CalcMeasCov2(measurement(i+1).range(j), measurement(i+1).bearing(j), ...
measurement(i+1).pos(1), measurement(i+1).pos(2), C_r/sqrt(12), C_b/sqrt(12));
plot(z2(1,k2), z2(2,k2), 'c*');
k2 = k2 + 1;
end
end
cost = zeros(2, k1, k2);
for j=1:k1-1
cost(1,j+1,1) = lr_kalman(track(1).X, track(1).Cov, z1(:,j), Qk, R1(:,:,j), vm, wm, F, G, H, Ik, Pd, lambda);
cost(2,j+1,1) = lr_kalman(track(2).X, track(2).Cov, z1(:,j), Qk, R1(:,:,j), vm, wm, F, G, H, Ik, Pd, lambda);
end
[tmp1X, tmp1Cov] = prekalman(track(1).X, track(1).Cov,vm,Qk,F,G);
[tmp2X, tmp2Cov] = prekalman(track(2).X, track(2).Cov,vm,Qk,F,G);
for j=1:k2-1
cost(1,1,j+1) = lr_kalman(tmp1X, tmp1Cov, z2(:,j), Qk, R2(:,:,j), vm, wm, F2, G2, H, Ik, Pd, lambda);
cost(2,1,j+1) = lr_kalman(tmp2X, tmp2Cov, z2(:,j), Qk, R2(:,:,j), vm, wm, F2, G2, H, Ik, Pd, lambda);
end
for j=1:k1-1
for k=1:k2-1
cost(1, j+1, k+1) = lr_3d_kalman(track(1).X, track(1).Cov, z1(:,j), R1(:,:,j), z2(:,k), R2(:,:,k), Qk, vm, wm, F, G, F2, G2, H, Ik, Pd, lambda);
cost(2, j+1, k+1) = lr_3d_kalman(track(2).X, track(2).Cov, z1(:,j), R1(:,:,j), z2(:,k), R2(:,:,k), Qk, vm, wm, F, G, F2, G2, H, Ik, Pd, lambda);
end
end
associate_prob = myLinProg(-cost);
if isempty(associate_prob)
[track(1).X, track(1).Cov] = prekalman(track(1).X, track(1).Cov,vm,Qk,F,G);
[track(2).X, track(2).Cov] = prekalman(track(2).X, track(2).Cov,vm,Qk,F,G);
[track(1).est(:,i+1), track(1).P(:,:,i+1)] = prekalman(track(1).X, track(1).Cov,vm,Qk,F2,G2);
[track(2).est(:,i+1), track(2).P(:,:,i+1)] = prekalman(track(2).X, track(2).Cov,vm,Qk,F2,G2);
else
prob1 = [];
prob2 = [];
prob3 = [];
prob4 = [];
% obtain the marginal pseudo-probabilities
for j=1:k1
prob1(j) = sum(associate_prob(1,j,:));
prob2(j) = sum(associate_prob(2,j,:));
end
for k=1:k2
prob3(k) = sum(associate_prob(1,:,k));
prob4(k) = sum(associate_prob(2,:,k));
end
% update tracks
[track(1).X, track(1).Cov] = pdakalman(track(1).X, track(1).Cov,...
z1, R1, prob1, Qk, vm, wm, F, G, H, Ik);
[track(2).X, track(2).Cov] = pdakalman(track(2).X, track(2).Cov,...
z1, R1, prob2, Qk, vm, wm, F, G, H, Ik);
[track(1).est(:,i+1), track(1).P(:,:,i+1)] = pdakalman(track(1).X, track(1).Cov,...
z2, R2, prob3, Qk, vm, wm, F2, G2, H, Ik);
[track(2).est(:,i+1), track(2).P(:,:,i+1)] = pdakalman(track(2).X, track(2).Cov,...
z2, R2, prob4, Qk, vm, wm, F, G, H, Ik);
end
end % if ind00 == 0 ...
end % for
plot(track(1).est(1, i+1), track(1).est(3, i+1), 'g.');
plot(track(2).est(1, i+1), track(2).est(3, i+1), 'g.');
center1 = [track(1).est(1,i+1), track(1).est(3, i+1)];
covariance1 = [track(1).P(1,1,i+1), track(1).P(1,3,i+1); track(1).P(3,1,i+1), track(1).P(3,3,i+1)];
[X1, Y1] = get_ellipsoid_gate(center1, covariance1, 1.96);
plot(X1, Y1, 'm');
center2 = [track(2).est(1,i+1), track(2).est(3, i+1)];
covariance2 = [track(2).P(1,1,i+1), track(2).P(1,3,i+1); track(2).P(3,1,i+1), track(2).P(3,3,i+1)];
[X2, Y2] = get_ellipsoid_gate(center2, covariance2, 1.96);
plot(X2, Y2, 'm');
axis([100e3,130e3,147e3,151e3]);
pause(.5);
frozen_time = measurement(i).time;
track(1).time(i+1) = measurement(i+1).time;
track(2).time(i+1) = measurement(i+1).time;
end
xlabel('x position (m)');
ylabel('y position (m)');
title('3-D soft assignment, sensor 1&2');
hold off;