粒子滤波程序一看就懂

clear all;

M=10000;%粒子数

P0=5; %初始状态协方差

Q=10; %过程噪声方差

R=1; %量测方差阵

tf=150; %终止时间

pdf_v=inline('1/(2*pi*1)^(1/2)*exp(-(x.^2)/(2*1))');

f=inline('x./2+25*x./(1+x.^2)+8*cos(1.2*t)','x','t');%状态转移方程

h=inline('(x.^2)/20'); %量测方程

x(1)=sqrtm(P0)*randn(1); %初始状态值

y(1)=feval(h,x(1))+sqrtm(R)*randn(1);

for t=2:tf %系统仿真

x(t)=feval(f,x(t-1),t-1)+sqrtm(Q)*randn(1);

y(t)=feval(h,x(t))+sqrtm(R)*randn(1);

end

xTrue=x;

xhat=PF(f,h,pdf_v,Q,P0,M,y);%状态值、量测值、高斯分布、过程噪声方差、初始方差阵、粒子数、包含噪声的量测值

plot(1:tf,xhat,'b--',1:tf,xTrue,'r');

xlabel('时间');

legend('状态估计值','状态真实值');

title('粒子滤波仿真实验');

grid on;

rms=sum((xTrue-xhat).^2);

rms=sqrt(rms/tf);

function xhat=PF(f,h,pdf_v,Q,P0,M,y)

n=size(P0,2);

x=sqrtm(P0)*randn(n,M);%初始化粒子

tf=size(y,2);

for t=1:tf

e=repmat(y(t),1,M)-h(x); %计算权重

w=feval(pdf_v,e); %似然函数

w=w/sum(w);

xhat(t)=sum(repmat(w,n,1).*x,2);%归一化权值 ind=resampling(w); %重采样

x=x(:,ind); %新粒子

x=feval(f,x,t)+sqrtm(Q)*randn(n,M);%时间更新end

function [i]=resampling(w)

wc=cumsum(w);M=length(w);

u=([0:M-1]+rand(1))/M;

i=zeros(1,M);k=1;

for j=1:M

while(wc(k)

k=k+1;

end

i(j)=k;

end