Experiment1a

 Simulated experiment.

 Simulated robot moving on a couple of concentric ellipses.

 The sensor parameters are set to define the plot in Fig. 1(a) in the 
 paper. 
 Note that this implements the standard Pose SLAM algorithm, without using
 the loop closure state recovery strategy introduced in the paper.
 Thus, whenever a loop is closed we invert the information matrix to
 recover the covariance to keep the blocks necessaries for data necessary 
 for data association and to recover the new mean.
 Note, though, that recovering the whole covariance matrix only works for
 small problems due to memory requirements.

 The (optional) paremeters of this function are
    stateRecoveryMethod:   
            2 -> Our method 
            1 -> Get whole Sigma (default)
            0 -> Get Sigma column by column
    nSteps: How many steps to simulate? The default value is 0, i.e.,
            simulate as far as sensor readings are available.
            Partial runs can be continued with ContinueSimulation.

 By directly editing this file you can execute the simulation with
 different filters and loop detection procedures.

 The possible filters are
      - EKF: Extended Kalman Filter. It uses the first available sensor
        reading to extend the state (Prediction) and the rest to
        improve its estimation (Update). The Update is also used when
        a loop is closed. This filter only works fine on small problems.

      - TRO: Our filter. It can be seen as an EIF where we take
        advantage that only very few loops are closed. Thanks to this,
        most of the time the filter is executed in constant time.
        Moreover, the TRO filter implements a strategy to remove
        redundant poses: poses that, with high probability
        are close to the current pose and that do not offer qualitatively 
        more inmore information than what is already available in the 
        filter.

  As far as loop detection procedures, you can use
      - NoLoops: The filter proposes no loop closures. Consequently, the
        simulation proceeds in open loop.

      - AllLoops: The filter proposes to close all possible loops (i.e.,
        loops linking the current robot pose with all the previous
        ones). In this case the loop detection completely relies on the
        sensors.

      - DetectLoops: The filter detects the poses that are with high 
        probability closer than a given threshold to the current robot 
        pose. 

      - TreeDetectLoops: Uses a tree structure that offers searches in 
        logarithmic time instead of the plain linear algorithm used by 
        DetectLoops. Note that the tree is only clearly advantageous in
        large problems.

 Returns:
     Result: A structure with all the parameters used in the experiment,
             the resulting filter structure, and the execution times.

 See also ContinueSimulation.

