• 大小: 3KB
    文件类型: .m
    金币: 1
    下载: 0 次
    发布日期: 2021-06-05
  • 语言: Matlab
  • 标签: SRUKF  

资源简介

本算法是基于UKF 的SRUKF 算法,其在运算效率和数值稳定性方面都比UKF快,且估计精度高。本程序已经过调试,切实可行,适合初学者。

资源截图

代码片段和文件信息

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

评论

共有 条评论

相关资源