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

资源简介

前段时间用平方根无迹卡尔曼滤波做了一个观测器算法,之前也找了好多该算法的资料,在CSDN也下载过,但是并不能用,后来自己结合之前下载的代码又上各种论坛,梳理了一下。最终代码可以用了,论文也顺利投出去了。现在分享给大家,顺便赚一点币

资源截图

代码片段和文件信息

function [xS]=sr_ukf(fstatexShmeaszQR)
    n=numel(x);                                 %numer of states
    m_z=numel(z);                               %numer of measurements
    k=0;                                       
    L=2*n+1;  
    alpha=0.5;%0.44;%0.5;%0.1; 
    beta=2.0;%2.0;
    m=alpha*alpha*(n+k)-n; %比例系数  为缩放比例,降低系统总的预测误差
    gama=1-alpha*alpha+beta;
    %%%%%%%%%%%%%%%%%%%分配sigma点权重
    Wm=zeros(L1);
    Wc=zeros(L1);
    Wm(1)=m/(n+m);
    Wc(1)=m/(n+m)+gama;
    for i=2:L
        Wm(i)=1/(2*(n+m));
        Wc(i)=1/(2*(n+m));
    end
    sqR=chol(R);
    sqQ=chol(Q);
    %------------------------时间更新过程---------------------------------------
    %计算sigma点(选择对称采样方法)
    I=sqrt(n+m)*S;
    x_sigma=zeros(nL);
    x_sigma(:1)=x;   
     for i=2:n+1
           x_sigma(:i) = x+I(:i-1);
     end
     for i=n+2:L
          x_sigma(:i) = x-I(:i-n-1);
     end 
    %传播sigma点
    r_sigma=zeros(nL);
       for i=1:L
          r_sigma(:i)=fstate(x_sigma(:i));
       end 
    %计算先验状态
    x_next=zeros(n1);
    for i=1:L
    x_next=x_next+Wm(i)*r_sigma(:i);
    end
    %qr分解,这一部分是SRUKF算法和UKF算法不同的地方
    A=zeros(nL-1);
     for i=2:L
         A(:i-1)=sqrt(Wc(i))*(r_sigma(:i)-x_next);      
     end
     [~S_xk]=qr([A sqQ]‘0);
    %cholupdate更新
    S_next=cholupdate(S_xksqrt(Wc(1))*(r_sigma(:1)-x_next));
% % % % % % % % % % % %     S_next=cholupdate(S_xkr_sigma(:1)-x_next‘-‘);  %已更改2018.12.2
% % % % % % % % % % % %     S_next=chol(S_next*S_next‘-((-Wc(1)^(1/2))

评论

共有 条评论