0001 function Result=Experiment1a(varargin)
0002 % Simulated experiment.
0003 %
0004 % Simulated robot moving on a couple of concentric ellipses.
0005 %
0006 % The sensor parameters are set to define the plot in Fig. 1(a) in the
0007 % paper.
0008 % Note that this implements the standard Pose SLAM algorithm, without using
0009 % the loop closure state recovery strategy introduced in the paper.
0010 % Thus, whenever a loop is closed we invert the information matrix to
0011 % recover the covariance to keep the blocks necessaries for data necessary
0012 % for data association and to recover the new mean.
0013 % Note, though, that recovering the whole covariance matrix only works for
0014 % small problems due to memory requirements.
0015 %
0016 % The (optional) paremeters of this function are
0017 %    stateRecoveryMethod:
0018 %            2 -> Our method
0019 %            1 -> Get whole Sigma (default)
0020 %            0 -> Get Sigma column by column
0021 %    nSteps: How many steps to simulate? The default value is 0, i.e.,
0022 %            simulate as far as sensor readings are available.
0023 %            Partial runs can be continued with ContinueSimulation.
0024 %
0025 % By directly editing this file you can execute the simulation with
0026 % different filters and loop detection procedures.
0027 %
0028 % The possible filters are
0029 %      - EKF: Extended Kalman Filter. It uses the first available sensor
0030 %        reading to extend the state (Prediction) and the rest to
0031 %        improve its estimation (Update). The Update is also used when
0032 %        a loop is closed. This filter only works fine on small problems.
0033 %
0034 %      - TRO: Our filter. It can be seen as an EIF where we take
0035 %        advantage that only very few loops are closed. Thanks to this,
0036 %        most of the time the filter is executed in constant time.
0037 %        Moreover, the TRO filter implements a strategy to remove
0038 %        redundant poses: poses that, with high probability
0039 %        are close to the current pose and that do not offer qualitatively
0040 %        more inmore information than what is already available in the
0041 %        filter.
0042 %
0043 %  As far as loop detection procedures, you can use
0044 %      - NoLoops: The filter proposes no loop closures. Consequently, the
0045 %        simulation proceeds in open loop.
0046 %
0047 %      - AllLoops: The filter proposes to close all possible loops (i.e.,
0048 %        loops linking the current robot pose with all the previous
0049 %        ones). In this case the loop detection completely relies on the
0050 %        sensors.
0051 %
0052 %      - DetectLoops: The filter detects the poses that are with high
0053 %        probability closer than a given threshold to the current robot
0054 %        pose.
0055 %
0056 %      - TreeDetectLoops: Uses a tree structure that offers searches in
0057 %        logarithmic time instead of the plain linear algorithm used by
0058 %        DetectLoops. Note that the tree is only clearly advantageous in
0059 %        large problems.
0060 %
0061 % Returns:
0062 %     Result: A structure with all the parameters used in the experiment,
0063 %             the resulting filter structure, and the execution times.
0064 %
0065 % See also ContinueSimulation.
0066  
0067   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0068   % Process the input parameters
0069   
0070   % For TRO_Filter: state update method when in loop closure
0071   %   2 -> Our method
0072   %   1 -> Get whole Sigma
0073   %   0 -> Get Sigma column by column
0074   if nargin>0
0075     stateRecoveryMethod=varargin{1};
0076   else
0077     stateRecoveryMethod=1;
0078   end
0079 
0080   % Steps to simulate (use 0 to execute all steps)
0081   if nargin>1
0082     Parameters.maxSteps=varargin{2};
0083   else
0084     Parameters.maxSteps=0;
0085   end
0086   
0087   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0088   % Now set the filter type and the data association procedure to use.
0089   
0090   % Type of filter to use: EKF, TRO.
0091   %   Note that EKF works fine for this small example but not for larger
0092   %   ones and that the stateRecovery parameter only applies to the TRO
0093   %   filter.
0094   
0095   % filterType='EKF';
0096   filterType='TRO';
0097   
0098   % Loop detection procedure to use:
0099   %  * NoLoops (simulation in open loop)
0100   %  * AllLoops (sensor-based loop clousure)
0101   %  * DetectLoops (lineal search),
0102   %  * treeDetectLoops (interval-based tree search introduced in the paper)
0103   %                    (This is only worth for large problems)
0104   
0105   % Parameters.loopClosure='NoLoops';
0106   % Parameters.loopClosure='AllLoops';
0107   Parameters.loopClosure='DetectLoops';
0108   % Parameters.loopClosure='TreeDetectLoops';
0109   
0110   % Distance prob. thresholds (only used for DetetLoops and
0111   % TreeDetectLoops).
0112   % - If the probability of two poses of being closer than  'matchArea'
0113   %   is larger than pdRange(1) we try to create a loop (if the information
0114   %   gain criterion also confirms it, see below)
0115   % - If the probability of two poses of being closer that  'matchArea'
0116   %   is larger than pdRange(2) the poses are close enough for one of them
0117   %   to be redundant  (if the information gain criterion also confirms it,
0118   %   see below)
0119   % This is 's' in the paper.
0120   Parameters.pdRange=[0.1 0.1];
0121 
0122   % information gain range (only used for DetetLoops and TreeDetectLoops).
0123   % - if the information gain is larger than igRange(2) we try to create a
0124   %   loop
0125   % - if the information gain is smaller than igRange(1) the pose is
0126   %   redundant and it can be overwrite next time slice
0127   % This is 'g' in the paper.
0128   Parameters.igRange=[0 0];
0129   
0130   % Set up  the trajectory
0131   Te1=Trajectory(pi/2,pi/2-2*pi,10,6,70); % First ellipse
0132   Te2=Trajectory(pi/2,pi/2-2*pi,20,6,100); % Second ellipse
0133   T=Trajectory(Te1,Te2); 
0134 
0135   % Error in odometry
0136   % 5% of the displacement, 1 degree in rotation
0137   % Values taken from "Data Association in O(n) for Divide and
0138   % Conquer Slam" by L.M Paz, J. Guivant, J.D. Tardos and J. Neira.
0139   ad=0.05*get(T,'averageDisplacement');
0140   SnOdometry=[[ad(1)^2,0,0];[0,ad(2)^2,0];[0,0,(1*pi/180)^2]]; 
0141 
0142   % Error in simulated "vision" (odometry + loop closure)
0143   SnVision=[[0.2^2,0,0];[0,0.2^2,0];[0,0,(0.5*pi/180)^2]];
0144   
0145   % Covariance of the 1st pose. The mean of the first pose in taken from
0146   % the trajectory (i.e., the ground truth)
0147   initialUncertainty=[[0.1^2,0,0];[0,0.1^2,0];[0,0,(5*pi/180)^2]];
0148 
0149   % Area around one pose where new images
0150   % tipycally match the current image
0151   % These are the  v_r in the paper.
0152   Parameters.matchArea=[3; 3; (15*pi/180)];
0153   
0154   % Noise used to simulate loop closures, i.e., to compute the
0155   % expected information gain of a candidate loop before actually
0156   % registering the sensor readings
0157   Parameters.noiseLoopClosure=SnVision;
0158 
0159   % Not to print info while solving
0160   Parameters.quiet=false;
0161   
0162   % True if you want graphical output while simulating
0163   Parameters.plot=true;
0164 
0165   % Use ground truth (this is used to evalute the NEES)
0166   Parameters.groundTruth=T;
0167   
0168   % Area where the robot will move. This is only used for plotting
0169   Parameters.Domain=get(T,'boundingBox');
0170   
0171   % Create the sensors: odometry and vision like sensors
0172   Sensors{1}=SimOdometry(SnOdometry,T);
0173   Sensors{2}=SimOdometryWithLoopClosure(SnVision,T,Parameters.matchArea);
0174 
0175   % Assemble a robot with the just defined sensors
0176   R=Robot('SimulatedRobot',Sensors);
0177 
0178   % Define the inital robot position: We use the initial position of the
0179   % true trajectory as an initial robot position. This is the ground truth
0180   % since this experiment is all simulation.
0181   initialEstimation=Gaussian(double(T{1}),initialUncertainty);
0182 
0183   % Initialize the selected filter
0184   switch filterType
0185     case 'EKF'
0186       F=P_EKF(initialEstimation);
0187     case 'TRO'
0188       F=P_TRO(initialEstimation,stateRecoveryMethod);
0189     otherwise
0190       error('Unknown filter type');
0191   end
0192   
0193   % and launch the simulation
0194   Result=Simulation(F,R,Parameters);
0195   
0196