2.ekf.m
function [Xekf,Pout]=ekf(Xin,Z,Pin,t,Qekf,Rekf)Xpre=feval('ffun',Xin,t);Jx=0.5;Pekfpre = Qekf + Jx*Pin*Jx';Zekfpre= feval('hfun',Xpre,t);if t<=30Jy = 2*0.2*Xpre;elseJy = 0.5;endM = Rekf + Jy*Pekfpre*Jy';K = Pekfpre*Jy'*inv(M);Xekf=Xpre+K*(Z-Zekfpre);Pout = Pekfpre - K*Jy*Pekfpre;%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
3.ukf.m
% 无迹卡尔曼滤波算法function [Xout,Pout]=ukf(Xin,Z,Pin,Qukf,Rukf,t) alpha = 1;beta= 0;kappa = 2;states= size(Xin(:),1);observations = size(Z(:),1);vNoise= size(Qukf,2);wNoise= size(Rukf,2);noises= vNoise+wNoise;if (noises)N=[Qukf zeros(vNoise,wNoise); zeros(wNoise,vNoise) Rukf];PQ=[Pin zeros(states,noises);zeros(noises,states) N];xQ=[Xin;zeros(noises,1)];elsePQ=Pin;xQ=Xin;end;[xSigmaPts, wSigmaPts, nsp] = scaledSymmetricSigmaPoints(xQ, PQ, alpha, beta, kappa);wSigmaPts_xmat = repmat(wSigmaPts(:,2:nsp),states,1);wSigmaPts_zmat = repmat(wSigmaPts(:,2:nsp),observations,1);xPredSigmaPts = feval('ffun',xSigmaPts(1:states,:),t)+xSigmaPts(states+1:states+vNoise,:);zPredSigmaPts = feval('hfun',xPredSigmaPts,t)+xSigmaPts(states+vNoise+1:states+noises,:);xPred = sum(wSigmaPts_xmat .* (xPredSigmaPts(:,2:nsp) - repmat(xPredSigmaPts(:,1),1,nsp-1)),2);zPred = sum(wSigmaPts_zmat .* (zPredSigmaPts(:,2:nsp) - repmat(zPredSigmaPts(:,1),1,nsp-1)),2);xPred=xPred+xPredSigmaPts(:,1);zPred=zPred+zPredSigmaPts(:,1);exSigmaPt = xPredSigmaPts(:,1)-xPred;ezSigmaPt = zPredSigmaPts(:,1)-zPred;PPred= wSigmaPts(nsp+1)*exSigmaPt*exSigmaPt';PxzPred = wSigmaPts(nsp+1)*exSigmaPt*ezSigmaPt';S= wSigmaPts(nsp+1)*ezSigmaPt*ezSigmaPt';exSigmaPt = xPredSigmaPts(:,2:nsp) - repmat(xPred,1,nsp-1);ezSigmaPt = zPredSigmaPts(:,2:nsp) - repmat(zPred,1,nsp-1);PPred= PPred + (wSigmaPts_xmat .* exSigmaPt) * exSigmaPt';S= S + (wSigmaPts_zmat .* ezSigmaPt) * ezSigmaPt';PxzPred= PxzPred + exSigmaPt * (wSigmaPts_zmat .* ezSigmaPt)';K= PxzPred / S;inovation = Z - zPred;Xout = xPred + K*inovation;Pout = PPred - K*S*K';%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
4.pf.m
% 基本粒子滤波算法function [Xo,Xoset]=pf(Xiset,Z,N,k,R,g1,g2)ticresamplingScheme=1;Zpre=ones(1,N);Xsetpre=ones(1,N);w = ones(1,N);for i=1:NXsetpre(i) = feval('ffun',Xiset(i),k) + gengamma(g1,g2);end;for i=1:N,Zpre(i) = feval('hfun',Xsetpre(i),k);w(i) = inv(sqrt(R)) * exp(-0.5*inv(R)*((Z-Zpre(i))^(2))) ...+ 1e-99; 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 = Xsetpre(outIndex); Xo=mean(Xoset);%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
5.epf.m
% 用EKF改进的粒子滤波算法--EPF% 用EKF产生建议分布% 输入参数说明：%Xiset是上t-1时刻的粒子集合 ， Z是t时刻的观测%Pin对应Xiset粒子集合的方差\% 输出参数说明：%Xo是epf算法最终的估计结果%Xoset是k时刻的粒子集合 ， 其均值就是Xo%Pout是Xoset对应的方差function [Xo,Xoset,Pout]=epf(Xiset,Z,t,Pin,N,R,Qekf,Rekf,g1,g2)resamplingScheme=1;Zpre=ones(1,N);Xsetpre=ones(1,N);w = ones(1,N);Pout=ones(1,N);Xekf=ones(1,N);Xekf_pre=ones(1,N); for i=1:N[Xekf(i),Pout(i)]=ekf(Xiset(i),Z,Pin(i),t,Qekf,Rekf);Xsetpre(i)=Xekf(i)+sqrtm(Pout(i))*randn;endfor i=1:N,Zpre(i) = feval('hfun',Xsetpre(i),t);lik = inv(sqrt(R)) * exp(-0.5*inv(R)*((Z-Zpre(i))^(2)))+1e-99;prior = ((Xsetpre(i)-Xiset(i))^(g1-1)) * exp(-g2*(Xsetpre(i)-Xiset(i)));proposal = inv(sqrt(Pout(i))) * ...exp(-0.5*inv(Pout(i)) *((Xsetpre(i)-Xekf(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 = Xsetpre(outIndex);Pout = Pout(outIndex);Xo = mean(Xoset);%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%