fusion1.txt

利用matlab实现kalman滤波以及信息融合

p1=170;p2=180;an2=pi/180;an1=an2;b=125000;a=b;c=235000;d=130000;r1=110000;r2=120000;r3=2000;r4=2500;
V0=rand(60,1)*1196+4;An=rand(60,1)*2*pi;            %随机产生初识航速，航向
X0=normrnd(200000,150000,60,1);Y0=normrnd(150000,100000,60,1);             %随机产生初始位置
m=0;
for i=1:60                   %初始状态向量
    X(1,i)=X0(i);
    X(2,i)=V0(i)*cos(An(i));
    X(3,i)=Y0(i);
    X(4,i)=V0(i)*sin(An(i));
end
T=[1 2 0 0;0 1 0 0;0 0 1 2;0 0 0 1];          %状态转移矩阵
D=[1 0;1 0;0 1;0 1];                          %过程噪声分布矩阵
H=[1 0 0 0;0 0 1 0];                          %观测矩阵
for j=1:60                                    %卡尔漫滤波状态估计
    A0=X(:,j);
    q11=15*10^(-2)*A0(2);q22=15*10^(-2)*A0(4);
    v=[normrnd(0,q11),normrnd(0,q22)]';       %过程噪声
    A(:,1)=T*A0+D*v;
    an(1)=atan((A(3,1)-A0(3))/(A(1,1)-A0(1)));r(1)=sqrt(A(1,1)^2+A(3,1)^2);        %定义目标方位角和斜距
    Z0=[A0(1),A0(3)]';
    sigmax(1)=p1^2*(cos(an(1)))^2+(r(1))^2*an1^2*(sin(an(1)))^2;
    sigmay(1)=p1^2*(sin(an(1)))^2+(r(1))^2*an1^2*(cos(an(1)))^2;
    sigmaxy(1)=(p1^2-r(1)^2*an1^2)*sin(an(1))*cos(an(1));
    R0=[sigmax(1) sigmaxy(1);sigmaxy(1) sigmay(1)];        %观测噪声协方差矩阵
    w(:,1)=f(R0);                                          %自相关二维正态分布向量
    Z(:,1)=[A(1,1);A(3,1)]+w(:,1);
    B(1,1)=Z(1,1);B(2,1)=(Z(1,1)-Z0(1))/2;B(3,1)=Z(2,1);B(4,1)=(Z(2,1)-Z0(2))/2;       %一步状态估计
    P=[sigmax(1) sigmax(1)/2 sigmaxy(1) sigmaxy(1)/2;
        sigmax(1)/2 2*sigmax(1)/4 sigmaxy(1)/2 2*sigmaxy(1)/4;
        sigmaxy(1) sigmaxy(1)/2 sigmay(1) sigmay(1)/2;
        sigmaxy(1)/2 2*sigmaxy(1)/4 sigmay(1)/2 2*sigmay(1)/4];                        %一步状态更新协方差
    for k=2:12                                                                         %滤波递推
        q11(k-1)=15*10^(-2)*A(2,k-1);
        q22(k-1)=15*10^(-2)*A(4,k-1);
        v(:,k-1)=[normrnd(0,q11(k-1));normrnd(0,q22(k-1))];
        A(:,k)=T*A(:,k-1)+D*v(:,k-1);
        an(k)=atan((A(3,k)-A(3,k-1))/(A(1,k)-A(1,k-1)));
        r(k)=sqrt(A(1,k)^2+A(3,k)^2);
        sigmax(k)=p1^2*(cos(an(k)))^2+(r(k))^2*an1^2*(sin(an(k)))^2;
        sigmay(k)=p1^2*(sin(an(k)))^2+(r(k))^2*an1^2*(cos(an(k)))^2;
        sigmaxy(k)=(p1^2-r(k)^2*an1^2)*sin(an(k))*cos(an(k));
        R=[sigmax(k) sigmaxy(k);sigmaxy(k) sigmay(k)];
        w(:,k)=f(R);
        Z(:,k)=H*A(:,k)+w(:,k);
        C(:,k)=T*B(:,k-1);
        T=T*P*T'+D*[q11(k-1) 0;0 q22(k-1)]*D';
        L(:,k)=H*C(:,k);
        S=H*T*H'+R;
        K=T*H'*inv(S);
        B(:,k)=C(:,k)+K*(Z(:,k)-L(:,k)); %第K步状态估计
        P=T-k*S*k';                      %第K+1步状态更新协方差
        B1(:,m+k)=B(:,k);A1(:,m+k)=A(:,k);
    end
    m=m+12;
end