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