function?[XP]?=?UKF_func（?ffun?X?P?hfun?Z?Q?R?）
%UKF_func：UKF濾波算法
%???此處顯示詳細(xì)說(shuō)明
????x_dim?=?numel（X）;??%狀態(tài)維數(shù)
????o_dim?=?numel（Z）;??%觀測(cè)維數(shù)
????alpha?=?0.01;??????%決定mean（x）周圍sigma點(diǎn)的分布狀態(tài)
????ki?=?0;
????beta?=?2;
????lambda?=?alpha^2*（x_dim?+?ki）?-?x_dim;?%比例因子
????c?=?x_dim?+?lambda;
????Wm?=?[lambda/c?0.5/c+zeros（1?2*x_dim）];??%權(quán)值
????Wc?=?Wm;
????Wc（1）?=?Wc（1）?+?（1?-?alpha^2?+?beta）;???%權(quán)值
????c?=?sqrt（c）;

????%%?獲得一組采樣點(diǎn)（Sigma點(diǎn)集）
????A?=?c*chol（P）‘;?%Cholesky分解
????Y?=?X（:ones（1numel（X）））;
????XsigmaSet?=?[X?Y+A?Y-A];??%Sigmada點(diǎn)集

????%%?計(jì)算2*x_dim+1個(gè)Sigma點(diǎn)集的一步預(yù)測(cè)，得到均值X1means和方差P1和新Sigma點(diǎn)集X1
????%對(duì)狀態(tài)UT變換
????SamNum?=?size（XsigmaSet2）;???%得到Xsigma樣本個(gè)數(shù)
????X1_pre?=?zeros（x_dim1）;??????%系統(tǒng)狀態(tài)量的一步預(yù)測(cè)（均值）
????X1?=?zeros（x_dimSamNum）;
????for?k=1:SamNum
??????