平方根无迹卡尔曼滤波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））