%一维粒子滤波代码
%状态方程：x(k)=x(k-1)/2+25*x(k-1)/(1+x(k-1)^2)+8cos(1.2(k-1))+vk;vk为噪声
%测量方程:y(k)=x(k)^2/20+nk;nk为噪声
%初始化时的状态 
x0=0.1;
%过程噪声的协方差，且其均值为0
Q=1;
%测量噪声的协方差，且其均值为0
R=1;
%仿真步数
simu_steps=70;
%粒子滤波中的粒子数
N=100;
%初始化分布的方差
V=2;
%初始化粒子滤波的估计值与初始状态一致
x_estimate=x0;
%用一个高斯分布随机的产生初始的粒子
%randn产生标准正太分布的随机数，其实它就是在x状态附近做一个随机样本抽样的过程，在这里x为均值，P为方差
for i=1 : N
    %用于存储当前采样到的粒子集的数组
    current_particle(i)=x0+sqrt(V)*randn;
end
%用于存储系统状态方程计算得到的每一步的状态值，此为数组
x_state=[x0];
%用于存储量测方程计算得到的每一步的状态值，也为数组
y_measure=[x0^2/20+sqrt(R)*randn];
%用于记录粒子滤波每一步估计的粒子均值（该均值即为对对应步状态的估计值），此为一个数组
x_estimate_Arr=[x_estimate];


%close all 是关闭所有窗口(程序运行产生的，不包括命令窗，editor窗和帮助窗）
close all;
%为方便下面进行for循环，而不用使用变量x0
x=x0;
for k=1:simu_steps
    %从状态方程当中获取下一时刻的状态值（称为预测）
    x=0.5*x+25*x/(1+x^2)+8*cos(1.2*(k-1))+sqrt(Q)*randn;
    %在当前状态下，通过观测方程获取的观测量的值
    y=x^2/20+sqrt(R)*randn;
    for i=1 : N
        %从先验分布（在这里当做粒子滤波中的重要性分布）p(x(k)|x(k-1))中采样,利用之前生成的粒子样本集current_particle(i)带入状态方程中，记做数组next_particle(i)
        next_particle(i) = 0.5 * current_particle(i)+25 * current_particle(i) / (1+(current_particle(i))^2)+8 * cos(1.2 * (k-1)) + sqrt(Q) * randn;
        %已经采样到了新的粒子，那么如何来计算每个粒子的权重呢，那么肯定要与观测量有关系，则将新的样本粒子中的粒子分别带入观测方程当中
        %计算出通过该粒子而预测出该粒子的量测值
        y_measure_particle=next_particle(i)^2/20;
        %由于上面已经计算出第i个粒子，带入观测方程后的预测值，现在与真实的测量值y进行比较，越接近则权重越大，或者说差值越小权重越大
        %这里的权重计算是关于p(y/x)的分布，即观测方程的分布，假设观测噪声满足高斯分布，那么particle_w=p(y/x)
        particle_w(i)=(1/sqrt(2*pi*R))*exp(-(y-y_measure_particle)^2/(2*R));
    end
    %将权值归一化,符号./是指两个矩阵对应元素相除，现在权值particle_w已经归一化了
    particle_w=particle_w./sum(particle_w);
    
    %下面进行重采样
    for i=1:N
        %用rand函数来产生在0到1之间服从均匀分布的随机数，用于找出归一化后权值较大的粒子
        u=rand;
        %在这里归一化后的权值太小了，很难单个粒子的权值会大于u=rand产生的随机数，这里用累加的方式来获得具有较大权值的粒子
        %临时权值求和变量particle_w_sum
        particle_w_sum=0;
        for j=1:N
            particle_w_sum=particle_w_sum+q(j);
            %如果particle_w_sum大于等于u，则将该权值的粒子保留下来
            if particle_w_sum>=u
                current_particle(i)=next_particle(i);
                %如果找到这样的大权值粒子，则退出，寻找下一个粒子，别忘了，在每一次寻找粒子的时候，都是从头到尾的
                %故可能会保留到重复的粒子,所以在这里容易造成粒子样本枯竭，即粒子的多样性失去，只剩下一个大的粒子，在不断的复制自己
                break;
            end
        end
    end
    %粒子滤波的状态估计，利用重采样以后的粒子计算其均值，那么得到的这个均值就是当前步粒子滤波对状态的估计值
    %x_estimate=mean(current_particle);怀疑取均值是错的，应该各粒子的权值乘以对应粒子值相加，才是粒子滤波的估计值
    x_estimate=sum(current_particle .* particle_w);
    %将上面的数据保存下来，以便之后的绘图
    %这是记录状态每一步值的数组，始终将新产生的x值插入数组
    x_state=[x_state x];
    %记录测量值的数组，始终将新 产生的y值插入数组
    y_measure=[y_measure y];
    %用于记录粒子滤波每一步估计的粒子均值（该均值即为对对应步状态的估计值），此为一个数组
    x_estimate_Arr=[x_estimate_Arr x_estimate];
end
%下面画图
t=0:simu_steps;
%figure

figure(1);
clf
plot(t,x_state,‘b.‘, t, x_estimate_Arr, ‘k-‘);
set(gca,‘FontSize‘,12);
set(gcf,‘Color‘,‘White‘);
xlabel(‘time step‘);
ylabel(‘state‘);
legend(‘True state‘,‘Particle filter estimate‘);