Experiment1c

 Simulated experiment.

 Simulated robot moving on a couple of concentric ellipses.

 The sensor parameters are set to define the plot in Fig. 1(c) in the 
 paper.

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