www.gusucode.com > MATLAB编程毕业设计 EKF SLAM仿真全部源代码 > data_associate.m

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%获取匹配项目zf，和新项目zn
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [zf,idf,zn]= data_associate(x,P,z,R, gate1, gate2)

zf= []; zn= [];
idf= []; 

Nxv= 3; %机器人状态向量的长度（x,y,theta）
Nf= (length(x) - Nxv)/2; %机器人库中LM的数目

%i：遍历所有观测到的landmark
for i=1:size(z,2)
    jbest= 0;
    %无限大
    nbest= inf;
    outer= inf;
    
    % search for neighbours
    %第i号观测到的Landmark与库中的所有landmark进行匹配比较，分别得到nis距离和nd距离
    %前提是nis距离小于gate1，在这个前提下选出nd距离最小的作为成功匹配项
    %若是没有一个nis距离小于gate1，那么选出nis距离的最小是outer，若是outer足够大...
    %...表明这个landmark和库中landmark距离最近的值是outer
    for j=1:Nf
        [nis, nd]= compute_association(x,P,z(:,i),R, j);
        if nis < gate1 & nd < nbest %小于阈值
            nbest= nd;
            jbest= j;
        elseif nis < outer
            outer= nis;
        end
    end
    
    % zn的发现
    if jbest ~= 0
        zf=  [zf  z(:,i)];
        idf= [idf jbest];
    elseif outer > gate2 % z太远了，不能够和库中的LM匹配成功，不过远度大于阈值可以认为发现了新LM
        zn= [zn z(:,i)];
    end
end


function [nis, nd]= compute_association(x,P,z,R,idf)
%返回规则化的normalised innovation squared（马氏距离）以及normalised distance
[zp,H]= observe_model(x, idf);
v= z-zp; 
v(2)= pi_to_pi(v(2));
S= H*P*H' + R;

nis= v'*inv(S)*v;
nd= nis + log(det(S));