stdpf1.m

标准的粒子滤波器matlab程序

% 一维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);
%
% Measurement function:
%           y(t) = (x(t).^2)/20 + e(t)
%
% Date: 3/31/2006

clear;
N = 1000;                 % Number of particles
P0 = 5;                   % Initial process noise covariance
Q = 10;                   % Process noise covariance
R = 1;                    % 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(xp,xTrue,'+');
hold on;
c=-25:1:25;
plot(c,c,'r');
axis([-25 25 -25 25]);


op=std(xp-xTrue)