%%?----------------------------------------------------------------
%?Bearing-only?tracking?model?for?tesing?the?cubature?Kalman?filter?（CKF）?
%?and?the?embedded?cubature?Kalman?filter?（ECKF）
%?
%?Version:?20140307
%?
%?
%?@All?rights?reserved?by?Xin-Chun?Zhang?from?UESTC
%?You?can?use?the?code?for?learning?teaching?and?education?do?not?use?it
%?for?commerical?purpose!
%?
%?
%?
%?參考：
%?[1]?I.Arasaratnam?S.Haykin.?Cubature?Kalman?Filters[J].?IEEE?Trans.?Automat.
%?????Control?2009?54（6）
%?[2]?張鑫春等.?一種用于目標跟蹤的嵌入式容積卡爾曼濾波器設計[C].?綜合電子技術教
%?????育部重點實驗室2012學術年會論文集，2012
%?[3]?張鑫春?等.?“均方根嵌入式容積卡爾曼濾波“?控制理論與應用?2013?30（9）.?????????
%?[4]?Xin-Chun?Zhang?Yun-Long?Teng.?A?new?derivation?of?the?cubature
%?????Kalman?filters?[J].?Asian?Journal?of?Control?2013?Accept
%?[5]?Xin-Chun?Zhang?A?novel?cubature?Kalman?filter?for?nonlinear?state
%?????estimation?[C].?52nd?IEEE?Conference?on?Decision?and?Control
%?????Florence?Italy?2013
%?[6]?Zhang?Xin-Chun?Guo?Cheng-Jun?Cubature?Kalman?filters:?Derivation
%?????and?extension?[J].?Chinese?Physics?B?2013?22（12）
%?[7]?Xin-Chun?Zhang.?Cubature?information?filter?using?embedded?and
%?????high-degree?cubature?rules?[J].?Circuits?Systems?and?Signal
%?????processing?Accept（2013）/Online（2014）
%
%?
%?
%?
%
%??????????????????????By?Xin-Chun?Zhang?
%?????????????????E-mail：irving_zhang@163.com
%???????University?of?Electronic?Science?and?Technology?of?China
%%?----------------------------------------------------------------

clear?all;
close?all;
clc;

h?=?waitbar（0?‘1‘?‘Name‘?‘Please?wait?...‘?‘Windowstyle‘?‘modal‘?...
????‘CreateCancelBtn‘?‘setappdata（gcbf‘‘canceling‘‘1）‘）;
setappdata（h?‘canceling‘?0）;

%?Global?variables
global?Q?R?fai?gama?kesi?w?n?m?w1?w2?kesi_im3?b?;

Q?=?0.0005^2;?%?process?noise
R?=?0.001^2;?%?measurement?noise
fai?=?[1?1?0?0;?0?1?0?0;?0?0?1?1;?0?0?0?1];
gama?=?[0.5?0;?1?0;?0?0.5;?0?1];
n?=?4;?%dimension?of?the?system
m?=?2?*?n;?%?Number?of?cubature?points

%?3rd-degree?CKF/SCKF
w?=?1?/?m;?%?Weight
kesi1?=?eye（n）;
kesi2?=?-eye（n）;
kesi?=?[kesi1?kesi2]?*?（sqrt（m?/?2））;?%?Construction?of?the?cubature?points

%?3rd-degree?ECKF
delta?=?1;
w1?=?1?/?（2^（n?+?1）?*?delta）;
w2?=?1?-?1?/?（2?*?delta）;
yitao?=?zeros（n?1）;
yitak1?=?[1?1?1?1;?1?1?1?-1;?1?1?-1?-1;?1?-1?-1?-1;?1?-1?1?1;?1?-1?-1?1;?...
????1?-1?1?-1;?1?1?-1?1]‘?*?sqrt（2?*?delta）;
kesi_im3?=?[yitak1?-yitak1?yitao];
[a?b]?=?size（kesi_im3）;

num?=?30;?%?steps?of?the?simulation

xarray_sum?=?zeros（n?num?+?1）;

xhatarray_sum?=?zeros（n?num?+?1）;
error_avg?=?zeros（n?num?+?1）;

xhatarray_S_sum?=?zeros（n?num?+?1）;
error_S_avg?=?zeros（n?num?+?1）;

xhatarray_5_sum?=?zeros（n?num?+?1）;
error_5_avg?=?zeros（n?num?+?1）;

xhatarray_I_sum?=?zeros（n?num?+?1）;
error_I_avg?=?zeros（n?num?+?1）;

len?=?100;
for?iter?=?1?:?len
????if?getappdata（h?‘canceling‘）
????????iter?=?iter?-?1;
????????break;
????end
????waitbar（iter?/?len?h?sprintf（‘%

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件???????4846??2014-03-07?22:36??BOT\Main.m

?????文件???????1556??2014-03-07?22:23??BOT\Third_degree_CKF.m

?????文件???????2401??2014-03-07?22:26??BOT\Third_degree_ECKF.m

?????目錄??????????0??2014-03-07?22:37??BOT

-----------?---------??----------?-----??----

?????????????????8803????????????????????4