三维SLAM路径规划

% 3D SLAM with linear KF - Moving vehicle - Relative measurement - Limited
% Sensor Range - Observing (x,v,landmarks) - Landmarks updated once
% observed
%
% A 3 DOF underwater robot is moving along a path detecting some
% motionless landmarks; the position/velocity of the robot and position
% of landmarks are computed by means of Simultaneos Localization and 
% Mapping using a linear Kalman Filter.  
% All the measures are relative to robot position.
% The measuring sensor is limited to a certain range distance.
% Landmarks are updated in the state once observed.
%
% Programmed by Joaquim Salvi

clear all;
NumberTimeStamps = 600;
MapDimension = [1,200;1,150;-100,0];   % X Y Z
MaximumRange = 50;

% INTRODUCE LANDMARKS
figure(1); clf;
title('Introduce 3D Landmarks');
v=[MapDimension(1,1)-10 MapDimension(1,2)+20 MapDimension(2,1)-10 MapDimension(2,2)+10];
axis(v); hold on;
xlabel('x'); ylabel('y');
text((MapDimension(1,2))/2,MapDimension(2,2)+5,'X-Y Plane');
text(MapDimension(1,2)+20-20,MapDimension(2,2)+5,'Z Plane');
plot([MapDimension(1,1); MapDimension(1,1)],[MapDimension(2,1); MapDimension(2,2)],'k-');
plot([MapDimension(1,1); MapDimension(1,2)],[MapDimension(2,1); MapDimension(2,1)],'k-');
plot([MapDimension(1,1); MapDimension(1,2)],[MapDimension(2,2); MapDimension(2,2)],'k-');
plot([MapDimension(1,2); MapDimension(1,2)],[MapDimension(2,1); MapDimension(2,2)],'k-');
plot([MapDimension(1,2); MapDimension(1,2)+10],[MapDimension(2,1); MapDimension(2,1)],'k-');
plot([MapDimension(1,2); MapDimension(1,2)+10],[MapDimension(2,2); MapDimension(2,2)],'k-');
plot([MapDimension(1,2)+10; MapDimension(1,2)+10],[MapDimension(2,1); MapDimension(2,2)],'k-');
stepz = (MapDimension(3,2)-MapDimension(3,1))/10;
stepy = (MapDimension(2,2)-MapDimension(2,1))/10; posy = MapDimension(2,1);
for i=MapDimension(3,1):stepz:MapDimension(3,2)
    st = sprintf('%0d',i);
    text(MapDimension(1,2)+11,posy,st);
    posy = posy + stepy;
end
pin=1; button = 0;
nlandmarks=0; z = 0;
poszx = MapDimension(1,2)+5;
poszy = MapDimension(2,2);
plot(poszx,poszy,'ro');
% Get input landmarks graphically, click right button to finish
while button ~= 3
   [x,y,button]=ginput(1);
   pin= ~isempty(x);
   if pin && button ~= 3
       if x > MapDimension(1,1) && x < MapDimension(1,2) && y > MapDimension(2,1) && y < MapDimension(2,2) 
          nlandmarks = nlandmarks+1;
          plot(x,y,'r*'); st = sprintf('%0d',round(z)); text(x+2,y+2,st);
          pp(:,nlandmarks)=[x;y;z];
       elseif x>MapDimension(1,2) && x < MapDimension(1,2)+10 && y > MapDimension(2,1) && y < MapDimension(2,2) 
           plot(poszx,poszy,'wo');
           z = MapDimension(3,1) + y*(MapDimension(3,2)-MapDimension(3,1))/(MapDimension(2,2)-MapDimension(2,1));
           poszy = y;
           plot(poszx,poszy,'ro');
       end
   end
end
hold off;
NumberLandmarks = size(pp,2);

% DEFINE TRAJECTORY
% Definition of the real trajectory; vehicle with constant velocity
figure(2); clf;
title('Introduce trajectory around landmarks');
v=[MapDimension(1,1)-10 MapDimension(1,2)+20 MapDimension(2,1)-10 MapDimension(2,2)+10];
axis(v); hold on;
xlabel('x'); ylabel('y');
text((MapDimension(1,2))/2,MapDimension(2,2)+5,'X-Y Plane');
text(MapDimension(1,2)+20-20,MapDimension(2,2)+5,'Z Plane');
plot([MapDimension(1,1); MapDimension(1,1)],[MapDimension(2,1); MapDimension(2,2)],'k-');
plot([MapDimension(1,1); MapDimension(1,2)],[MapDimension(2,1); MapDimension(2,1)],'k-');
plot([MapDimension(1,1); MapDimension(1,2)],[MapDimension(2,2); MapDimension(2,2)],'k-');
plot([MapDimension(1,2); MapDimension(1,2)],[MapDimension(2,1); MapDimension(2,2)],'k-');
plot([MapDimension(1,2); MapDimension(1,2)+10],[MapDimension(2,1); MapDimension(2,1)],'k-');
plot([MapDimension(1,2); MapDimension(1,2)+10],[MapDimension(2,2); MapDimension(2,2)],'k-');
plot([MapDimension(1,2)+10; MapDimension(1,2)+10],[MapDimension(2,1); MapDimension(2,2)],'k-');
stepz = (MapDimension(3,2)-MapDimension(3,1))/10;
stepy = (MapDimension(2,2)-MapDimension(2,1))/10; posy = MapDimension(2,1);
for i=MapDimension(3,1):stepz:MapDimension(3,2)
    st = sprintf('%0d',i);
    text(MapDimension(1,2)+11,posy,st);
    posy = posy + stepy;
