Simulation
PURPOSE 
Simulates a robot performing Pose SLAM.
SYNOPSIS
function Result=Simulation(F,R,Parameters)
DESCRIPTION
Simulates a robot performing Pose SLAM.
This function is also used to continue a previously started simulation.
Parameters:
F: the P_Filter to use to estimate the trajectory of the robot.
R: The robot to use in the simulation
Parameters.maxSteps: Number of steps to run the simulation.
Set to 0 to use as many steps as sensor readings available.
This parameter and the use of this function as well as
ContinueSimulation allow to run a full simulation in
separate phases.
The first step is the initial estimation. So use at maxSteps
2 to perform one filter step and so on.
Parameters.plot: true if the trajectory as estimated by the filter
has to be plotted while simulating.
Parameters.plotSensors: Array of booleans indicating which sensors
are to be plotted along with the filter information.
Right now only RealVison and RealLaser sensors have
non-void Plot functions. In the first case the images
of the cameras are shown at the right of the
trajectory and in the second case the laser readings
are plotted at the corresponding pose.
Parameters.Domain: Area where the robot is expected to move. Used
to define the tree of poses and the axis of the plot.
Parameters.loopClosure: Selects the method to determine the loop
closure points.
The rest of parametres depend on the 'loopClosure' value
For 'NoLoops' or 'AllLoos' no more parameters are needed.
For 'DetectLoops'or 'TreeDetectLoops' we have to provide
- Parameters.matchArea: Area arround a pose where it is likely
to get similar sensor readings (sensor readings that can be
registered to the sensor readings of the current pose).
This is 'v_r' in the paper.
Todo: integrate this threshold in the sensor information.
- Parameters.loopNoise: Typical noise of the sensors closing
loops. The sensors that will be used to register poses.
This is \bar{\Sigma_y} in the paper.
- Parameters.pdRange: An interval [p1,p2]. If a pose has a
probability higher p1 of being in the match area of the
current robot's pose it is considered for a loop (if the
information test below agrees to do so). If the pose has
probability higher than p2 of being in the match area of the
current robot's pose the current pose is considered for
removal (see information test below)
This is 's' in the paper. However here we allow to define two
values for it.
- Parameters.igRange: An interval [ig1,ig2]. If the information
gain of apossible loop closure (suggested by the distance
criterion) is above ig2 the loops closure is considered
informativeenough. If the information gain is below ig1, the
current robot pose is considered redundant (it can be removed
since there is a very similar pose already in the trajectory).
The information criterion is only considered after the
distance one.
This is 'g' in the paper. However we allow to define two
values for it.
Outputs:
Results: A structure with the Filter, the Robot, the Parameters and
some outputs (time, inf. gain,...)CROSS-REFERENCE INFORMATION
This function calls:
- GetOdometry Get the odometry readings witht the different available sensors.
- MoveRobot Moves the robot by one time slice.
- display Displays the name of the robot.
- get Get function for robots.
- display Displays a trajectory.
- get Get function for trajectories.
- size Number of poses in a trajectory.
- Update Adds a new pose to an EKF filter.
- UpdateLoop Adds a new loop to an EKF filter.
- get Get function for EKF filters.
- AllLoops Selects all possible pairs of poses as potential loops.
- ComputeNEES Quantifies the consitency of a filter.
- DetectLoops Propose loops based on the distance between poses searching linearly.
- GetPoseData Defines a PoseData objects from the information stored in the filter.
- InformationGain Information gain by closing a loop with a previous pose.
- InitFilterPlot Initializes the plot of a filter.
- NoLoops Avoids any loop to be stablished.
- PlotFilter (Re)plots a filter.
- PreviousPose Identifies the previous pose.
- ShowLinkToNeighbour Shows the line to a possible neighbour.
- TreeDetectLoops Propose loops based on the distance between poses using a tree for the search.
- Update Adds a pose to the filter.
- UpdateFilterPlot Updates the plot of a filter with a new pose.
- UpdateLoop Adds a loop to the filter.
- display Displays the type of filter F.
- get Generic get function for filters.
- size Size of the state estimated in the filter
- GetPoseData Defines a PoseData objects from the information stored in the filter.
- Update Adds a new pose to a TRO filter.
- UpdateLoop Adds a new loop to a TRO filter.
- get Get function for TRO filters.
- BTree Balanced tree constructor.
- InsertInTree Inserts an element in a BTree.
- UpdateTree Updates the information in a BTree.
- display Displays a balanced tree.
- get Get function for BTrees.
- display Displays a PoseData object.
- get Get function for PoseData objects.
- display Displays the information of a Gaussian (mean and covariance).
- get Get function for Gaussians.
- display Displays an interval matrix.
- size Size (rows/columns) of an interval matrix.
- HideLinkToNeighbour Hides the line to a possible neighbour.
- display Displays a pose.
- get Get function for poses.
- size Number of parameters of the pose.
- get Generic get for relative positioning sensors
- SensorLoopCloses Checks if two laser scan can be properly aligned.
- display Displays the type of sensor.
- get Generic get for sensors
- size Number of readings stored in the Sensor.
- SensorLoopCloses Checks if two sensor readigs can be properly registered.
- Experiment1a Simulated experiment.
- Experiment1b Simulated experiment.
- Experiment1c Simulated experiment.
- ContinueSimulation Continues a previous partial simulation.
SOURCE CODE
0001 function Result=Simulation(F,R,Parameters) 0002 % Simulates a robot performing Pose SLAM. 0003 % This function is also used to continue a previously started simulation. 0004 % 0005 % Parameters: 0006 % F: the P_Filter to use to estimate the trajectory of the robot. 0007 % R: The robot to use in the simulation 0008 % Parameters.maxSteps: Number of steps to run the simulation. 0009 % Set to 0 to use as many steps as sensor readings available. 0010 % This parameter and the use of this function as well as 0011 % ContinueSimulation allow to run a full simulation in 0012 % separate phases. 0013 % The first step is the initial estimation. So use at maxSteps 0014 % 2 to perform one filter step and so on. 0015 % Parameters.plot: true if the trajectory as estimated by the filter 0016 % has to be plotted while simulating. 0017 % Parameters.plotSensors: Array of booleans indicating which sensors 0018 % are to be plotted along with the filter information. 0019 % Right now only RealVison and RealLaser sensors have 0020 % non-void Plot functions. In the first case the images 0021 % of the cameras are shown at the right of the 0022 % trajectory and in the second case the laser readings 0023 % are plotted at the corresponding pose. 0024 % Parameters.Domain: Area where the robot is expected to move. Used 0025 % to define the tree of poses and the axis of the plot. 0026 % Parameters.loopClosure: Selects the method to determine the loop 0027 % closure points. 0028 % The rest of parametres depend on the 'loopClosure' value 0029 % For 'NoLoops' or 'AllLoos' no more parameters are needed. 0030 % For 'DetectLoops'or 'TreeDetectLoops' we have to provide 0031 % - Parameters.matchArea: Area arround a pose where it is likely 0032 % to get similar sensor readings (sensor readings that can be 0033 % registered to the sensor readings of the current pose). 0034 % This is 'v_r' in the paper. 0035 % Todo: integrate this threshold in the sensor information. 0036 % - Parameters.loopNoise: Typical noise of the sensors closing 0037 % loops. The sensors that will be used to register poses. 0038 % This is \bar{\Sigma_y} in the paper. 0039 % - Parameters.pdRange: An interval [p1,p2]. If a pose has a 0040 % probability higher p1 of being in the match area of the 0041 % current robot's pose it is considered for a loop (if the 0042 % information test below agrees to do so). If the pose has 0043 % probability higher than p2 of being in the match area of the 0044 % current robot's pose the current pose is considered for 0045 % removal (see information test below) 0046 % This is 's' in the paper. However here we allow to define two 0047 % values for it. 0048 % - Parameters.igRange: An interval [ig1,ig2]. If the information 0049 % gain of apossible loop closure (suggested by the distance 0050 % criterion) is above ig2 the loops closure is considered 0051 % informativeenough. If the information gain is below ig1, the 0052 % current robot pose is considered redundant (it can be removed 0053 % since there is a very similar pose already in the trajectory). 0054 % The information criterion is only considered after the 0055 % distance one. 0056 % This is 'g' in the paper. However we allow to define two 0057 % values for it. 0058 % 0059 % Outputs: 0060 % Results: A structure with the Filter, the Robot, the Parameters and 0061 % some outputs (time, inf. gain,...) 0062 0063 if ~Parameters.quiet 0064 fprintf('Simulating: \n'); 0065 end 0066 0067 % Robot's number of sensors 0068 Sensors=get(R,'sensors'); 0069 numSensors=size(Sensors,2); 0070 0071 pt=InitFilterPlot(F,Parameters); 0072 0073 % Default output is a tree from the filter data (non-empty if we 0074 % are continuing a previously stopped simulation). 0075 if strcmp(Parameters.loopClosure,'TreeDetectLoops') 0076 pd=GetPoseData(F); 0077 T=BTree({pd{1:(end-1)}}); 0078 else 0079 T=0; 0080 end 0081 0082 % sometimes, when detecting possible points for loop closure we 0083 % detect that current pose is redundant (i.e., we already have 0084 % a nearby pose in the filter). If so, next pose to be added 0085 % replaces the current one. This helps to control the size 0086 % of the filter. 0087 redundantPose=false; 0088 0089 % To gather estatistics about the loop closure 0090 NPL=0; % number of proposed loops 0091 NCL=0; % number of actually closed loops 0092 0093 % To gather statistics about execution time 0094 timeSA=0; % Time used in state augmentation (Update) 0095 timeLC=0; % Time used in loop closure (UpdateLoop) 0096 timeS=0; % Time used searching for neighbours 0097 timeT=0; % Time used inserting in the tree and re-building it 0098 0099 totalIG=0; % total information gain of actually closed loops 0100 0101 % To comptute the filter consitency (only when grount truth is 0102 % available). 0103 groundTruthAvailable=isfield(Parameters,'groundTruth'); 0104 NEES=[]; 0105 0106 % continue from last step 0107 step=get(F,'nSteps'); 0108 0109 done=false; 0110 0111 while ~done 0112 0113 step=step+1; 0114 0115 if Parameters.maxSteps>0 0116 done=(step>=Parameters.maxSteps); 0117 end 0118 0119 if ~done 0120 % And now iterate reading the sensors and updating the filter 0121 % done is set to true when no more sensor readings are available 0122 [done R]=MoveRobot(R); % returns done=false whe no more sensor readings 0123 end 0124 0125 if ~done 0126 0127 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0128 % Update the filter with the new sensor readings -> add a new state 0129 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0130 [SR Sn]=GetOdometry(R); 0131 0132 ck=cputime; 0133 F=Update(F,step,SR,Sn,redundantPose); 0134 timeSA(step)=(cputime-ck); 0135 0136 if Parameters.plot 0137 if redundantPose 0138 pt=PlotFilter(F,Parameters); 0139 else 0140 pt=UpdateFilterPlot(F,step,pt,true); 0141 end 0142 end 0143 0144 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0145 % Use the filter information to detect possible points for loop 0146 % closuse (i.e., if the current pose is close enough) 0147 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0148 switch Parameters.loopClosure 0149 case 'NoLoops' 0150 L=NoLoops(F); 0151 0152 case 'AllLoops' 0153 L=AllLoops(F); 0154 0155 case 'DetectLoops' 0156 ck=cputime; 0157 L=DetectLoops(F,Parameters.matchArea,Parameters.pdRange); 0158 timeS(step)=(cputime-ck); 0159 0160 case 'TreeDetectLoops' 0161 if step>1 0162 pd=GetPoseData(F,step-1); 0163 if isa(pd,'PoseData') 0164 % if 'step' is not one of the removed steps 0165 ck=cputime; 0166 T=InsertInTree(T,pd); 0167 timeT(step)=(cputime-ck); 0168 end 0169 end 0170 ck=cputime; 0171 L=TreeDetectLoops(F,T,Parameters.matchArea,Parameters.pdRange); 0172 timeS(step)=(cputime-ck); 0173 otherwise 0174 error('Unknow loop closure procedure'); 0175 end 0176 0177 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0178 % Check if at least one of the sensors confirm the loop closure 0179 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0180 n=size(L,2); 0181 0182 NPL=NPL+n; % number of tentative loop closures 0183 0184 redundantPose=false; 0185 0186 ncl=0; % number of closed loops in this time slice 0187 0188 % Compute the information gain for all the tentative links 0189 allI=cellfun(@(n)(InformationGain(F,Parameters.noiseLoopClosure,n.step)),L); 0190 0191 while n>0 0192 0193 % select the maximally informative link. 0194 [I mi]=max(allI); 0195 link=L{mi}; 0196 cl=false; % the loop is not closed so far 0197 0198 % link to the neighbouring pose 0199 ShowLinkToNeighbour(F,Parameters,pt,step,link.step,'y'); 0200 0201 if (I>Parameters.igRange(2)) && ~PreviousPose(F,link.step) 0202 0203 % link to the neighbouring informative pose 0204 ShowLinkToNeighbour(F,Parameters,pt,step,link.step,'b'); 0205 0206 % Look for the sensor that confirm the loop closure link 0207 j=1; 0208 nSLC=0; % Number of sensors that confirm the loop closure 0209 0210 while (j<=numSensors) && (I>Parameters.igRange(2)) 0211 [c d Sn]=SensorLoopCloses(get(R,'sensor',j),link.step,step); 0212 0213 if c 0214 % If the sensor confirms the loop 0215 % we compute the true information gain 0216 Is=InformationGain(F,Sn,link.step); 0217 I=min(I,Is); 0218 0219 if Is>Parameters.igRange(2) 0220 % This sensor confirms the loop and it is truly 0221 % informative: Store the link data (link mean and noise) 0222 % according to this sensor 0223 nSLC=nSLC+1; 0224 dSLC{nSLC}=d; 0225 sSLC{nSLC}=Sn; 0226 end 0227 end 0228 j=j+1; 0229 end 0230 0231 % if at least one sensor confirms the infomrative link 0232 if nSLC>0 0233 cl=true; % the link is actually confirmed and the loop closed 0234 0235 % if at least one sensor confirms the loop closure, execute it. 0236 ck=cputime; 0237 F=UpdateLoop(F,link.step,dSLC,sSLC); 0238 timeLC(step)=(cputime-ck); 0239 0240 ncl=ncl+1; 0241 0242 totalIG=totalIG+I; 0243 0244 if ~Parameters.quiet 0245 fprintf(' Loop closure: [%u %u] ig:%f pd:%f -> ',link.step,step,I,link.pd); 0246 display(dSLC{1}); 0247 end 0248 0249 if Parameters.plot 0250 % Replot the filter after the loop closure 0251 pt=PlotFilter(F,Parameters); 0252 end 0253 end 0254 end % information is large enough 0255 0256 % If the pose is close to another pose and the change in the map when 0257 % linking the two poses is small -> the current pose is redundant 0258 if (link.pd>Parameters.pdRange(2)) && (I<Parameters.igRange(1)) 0259 redundantPose=true; 0260 end 0261 0262 L(mi)=[]; % Removes element mi from L 0263 0264 if cl 0265 % If the link resulted in a loop we have to recompute the 0266 % information gain 0267 allI=cellfun(@(n)(InformationGain(F,Parameters.noiseLoopClosure,n.step)),L); 0268 else 0269 allI(mi)=[]; % Removes element mi from allI 0270 end 0271 0272 n=size(L,2); % n=n-1 0273 0274 end % while n>0 0275 0276 if ncl>0 0277 redundantPose=false; 0278 if strcmp(Parameters.loopClosure,'TreeDetectLoops') 0279 % If we closed a loop, we have to update the tree 0280 pd=GetPoseData(F); 0281 ck=cputime; 0282 T=UpdateTree(T,F,pd); 0283 timeT(step)=(cputime-ck); 0284 end 0285 end 0286 0287 NCL=NCL+ncl; %number of loops actually closed 0288 0289 if groundTruthAvailable 0290 NEES(step)=ComputeNEES(F,Parameters.groundTruth); 0291 end 0292 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0293 0294 pt=HideLinkToNeighbour(Parameters,pt); 0295 0296 end % if ~done 0297 0298 end % while 0299 0300 Result.Filter=F; % Filter at the end of the simulation 0301 Result.Robot=R; % Robot used in the simulation 0302 Result.Tree=T; % Tree used for data association, possibly empty 0303 Result.Parameters=Parameters; % Parameters used in the simulation 0304 0305 Result.timeS=timeS; % Seach time during data association 0306 Result.timeT=timeT; % Time used building and updating the tree 0307 Result.timeSA=timeSA; % Time used in state augmentation (one value per iteration) 0308 Result.timeLC=timeLC; % Time used in loop closure (one value per iteration) 0309 Result.timeF=sum(timeSA)+sum(timeLC); % Total time used in filter operations 0310 Result.time=Result.timeF+sum(Result.timeS)+sum(Result.timeT); % Total time used in the simulation 0311 Result.numClosedLoops=NCL; % Number of closed loops 0312 Result.numProposedLoops=NPL; % Number of proposed loops 0313 0314 Result.totalIG=totalIG; % Sum of the information gain for all loops 0315 Result.NEES=NEES; % NEES per iteration 0316 0317 if ~Parameters.quiet 0318 fprintf('\nSimulation done:\n'); 0319 fprintf(' Execution time: %f s (%f s per step)\n',Result.time,Result.time/step); 0320 fprintf(' NN search time : %f\n',sum(Result.timeS)); 0321 if strcmp(Parameters.loopClosure,'TreeDetectLoops') 0322 fprintf(' Tree update time : %f\n',sum(Result.timeT)); 0323 end 0324 fprintf(' State Augmenation time: %f\n',sum(Result.timeSA)); 0325 fprintf(' Loop Closure time : %f\n',sum(Result.timeLC)); 0326 if Result.numProposedLoops>0 0327 fprintf(' Closed vs. Proposed loops: %u/%u = %.2f %%\n',Result.numClosedLoops,Result.numProposedLoops,Result.numClosedLoops/Result.numProposedLoops*100); 0328 else 0329 fprintf(' No loop closure was proposed\n'); 0330 end 0331 end 0332