%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %
% Demo for Multitarget Bearing Only Tracking %
% %
% cf. " Sequential Monte-Carlo methods for multple target tracking and data fusion" %
% %
% Carine. Hue, Jean-Pierre Le Cadre and Patrick Perez, IEEE AES,
% Vol50, no 2, 2002
% %
% ***************************************************************************%%
% Author Sbastien PARIS (sebastien.paris@lsis.org), 09/2002 %
%% ***************************************************************************%%
clear;
close all;
boites_dialogue = 1; % 0 pas de boite de dialogue pour la saisie des paramtres, 1 boite de dialogue
verbose = 2; % 0 pas de display, 1 affichage 2D, 2 affichage 3D, >2 seulement les itrations
%---------------------------- Constantes du scnario -------------------------------%
nx = 4; % dimension de l'tat, i.e. [rx , ry , vx , vy]'.
nz = 1; % dimension de la mesure nz = 1 , 2 i.e. angle ou angle + distance
M = 3; % Nombre de sources.
N = 5000; % Nombre de particules.
T = 1000; % Nombre d'instant de mesure.
LambdaV = 1; % Nombre moyen de Fausses Alarmes par unit de volume V .
Tau_beg = 200; % Nombre d'echantillons de "Chauffe" de l'chantillonneur de Gibbs.
Tau_end = 1000; % Nombre d'itrations totales de l'chantillonneur de Gibbs.
N_threshold = N/4; % Seuil pour dcider du retirage des particules.
delta_t = 6; % Temps d'chantillonage entre chaque mesure.
%% Suppression ventuelle de sources %%
ind_D = [1]; % Numro des sources que l'on supprime de l'observation
D = cat(3 , [600 ; 800 ]); % Periodes de supprssion des sources %
if (nz == 1)
V = [-pi ; pi]; % Bearings only.
end
if (nz == 2)
V = [-pi , 0 ; pi , 500]; %[Bearings , range]
end
if(boites_dialogue)
question ={'Particles Number N' , 'Number of iterations T' , 'Mean number of False Alarm per unit \lambdaV' , 'Number of iteration of the Gibbs sampleur \tau_{end}>0' , 'Burn-in of the Gibbs sanpleur 0<\tau_{beg}<\tau_{end}' ,...
'Redistribution Threshold N_{thres}' , 'Sampling time \Deltat' , 'verbose 0: nodisplay, 1: 2D display , 2: 3D display , >2: text information' };
def = {'5000' , '1000' , '1' , '1000' , '200' , '0.5*1000' , '6' , '3'};
Titreboite = 'Simulation Parameters';
nbligne = 1;
AddOpts.Resize = 'on';
AddOpts.WindowStyle = 'normal';
AddOpts.Interpreter = 'tex';
reponse = inputdlg(question , Titreboite , nbligne , def , AddOpts);
N = str2num(char(reponse(1)));
T = str2num(char(reponse(2)));
LambdaV = str2num(char(reponse(3)));
Tau_end = str2num(char(reponse(4)));
Tau_beg = str2num(char(reponse(5)));
N_threshold = str2num(char(reponse(6)));
deltat = str2num(char(reponse(7)));
verbose = str2num(char(reponse(8)));
end
OT = ones(T , 1);
dt = delta_t(OT , :);
temps = cumsum(dt , 1);
% -------------- Mouvement de l'observateur --------------- %
Xobs_ini = [ 200 ; -3000 ; 1.2 ; 0.5]; %[m ; m ; m/s ; m/s]
sigobs_rx = 0.001; % cart - type en position x, observateur
sigobs_ry = 0.001; % cart - type en position y, observateur
Fobs = @(delta_t) [1 0 delta_t 0 ; 0 1 0 delta_t ; 0 0 1 0 ; 0 0 0 1];
Gobs = @(delta_t) diag([delta_t.^2/2 delta_t.^2/2 delta_t delta_t]);
Qobs = diag([sigobs_rx sigobs_ry sigobs_rx sigobs_ry].^2) ;
Vobs = [ -0.6 , 2.0 , -0.6 , 2.0 , -0.6 ; 0.3 , 0.3 , 0.3 , 0.3 , 0.3 ; 200 , 400 , 600 , 800 , 900 ];
Xobs = obs_mvt(Xobs_ini , Fobs , Gobs , Qobs , dt , Vobs); %(nx x 1 x T)
% -------------- Mouvement des sources (M x N) --------------- %
X1_ini = [ 200 ; 1500 ; 1 ; -0.5]; %[m ; m ; m/s ; m/s]
X2_ini = [ 0 ; 0 ; 1 ; 0]; %[m ; m ; m/s ; m/s]
X3_ini = [ -200 ; -1500 ; 1 ; 0.5]; %[m ; m ; m/s ; m/s]
X_ini = reshape([X1_ini , X2_ini , X3_ini ] , [nx , 1 , M]); %(nx x 1 x M)
P1_cov = diag([20 100 0.05 0.05].^2); % covariance initiales des particules
P2_cov = diag([20 100 0.05 0.05].^2); % covariance initiales des particules
P3_cov = diag([20 100 0.05 0.05].^2); % covariance initiales des particules
P_ini = reshape([P1_cov , P2_cov , P3_cov ] , [nx , nx , M ]);
sig_rx1 = 0.0005; % cart - type en position x (m/s), source 1
sig_ry1 = 0.0005; % cart - type en position y (m/s), source 1
sig_rx2 = 0.0005; % cart - type en position x (m/s), source 2
sig_ry2 = 0.0005; % cart - type en position y (m/s), source 2
sig_rx3 = 0.0005; % cart - type en position x (m/s), source 3
sig_ry3 = 0.0005; % cart - type en position y (m/s), source 3
G = @(delta_t) cat(3 , diag([delta_t.^2/2 delta_t.^2/2 delta_t delta_t]) , diag([delta_t.^2/2 delta_t.^2/2 delta_t delta_t]) , diag([delta_t.^2/2 delta_t.^2/2 delta_t delta_t]));
F = @(delta_t) cat(3 , [1 0 delta_t 0 ; 0 1 0 delta_t ; 0 0 1 0 ; 0 0 0 1] , [1 0 delta_t 0 ; 0 1 0 delta_t ; 0 0 1 0 ; 0 0 0 1] , [1 0 delta_t 0 ; 0 1 0 delta_t ; 0 0 1 0 ; 0 0 0 1]);
Q = reshape([ diag([sig_rx1 sig_ry1 sig_rx1 sig_ry1].^2) , diag([sig_rx2 sig_ry2 sig_rx2 sig_ry2].^2) , diag([sig_rx3 sig_ry3 sig_rx3 sig_ry3].^2) ] , [nx nx M]);
X = sources_mvt(X_ini , F , G , Q , dt); %(nx x 1 x M x T)
%---------- Gnration des mesures partir des M sources ------------
sigr = 10; % cart - type sur le bruit de mesure en distance (m/s)
siga = 0.005%0.05; % cart - type sur le bruit de mesure des angle (radians/s)
if (nz == 1)
R = diag([siga].^2);
end
if (nz == 2)
R = diag([siga , sigr].^2);
end
Y = mkmeasure(X , Xobs , R); % Mesures vraie (nz x 1 x M x T)
% ----------- Gnration du clutter ----------------
clutter = mkclutterV(T , LambdaV , V); % clutter (N_clutter x (nz + 2))
%----------- Concatnation des mesures vraies avec le clutter -----------
Z = concatenate_Yclutter(Y , clutter); % (N_Z x (nz + 2)), N_Z = N_clutter + M*T
Z = delete_sources(Z , D , ind_D , M);
%----------- Initiatialisation de l'algorithme MTPF -----------
%--- Ajout de Biais l'initialisation ----%
X1_biais = [-10 ; 100 ; 0 ; 0]; % biais pour l'initilisation des particules
X2_biais = [-10 ; -100 ; 0 ; 0];
X3_biais = [20 ; 100 ; 0 ; 0];
X_ini_part = reshape([X1_ini + X1_biais , X2_ini + X2_biais , X3_ini + X3_biais ] , [nx 1 M]);
P_ini_part = reshape([P1_cov , P2_cov , P3_cov ] , [nx , nx , M ]);
R = R(: , : , : , OT);
%----------- Lancement de l'algorithme -----------
t0 = clock;
[Xmean , Pcov , PI_Tau_moy , PI0t , mt , instant_redistribution] = ndmtpf(Z , Xobs , X_ini_part , P_ini_part , F , G , Q , R , dt , LambdaV , N , N_threshold, Tau_end , Tau_beg , prod(diff(V)) , verbose , X );
t1 = etime(clock , t0);
disp( ['Temps moyen par appel = ' num2str(t1/T) 's pour N = ' int2str(N) ' particules'] );
%----------- Affichage rsultat -------------- %
fig2 = figure(2);
p21 = plot(squeeze(Xmean(1 , : , : , :))' , squeeze(Xmean(2 , : , : , :))' , '-+' , 'markersize' , 2 , 'linewidth' , 1);
hold on
p22 = plot(squeeze(Xobs(1 , : , :)) , squeeze(Xobs(2 , : , :)), 'm-+' , 'markersize' , 3 , 'linewidth' , 2 );
p23 = plot(squeeze(X(1 , : , : , :))' , squeeze(X(2 , : , : , :))' , 'markersize' , 3 , 'linewidth' , 2 );
p24 = plot( squeeze(Xmean(1 , : , : , instant_redistribution))' , squeeze(Xmean(2 , : , : , instant_redistribution))' , '^' , 'color' , [0.34 0.1 0.76] , 'markersize' , 4 , 'linewidth' , 2 );
hold off;
xlabel('X(m)' , 'fontsize' , 11, 'fontname' , 'times' );
ylabel('Y(m)' , 'fontsize' , 11, 'fontname' , 'times' );
title(['N = ' num2str(N) ', \tau_{beg} = ' num2str(Tau_beg) ', \tau_{end} = ' num2str(Tau_end)...
', \lambdaV = ' num2str(LambdaV) ', N_{thres} = ' num2str(N_threshold) ], 'fontsize' , 9, 'fontname' , 'times')
p25 = legend( [p22 ; p21 ; p23 ; p24(end)] , [{'Obs'} ; cellstr(num2str((1:M)' , 'X%0.0f_{est}')) ; cellstr(num2str((1:M)' , 'X%0.0f_{vraie}')) ; {'Redistribution'} ] );
set(p25 , 'fontsize' , 8 , 'fontname' , 'times');
grid on
fig3 = figure(3);
p31 = plot(temps , [ PI_Tau_moy' , PI0t]);
p32 = legend( p31 , num2str( [(1:M)' ; 0] , '\\pi_t^{%0.0f}' ));
set(p32 , 'fontsize' , 8 , 'fontname' , 'times');
xlabel('t (s)' , 'fontsize' , 11, 'fontname' , 'times' );
ylabel('\pi_t^i' , 'fontsize' , 11, 'fontname' , 'times' );
title(['\pi_t^i, i=0,...,' num2str(M) ], 'fontsize' , 9, 'fontname' , 'times');
fig4 = figure(4);
p41 = plot(temps(Z(: , 1)) , Z(: , 2) , 'b+' , 'markersize' , 4);
axis ij
xlabel('t (s)' , 'fontsize' , 11, 'fontname' , 'times' );
ylabel('y_t' , 'fontsize' , 11, 'fontname' , 'times' );
title(['Bearings measurements' ], 'fontsize' , 9, 'fontname' , 'times');