end
for i = 1:NumberLandmarks
    plot(pp(1,i),pp(2,i),'r*'); 
    st = sprintf('%0d',round(pp(3,i))); text(pp(1,i)+2,pp(2,i)+2,st);
end;
pin = 1; button = 0; fi = 0;
npoints = 0; dist = 0; z = 0;
poszx = MapDimension(1,2)+5;
poszy = MapDimension(2,2);
plot(poszx,poszy,'ro');
% get trajectory segments, click right button to finish 
while button ~= 3
   [x,y,button]=ginput(1);
   pin= ~isempty(x);
   if pin && button ~= 3
       if x > MapDimension(1,1) && x < MapDimension(1,2) && y > MapDimension(2,1) && y < MapDimension(2,2) 
          npoints = npoints + 1;
          t(:,npoints)=[x;y;z];
          plot(x,y,'go'); st = sprintf('%0d',round(z)); text(x+2,y+2,st);
          if npoints > 1
            plot(t(1,npoints-1:npoints),t(2,npoints-1:npoints),'g-');
            dist = dist + norm(t(:,npoints) - t(:,npoints-1));
          end
       elseif x>MapDimension(1,2) && x < MapDimension(1,2)+10 && y > MapDimension(2,1) && y < MapDimension(2,2) 
           plot(poszx,poszy,'wo');
           z = MapDimension(3,1) + y*(MapDimension(3,2)-MapDimension(3,1))/(MapDimension(2,2)-MapDimension(2,1));
           poszy = y;
           plot(poszx,poszy,'ro');
       end
   end
end
% Sampling NumberTimeStamps points in the given trajectory.
point = 2; dist2=0; incdist=dist/NumberTimeStamps;
tt(:,1)=t(:,1);
for i = 2:NumberTimeStamps
    tt(:,i)=tt(:,i-1)+ incdist*((t(:,point)-t(:,point-1))/norm(t(:,point)-t(:,point-1))); % tx,ty,tz trajectories
    vv(:,i-1)=tt(:,i)-tt(:,i-1); % vx,vy,vz velocities
    plot(tt(1,i),tt(2,i),'b.');
    dist2 = dist2 + incdist;
    if (dist2 + incdist) > norm(t(:,point)-t(:,point-1)) && abs((dist2 + incdist)-norm(t(:,point)-t(:,point-1))) > abs(dist2-norm(t(:,point)-t(:,point-1)))
        point = point + 1; dist2 = 0;
    end
end
plot(tt(1,:),tt(2,:),'b.');
hold off;

% DEFINE INITIAL PARAMETERS
v = vv;  % Velocity is the discrepancy between trajectory points
x = tt;  % Trajectory is the interpolated trajectory introduced by user
         % Landmarks are arranged in a vector form (x1,y1,x2,y2,... yn)
clear vv;
clear z;
for i=1:NumberLandmarks
    p(3*i-2)=pp(1,i);
    p(3*i-1)=pp(2,i);
    p(3*i)=pp(3,i);
end     

% Definition of process noise and initial noises
ri = (30*0.5)^2;  % landmark measurement noise, change to see effect
rxi = (3*0.5)^2; % position measurement noise, change to see effect
rvi = (1*0.5)^2; % velocity measurement noise, change to see effect
pn = (10*0.1)^2; % process noise, change to see effect

% Definition of Measurement matrix to use in prediction
% Landmark distance are relative to robot position
HP = diag(ones(3+3,1));

% Definition of Measurement matrix to use in ground truth
% Landmark distance are relative to robot position
HO = [diag(ones(3+3,1)) zeros(3+3,3*NumberLandmarks)];
for i=1:NumberLandmarks
    HO = [HO;-1 0 0 0 0 0 zeros(1,3*(i-1)) 1 0 0 zeros(1,3*(NumberLandmarks-i))];
    HO = [HO;0 -1 0 0 0 0 zeros(1,3*(i-1)) 0 1 0 zeros(1,3*(NumberLandmarks-i))];
    HO = [HO;0 0 -1 0 0 0 zeros(1,3*(i-1)) 0 0 1 zeros(1,3*(NumberLandmarks-i))];    
