UKF与EKF算法应用比较

为了对比UKF与EKF两种算法的滤波效果，进行的仿真分析

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 程序说明：对比UKF与EKF在非线性系统中应用的算法性能
%  详细原理介绍及中文注释请参考：
%  《卡尔曼滤波原理及应用-MATLAB仿真》，电子工业出版社，黄小平著。
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function ukf_ekf_compair_example
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
N=50;
L=1;
Q=10;
R=1;
W=sqrtm（Q）*randn（LN）;
V=sqrt（R）*randn（1N）;
X=zeros（LN）;
X（:1）=[0.1]‘;
Z=zeros（1N）;
Z（1）=X（:1）^2/20+V（1）;
Xukf=zeros（LN）;
Xukf（:1）=X（:1）+sqrtm（Q）*randn（L1）;
Pukf=eye（L）;
Xekf=zeros（LN）;
Xekf（:1）=X（:1）+sqrtm（Q）*randn（L1）;
Pekf=eye（L）;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for k=2:N
    X（: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）PekfZ（k）QRk）;
    [Xukf（:k）Pukf]=ukf（Xukf（:k-1）PukfZ（k）QRk）;
end
err_ekf=zeros（1N）;
err_ukf=zeros（1N）;
for k=1:N
    err_ekf（k）=abs（Xekf（1k）-X（1k））;
    err_ukf（k）=abs（Xukf（1k）-X（1k））;
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 [XoutPout]=ekf（XinPZinQRk）
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 [XoutPout]=ukf（XP0ZQRk）
L=1;
alpha=1;
kalpha=0;
belta=2;
ramda=3-L;
for j=1:2*L+1
    Wm（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:L
    xgamaP1（:j）=xestimate+cho（:j）;
    xgamaP2（:j）=xestimate-cho（:j）;
end
Xsigma=[xestimatexgamaP1xgamaP2];
for j=1:2*L+1
    Xsigmapre（: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+1
    Xpred=Xpred+Wm（j）*Xsigmapre（:j）;
end
Ppred=0;
for j=1:2*L+1
    Ppred=Ppred+Wc（j）*（Xsigmapre（:j）-Xpred）*（Xsigmapre（:j）-Xpred）‘;
end
Ppred=Ppred+Q;
chor=（chol（（L+ramda）*Ppred））‘;
for j=1:L
    XaugsigmaP1（:j）=Xpred+chor（:j）;
    XaugsigmaP2（:j）=Xpred-chor（:j）;
end
Xaugsigma=[Xpred XaugsigmaP1 XaugsigmaP2];
for j=1:2*L+1
    Zsigmapre（1j）=Xaugsigma（:j）^2/20;
end
Zpred=0;
for j=1:2*L+1
    Zpred=Zpred+Wm（j）*Zsigmapre（1j）;
end
Pzz=0;
for j=1:2*L+1
    Pzz=Pzz+Wc（j）*（Zsigmapre（1j）-Zpred）*（Zsigmapre（1j）-Zpred）‘;
end
Pzz=Pzz+R;

Pxz=0;
for j=1:2*L+1
    Pxz=Pxz+Wc（j）*（Xaugsigma（:j）-Xpred）*（Zsigmapre（1j）-Zpred）‘;
end
K=Pxz*inv（Pzz）;
xestimate=Xpred+K*（Z-Zpred）;
P=Ppred-K*Pzz

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件       25573  2018-01-11 13:53  5.4\EKF-UKF2.fig
     文件        3201  2015-06-25 17:41  5.4\ukf_ekf_compair_example.m
     目录           0  2018-03-14 21:20  5.4\