guji.m

来自「kalman滤波程序 关于信息融合方面的程序」· M 代码 · 共 28 行

function [x_gj,y_gj,Px,Py,K]=guji(X,Y,Rw,Rv)
    x_gj=0;
    Px=0;
    y_gj=0;
    Py=0;
    Pxy=0;
    Wx(1)=(3-4)/3;
    Wx(2:9)=1/6;
    Wy(1)=(3-2)/3;
    Wy(2:5)=1/6;
    for i=1:9 %此时为状态估计值，其中n为4
        x_gj=x_gj+Wx(i)*X(:,i);       
    end
    for i=1:5%此量测值估，其中n为2
         y_gj=y_gj+Wy(i)*Y(:,i);
     end
    for i=1:9
        Px=Px+Wx(i)*(X(:,i)-x_gj)*(X(:,i)-x_gj)';       
    end
    for i=1:5
         Py=Py+Wy(i)*(Y(:,i)-y_gj)*(Y(:,i)-y_gj)';
        Pxy=Pxy+Wy(i)*(X(:,i)-x_gj)*(Y(:,i)-y_gj)';
    end
    Px=Px+Rw;
    Py=Py+Rv;
    K=Pxy*inv(Py);