end

% Definition of the State matrix
% The State matrix is a diagonal of ones, which means that the prediction
% of next state is equivalent to current state, except the position which
% is updated with the current velocity at every time stamp.
Fk = diag(ones(1,3+3)); Fk(1,4)=1; Fk(2,5)=1; Fk(3,6)=1;   

% Definition of the Control vector
% The control vector is null, which means that the are no external inputs
% that change the robot state.
Uk = zeros(3+3,1);  

% Definition of the Covariance matrix
% The initial covariance matrix is defined with a large diagonal uncertainity
% in both state of the robot and position of the landmarks and with an
% equivalent uncertainity which means that none prevail over the other.
Pk = [(rxi+rvi) 0 0 rvi 0 0;0 (rxi+rvi) 0 0 rvi 0; 0 0 (rxi+rvi) 0 0 rvi; rvi 0 0 rvi 0 0 ;0 rvi 0 0 rvi 0;0 0 rvi 0 0 rvi];

%Definition of the Process Noise Matrix
Q = diag(pn*ones(1,3+3));

% Definition of measurement noises (may differ from initial noises
r = (10*0.5)^2;  % landmark measurement noise, change to see effect
rx = (2*0.5)^2; % position measurement noise, change to see effect
rv = (0.1*0.5)^2; % velocity measurement noise, change to see effect

%Definition of the Measurement Noise Matrix
R = diag([rx*ones(1,3) rv*ones(1,3)]); 

% Initial prediction of the vector state
for kk = 1:NumberTimeStamps
    xhat{kk} = [];
    zhat{kk} = [];
    Un{kk} = [];
end
xhat{1}= [x(:,1)' v(:,1)']';
N = sqrt(pn)*randn(3+3,1); xhat{1}(1:6) = xhat{1}(1:6) + N; % adding process noise
xtt(:,1) = xhat{1}(1:3);  % Only for plotting purposes

% KALMAN FILTER LOOP
figure(3); clf; 
handle = gcf;
set(handle,'DoubleBuffer','on'); 
view(3);

landobs = []; % Set of landmarks observed at least once, i.e. already updated in Pk.
plotland(:,1) = zeros(NumberLandmarks,1);
for k = 1:NumberTimeStamps-2
    
    % Prediction
    xhat{k+1} = Fk*xhat{k}+Uk;     % Prediction of next state
    zhat{k+1} = HP*xhat{k+1};      % Measure at the predicted position
    Pk = Fk*Pk*Fk' + Q;
    
    % Observation
    N = [sqrt(rx)*randn(3,1); sqrt(rv)*randn(3,1); sqrt(r)*randn(3*NumberLandmarks,1)];
    z(:,k+1)= HO*[x(:,k+1)' v(:,k+1)' p]' + N; % Measure at the correct position x(k+1) v(k+1) and p
    vvt=z(1:6,k+1)-zhat{k+1}(1:6);  % Innovation vector, i.e. discrepancy between measures
    znot(:,k+1)= HO*[x(:,k+1)' v(:,k+1)' p]'; % Measure at the correct position x(k+1) v(k+1) and p
    
    landdec = []; landnotdec = []; % landmarks detected/non-detected sets
    plotland(:,k+1) = zeros(NumberLandmarks,1); % only for plotting purposes
    for kk=7:3:7+3*NumberLandmarks-1  % Explore the detection of landmarks
        if norm(znot(kk:kk+2,k+1)) < MaximumRange % Landmark in not-noisy range?       
            landdec = [landdec; (kk-1)/3-1]; % Add detected landmark identifier to the set
            [check,pos] = ismember((kk-1)/3-1,landobs); % Check if landmark was observed in the past
            if check
                vvt=[vvt;z(kk:kk+2,k+1)-zhat{k+1}(6+3*pos-2:6+3*pos)];  % Innovation vector, i.e. discrepancy between measures
                plotland((kk-1)/3-1,k+1) = 1;
            else
                landnotdec = [landnotdec; (kk-1)/3-1]; % Add landmark as non-detected for another step
            end
        else
            landnotdec = [landnotdec; (kk-1)/3-1]; % Add non-detected landmark identifier to the set
        end
    end
    
    Ht = HP; 
    for kk=size(landobs,1):-1:1  % Removing rows/column of once observed but now non-detected landmarks
        [check,pos] = ismember(landobs(kk),landnotdec);
        if check
            Ht(6+3*kk-2:6+3*kk,:) = []; 
        end
    end

    Rt = diag([rx*ones(1,3) rv*ones(1,3) r*ones(1,size(Ht,1)-6)]);
 

D62