clear
x_reality=[-0.7;1;1];???????????%初始狀態(tài)
x_estimate=[0;0;0];?????%初始狀態(tài)的估計

Q=0.7;????????????????%過程狀態(tài)協(xié)方差
R=1;??????????????%?測量噪聲協(xié)方差
P=[1?0?0;0?1?0;0?0?1];??????%初始估計方差


n=3;?????%系統(tǒng)的維數(shù)
m=0.5;?%比例系數(shù)
L?=?2?*?n?+?1;?%總的采樣點的個數(shù)
for?k=1:80;
????x_reality（:1）=[-0.7;1;1];
????x_reality（:k+1）=[3*sin（2*x_reality（2k））;x_reality（1k）+exp（-0.05*x_reality（3k））+10;x_reality（1k）*（x_reality（2k）+x_reality（3k））/5]+0.3*randn;
????x_array?=?[x_reality];??%真實值數(shù)組
????z（k）=x_reality（1k）+x_reality（2k）*x_reality（3k）+0.5*randn;
end??
S=chol（P）;
?%%%%%%%%%狀態(tài)估計
%%%%選擇對稱采樣，構(gòu)造狀態(tài)的sigma點
I=sqrt（n+m）*S;
x_sigma=x_estimate;
?for?i=2:n+1
???????x_sigma（:i）?=?x_estimate+I（:i-1）;
?end
???for?i=n+2:L
??????x_sigma（:i）?=?x_estimate-I（:i-n-1）;
???end
???%%%%對應于各sigma點的權(quán)值
??w_1=m/（n+m）;
??w_2=1/（2*（n+m））;
??%%%%時間更新過程
?
%%把這些粒子通過傳遞方程?得到下一個狀態(tài)
???for?i=1:L
??????r_sigma（:i）=[3*sin（2*x_sigma（2i））;x_sigma（1i）+exp（-0.05*x_sigma（3i））+10;（x_sigma（1i）*（x_sigma（2i）+x_sigma（3i）））/5+x_sigma（1i）/2];
???end?
???%%傳遞后的均值（均值的一步預測）
??x_next=zeros（31）;
??for?i=1:L
??x_next=x_next+w_1*r_sigma（:i）;
??end?
???%%傳遞后的方差（方差的一步預測）

?for?i=1:L;
????if?i>1;
????????w=w_2;
?
????else?
????????w=w_1;
????end

?end
?%%qr分解
?
?for?i=1:L;
?????A（:i）=sqrt（w）*（r_sigma（:i）-x_next）;
?end
??[Q?S_next]=qr（[A??sqrt（Q*eye（3））]‘0）;
??%%cholupdate更新
S_next=cholupdate（S_n