sigma.m

kalman滤波程序 关于信息融合方面的程序

function [X,Y]=sigma(x,p,F,Rw)
    r=chol(3*p);
    t=chol(3*Rw);
    s(:,1)=x;
   for j=1:4
    xgamaP1(:,j)=s(:,1)+r(:,j);
    xgamaP2(:,j)=s(:,1)-r(:,j);
   end
Xsigma=[s(:,1) xgamaP1 xgamaP2];
X=F*Xsigma;


   for j=1:2
    XaugsigmaP1(:,j)=X(:,1)+t(:,j);%与UT变换中第二大步的第一小步对映不上？
    XaugsigmaP2(:,j)=X(:,1)-t(:,j);
   end
Xaugsigma=[X(:,1) XaugsigmaP1 XaugsigmaP2];
   for j=1:5
    Y(1,j)=sqrt((Xaugsigma(1,j))*(Xaugsigma(1,j))'+(Xaugsigma(3,j))*(Xaugsigma(3,j))');
    Y(2,j)=(Xaugsigma(1,j)*Xaugsigma(2,j)+Xaugsigma(3,j)*Xaugsigma(4,j))/Y(1,j);
   end