fform_validation_matrix.asv

IMM滤波的 matlab程序

function [omega,Z]=fform_validation_matrix(Z1,Z2)
%如何生成确认矩阵

[n,n1]=size(Z1);
[n,n2]=size(Z2);
k=1;%落入门限公共部分量测个数
flag(:,k)=[0;0];%量测为同一量测的判断标志。
flag1(k)=0;%Z1中有和Z2相同量测标志
flag2(k)=0;%Z2中有和Z1相同量测标志
for i=1:n1
    for j=1:n2
        sign(:,j)=Z1(:,i)-Z2(:,j);
        if sign(1,j)==sign(2,j)&sign(1,j)==0 % 判断Z1(:,i)和Z2(:,j)是否为同一量测。
            k=k+1;
            flag(:,k)=[i;j]; %量测为同一量测的判断标志。
            flag1(k)=i; %Z1中有和Z2相同量测标志
            flag2(k)=j; %Z2中有和Z1相同量测标志
        end
    end
end

omega=zeros(n1+n2-k+1,3);%预先生成空的确认阵

for i=1:n1
    a=find(flag1==i); % 判断第i个量测是否为公共量测
    if isempty(a)==1%若不为公共量测
        omega(i,:)=[1,1,0];
    else%若为公共量测
        omega(i,:)=[1,1,1];
    end
    Z(:,i)=Z1(:,i);%统一的量测
end

%在剩余的确认阵内放入量测
for i=n1+1:n1+n2-k+1
    omega(i,:)=[1,0,1];
end
%对Z2重新统一在Z中编号
L=n1+1;
for i=1:n2
    aa=find(flag2==i);
    if isempty(aa)==1
        Z(:,L)=Z2(:,i);
        L=L+1;
    end
end