%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%??功能說明：?UKF在直線定位的應用
%??參數說明：?1、狀態3維，x方向的位置；
%???????????????y方向的位置、；航向角；
%?????????????2、觀測信息為距離和角度；
%??詳細原理介紹及中文注釋請參考如下：
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function?ukf_for_track_6_div_system
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
n=3;
t=1;
Q=diag（[111]）;
R?=?diag（[0.01
????0.01/180]）;
f=@（x）[x（1）+10*cos（pi/4）;x（2）+10*sin（pi/4）;pi/4];
h=@（x）[sqrt（x（1）^2+x（2）^2）;atan（x（2）/x（1））];
s=[0;0;pi/4];
x0=s+sqrtm（Q）*randn（n1）;
P0?=eye（3）;
N=200;
Xukf?=?zeros（nN）;
X?=?zeros（nN）;
Z?=?zeros（2N）;
for?i=1:N
????X（:i）=?f（s）+sqrtm（Q）*randn（31）;%?此處有誤，請修改為P114頁一致即可運行
????s?=?X（:i）;
end
ux=x0;
for?k=1:N
????Z（:k）=?h（X（:k））?+?sqrtm（R）*randn（21）;
????[Xukf（:k）?P0]?=?ukf（fuxP0hZ（:k）QR）;
????ux=Xukf（:k）;
end

err_ukf_x=zeros（1N）;
for?k=1:N
????err_ukf_x（k）=（X（1k）-Xukf（1k））;
end
ukf_xmax=max（abs（err_ukf_x））;
err_ave_ukf=sum（abs（err_ukf_x））/N;
fprintf（?‘ukf_xmax=%f‘ukf_xmax）;???
fprintf（‘err_ave_ukf=%f‘err_ave_ukf）;

err_ukf_y=zeros（1N）;
for?k=1:N
????err_ukf_y（k）=（X（2k）-Xukf（2k））;
end
ukf_ymax=max（abs（err_ukf_y））;
err_ave_ukf=sum（abs（err_ukf_y））/N;
fprintf（?‘ukf_ymax=%f‘ukf_ymax）?;???
fprintf（‘err_ave_ukf=%f‘err_ave_ukf）;

err_ukf_t=zeros（1N）;
for?k=1:N
????err_ukf_t（k）=（X（3k）-Xukf（3k））;
end
ukf_tmax=max（abs（err_ukf_t））;
err_ave_ukf=sum（abs（err_ukf_t））/N;
fprintf（?‘?ukf_tmax=%f‘ukf_tmax）?;???
fprintf（‘err_ave_ukf=%f‘err_ave_ukf）;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure
t=1:N;
hold?on;box?on;
plot（?X（1t）X（2t）?‘-k*‘‘LineWidth‘1）
plot（Xukf（1t）Xukf（2t）‘-b+‘‘LineWidth‘1）
grid?on;
legend（‘真實軌跡‘‘UKF軌跡‘）;
xlabel（‘x方向位置/cm‘）
ylabel（‘y方向位置/cm‘）

figure
box?on;
plot（err_ukf_x‘-b*‘‘LineWidth‘1）
grid?on;
xlabel（‘時間t/秒‘）
ylabel（‘x方向位置誤差/cm‘）

figure
box?on;
plot（err_ukf_y‘-r*‘‘LineWidth‘1）
grid?on;
xlabel（‘時間t/秒‘）
ylabel（‘y方向位置誤差/cm‘）

figure
box?on;
plot（err_ukf_t‘-m‘‘LineWidth‘1）
grid?on;
xlabel（‘時間t/秒‘）
ylabel（‘航向角誤差/°‘）
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function?[XP]=ukf（ffunXPhfunZQR）
L=numel（X）;
m=numel（Z）;
alpha=1e-2;
ki=0;
beta=2;
lambda=alpha^2*（L+ki）-L;
c=L+lambda;
Wm=[lambda/c?0.5/c+zeros（12*L）];
Wc=Wm;
Wc（1）=Wc（1）+（1-alpha^2+beta）;
c=sqrt（c）;
Xsigmaset=sigmas（XPc）;?
[X1meansX1P1X2]=ut（ffunXsigmasetWmWcLQ）;???
[ZpreZ1PzzZ2]=ut（hfunX1WmWcmR）;
Pxz=X2*diag（Wc）*Z2‘;
K=Pxz*inv（Pzz）;
X=X1means+K*（Z-Zpre）;
P=P1-K*Pxz‘;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function?[XmeansXsigma_prePXdiv]=ut（funXsigmaWmWcnCOV）
LL=size（Xsigma2）;
Xmeans=zeros（n1）;
Xsigma_pre=zeros（nLL）;
for?k=1:LL
????Xsigma_pre（:k）=fun（Xsigma（:k））;
????Xmeans=Xmeans+Wm（k）*Xsigma_pre（:k）;
end
Xdiv=Xsigma_pre-Xmeans（:ones（1LL

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件????????3406??2018-05-04?16:15??ukf_for_track_6_div_system.m