%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%??無跡Kalman濾波在目標跟蹤中的應用
%??詳細原理介紹及中文注釋請參考：
%??《卡爾曼濾波原理及應用-MATLAB仿真》，電子工業出版社，黃小平著。
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function?UKF
clc;clear;
T=1;?
N=60/T;?
X=zeros（4N）;?
X（:1）=[-100220020];?
Z=zeros（1N）;?
delta_w=1e-3;?
Q=delta_w*diag（[0.51]）?;
G=[T^2/20;T0;0T^2/2;0T];
R=5;?
F=[1T00;0100;001T;0001];
x0=200;?
y0=300;
Xstation=[x0y0];?
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
w=sqrtm（R）*randn（1N）;
for?t=2:N
????X（:t）=F*X（:t-1）+G*sqrtm（Q）*randn（21）;
end
for?t=1:N
????Z（t）=Dist（X（:t）Xstation）+w（t）;
end
L=4;
alpha=1;
kalpha=0;
belta=2;
ramda=alpha^2*（L+kalpha）-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;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Xukf=zeros（4N）;
Xukf（:1）=X（:1）;
P0=eye（4）;
for?t=2:N
????xestimate=?Xukf（:t-1）;
????P=P0;
????cho=（chol（P*（L+ramda）））‘;
????for?k=1:L
????????xgamaP1（:k）=xestimate+cho（:k）;
????????xgamaP2（:k）=xestimate-cho（:k）;
????end
????Xsigma=[xestimatexgamaP1xgamaP2];
????Xsigmapre=F*Xsigma;
????Xpred=zeros（41）;
????for?k=1:2*L+1
????????Xpred=Xpred+Wm（k）*Xsigmapre（:k）;
????end
????Ppred=zeros（44）;
????for?k=1:2*L+1
????????Ppred=Ppred+Wc（k）*（Xsigmapre（:k）-Xpred）*（Xs