标签：plot ones UPF 卡尔曼滤波 源码 zeros lineWidth
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.
标签：plot,ones,UPF,卡尔曼滤波,源码,zeros,lineWidth 来源： https://blog.csdn.net/qq_59747472/article/details/120792064
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。