首页 > 代码库 > 一维粒子滤波纯代码

一维粒子滤波纯代码

%一维粒子滤波代码
%状态方程: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);

 

一维粒子滤波纯代码