slam2dprob.m

These Matlab files are a convenient interface for the Java library containing the implementation of

% SLAM2DPROB - Generates a 2D SLAM problem structure.  
%
% A world consisting of a square plane with point landmarks is
% constructed; the robot travels along a prescribed path (subject to
% velocity limits), observing the landmarks that lie within a
% cone-shaped visual field.
%
% Options:
%
%   'seed'                 - the seed used to initialize both RAND and
%                            RANDN so that the simulation generated is
%                            a deterministic function of this
%                            functions inputs; if this is [], then the
%                            random number generator is not seeded
%   'shape'                - the shape of the robot path; this can be
%                            'square', 'circle', 'line', or
%                            'switchbacks' (default: 'square')
%   'side'                 - the length of the side of the square region
%                            to be mapped (in meters)
%   'num-landmarks'        - the number of landmarks (default: 1000)
%   'random-landmarks'     - a flag; if this is nonzero, the
%                            landmark locations are sampled
%                            uniformly on the square region; if
%                            not, they are spaced on a grid
%                            (default: 1)
%   'max-range'            - defines the length of the visual cone
%                            (in meters; default: 10 m)
%   'max-bearing'          - defines half of the arc of the visual cone
%                            (in radians; default: pi / 2)
%   'num-obs'              - if nonzero, then the maximum range is
%                            chosen dynamically to ensure that this
%                            num-obs landmark observations are made
%                            at each time step (when this is
%                            possible; default: [])
%   'max-trans-ctrl'       - the maximum translation velocity of
%                            the robot (default: 0.5 meters/second)
%   'max-rot-ctrl'         - the maximum rotational velocity of the
%                            robot (default: 30*pi/180 radians/second)
%   'ctrl-rel-trans-noise' - the variance of the relative noise in
%                            the robot's translation control signal
%                            (default: 0.03^2)
%   'ctrl-rel-rot-noise'   - the variance of the relative noise in
%                            the robot's rotation control signal
%                            (default: 0.05^2)
%   'ctrl-abs-trans-noise' - the variance of the absolute noise in
%                            the robot's translation control signal
%                            (default: (0.02 meters/sec)^2)
%   'ctrl-abs-rot-noise'   - the variance of the absolute noise in
%                            the robot's rotation control signal
%                            (default: (pi/180 radians/sec)^2)
%   'odo-trans-noise'      - the variance of the absolute noise in
%                            the robot's translation odometry
%                            (default: (0.05 meters/sec)^2)
%   'odo-rot-noise'        - the variance of the absolute noise in
%                            the robot's rotation odometry
%                            (default: (pi/180 radians/sec)^2)
%   'bearing-noise'        - the variance of the absolute noise in
%                            the robot's bearing measurements
%                            (default: (2 * pi/180 radians/sec)^2)
%   'rel-range-noise'      - the variance of the relative noise in
%                            the robot's range measurements
%                            (default: (0.1)^2)
%   'abs-range-noise'      - the variance of the absolute noise in
%                            the robot's range measurements
%                            (default: (0.5 meters)^2)
%
% See also: SLAMPROB, SLAM2DPLOT
%

% Copyright (C) 2002 Mark A. Paskin
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful, but
% WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
% General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
% USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function p = slam2dprob(varargin)

[seed, ...
 shape, ...
 side, ...
 random_lm, ...
 num_obs, ...
 max_range, ...
 max_bearing, ...
 max_trans_ctrl, ...
 max_rot_ctrl, ...
 ctrl_rel_trans_noise, ...
 ctrl_rel_rot_noise, ...
 ctrl_abs_trans_noise, ...
 ctrl_abs_rot_noise, ...
 odo_trans_noise, ...
 odo_rot_noise, ...
 bearing_noise, ...
 rel_range_noise, ...
 abs_range_noise, ...
 n] = process_options(varargin, 'seed', [], ...
			        'shape', 'square', ...
			        'side', 100, ...
			        'random-landmarks', 1, ...
		                'num-obs', [], ...
			        'max-range', 10, ...
			        'max-bearing', pi / 2, ...
			        'max-trans-ctrl', 0.5, ...
			        'max-rot-ctrl', (30 * pi/180), ...
			        'ctrl-rel-trans-noise', 0.03^2, ...
			        'ctrl-rel-rot-noise', 0.05^2, ...
			        'ctrl-abs-trans-noise', 0.02^2, ...
			        'ctrl-abs-rot-noise', (1 * pi / 180)^2, ...
			        'odo-trans-noise', (0.05)^2, ...
			        'odo-rot-noise', (1 * pi / 180)^2, ...
			        'bearing-noise', (2 * pi / 180)^2, ...
			        'rel-range-noise', 0.1^2, ...
			        'abs-range-noise', 0.5^2, ...
			        'num-landmarks', 1000);

