%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%?程序說(shuō)明：對(duì)比UKF與EKF在非線性系統(tǒng)中應(yīng)用的算法性能
%??詳細(xì)原理介紹及中文注釋請(qǐng)參考：
%??《卡爾曼濾波原理及應(yīng)用-MATLAB仿真》，電子工業(yè)出版社，黃小平著。
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
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（‘真實(shí)狀態(tài)‘‘EKF估計(jì)‘‘UKF估計(jì)‘）
xlabel（‘時(shí)間k/s‘）
ylabel（‘狀態(tài)值‘）
figure
hold?on;box?on;
plot（err_ekf‘-ro‘）;
plot（err_ukf‘-b+‘）;
xlabel（‘時(shí)間k/s‘）
ylabel（‘偏差絕對(duì)值‘）
legend（‘EKF估計(jì)‘‘UKF估計(jì)‘）
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

?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----
?????文件???????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\