%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
????%??狀態方程：x（:k+1）?=?F?*?x（:k）?+[sqrt（Q）?*?randn;0];?
????%??觀測方程：z（k+1）?=?atan（0.1?*?x（1k+1））?+?sqrt（R）?*?randn;?
?%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
????clc;?
????clear?all;
????close?all;
????n=501;
????tf?=?500;?????????????????????????????????????%?模擬長度?
????x=zeros（2n）;
????x（:1）?=[1;0.1];??????????????????????????????%?初始狀態?
????x_ckf=zeros（2n）;
?????%x_estimate（:1）?=?[1;0.1];??????????????????%狀態的估計
????x_ckf（:1）=[1;0.1];
????%?e_x_estimate?=?x_estimate（:1）;?????????????%EKF的初始估計
????xhat=x_ckf（:1）;
????x_e_error=zeros（1n）;
????x_c_error=zeros（1n）;
????z_e_error=zeros（1n）;
????z_c_error=zeros（1n）;
????Q?=?0.0001;????????????????????????????????????%?過程狀態協方差?
????R?=?0.16;??????????????????????????????????????%?測量噪聲協方差?
????P?=[0.00990;00.0001];????????????????????????%初始估計方差
????Pplus=P;
????F=[11;
????????01];
????Gamma=[0.5;1];
????w=0.25;??
????kesi=sqrt（2）*[10-10;
????????010-1];
????for?k?=?1?:?tf?
????????%?模擬系統?
???????x（:k+1）?=?F?*?x（:k）;??????%狀態值?
???????%x（:k+1）?=?F?*?x（:k）?+[sqrt（Q）?*?randn;0];?
???????if?rand（）>0.4
???????z（k+1）?=??x（1k+1）?+randn（）*2;?????%觀測值
???????else
???????????z（k+1）=x（1k+1）+randn（）*3+4;
???????end
????end
????for?k?=?1?:?tf?
????????%Cubature卡爾曼濾波器
????????%%%%%（1）求協方差矩陣平方根
????????S=chol（Pplus‘lower‘）;
????????%%%%%（2）計算求容積點
????????rjpoint（:1）=S*kesi（:1）+xhat;
????????rjpoint（:2）=S*kesi（:2）+xhat;
????????rjpoint（:3）=S*kesi（:3）+xhat;
????????rjpoint（:4）=S*kesi（:4）+xhat;
????????%%%%%（3）傳播求容積點
????????Xminus（:1）=F*rjpoint（:1）;???????????????????????????%容積點經過非線性函數后的值
????????Xminus（:2）=F*rjpoint（:2）;
????????Xminus（:3）=F*rjpoint（:3）;?
????????Xminus（:4）=F*rjpoint（:4）;?
????????%%%%（4）狀態預測
????????xminus=w*Xminus（:1）+w*Xminus（:2）+w*Xminus（:3）+w*Xminus（:4）;
????????%%%%（5）狀態預測協方差陣
????????Pminus=w*（Xminus（:1）*Xminus（:1）‘+Xminus（:2）*Xminus（:2）‘+Xminus（:3）*Xminus（:3）‘+Xminus（:4）*Xminus（:4）‘）-xminus*xminus‘+Gamma?*?Q*?Gamma‘;
???????%Pminus=w*（Xminus（:1）*Xminus（:1）‘+Xminus（:2）*Xminus（:2）‘+Xminus（:3）*Xminus（:3）‘+Xminus（:4）*Xminus（:4）‘）-xminus*xminus‘+[Q0;00];?
???????%%%%觀測更新
????????%%%%%（1）矩陣分解
????????Sminus=chol（Pminus‘lower‘）;
????????%%%%%（2）計算求容積點
????????rjpoint1（:1）=Sminus*kesi（:1）+xminus;
????????rjpoint1（:2）=Sminus*kesi（:2）+xminus;
????????rjpoint1（:3）=Sminus*kesi（:3）+xminus;
????????rjpoint1（:4）=Sminus*kesi（:4）+xminus;
????????%%%%%（3）傳播求容積點
????????Z（1）=rjpoint1（11）;
????????Z（2）=rjpoint1（12）;
????????Z（3）=rjpoint1（13）;
????????Z（4）=rjpoint1（14）;
???????%?Z（:4）=[atan（0.1*rjpoint1（14））;0];
????????%%%%%%%（4）觀測預測
????????zhat=w*（Z（1）+Z（2）+Z（3）+Z（4））;
????????%%%%（5）觀測預測協方差陣
????????%Pzminus=w*（Z（:1）*Z（:1）‘+Z（:2）*Z（:2）‘+Z（:3）*Z（:3）‘+Z（:4）*Z（:4）‘）-zhat*zhat‘+[R0;0Q];
????????Pzminus=w*（Z（1）^2+Z（2）^2+Z（3）^2+Z（4）^2）-z