nis.m

利用人工合成数据进行了仿真实验

%***************************************************************
%*****         Nearest Neighbor Data Association      **********
%***************************************************************

% 传感器测量范围25米
% Firstly, use NIS;
% Secondly, use normalised distance;
clc;
clear all;
load all8;
% mnoise=0.1*randn(120,5);     % generate random noise 
load mnoise;                   % load noise used to all simulations

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

sensordis=25;         % scope of external sensor is circular with a radius of 25
inity=40;
NUM=87;               % the max number which can be selected is 87, or the filter will diverge.
[dimf temp0]=size(initf);  % dimf=the number of features

%-------设置初始环境及规划航迹--------------
x1=1:NUM;                               % 规划航迹,横坐标
dimx1=length(x1);
x2=inity*ones(1,dimx1);                 % 规划航迹,纵坐标

figure(1)
hold on;
bound1=0;bound2=120;disarea=bound2-bound1+1;
area1=[bound1:bound2;bound1*ones(1,disarea)];
area2=[bound2*ones(1,disarea);bound1:bound2];
area3=[bound2:-1:bound1;bound2*ones(1,disarea)];
area4=[bound1*ones(1,disarea);bound2:-1:bound1];
area=[area1 area2 area3 area4];
plot(area(1,:),area(2,:));
scatter(initf(:,1),initf(:,2),5,'bo');  % 全部特征的真实位置
plot(x1,x2,'bx-');

%-------------状态变量初始化------------
xv=[1.4 inity-0.2 0.003]';
j=1;
k=1;
for i=1:dimf
    if(sqrt((initf(i,1)-xv(1))^2+(initf(i,2)-xv(2))^2)<=sensordis)
        xm(j)=initf(i,1)+0.05*mnoise(i,1);
        xm(j+1)=initf(i,2)+0.05*mnoise(i,2);
        xmf(k,1)=i;
        k=k+1;
        j=j+2;
    end
end

xm=xm';
dim1=length(xm);
xmf(1:dim1/2,3:5)=0;
xmf(1:dim1/2,2)=1;
x=[xv;xm];
xpre(1,:)=xv';

P0=2;
Pa=P0*eye(dim1+3);
Paall(1,1)=Pa(1,1);
Paall(1,2)=Pa(2,2);
Paall(1,3)=0.1;

dim=length(x);
allx=zeros(NUM,NUM);
allx(1,1:dim)=x';
fnum(1)=0;       % 第i个位置处的关联对数
tempkff=1;
varianceall(1)=P0;
varianceall1(1)=P0;

