6.upf.m
% 用UKF改进的粒子滤波算法--UPF% 用UKF产生建议分布% 输入参数说明：%Xiset是上t-1时刻的粒子集合 ， Z是t时刻的观测%Pin对应Xiset粒子集合的方差\% 输出参数说明：%Xo是upf算法最终的估计结果%Xoset是k时刻的粒子集合 ， 其均值就是Xo%Pout是Xoset对应的方差function [Xo,Xoset,Pout]=upf(Xiset,Z,t,Pin,N,R,Qukf,Rukf,g1,g2)resamplingScheme=1;Xukf=ones(1,N);Xset_pre=ones(1,N);Zpre=ones(1,N); for i=1:N[Xukf(i),Pout(i)]=ukf(Xiset(i),Z,Pin(i),Qukf,Rukf,t);Xset_pre(i) = Xukf(i) + sqrtm(Pout(i))*randn;endfor i=1:NZpre(i) = feval('hfun',Xset_pre(i),t);lik = inv(sqrt(R)) * exp(-0.5*inv(R)*((Z-Zpre(i))^(2)))+1e-99;prior = ((Xset_pre(i)-Xiset(i))^(g1-1)) * exp(-g2*(Xset_pre(i)-Xiset(i)));proposal = inv(sqrt(Pout(i))) * ...exp(-0.5*inv(Pout(i)) *((Xset_pre(i)-Xukf(i))^(2)));w(i) = lik*prior/proposal;end;w = w./sum(w);if resamplingScheme == 1outIndex = residualR(1:N,w');elseif resamplingScheme == 2outIndex = systematicR(1:N,w');elseoutIndex = multinomialR(1:N,w');end;Xoset = Xset_pre(outIndex);Pout = Pout(outIndex);Xo = mean(Xoset);%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
7.ffun.m
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 状态方程函数function [y] = ffun(x,t);if nargin < 2error('Not enough input arguments.'); endbeta = 0.5;y = 1 + sin(4e-2*pi*t) + beta*x;
8.hfun.m
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 观测方程函数function [y] = hfun(x,t);if nargin < 2, error('Not enough input arguments.');endif t<=30y = (x.^(2))/5;elsey = -2 + x/2;end;
9..m
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 产生一个符合gamma分布的噪声function x = gengamma(alpha, beta)if (alpha==1)x = -log(1-rand(1,1))/beta;returnendflag=0;if (alpha<1)flag=1;alpha=alpha+1;endgamma=alpha-1;eta=sqrt(2.0*alpha-1.0);c=.5-atan(gamma/eta)/pi;aux=-.5;while(aux<0)y=-.5;while(y<=0)u=rand(1,1);y = gamma + eta * tan(pi*(u-c)+c-.5);endv=-log(rand(1,1));aux=v+log(1.0+((y-gamma)/eta)^2)+gamma*log(y/gamma)-y+gamma;end;if (flag==1) x = y/beta*(rand(1))^(1.0/(alpha-1));elsex = y/beta;end
10..m
function [xPts, wPts, nPts] = scaledSymmetricSigmaPoints(x,P,alpha,beta,kappa)n= size(x(:),1);nPts = 2*n+1;kappa = alpha^2*(n+kappa)-n;wPts=zeros(1,nPts);xPts=zeros(n,nPts);Psqrtm=(chol((n+kappa)*P))';xPts=[zeros(size(P,1),1) -Psqrtm Psqrtm];xPts = xPts + repmat(x,1,nPts);wPts=[kappa 0.5*ones(1,nPts-1) 0]/(n+kappa);wPts(nPts+1) = wPts(1) + (1-alpha^2) + beta;%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
11..m
%% 多项式重采样function outIndex = multinomialR(inIndex,q);error('下面的参数nargin请参考书中的值设置 ， 然后删除本行代码') if nargin < 0, error('Not enough input arguments.'); end[S,arb] = size(q);N_babies= zeros(1,S);cumDist= cumsum(q');u = fliplr(cumprod(rand(1,S).^(1./(S:-1:1))));j=1;for i=1:Swhile (u(1,i)>cumDist(1,j))j=j+1;endN_babies(1,j)=N_babies(1,j)+1;end;index=1;for i=1:Sif (N_babies(1,i)>0)for j=index:index+N_babies(1,i)-1outIndex(j) = inIndex(i);end;end;index= index+N_babies(1,i);end
12..m
%% 随机重采样function outIndex = residualR(inIndex,q);if nargin < 2, error('Not enough input arguments.'); end[S,arb] = size(q); N_babies= zeros(1,S);q_res = S.*q';N_babies = fix(q_res);N_res=S-sum(N_babies);if (N_res~=0)q_res=(q_res-N_babies)/N_res;cumDist= cumsum(q_res);u = fliplr(cumprod(rand(1,N_res).^(1./(N_res:-1:1))));j=1;for i=1:N_reswhile (u(1,i)>cumDist(1,j))j=j+1;endN_babies(1,j)=N_babies(1,j)+1;end;end;index=1;for i=1:Sif (N_babies(1,i)>0)for j=index:index+N_babies(1,i)-1outIndex(j) = inIndex(i);end;end;index= index+N_babies(1,i);end
13..m
%% 系统重采样function outIndex = systematicR(inIndex,wn);error('下面的参数nargin 请参考书中的值设置 ， 然后删除本行代码')if nargin