平方根无迹卡尔曼滤波算法程序

clear
x_reality=[-0.7;1;1];           %初始状态
x_estimate=[0;0;0];     %初始状态的估计

Q=0.7;                %过程状态协方差
R=1;              % 测量噪声协方差
P=[1 0 0;0 1 0;0 0 1];      %初始估计方差


n=3;     %系统的维数
m=0.5; %比例系数
L = 2 * n + 1; %总的采样点的个数
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];  %真实值数组
    z（k）=x_reality（1k）+x_reality（2k）*x_reality（3k）+0.5*randn;
end  
S=chol（P）;
 %%%%%%%%%状态估计
%%%%选择对称采样，构造状态的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点的权值
  w_1=m/（n+m）;
  w_2=1/（2*（n+m））;
  %%%%时间更新过程
 
%%把这些粒子通过传递方程 得到下一个状态
   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