pf_nav.m

本文件采用Matlab软件

function [X_pf , Pcov , resZ , resE , compteur] = pf_nav(Z , X1 , Q1  , dt , map , R , Hp  , F ,  U , Q  , N , N_threshold , Y_min , Y_max , X_min , X_max , verbose , Y_traj , Z_traj);

% Particle fiter for navigation associated with the following dynamic
% system :
%
% X_1       = N(X1 , Q1)
% X_{k+1}   = FkXk + Uk + N(0 , Qk)
% Zk        = h(Xk) + N(0 , Rk(Xk))
%
%
% Usage
% ------
%
% [X_pf , Pcov , resZ , resE , compteur] = pf_nav(Z , X1 , Q1 , dt , map , R , Hp  , Q1 , F , U , Q , [N] , [N_threshold] , [Y_min] , [Y_max] , [X_min] , [X_max] , [verbose] , [Y_traj] , [Z_traj]);
%
%
% Inputs
% -------
%
% Z               Measurements (m x K)
% X1              Initial state vector (d x 1)
% Q1              Initial state covariance matrix (d x d)
% dt              Time step increment (1 x K-1)
% map             Measurement map (nR x nC x m)
% R               Covariance measurement map (nR x nC x m x m)
% Hp              Projection matrix (2 x d) to retrieve position
% F               State transition matrix inlined (d x d x K-1)
% U               Command low (d x K-1)
% Q               State covariance matrix inlined (d x d x K-1)
% N               Number of particules (default N = 10000)
% N_threshold     Threshold for particules redistribution (default 0.85*N)
% Y_min           Minimum Y value (default 1)
% Y_min           Maximum Y value (default nR)
% X_min           Minimum X value (default 1)
% X_min           Maximum X value (default nC)
% verbose         Verbose (0 no display, 1 display map, 2 display map + measurements
% Y_traj          True Trajectory (for display if verbose >= 1)
% Z_traj          True Measurement (for display if verbose = 2)
%
% Outputs
% -------
%
% X_pf            Estimated state sequence by particle filter (d x K)
% Pcov            Estimated covariance of the particles (d x d x K)
%
%
%
%  Author          S閎astien PARIS ([e-mail]) (5/4/08)
%  -------



%% ------------------ Parse inputs --------------------- %%

if (nargin < 10)
    
    error('Not enougth parameters for pf_nav function');
    
end


warning off

display_X_traj      = 1;
[nR , nC , m]       = size(map);                          % Measurement map
[d1 , d2 , d3 , d4] = size(R);                            % Covariance Measurement map (nR x Nc x m x m)

if ((d1 ~= nR) || (d2 ~= nC) || (d3 ~= m) || (d4 ~= m))
    
    error('map must be (nR x nC x m) and R (nR x nC x m x m)');
    
end


alpha             = 2.4477;                             % sqrt(chi2inv(0.95 , 2))


[d , K]           = size(U);                            % Law control
K                 = K + 1;

if (nargin < 17)
    
    verbose       = 0;
    
end

if (nargin < 19)
    
    verbose       = min(verbose , 1);
    
end

if (nargin < 18)
    
    display_X_traj    = 0;
    
end

if (nargin < 16)
    
    X_max = nC;

end

if (nargin < 15)
    
    X_min = 1;

end

if (nargin < 14)
    
    Y_max = nR;

end

if (nargin < 13)
    
    Y_min = 1;

end

if (nargin < 11)
    
    N     = 100000;

end

if (nargin < 12)
    
    N_threshold     = (8.5*N/10);

end



if (verbose > 0)
    
    time          = [0 , cumsum(dt)];
    diff_Y        = abs(Y_max - Y_min);
    diff_X        = abs(X_max - X_min);
    pente_Y       = diff_Y/(nR - 1);
    pente_X       = diff_X/(nC - 1);
    vect_X        = (X_min : pente_X : X_max);
    vect_Y        = (Y_min : pente_Y : Y_max);
    min_Z         = min(Z(:));
    max_Z         = max(Z(:));
    fig1          = figure(1);
    set(fig1 , 'renderer' , 'zbuffer' , 'doublebuffer' , 'on' , 'nextplot' , 'replacechildren' );
    bright        = 0.1;
    colmap        = flipud(pink);
    
end


if (verbose > 1)
    
    fig2             = figure(2);
    set(fig2 , 'renderer' , 'zbuffer' , 'doublebuffer' , 'on' , 'nextplot' , 'replacechildren' );    

end


ON                    = ones(1 , N);
cte                   = 1/N;
cteN                  = cte(1 , ON);
cteE                  = 1/(2*pi)^(m*0.5);
tiny                  = 10e-99;
w                     = cteN;
compteur              = 0;


%% --------------------- Memory allocation --------------% %


X_pf                  = zeros(d , K);
Pcov                  = zeros(d , d , K);
resZ                  = zeros(1 , K);
resE                  = zeros(1 , K);
N_eff                 = zeros(1 , K);
Z_pf                  = zeros(m , K);
PcovZ                 = zeros(m , m , K);

%% ----------------------------- Initialisation  k = 1    ------------------- %%

k                               = 1;

X_part                          = X1(: , ON)  + chol(Q1)'*randn(d , N );

%% ------------------ Initial Likelihood for k = 1--------------------% %

[like , Zi]                     = likelihood_nav(Z(k) , map , R , X_part , Hp , X_min , X_max , Y_min , Y_max);

%% ------------------ Normalize weights ------------------------- %%

w                               = w.*like;
w                               = w/sum(w);
N_eff(k)                        = 1./sum(w.*w);


%% ---------------------- State and Measurements Mean & covariance ------ %%


[X_pf(: , k) , Pcov(: , : , k)]  = part_moment(X_part , w);
[Z_pf(: , k) , PcovZ(: , : , k)] = part_moment(Zi , w);

%[Z_pf , PcovZ]                   = part_moment(Zi , cteN);

temp                             = (Z(: , k) - Z_pf(: , k));
resZ(k)                          = temp'*inv(PcovZ(: , : , k))*temp;
resE(k)                          = (cteE/sqrt(abs(det(PcovZ(: , : , k)))))*exp(-0.5*resZ(k)) + tiny;


%% ------------------ Display ---------------------------- %%

if (verbose > 0)
    
    figure(fig1) 
    imagesc(vect_X  , vect_Y , map);
    hold on
    [x , y] = ndellipse(X_pf(: , 1) , Pcov(: , : , 1) , Hp , alpha);
    Ypart   = Hp*X_part;
    Ypf     = Hp*X_pf(: , 1);
    plot(Ypart(1 , :) , Ypart(2 , :) , 'b+');
    plot(Ypf(1 , 1) , Ypf(2 , 1) , 'm-^' , 'linewidth' , 1 ,  'markersize' , 3)
    plot(Ypart(1 , :) , Ypart(2 , :) , 'b+');
    plot(x , y , 'k' , 'linewidth' , 2 )
    if(display_X_traj)
        plot(Y_traj(1 , 1) , Y_traj(2 , 1) , 'g-o' , 'linewidth' , 1 , 'markersize' , 3)
    end
    hold off
    colormap(colmap);  
    brighten(bright)
    axis xy;
    title(sprintf('k = %d/%d, %6.2f/%6.2f' , 1 , K , N_eff(1) , N_threshold));
    
end

if(verbose  > 1)
      
    figure(fig2)
    plot(0 , Z(: , 1) , 0 , Z_pf(: , k) , 'r' , 0 , Z_traj(: , k) , 'g' )
    title(sprintf('dist(Z_{pf},Z_{true}) = %5.5f' , resZ(k)));
    axis([0 , time(end) , min_Z , max_Z])
   
end

drawnow


%% ----------------------------- Main Loop  k = 2,...,K    ------------------ %%

for k = 2 : K 
    
    %% ----------------------------   Draw X_part -------------------------- %%
    
    tp_U                             = U(: , k - 1);
        
    X_part                           = F(: , : , k - 1)*X_part + tp_U(: , ON) + chol(Q(: , : , k - 1))'*randn(d , N ); 
    
    %% ---------------------------  LikeLihood ----------------------------- %%
    
    [like , Zi]                      = likelihood_nav(Z(k) , map , R , X_part , Hp , X_min , X_max , Y_min , Y_max);
    
    %% ------------------------ Update weights ----------------------------- %%
    
    w                                = w.*like;  
    w                                = w/sum(w);   
    N_eff(k)                         = 1./sum(w.*w);


    %% -------- State and Measurements Mean & covariance ------------------------- %%
    
    [X_pf(: , k) , Pcov(: , : , k )] = part_moment(X_part , w);
    [Z_pf(: , k) , PcovZ(: , : , k)] = part_moment(Zi , w);
    
    %[Z_pf , PcovZ]                   = part_moment(Zi , cteN);
    
    temp                             = (Z(: , k) - Z_pf(: , k)); 
    resZ(k)                          = temp'*inv(PcovZ(: , : , k))*temp;
    resE(k)                          = (cteE/sqrt(abs(det(PcovZ(: , : , k)))))*exp(-0.5*resZ(k)) + tiny;
    
    
    %% ----- Particles redistribution ? if N_eff < N_threshol -------- %%
    
    
    if (N_eff(k) < N_threshold)
        
        compteur                     = compteur + 1;     
        indice_resampling            = particle_resampling(w);
        
        % Copy particles given resampling results
        
        X_part                       = X_part(: , indice_resampling);
        w                            = cteN;
        
    end
    
    if (verbose > 0)
        
        figure(fig1)        
        imagesc(vect_X  , vect_Y , map);
        ind_k   = (1 : k);
        hold on
        [x , y] = ndellipse(X_pf(: , k) , Pcov(: , : , k) , Hp , alpha);
        Ypart   = Hp*X_part;
        Ypf     = Hp*X_pf(: , ind_k);
        plot(Ypart(1 , :) , Ypart(2 , :) , 'b+');
        plot(Ypf(1 , ind_k) , Ypf(2 , ind_k) , 'm-^' , 'linewidth' , 1  , 'markersize' , 3)
        plot(x , y , 'k' , 'linewidth' , 2 )
        if(display_X_traj)
            plot(Y_traj(1 , ind_k) , Y_traj(2 , ind_k) , 'g-o' , 'linewidth' , 1 , 'markersize' , 3)
        end        
        hold off
        colormap(colmap);
        brighten(bright)
        axis xy;
        title(sprintf('k = %d/%d, %6.2f/%6.2f, Redist = %d' , k , K , N_eff(k) , N_threshold , compteur));
        
    end
    
    if (verbose > 1)
        
        figure(fig2) 
        plot(time(ind_k) , Z(: , ind_k) , time(ind_k) , Z_pf(: , ind_k) , 'r' , time(ind_k) , Z_traj(: , ind_k) , 'g')
        axis([0 , time(end) , min_Z , max_Z])
        title(sprintf('dist(Z_{pf},Z_{true}) = %5.5f' , resZ(k)));
        
    end
    
    drawnow
    
    
end

%%


warning on