《卡尔曼滤波原理及应用-MATLAB仿真》程序-5.3UKF

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 程序说明：对比UKF与EKF在非线性系统中应用的算法性能
%  详细原理介绍及中文注释请参考：
%  《卡尔曼滤波原理及应用-MATLAB仿真》，电子工业出版社，黄小平著。
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function ukf_ekf_compair_example
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N=50;
L=1;
Q=10;
R=1;
W=sqrtm(Q)*randn(L,N);
V=sqrt(R)*randn(1,N);
X=zeros(L,N);
X(:,1)=[0.1]';
Z=zeros(1,N);
Z(1)=X(:,1)^2/20+V(1);
Xukf=zeros(L,N);
Xukf(:,1)=X(:,1)+sqrtm(Q)*randn(L,1);
Pukf=eye(L);
Xekf=zeros(L,N);
Xekf(:,1)=X(:,1)+sqrtm(Q)*randn(L,1);
Pekf=eye(L);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for k=2:NX(:,k)=0.5*X(:,k-1)+2.5*X(:,k-1)/(1+X(:,k-1)^2)+8*cos(1.2*k)+W(k);Z(k)=X(:,k)^2/20+V(k);[Xekf(:,k),Pekf]=ekf(Xekf(:,k-1),Pekf,Z(k),Q,R,k);[Xukf(:,k),Pukf]=ukf(Xukf(:,k-1),Pukf,Z(k),Q,R,k);
end
err_ekf=zeros(1,N);
err_ukf=zeros(1,N);
for k=1:Nerr_ekf(k)=abs(Xekf(1,k)-X(1,k));err_ukf(k)=abs(Xukf(1,k)-X(1,k));
end
XX=X-W;
err_ave_ekf=sum(err_ekf)/N
err_ave_ukf=sum(err_ukf)/N
figure
hold on;box on;
plot(X,'-r*');
plot(Xekf,'-ko');
plot(Xukf,'-b+');
legend('真实状态','EKF估计','UKF估计')
xlabel('时间k/s')
ylabel('状态值')
figure
hold on;box on;
plot(err_ekf,'-ro');
plot(err_ukf,'-b+');
xlabel('时间k/s')
ylabel('偏差绝对值')
legend('EKF估计','UKF估计')
function [Xout,Pout]=ekf(Xin,P,Zin,Q,R,k)
Xpre=0.5*Xin+2.5*Xin/(1+Xin^2)+8*cos(1.2*k);
F=[0.5+(2.5*(1+Xpre^2)-2.5*Xpre*2*Xpre)/(1+Xpre^2)^2];
Ppre=F*P*F'+Q;
Zpre=Xpre^2/20;
H=[Xpre/10];
K=Ppre*H'/(H*Ppre*H'+R);
Xout=Xpre+K*(Zin-Zpre);
Pout=(eye(1)-K*H)*Ppre;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Xout,Pout]=ukf(X,P0,Z,Q,R,k)
L=1;
alpha=1;
kalpha=0;
belta=2;
ramda=3-L;
for j=1:2*L+1Wm(j)=1/(2*(L+ramda));Wc(j)=1/(2*(L+ramda));
end
Wm(1)=ramda/(L+ramda);
Wc(1)=ramda/(L+ramda)+1-alpha^2+belta;
xestimate= X;
P=P0;
cho=(chol(P*(L+ramda)))';
for j=1:LxgamaP1(:,j)=xestimate+cho(:,j);xgamaP2(:,j)=xestimate-cho(:,j);
end
Xsigma=[xestimate,xgamaP1,xgamaP2];
for j=1:2*L+1Xsigmapre(:,j)=0.5*Xsigma(:,j)+2.5*Xsigma(:,j)/(1+Xsigma(:,j)^2)+8*cos(1.2*k);
end
Xpred=0;
for j=1:2*L+1Xpred=Xpred+Wm(j)*Xsigmapre(:,j);
end
Ppred=0;
for j=1:2*L+1Ppred=Ppred+Wc(j)*(Xsigmapre(:,j)-Xpred)*(Xsigmapre(:,j)-Xpred)';
end
Ppred=Ppred+Q;
chor=(chol((L+ramda)*Ppred))';
for j=1:LXaugsigmaP1(:,j)=Xpred+chor(:,j);XaugsigmaP2(:,j)=Xpred-chor(:,j);
end
Xaugsigma=[Xpred XaugsigmaP1 XaugsigmaP2];
for j=1:2*L+1Zsigmapre(1,j)=Xaugsigma(:,j)^2/20;
end
Zpred=0;
for j=1:2*L+1Zpred=Zpred+Wm(j)*Zsigmapre(1,j);
end
Pzz=0;
for j=1:2*L+1Pzz=Pzz+Wc(j)*(Zsigmapre(1,j)-Zpred)*(Zsigmapre(1,j)-Zpred)';
end
Pzz=Pzz+R;Pxz=0;
for j=1:2*L+1Pxz=Pxz+Wc(j)*(Xaugsigma(:,j)-Xpred)*(Zsigmapre(1,j)-Zpred)';
end
K=Pxz*inv(Pzz);
xestimate=Xpred+K*(Z-Zpred);
P=Ppred-K*Pzz*K';
Pout=P;
Xout=xestimate;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