if (~isempty(seed))
  % It is necessary to seed these two random number generators
  % independently.
  rand('state', seed);
  randn('state', seed);
end

% The dimensions of the robot and landmark states
p.dr = 5;  % X = [x, y, h, t, r]
p.dl = 2;  % L = [x, y]

% Robot state evolution noise covariance.
p.G = diag([ctrl_abs_trans_noise, ctrl_rel_trans_noise, ...
	    ctrl_abs_rot_noise, ctrl_rel_rot_noise]);

% Odometric noise covariance.
p.oC = diag([odo_trans_noise, odo_rot_noise]);

% Landmark measurement noise covariance.
p.yC = diag([bearing_noise, rel_range_noise, abs_range_noise]); 

% Generate the desired pose of the robot at every time step.
if (strcmpi(shape, 'square'))
  % The nubmer of steps necessary to traverse a side and make a
  % 90-degree turn.
  side_steps = ceil(side / max_trans_ctrl);
  rot_steps = ceil((pi / 2) / max_rot_ctrl);
  % First side.
  traj = [linspace(0, side, side_steps); zeros(2, side_steps)];
  % First rotation.
  traj = [traj, [repmat(traj(1:2, end), [1 rot_steps]); ...
		 linspace(0, -pi / 2, rot_steps)]];
  % Second side.
  traj = [traj, [repmat(traj(1, end), [1 side_steps]); ...
		 linspace(0, side, side_steps); ...
		 repmat(traj(3, end), [1 side_steps])]];
  % Second rotation.
  traj = [traj, [repmat(traj(1:2, end), [1 rot_steps]); ...
		 linspace(-pi / 2, -pi, rot_steps)]];
  % Third side.
  traj = [traj, [linspace(side, 0, side_steps); ...
		 repmat(traj(2, end), [1 side_steps]); ...
		 repmat(traj(3, end), [1 side_steps])]];
  % Third rotation.
  traj = [traj, [repmat(traj(1:2, end), [1 rot_steps]); ...
		 linspace(-pi, -3 * pi / 2, rot_steps)]];
  % Fourth side.
  traj = [traj, [repmat(traj(1, end), [1 side_steps]); ...
		 linspace(side, 0, side_steps); ...
		 repmat(traj(3, end), [1 side_steps])]];
elseif (strcmpi(shape, 'circle'))
  steps = ceil(side * pi / max_trans_ctrl);
  rot_v = 2 * pi / steps;
  trans_v = side * pi / steps;
  traj = zeros(3, steps);
  traj(3, :) = linspace(0, 2 * pi, steps);
  for i=2:steps
    traj(1:2, i) = traj(1:2, i - 1) + h2rot(traj(3, i)) * [trans_v; 0];
  end
elseif (strcmpi(shape, 'line'))
  steps = ceil(side / max_trans_ctrl);
  traj = zeros(3, steps);
  traj(1, :) = linspace(0, side, steps);
elseif strcmpi(shape, 'switchbacks')
  % The nubmer of steps necessary to traverse a side and make a
  % 90-degree turn.
  side_steps = ceil(side / max_trans_ctrl);
  rot_steps = ceil((pi / 2) / max_rot_ctrl);
  % Choose the number of switchbacks so that the field of view
  % overlaps on consecutive passes.
  sb_side = max_range * sin(max_bearing);
  num_sb = round(side / (sb_side * 2));
  sb_steps = ceil(sb_side / max_trans_ctrl);
  % Compute the trajectory.
  traj = [0; 0; 0];
  for i=1:num_sb
    % First long side.
    traj = [traj, [linspace(0, side, side_steps); ...
		   repmat(traj(2, end), [1 side_steps]); ...
		   repmat(traj(3, end), [1 side_steps])]];