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); for t=2:T processNoise(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); figure hold on;box on; 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) figure hold on;box on; 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偏差') figure hold on;box on; %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]. 电网技术, 2008, 32(7):82-86.