结果：

《卡尔曼滤波原理及应用-MATLAB仿真》程序-5.3UKF相关推荐

《卡尔曼滤波原理及应用-MATLAB仿真》程序-2.2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 程序说明:练习使用plot函数,线型设置 % ...
ukf源程序 matlab,《卡尔曼滤波原理及应用-MATLAB仿真》程序-5.1UKF
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 无迹Kalman滤波在目标跟踪中的应用 % ...
《卡尔曼滤波原理及应用-MATLAB仿真》程序-5.2UKF
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % 功能说明: UKF在目标跟踪中的应用 % 参数 ...
matlab 脉冲压缩算法,线性调频脉冲压缩原理及其MATLAB仿真+程序
摘要:脉冲压缩技术在现代雷达系统和超带宽通信系统等领域具有越来越重要的应用.本文首先阐述了线性调频脉冲压缩的基本原理,并提出了增大信号等效带宽可以提高雷达距离分辨率,然后分析了匹配滤波器的特性,给出 ...
滑模变结构控制matlab仿真程序,滑模变结构控制MATLAB仿真程序代码
[实例简介] 滑模变结构控制MATLAB仿真程序代码,值得借鉴 [实例截图] [核心代码] 滑模变结构控制MATLAB仿真 └── 滑模变结构控制MATLAB仿真 ├── 43188036Slidi ...
光刻原理-偏振光（MATLAB仿真）（1）
光的偏振光的偏振性质线偏光圆偏振光椭圆偏振光自然偏振光 MATLAB仿真程序琼斯矢量线偏光的琼斯矢量圆偏光的琼斯矢量 MATLBA琼斯矢量表示偏振光马吕斯定律光的偏振性质对于在直 ...
matlab2014仿真pid程序,先进 PID 控制及其 MATLAB 仿真（PDF+程序）
[实例简介] 目录前言第 1 章数字 PID 控制------------------------(1) 1.1PID 控制原理 --------------------------(1) ...
脉冲信号matlab仿真,脉冲积累matlab仿真
%T_7_1.m % 相干积累 clear all clc clf c=3e8; % speed of light Te=100e-6; % 发射脉冲宽度 Be=1e6; %带宽 mu=Be/Te; ...
控制系统数学模型的matlab仿真,第7章控制系统的MATLAB仿真
<第7章控制系统的MATLAB仿真>由会员分享,可在线阅读,更多相关<第7章控制系统的MATLAB仿真(101页珍藏版)>请在人人文库网上搜索. 1.1,本章主要教学内容在 ...

《卡尔曼滤波原理及应用-MATLAB仿真》程序-5.3UKF

《卡尔曼滤波原理及应用-MATLAB仿真》程序-5.3UKF相关推荐

最新文章

热门文章