nisda.m

本文针对SLAM算法中数据关联过程

function NISDA(k,dim1)

global x;
global xmf;
global Pa;
global z;
global zhat;
global count;
global daall;


zhat=[];
for m=1:dim1/2
    zhat(m,1)=sqrt((x(2+m*2)-x(1))^2+(x(3+m*2)-x(2))^2); 
    if(x(2+m*2)-x(1)~=0)
        zhat(m,2)=atan((x(3+m*2)-x(2))/(x(2+m*2)-x(1)))-x(3); 
    else
        zhat(m,2)=atan((x(3+m*2)-x(2))/0.000001)-x(3);
    end
end    

count=1;
daall=[];
R=1*eye(2);
j=1;
for m=1:dim1/2
    temp=[];
    i=1;
    for n=1:(k-1)
        v=(z(n,1:2)-zhat(m,:))';
        r=sqrt((x(2+m*2)-x(1))^2+(x(3+m*2)-x(2))^2);
        Jhx=[    (x(2+m*2)-x(1))/r         (x(3+m*2)-x(2))/r;
             -(x(3+m*2)-x(2))/(r^2)      (x(2+m*2)-x(1))/(r^2)];
        P=Pa((2+m*2):(3+m*2),(2+m*2):(3+m*2));
        S=Jhx*P*Jhx'+R;
        g=v'*inv(S)*v;
        if(g<6)
            temp(i,1)=g;
            temp(i,2)=n;
            i=i+1;   
%             disp(n);
%             disp(g);
        end
    end
    if(temp~=[])
        min0=min(temp(:,1));
        if(min0<0.08)
            daall(j,1)=m;          
            [p q]=find(temp(:,1)==min0);
            daall(j,2)=temp(p,2);
            daall(j,3)=temp(p,1);
            z(temp(p,2),4)=1;
%             disp(daall(j,3));                % if matched, display the associated innovation
            j=j+1;
        end
    end
end    
count=j;  

% disp(daall);
i=1;
delrow=[];
sorted=[];
for m=1:(k-1)
    predel=[];
    [p q]=find(daall(:,2)==m);
    dimp=length(p);
    if(dimp>1)
        for n=1:dimp
            predel(n)=daall(p(n),3);
        end
%         disp(predel);
%         disp(m);
        min0=min(predel');
        for n=1:(count-1)
            if((daall(n,2)==m)&(daall(n,3)~=min0))
                delrow(i)=n;
                i=i+1;
            end
        end
    end
end

% disp(delrow);
if(i>1)
    sorted=sort(delrow);
%     disp(sorted);
    for m=(i-1):-1:1
        daall(sorted(m),:)=[];
    end
    count=count-(i-1);
end
% disp(daall);

count0=1;
daall0=[];
for n=1:(k-1)
    [p,q]=find(xmf(:,1)==z(n,3));
%     disp(p);
    if(p~=[])                
        daall0(count0,1)=p;              % 关联特征序号存入第一列,数值无重复
        daall0(count0,2)=n;              % 关联测量序号存入第二列,数值无重复    
        count0=count0+1;                 % 关联总对数为count-1
        xmf(p,2)=xmf(p,2)+1;           % 特征关联次数加1
    end
end    
% disp(daall0);

pair=0;
for n=1:count-1
    for m=1:count0-1
        if(daall(n,1:2)==daall0(m,:))
            pair=pair+1;
        end
    end
end
if(pair~=count0-1)
    disp('NNDA is wrong!');
end