stdpf.m

一个标准的粒子滤波程序

% 一维particle滤波 
% Process function: 
% x(t) = x(t-1)./2 + 25*x(t-1)./(1 + x(t-1).^2) + 8*cos(1.2*t) + w(t);
% w(t) is obey to the Guass distribution N(0,1)
% 
% Measurement function: 
% y(t) = (x(t).^2)/20 + e(t) 
% e(t) is obey to the Guass distribution N(0,10)


clear; 
N = 1000; % Number of particles 
P0 = 5; % Initial process noise covariance 
Q = 1; % Process noise covariance 
R = 10; % Measurement noise covariance 
T=100; % Step of time 

pe = inline('1/(2*pi*1)^(1/2)*exp(-(x.^2)/(2*1))'); % 表达式赋值给pe 
f = inline('x./2+25*x./(1+x.^2)+8*cos(1.2*t)','x','t'); % 表达式赋值给f 
h = inline('(x.^2)/20'); % 表达式赋值给h 

x(1) = sqrt(P0)*randn(1); % Initial state value 
y(1) = feval(h,x(1)) + sqrt(R)*randn(1); 

for t = 2:T % Simulate the system 
    x(t) = feval(f,x(t-1),t)+ sqrt(Q)*randn(1);% 计算真实值x 
    y(t) = feval(h,x(t))+ sqrt(R)*randn(1); % 计算真实值y 
end 
xTrue = x; % 真实值 

x = sqrt(P0)*randn(1,N); % Initialize the particles 

tic; 
for t = 2:T 
    x = feval(f,x,t)+sqrt(Q)*randn(1,N); 
    e = repmat(y(t),1,N) - h(x); % Calculate weights 
    q0 = feval(pe,e); % The likelihood function 
    q = q0/sum(q0); % Normalize the importance weights 

    % 重采样 
    P = cumsum(q); % 计算q的累加值，维数和q一样 


    ut(1)=rand(1)/N; 
    k = 1; 
    i = zeros(1,N); 
    for j = 1:N 
       ut(j)=ut(1)+(j-1)/N; 
         while(P(k)<ut(j)); 
          k = k + 1; 
        end; 
      i(j) = k; 
      q(j)=1/N; 
    end; 

    x = x(:,i); % The new particles 
    xp(t) = mean(x); % Compute the estimate 

end

time = toc

figure(1) 
clf; 
plot(1:T,xTrue,'b*-',1:T,xp,'r^-'); % 真实值蓝色*表示，滤波值红色三角表示 
legend('原始值','particle滤波值'); 
xlabel('Time'); 

figure(2)
clf;
plot(1:T,xTrue-xp,'b-');
xlabel('Time'); 
ylabel('Residual Error');

figure(3) 
clf; 
plot(xp,xTrue,'+'); 
hold on; 
c=-25:1:25; 
plot(c,c,'r'); 
axis([-25 25 -25 25]); 

op=std(xp-xTrue)