| ndmtpf(Z , Xobs , X_ini_part , P_ini_part , F , G , Q , R , dt , LambdaV , N , N_threshold, Tau_end , Tau_beg , V , verbose , X)
|
function [Xmean , Pcov , PI_Tau_moy , PI0t , mt , instant , N_eff] = ndmtpf(Z , Xobs , X_ini_part , P_ini_part , F , G , Q , R , dt , LambdaV , N , N_threshold, Tau_end , Tau_beg , V , verbose , X)
%
% NDMTPF Multiple-Target Particle Filter
%
% Underlaying discrete time dynamic system
%
% X_{k+1} = F_{k}X_{k} + Uobs_{k} + V_{k}, V_{k} = N(0 , Q_{k})
% Z_{k} = H(X_{k}) + W_{k}, W_{k} = N(0 , R_{k})
%
%
% Usage
% -----
% [Xmean , Pcov , PI_Tau_moy , PI0t , mt , instant , N_eff]= ndmtpf(Z , Xobs , X_ini_part , P_ini_part , F , G , Q , R ...
% , dt , LambdaV , N , N_threshold, Tau_end , Tau_beg , V , verbose , X)
%
% Inputs
% ------
%
% Z Measurement, Matrix (sum(mt,t=1,t=T)*T x (nz + 2)), T is the
% number of temporal observations, nz measurement dimension.
% nz = {1 , 2}, i.e. {bearings , bearings + range}
%
% Xobs Observer maneuverer (d x 1 x T). Xobs(1 , :) represents the
% X-axis and Xobs(2 , :) Y-axis. d is the state dimension at
% least >=2.
%
% X_ini_part Initialization of the PF, (d x 1 x M), M is the number of
% targets eventually present in the observation Z.
%
% P_ini_part Initial covariance matrix of the PF (d x d x M)
%
% F State transition matrix (d x d x M)
%
% G Covariance state process matrix dependend of the time (d x d x M x T)
%
% Q Covariance state process matrix independend of the time (d x d x M)
%
% R Measurement covariance matrix (nz x nz) or (nz x nz x M) or
% (nz x nz x M x T)
%
% dt Sampling time vector (T - 1 x 1)
%
% LambdaV Mean number of false alarms per Volume unit
%
% Optionnal entries
%
% N Number of particles (default = 1000)
%
% N_threshold Particle redistribution threshold (default = N/20);
%
% Tau_end Number of iteration of the Gibbs sampleur (default = 1000);
%
% Tau_beg Burn-in number of iteration of the Gibbs sampleur (default = 200)
%
% V Elementary volume (default = [-pi ; pi], nz = 1)
%
% verbose 0 none display , 1 = 2D display , 2 = 3D display , >2 verbose display (dfaut = 1)
%
% X Trues sources (d x 1 x M x T ) (For the display)
%
% Outputs
% -------
%
% Xmean Sources estimated (d x 1 x M x T)
%
% Pcov Sources covariance matrices (d x d x M x T)
%
% PI_Tau_moy Measurement probabilities association to a target estimated via the Gibbs sampleur (T x M)
%
% PI0t Measurement probabilities association to a false alarm (T x 1)
%
% mt Number of measurement at time t (T x 1)
%
% instant Particles redistribution instant
%
% N_eff 1/sum(weight).^2, (T x 1)
%
%
% Ref : " Sequential Monte-Carlo methods for multple target tracking and data fusion"
% --- Carine. Hue, Jean-Pierre Le Cadre and Patrick Perez, IEEE AES, Vol 50, no 2, 2002
%
%
% SEE Also PR_PIOt, LIKELIHOOD_BOT, LIKELIHOOD_PI
%
%
%
% *************************************************************************%
% Author Sbastien PARIS (sebastien.paris@lsis.org), Septembre 2002 %
%%*************************************************************************%
%-------------- Traitement valeurs par dfaut ------------------
if(nargin < 11)
N = 1000;
end
if(nargin < 12)
N_threshold = N/10;
end
if(nargin < 13)
Tau_end = 1000;
end
if(nargin < 14)
Tau_beg = 200;
end
if(nargin < 15)
V = 2*pi;
end
if(nargin < 16)
verbose = 1;
end
sources = 1;
if (nargin < 17)
sources = 0;
end
warning off
%-------------------------------------------------------------------------
[nx , a , T] = size(Xobs);
[nx , a , M] = size(X_ini_part);
[le_Z , nz2] = size(Z);
if (nx <2)
error('State dimension must be at least equal to 2 !!!');
end
if (nz2 >4)
error('Measurement dimension must be less equal to 3 !!!');
end
%------------------ Pr-allocation des tailles des matrices ------------------
X_part = zeros(nx , 1 , N , M); % particules l'instant k, i.e. X_{k|k}^{i}
Xmean = zeros(nx , 1 , M , T);
Pcov = zeros(nx , nx , M , T);
ON = ones(N , 1);
OM = ones(M , 1);
Onx = ones(nx , 1);
N_eff = zeros(1 , T);
PI_Tau_moy = zeros(M , T);
std_Tau = zeros(M , T);
ind_X1 = [2 , 1 , 3 , 4];
ind_X2 = [2 , 3 , 4 , 1];
ind_X3 = [1 , 2 , 4 , 3];
ind_Pcov = [2 , 1 , 3];
ind_N = (1 : N)';
ind_nz = (2 : nz2 - 1);
H = [1 0 0 0 ; 0 1 0 0];
cte = 1/N;
cteN1 = 1/(N - 1);
cteN = cte(ON , 1);
q_part = cteN; % poids des particules (N x 1)
instant = [];
compteur = 1;
temps = cumsum(dt);
temps_total = temps(end);
%%%%%%%%----------- Number of measurements per scan -------------%%%%%%%%%%%
mt = diff(find(diff([0 ; Z(: , 1) ; 1]))); %(vecteur T x 1);
%------------------ Computation of PI_t^0 ---------------
PI0t = pr_PI0t(mt , LambdaV); %(vecteur T x 1);
%----- Initialization of the initial particules --------%
X_part = X_ini_part(: , : , : , ON) + ndtimes(permute( ndchol(P_ini_part) , ind_X1) , randn(nx , 1 , M , N));
%----- Itrations versus t --------
for t = 1 : T
%---------------------------- 1) Prediction step Xt_part => Xt_pred --------------------
X_part = ndtimes( F( dt(t) ) , X_part ) + ndtimes(permute( ndchol( G( dt(t) ).*Q) , ind_X1) , randn(nx , 1 , M , N));
%--------------------------- 2) Retrieve the measurement at time t -------------------------------
ytj = Z(Z(: , 1) == t , ind_nz); % Rcupration des mesures (mt x nz)
%--------------------------- 3) Estimation of PI_{t}^{i} via a Gibbs Sampler -------------------------
[vraisemblance , PI_Tau_moy(: , t)] = likelihood_bot(ytj , PI0t(t) , Xobs(: , t) , q_part , X_part , R(: , : , : , t) , V , Tau_end , Tau_beg );
%--------------------------- 5) Update particule's weight -------------------------
q_part = q_part.*vraisemblance;
q_part = q_part./sum(q_part , 1);
%--------------------------- 6) MMSE estimate & covariance -------------------------
[Xmean(: , : , : , t) , Pcov(: , : , : , t )] = part_moment(permute(X_part , [1 4 3 2]) , q_part);
%--------------------------- 7) Particles redistribution ? if N_eff < N_threshold -------------------------
N_eff(t) = 1./sum(q_part.*q_part , 1);
if (N_eff(t) < N_threshold)
instant(compteur) = t;
compteur = compteur + 1;
indice_resampling = particle_resampling(q_part);
% Recopie des particules selon le tirage des indices prcdents
X_part = X_part(: , : , : , indice_resampling);
q_part = cteN;
end
if (verbose > 0)
disp( sprintf('t = %d / %d , N = %d , Tau_beg = %d, Tau_end = %d, N_eff = %4.2f' , t , T , N , Tau_beg , Tau_end , N_eff(t)));
if (verbose == 1)
ind_t = (1 : t);
[xx , yy] = ndellipse(Xmean(: , : , : , t) , Pcov(: , : , : , t) , H , 2);
fig1 = figure(1);
%set(fig1 , 'renderer' , 'opengl');
set(fig1 , 'nextplot' , 'replacechildren' );
set(fig1 , 'DoubleBuffer' , 'on');
set(gca , 'drawmode' , 'fast');
p11 = plot(reshape(X_part(1 , : , : , :) , [M N]) , reshape(X_part(2 , : , : , :) , [M N]) , '+' , 'markersize' , 3 );
hold on;
p12 = plot(xx , yy , 'y' , 'markersize' , 4 , 'linewidth' , 2);
p13 = plot( reshape(Xmean(1 , : , : , ind_t) , [M , t])' , reshape(Xmean(2 , : , : , ind_t) , [M , t])' , '-+' , 'markersize' , 2 , 'linewidth' , 1);
p14 = plot( reshape(Xobs(1 , : , ind_t) , [1 t]) , reshape(Xobs(2 , : , ind_t) , [1 t]) , 'm-+' , 'markersize' , 3 , 'linewidth' , 2 );
p15 = plot( squeeze(Xmean(1 , : , : , instant))' , squeeze(Xmean(2 , : , : , instant))' , '^' , 'color' , [0.34 0.1 0.76] , 'markersize' , 4 , 'linewidth' , 2 );
%p15 = plot( reshape(Xmean(1 , : , : , instant) , [1 , M])' , reshape(Xmean(2 , : , : , instant) , [1 ,M])' , '^' , 'color' , [0.34 0.1 0.76] , 'markersize' , 4 , 'linewidth' , 2 );
if (sources)
p16 = plot(reshape(X(1 , : , : , ind_t) , [M , t] )' , reshape(X(2 , : , : , ind_t ) , [M , t])' , 'markersize' , 3 , 'linewidth' , 2 );
end
hold off;
axis([-500 10500 -4500 2500]) % Dfinition du domaine d'tat %
xlabel('X(m)' , 'fontsize' , 11, 'fontname' , 'times' )
ylabel('Y(m)' , 'fontsize' , 11, 'fontname' , 'times' )
title(['t = ' int2str(t) '/' int2str(T) ', N = ' int2str(N) ', \tau_{beg} = ' int2str(Tau_beg) ', \tau_{end} = ' int2str(Tau_end)...
', \lambdaV = ' num2str(LambdaV) ', N_{eff} = ' num2str(N_eff(t) , '%4.2f') ', N_{thres} = ' num2str(N_threshold) ], 'fontsize' , 9, 'fontname' , 'times')
if(sources)
p110 = legend([p11 ; p12(end) ; p13 ; p16 ; p14 ; p15] , ['Part M=1'] , ['Part M=2'] , ['Part M=3'] , ...
['95% Part'] , ['X_{moy} M=1'] , ['X_{moy} M=2'], ['X_{moy} M=3'] , ['X_{vrai} M=1'] , ['X_{vrai} M=2'], ['X_{vrai} M=3'] , ['Obs'] , ['Redis'] );
else
p110 = legend([p11 ; p12(end) ; p13 ; p14 ; p15] , ['Part M=1'] , ['Part M=2'] , ['Part M=3'] , ['95% Part'] , ['X_{moy} M=1'] , ['X_{moy} M=2'], ['X_{moy} M=3'] , ['Obs'] , ['Redis'] );
end
set(p110 , 'fontsize' , 7 , 'fontname' , 'times');
grid on
%---------- Axe des mesures -------------- %
ax1 = axes('position', [0.635 , 0.137 , 0.25 , 0.24]); % axe des mesures
tt = sum(mt(ind_t));
ind_tt = (1:tt);
plot(temps(Z(ind_tt , 1) ) , Z(ind_tt , 2) , 'b+' , 'markersize' , 1 )
axis([1 temps_total -pi +pi])
axis ij
axis on
set(ax1 , 'fontsize' , 6)
set(ax1 , 'Ytick' , [-pi -pi/2 0 pi/2 pi]');
set(ax1 , 'Yticklabel' , {'-pi' ; '-pi/2' ;'0'; 'pi/2'; 'pi'})
%---------- Axe des PI -------------- %
ax2 = axes('position', [0.635 , 0.165 + 0.24 , 0.25 , 0.24]); % axe des mesures
ttt = temps(ind_t);
plot(ttt , PI_Tau_moy(: , ind_t) , 'markersize' , 1);
%std_Tau(: , t) = std(PI_Tau(Tau_beg : Tau_end , :))';
%errorbar(ttt(: , OM) , PI_Tau_moy(1:t , :) , std_Tau(1:t , :) );
axis([1 temps_total 0 1])
set(ax2 , 'fontsize' , 6);
end
if (verbose == 2)
H = [1 0 0 0 ; 0 1 0 0];
ind_t = (1:t);
% for e = 1 : M
%
% [xx(e , :) , yy(e , :)] = ellipse(Xmean(: , : , e , t) , Pcov(: , : , e , t) , 2 , H);
%
% end
[xx , yy] = ndellipse(Xmean(: , : , : , t) , Pcov(: , : , : , t) , H , 2);
fig1 = figure(1);
set(fig1 , 'nextplot' , 'replacechildren');
set(fig1 , 'DoubleBuffer' , 'on');
set(gca , 'drawmode' , 'fast');
p11 = plot3(reshape(X_part(1 , : , : , :) , [M N])' , reshape(X_part(2 , : , : , :) , [M N])' , q_part , 'o' , 'markersize' , 2);
hold on;
p12 = plot(xx , yy , 'y' , 'markersize' , 4 , 'linewidth' , 2);
p13 = plot(reshape(Xmean(1 , : , : , ind_t) , [M , t])' , reshape(Xmean(2 , : , : , ind_t) , [M , t])' , '-+' , 'markersize' , 2 , 'linewidth' , 1);
p14 = plot(reshape(Xobs(1 , : , ind_t) , [1 , t]) , reshape(Xobs(2 , : , ind_t) , [1 , t]), 'm-+' , 'markersize' , 3 , 'linewidth' , 2 );
p15 = plot( squeeze(Xmean(1 , : , : , instant))' , squeeze(Xmean(2 , : , : , instant))' , '^' , 'color' , [0.34 0.1 0.76] , 'markersize' , 4 , 'linewidth' , 2 );
if (sources)
p16 = plot(reshape(X(1 , : , : , ind_t) , [M , t])' , reshape(X(2 , : , : , ind_t ) , [M , t])' , 'markersize' , 3 , 'linewidth' , 2 );
end
hold off;
axis([-500 8000 -3500 2100 0 0.15]) % Dfinition du domaine d'tat %
%view([33 35]);
view([82 65]);
%camva(7.7);
xlabel('X(m)' , 'fontsize' , 11, 'fontname' , 'times' )
ylabel('Y(m)' , 'fontsize' , 11, 'fontname' , 'times' )
zlabel('w_{k}^{i}' , 'fontsize' , 11, 'fontname' , 'times' )
title(['t = ' int2str(t) '/' int2str(T) ', N = ' int2str(N) ', \tau_{beg} = ' int2str(Tau_beg) ', \tau_{end} = ' int2str(Tau_end)...
', \lambdaV = ' num2str(LambdaV) ', N_{eff} = ' num2str(N_eff(t) , '%4.2f') ', N_{thres} = ' num2str(N_threshold) ], 'fontsize' , 9, 'fontname' , 'times')
if(sources)
p110 = legend([p11 ; p12(end) ; p13 ; p16 ; p14 ; p15] , ['Part M=1'] , ['Part M=2'] , ['Part M=3'] , ...
['95% Part'] , ['X_{moy} M=1'] , ['X_{moy} M=2'], ['X_{moy} M=3'] , ['X_{vrai} M=1'] , ['X_{vrai} M=2'], ['X_{vrai} M=3'] , ['Obs'] , ['Redis'] );
else
p110 = legend([p11 ; p12(end) ; p13 ; p14 ; p15] , ['Part M=1'] , ['Part M=2'] , ['Part M=3'] , ['95% Part'] , ['X_{moy} M=1'] , ['X_{moy} M=2'], ['X_{moy} M=3'] , ['Obs'] , ['Redis'] );
end
set(p110 , 'fontsize' , 7 , 'fontname' , 'times');
%set(fig1 , 'resizefcn' , '');
grid on
%---------- Axe des mesures -------------- %
ax1 = axes('position', [0.73 , 0.11 , 0.25 , 0.24]); % axe des mesures
tt = sum(mt(ind_t));
ind_tt = (1:tt);
plot(temps(Z(ind_tt , 1) ) , Z(ind_tt , 2) , 'b+' , 'markersize' , 1 )
axis([1 temps_total -pi +pi])
axis ij
axis on
set(ax1 , 'fontsize' , 6)
set(ax1 , 'Ytick' , [-pi -pi/2 0 pi/2 pi]');
set(ax1 , 'Yticklabel' , {'-pi' ; '-pi/2' ;'0'; 'pi/2'; 'pi'})
%---------- Axe des PI -------------- %
ax2 = axes('position', [0.73 , 0.142 + 0.24 , 0.25 , 0.24]); % axe des mesures
ttt = temps(ind_t);
plot(ttt , PI_Tau_moy(: , ind_t) , 'markersize' , 1);
%std_Tau(: , t) = std(PI_Tau(Tau_beg:Tau_end , :))';
%errorbar(ttt(: , OM) , PI_Tau_moy(1:t , :) , std_Tau(1:t , :) );
axis([1 temps_total 0 1])
set(ax2 , 'fontsize' , 6)
end
drawnow
end
end
warning on
|
|