for i=2:NUM                             % i为采样位置代号,第几个载体位置    
    
    %-------- prediction stage -------------    
    % compute predicted Xa
    xdelta=[];
    xdelta(1)=x1(i)-x1(i-1)+0.5*mnoise(i,3);  % 载体姿态变化量=真实值+噪声
    xdelta(2)=0.5*mnoise(i,4);         % ydelta real value equals zero
    xdelta(3)=0.5*mnoise(i,5);         % fidelta real value equals zero   
    
    x(1)=x(1)+xdelta(1)*cos(x(3))-xdelta(2)*sin(x(3));
    x(2)=x(2)+xdelta(1)*sin(x(3))+xdelta(2)*cos(x(3));
    x(3)=x(3)+xdelta(3);
    
    xpre(i,1)=xpre(i-1,1)+xdelta(1)*cos(xpre(i-1,3))-xdelta(2)*sin(xpre(i-1,3));
    xpre(i,2)=xpre(i-1,2)+xdelta(1)*sin(xpre(i-1,3))+xdelta(2)*cos(xpre(i-1,3));
    xpre(i,3)=xpre(i-1,3)+xdelta(3);

    %------------compute Pdelta-----------------
    Pdelta=2*eye(3);
    Pdelta(1,1)=2;
    Pdelta(2,2)=0.5;
    Pdelta(3,3)=0.5;
    
    % compute predicted Pa    
    xv=x(1:3);
    xm=x(4:dim);  
    dim1=length(xm);    % dim1/2为现有特征个数
    Pv=Pa(1:3,1:3);
    Pvm=Pa(1:3,4:dim);
    Pm=Pa(4:dim,4:dim);
    Jgxv=[1     0      -xdelta(1)*sin(x(3))-xdelta(2)*cos(x(3));...
          0     1       xdelta(1)*cos(x(3))-xdelta(2)*sin(x(3));...
          0     0                   1                          ];
    Jgxdelta=[cos(x(3))    -sin(x(3))      0;...
              sin(x(3))    cos(x(3))       0;...
                  0           0            1];
    temp=Jgxv*Pvm;
    Pa=[Jgxv*Pv*Jgxv'+Jgxdelta*Pdelta*Jgxdelta'   temp  ;...
                     temp'                         Pm   ];    
    
    %--------get observations-------------
    z=[];    % 重新为此次的观测分配存储空间
    R=[];
    k=1;             % k-1为测量个数,每次载体位置变动后归一
    for j=1:dimf       
        realdis=sqrt((initf(j,1)-x1(i))^2+(initf(j,2)-x2(i))^2);   
        
        if((realdis<=sensordis)&(initf(j,1)>x1(i)))
%             scatter(initf(j,1),initf(j,2),'^');
            z(k,1)=realdis+0.05*mnoise(k,1);            % 量测量=相对距离真实值+噪声    
            if(initf(j,1)-x1(i)~=0)
                z(k,2)=atan((initf(j,2)-x2(i))/(initf(j,1)-x1(i)));         % 计算观测相对于载体的角度,0度线为沿载体航向
            else
                z(k,2)=atan((initf(j,2)-x2(i))/0.000001);
            end
            z(k,3)=j;     % the index of the feature the observation corresponds to
            z(k,4)=0;     % this bit indicates whether the observation has been associated, default value is 0(not associated).
                        
            Rtemp=1*eye(2);
            R((2*k-1):2*k,1:2)=Rtemp;
            k=k+1;
        end           
    end
    if(z==[])    
        disp('Attention!Attention!Attention!Attention!!');      % no observation exists.    
    end
    % up to now,observations are ready.
    
    % ***************在此添加数据关联程序**********************
    
    disp(i);
    NISDA(k,dim1);
    
    %**********************************************************
    
    S=[];
    v=[]; 
    vda=[];
    Jhxa=[];
    Rda=[];
    for m=1:(count-1)
        p=daall(m,1);             % feature associated
        q=daall(m,2);             % observation associated        
        vda((2*m-1):2*m)=z(q,1:2)-zhat(p,:);
        
        deltax=x(2+p*2)-x(1);
        deltay=x(3+p*2)-x(2);
        d=sqrt((x(2+p*2)-x(1))^2+(x(3+p*2)-x(2))^2);
        if(d==0)
            d=0.000001;
        end
        
        part1=[1               1              1           1            2             2            2        2            2      ];
        part2=[1               2             2+2*m       3+2*m         1             2            3       2+2*m        3+2*m   ];        
        part3=[-deltax/d  -deltay/d         deltax/d    deltay/d   deltay/d/d    -deltax/d/d     -1   -deltay/d/d   deltax/d/d ];
        part3(find(abs(part3)<0.0001))=0.000001;
        Jhxa((2*m-1):2*m,:)=sparse(part1,part2,part3,2,dim);
        
        Rda((2*m-1):2*m,(2*m-1):2*m)=R((2*q-1):2*q,1:2); 
    end 
    vda=vda';
    vdaall(1:2*(count-1),i)=vda;
    
    if(count~=1)   % 若有关联上的特征，则更新，否则越过
        S=[];
        W=[];
        S=Jhxa*Pa*Jhxa'+Rda;
        W=Pa*Jhxa'*inv(S);
        x=x+W*vda;
        Pa=Pa-W*S*W';    
        
        fnum(i)=count-1;        % 该位置处的总关联对数
    else
        disp(i);        
    end
    
    m=19;
    if((dim1/2)>=11)
        temp0=any(daall(:,1)==m);
        if(temp0==1)
            kff(tempkff,:)=x((2+2*m):(3+2*m))';  % 保存第m个特征每次关联后的位置在tempkff中
            tempkff=tempkff+1;
        end
    end
            

    %--------- State Argumentation ----------------
    if(count~=k)    
        for n=1:(k-1)
            if((z(n,4)==0)&(~any(xmf(:,1)==z(n,3))))
                % state argumentation  
                dim=length(x);
                dim1=dim-3;
                Pv=Pa(1:3,1:3);
                Pvm=Pa(1:3,4:dim);
                Pm=Pa(4:dim,4:dim);
            
                temp2=x(1)+z(n,1)*cos(z(n,2)+x(3));
                temp3=x(2)+z(n,1)*sin(z(n,2)+x(3));
                x=[x;temp2;temp3];
                xmf(dim1/2+1,1)=z(n,3);
                disp('new augmented feature:')
                disp(z(n,3));
            
                mx1=zeros(dim,2);
                mx2=R((2*n-1):2*n,1:2);            
                Patemp=[Pa mx1;mx1' mx2];
                Pa=Patemp;
            
                Jgxv =[1               0                  -z(n,2)*sin(z(n,2)+x(3))  ;...
                       0               1                z(n,2)*cos(z(n,2)+x(3))     ];
                Jgz  =[cos(z(n,2)+x(3))                   -z(n,2)*sin(z(n,2)+x(3))  ;...
                       sin(z(n,2)+x(3))                   z(n,2)*cos(z(n,2)+x(3))   ];
                 Pa  =[   Pv            Pvm                         Pv*Jgxv'        ;...
                          Pvm'          Pm                          Pvm'*Jgxv'      ;...
                         Jgxv*Pv       Jgxv*Pvm           Jgxv*Pv*Jgxv'+Jgz*mx2*Jgz'];   
%              end
            end
        end                    
    end
    
    dim=length(x);
    Paall(i,1)=Pa(1,1);  % 状态加入前与加入后的Paall的值相同
    Paall(i,2)=Pa(2,2);
    Paall(i,3)=Pa(3,3);   
%     Paall(i,4)=det(Pa(4:dim,4:dim));
    Paall(i,4)=det(Pa(4:dim,4:dim));    
    varianceall(i)=Pa(11,11);
    varianceall1(i)=Pa(10,10);
    
    allx(i,1:3)=x(1:3)';  
    allx(i,4:dim)=x(4:dim)';
end

plot(xpre(:,1),xpre(:,2),'m*-');                         % 未滤波的位置
plot(allx(:,1),allx(:,2),'go-');                         % 滤波状态位置
dimf=length(xmf(:,1));
for i=1:dimf
     scatter(x(2+2*i),x(3+2*i),'rp');
end
legend('simulated area','designed path','dead-reckoning path','SLAM path');
title('posions of vehicle and features');
xlabel('x(m)');ylabel('y(m)');

%----------- vehicle state error -------------
for i=1:NUM
    error1(i)=allx(i,1)-i;    
    error2(i)=allx(i,2)-inity;
    error3(i)=allx(i,3);
    
    Psigma1(i)=Paall(i,1)^0.5;
    Psigma2(i)=Paall(i,2)^0.5;
    Psigma3(i)=Paall(i,3)^0.5;

end
maxerror1=max(abs(error1(2:NUM)));     
maxerror2=max(abs(error2(2:NUM)));
maxerror3=max(abs(error3(2:NUM))); 
figure(2)
hold on;
subplot(3,1,1);plot(1:NUM,error1,1:NUM,2*Psigma1,'k',1:NUM,-2*Psigma1,'k');xlabel('time(s)');ylabel('x error(m)');
title('vehicle state estimation error');
subplot(3,1,2);plot(1:NUM,error2,1:NUM,2*Psigma2,'k',1:NUM,-2*Psigma2,'k');xlabel('time(s)');ylabel('y error(m)');
subplot(3,1,3);plot(1:NUM,error3,1:NUM,2*Psigma3,'k',1:NUM,-2*Psigma3,'k');xlabel('time(s)');ylabel('psi error(rad)');

%----------- feature estimate error -----------
figure(3)
[p q]=find(xmf(:,4)==0);
if(p~=[])
    xmf(p,4)=x(2+2*p);
    xmf(p,5)=x(3+2*p);
end    
for i=1:dimf
    ferror(i,1)=abs(x(2+2*i)-initf(xmf(i,1),1));         
    ferror(i,2)=abs(x(3+2*i)-initf(xmf(i,1),2));
    fjulierror(i)=((x(2+2*i)-initf(xmf(i,1),1))^2+(x(3+2*i)-initf(xmf(i,1),2))^2)^0.5;
end
stem(1:dimf,fjulierror,'o-','fill','r-p');xlabel('特征序号');ylabel('估计绝对误差(m)');
title('所有特征估计的绝对误差');

%---------- innovation error --------------
dimvda=length(vdaall(:,1));
for i=1:dimvda/2
    vdaall1(i,:)=vdaall(2*i-1,:);
    vdaall2(i,:)=vdaall(2*i,:);
end    
figure(4)
vdaave1=mean(vdaall1);          % the mean value of innovation at each time step
subplot(2,1,1);plot(1:NUM,vdaave1,'b*-');xlabel('时间(s)');ylabel('距离新息(m)');axis([1 NUM -0.25 0.25]);
title('新息序列');
vdaave2=mean(vdaall2);
subplot(2,1,2);plot(1:NUM,vdaave2,'b*-');xlabel('时间(s)');ylabel('方位新息(rad)');axis([1 NUM -0.1 0.1]);

disp('the code execution is over!')