www.pudn.com > stdpf1.rar > stdpf1.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); 
% 
% 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)