demo_particulaire_gaussian.m

matlab实现的粒子滤波器源代码

% Demo of  the PF algorithm for linear and gaussian model
% author : D. Le  Ruyet
% date :01/07/2006

clear all
close all

% initialization
sigma_x=0.2; % state noise
sigma_n=0.1; % observation noise
number_of_symbols=150;
NP=100; % number of particles

%signal generation
x(1)=0;
for (t=2:number_of_symbols)
   x(t)=x(t-1)+sigma_x*randn(1,1); 
end
y=x+ sigma_n*randn(1,number_of_symbols);


weight(1:NP,1:number_of_symbols)=1/NP;
x_part(1:NP,1)=randn(NP,1); % unknown initial value

for (t=2:number_of_symbols)
    x_part(:,t)=x_part(:,t-1)+ sigma_x*randn(NP,1);

    for (index=1:NP)
        pd(index,t)=exp((-1/(2*sigma_n^2))*(abs(y(t)-x_part(index,t))));
        weight(index,t)=weight(index,t-1)*pd(index,t);
    end
    % normalization
    weight(:,t)=weight(:,t)/sum(weight(:,t));
    
    % Resampling if necessary
    Neff=1/sum(weight(:,t).^2)
   
    if (Neff<NP/10)
        [x_part_new(1:NP,t),weight_new(1:NP,t),weight_n_new(1:NP,t)] =  redistribution_uniform(NP,x_part(:,t),weight(:,t));    
        x_part(1:NP,t)=x_part_new(1:NP,t);
        weight(1:NP,t)=weight_n_new(1:NP,t);
    end
   
end

result=sum(weight.*x_part);

% plotting
figure(1)
plot(x_part','b')
hold on
plot(x,'m')
figure(2)
plot(result,'b')
hold on
plot(x,'m')