1 模型
本文重点论述了EKF、UPF、PF、EPF、UPF多种卡尔曼滤波的理论基础,以 离散时间系统为主,介绍了各种滤波方法的递推公式,分析 了各种方法的特点,理顺了种种方法之间的区别和联系,阐 述了卡尔曼滤波方法在动态测量中的应用.
2 部分代码
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 主函数%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 功能说明：ekf,ukf,pf,epf,upf算法的综合比较程序rand('seed',3);randn('seed',6);%error('下面的参数T请参考书中的值设置，然后删除本行代码')T=60;R= 1e-5; g1=3; g2=2; X=zeros(1,T);Z=zeros(1,T);processNoise=zeros(T,1);measureNoise=zeros(T,1);X(1) =1; P0=3/4;Qekf=10*3/4; Rekf=1e-1;Xekf=zeros(1,T); Pekf=P0*ones(1,T); Tekf=zeros(1,T); Qukf=2*3/4; Rukf=1e-1;Xukf=zeros(1,T); Pukf=P0*ones(1,T);Tukf=zeros(1,T); N=200; Xpf=zeros(1,T);Xpfset=ones(T,N); Tpf=zeros(1,T);Xepf=zeros(1,T);Xepfset=ones(T,N); Pepf=P0*ones(T,N);Tepf=zeros(1,T); Xupf=zeros(1,T);Xupfset=ones(T,N); Pupf=P0*ones(T,N); Tupf=zeros(1,T); fort=2:TprocessNoise(t) = gengamma(g1,g2); measureNoise(t) = sqrt(R)*randn; X(t) =feval('ffun',X(t-1),t)+processNoise(t);Z(t) =feval('hfun',X(t),t)+measureNoise(t);tic[Xekf(t),Pekf(t)]=ekf(Xekf(t-1),Z(t),Pekf(t-1),t,Qekf,Rekf);Tekf(t)=toc;tic[Xukf(t),Pukf(t)]=ukf(Xukf(t-1),Z(t),Pukf(t-1),Qukf,Rukf,t);Tukf(t)=toc;tic[Xpf(t),Xpfset(t,:)]=pf(Xpfset(t-1,:),Z(t),N,t,R,g1,g2);Tpf(t)=toc;tic[Xepf(t),Xepfset(t,:),Pepf(t,:)]=epf(Xepfset(t-1,:),Z(t),t,Pepf(t-1,:),N,R,Qekf,Rekf,g1,g2);Tepf(t)=toc;tic[Xupf(t),Xupfset(t,:),Pupf(t,:)]=upf(Xupfset(t-1,:),Z(t),t,Pupf(t-1,:),N,R,Qukf,Rukf,g1,g2);Tupf(t)=toc;end;ErrorEkf=abs(Xekf-X);ErrorUkf=abs(Xukf-X); ErrorPf=abs(Xpf-X); ErrorEpf=abs(Xepf-X); ErrorUpf=abs(Xupf-X); figureholdon;boxon;p1=plot(1:T,X,'-k.','lineWidth',2);%p2=plot(1:T,Xekf,'m:','lineWidth',2);%p3=plot(1:T,Xukf,'--','lineWidth',2);p4=plot(1:T,Xpf,'-ro','lineWidth',2);p5=plot(1:T,Xepf,'-g*','lineWidth',2);p6=plot(1:T,Xupf,'-b^','lineWidth',2);legend([p1,p4,p5,p6],'真实状态','PF估计','EPF估计','UPF估计')xlabel('Time','fontsize',10)title('Filter estimates (posterior means) vs. True state','fontsize',10)figureholdon;boxon;p1=plot(1:T,ErrorEkf,'-k.','lineWidth',2);p2=plot(1:T,ErrorUkf,'-m^','lineWidth',2);p3=plot(1:T,ErrorPf,'-ro','lineWidth',2);p4=plot(1:T,ErrorEpf,'-g*','lineWidth',2);p5=plot(1:T,ErrorUpf,'-bd','lineWidth',2);legend([p1,p2,p3,p4,p5],'EKF偏差','UKF偏差','PF偏差','EPF偏差','UPF偏差')figureholdon;boxon;%p1=plot(1:T,Tekf,'-k.','lineWidth',2);%p2=plot(1:T,Tukf,'-m^','lineWidth',2);p3=plot(1:T,Tpf,'-ro','lineWidth',2);p4=plot(1:T,Tepf+0.02,'k:o','lineWidth',2);p5=plot(1:T,Tupf,'-bo','lineWidth',2);p6=plot(1:T,Tepf+0.015,'-g*','lineWidth',2);legend([p3,p4,p6,p5],'PF','DCS-UPF-X','DCS-UPF-Y','UPF')xlabel('Time','fontsize',10)ylabel('Single step running time','fontsize',10)%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
3 仿真结果
4 参考文献
[1]宋文尧, and 张牙. 卡尔曼滤波. 科学出版社, 1991.
[2]潘迪夫, 刘辉, 李燕飞. 基于时间序列分析和卡尔曼滤波算法的风电场风速预测优化模型[J]. 电网技术, , 32(7):82-86.
如果觉得《【卡尔曼滤波】基于EKF UPF PF EPF UPF多种卡尔曼滤波实现航迹滤波跟踪matlab源码》对你有帮助，请点赞、收藏，并留下你的观点哦！
