%%?----------------------------------------------------------------
%容積卡爾曼濾波（CKF）——單變量非平穩增長模型
%參考文獻：
%[1]?I.Arasaratnam?S.Haykin.?Cubature?Kalman?Filters[J].?IEEE?Trans.?Automat.
%????Control?2009?54（6）?pp.1254-1269
%[2]?Y.X.WuD.W.HuM.P.WuX.P.Hu.?Quasi-Gaussian?Particle?Filtering[J].
%????Comput.?Sci.?2006?（3991）?pp.689-696
%
%???????????????????????????????????????????????????????
%???????????????????????????????????????????By?Irvingzhang?（UESTC）?2012-9-30
%??????????????????????????????????????????????E-mail：irving_zhang@163.com
%%?----------------------------------------------------------------
function?CubatureKF
clear?all;
close?all;
clc;


n=1;%系統的維數
m=2*n;%容積點數
w=1/m;%權值w=1/m
kesi=sqrt（m/2）*[1-1];%kesi=sqrt（m/2）*[1]_i
Q=10;%過程噪聲
R=1;%量測噪聲

x=0.1;
Pplus=10;
xhat=x;%x（0|0）的初始-值隨機取值
xarray=[x];
zarray=[x^2/20+sqrt（R）*randn];
xhatarray=[x];

num=100;%仿真長度
for?i=1:num
????x=0.5*x+25*x/（1+x^2）+8*cos（1.2*（i-1））+sqrt（Q）*randn;%系統方程
????z=x^2/20+sqrt（R）*randn;%量測方程
????xarray=[xarray?x];
????zarray=[zarrayz];
%%?----------------------------CKF濾波----------------------------

%%?----------------------------時間更新----------------------------
????%（1）協方差矩陣Cholesky分解
????Shat=chol（Pplus‘lower‘）;
????for?cpoint=1:m
????????%（2）計算容積點
????????rjpoint（cpoint）=Shat*kesi（cpoint）+xhat;
????????%（3）傳播容積點
????????Xminus（cpoint）=0.5*rjpoint（cpoint）+25*rjpoint（cpoint）/（1+rjpoint（cpoint）^2）+8*cos（1.2*（i-1））;?%容積點經過非線性函數后的值
????end
????%（4）狀態預測
????xhat=w*sum（Xminus）;
????%（5）狀態預測協方差陣
????Pminus=w*sum（Xminus.^2）-xhat*xhat‘+Q;
%%?---------------------------------------------------------------

%%?----------------------------量測更新----------------------------
????%（1）矩陣Cholesky分解
????Sminus=chol（Pminus‘lower‘）;
????for?cpoint=1:m
????????%（2）計算容積點
????????rjpoint1（cpoint）=Sminus*kesi（cpoint）+xhat;
????????%（3）傳播容積點
????????Z（cpoint）=rjpoint1（cpoint）^2/20;%容積點經過非線性函數后的值
????end????
????%（4）觀測預測
????zhat=w*sum（Z）;
????%（5）觀測預測協方差陣
????Pzminus=w*sum（Z.^2）-zhat^2+R;
????%（6）互協方差陣
????Pxzminus=w*rjpoint1*Z‘-xhat*zhat;
????%（7）計算卡爾曼增益
????W=Pxzminus*inv（Pzminus）;
????%（8）狀態更新
????xhat=xhat+W*（z-zhat）;
????%（9）狀態協方差矩陣更新
????Pplus=Pminus-W*Pzminus*W‘;
%%?---------------------------------------------------------------

????xhatarray=[xhatarray?xhat];????
end
%%?---------------------------------------------------------------


k=0:num;
figure（1）
plot（kxarray‘b.‘kxhatarray‘r-‘）;
set（gca‘fontname‘‘Times?New?Roman‘‘fontsize‘12）;
set（gcf‘Color‘‘White‘）;
xlabel（‘Time?step‘‘fontname‘‘Times?New?Roman‘‘fontsize‘16）;
ylabel（‘State‘‘fontname‘‘Times?New?Roman‘‘fontsize‘16）;
axis?tight;
legend（‘True?state‘‘CKF?estimates‘）;
title（‘CKF?estimates‘‘fontname‘‘Times?New?Roman‘‘fontsize‘16）?;

error=xarray-xhatarray;
CKF_RMS=rms（error）;
fa（num）=CKF_RMS;

figure（2）
plot（kerror）;
set（gca‘fontname‘‘Times?New?Roman‘‘fontsize‘12）;
set（gcf‘Color‘‘White‘）;
xlabel

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

?????文件???????2984??2015-03-19?11:34??Third_degree_CKF.m

?????文件???????3487??2015-03-19?11:33??CubatureKF.m

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

?????????????????6471????????????????????2