Particle Filter 粒子滤波 MATLAB程序 Sampling Importance Resampling

clear all  
close all  
clc  
%% initialize the variables  
x = 0.1; % initial actual state  
x_N = 1; % 系统过程噪声的协方差  （由于是一维的，这里就是方差）  
x_R = 1; % 测量的协方差  
T = 75;  % 共进行75次  
N = 100; % 粒子数，越大效果越好，计算量也越大  
  
%initilize our initial prior particle distribution as a gaussian around  
%the true initial value  
  
V = 2; %初始分布的方差  
x_P = []; % 粒子  
% 用一个高斯分布随机的产生初始的粒子  
for i = 1:N  
    x_P（i） = x + sqrt（V） * randn;  
end  
  
z_out = [x^2 / 20 + sqrt（x_R） * randn];  %实际测量值  
x_out = [x];  %the actual output vector for measurement values.  
x_est = [x]; % time by time output of the particle filters estimate  
x_est_out = [x_est]; % the vector of particle filter estimates.  
  
for t = 1:T  
    x = 0.5*x + 25*x/（1 + x^2） + 8*cos（1.2*（t-1）） +  sqrt（x_N）*randn;  
    z = x^2/20 + sqrt（x_R）*randn;  
    for i = 1:N  
        %从先验p（x（k）|x（k-1））中采样  
        x_P_update（i） = 0.5*x_P（i） + 25*x_P（i）/（1 + x_P（i）^2） + 8*cos（1.2*（t-1）） + sqrt（x_N）*randn;  
        %计算采