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