general tidyup

Author: petercorke

demos/car_anim_rs.m

@@ -25,14 +25,14 @@
 maxcurv = 1/5;   % 5m turning circle
 rs = ReedsShepp(q0, qf, maxcurv, dl)
 
-%% set up a vehicle model
+% set up a vehicle model
 [car.image,~,car.alpha] = imread('car2.png');
 car.rotation = 180;
 car.centre = [81,110];
 car.centre = [648; 173];
 car.length = 4.2;
 
-%% now animate
+% now animate
 clf; plotvol([-4 8 -6 6])
 
 a = gca;

demos/fdyn.m

@@ -40,7 +40,7 @@
 % with zero applied joint torques
 
 tic
-[t q qd] = p560.nofriction().fdyn(10, [], qz);
+[t,q,qd] = p560.nofriction().fdyn(10, [], qz);
 toc
 
 % and the resulting motion can be plotted versus time

demos/particlefilt.m

@@ -33,7 +33,7 @@
 V = diag([0.1, 1*pi/180].^2);
 
 % then use this to create an instance of a Vehicle class
-veh = Vehicle(V);
+veh = Bicycle(V);
 
 % and then add a "driver" to move it between random waypoints in a square
 % region with dimensions from -10 to +10
@@ -42,14 +42,14 @@
 
 % Creating the map.  The map covers a square region with dimensions from 
 % -10 to +10 and contains 20 randomly placed landmarks
-map = Map(20, 10);
+map = LandmarkMap(20, 10);
 
 % Creating the sensor.  We firstly define the covariance of the sensor measurements
 % which report distance and bearing angle
 W = diag([0.1, 1*pi/180].^2);
 
 % and then use this to create an instance of the Sensor class.
-sensor = RangeBearingSensor(veh, map, W, 'animate');
+sensor = RangeBearingSensor(veh, map, 'covar', W, 'animate');
 % Note that the sensor is mounted on the moving robot and observes the features
 % in the world so it is connected to the already created Vehicle and Map objects.
 
@@ -80,7 +80,7 @@
 % hypotheses are very spread out, but soon start to cluster around the actual pose
 % of the robot.  The pose is 3D (x,y, theta) so if you rotate the graph while the
 % simulation is running you can see the theta dimension as well.
-pf.run(200);
+pf.run(100);
 % all the results of the simulation are stored within the ParticleFilter object
 
 % First let's plot the map

demos/pgslam.m

@@ -31,7 +31,7 @@
 clf; pg.plot()
 
 % we optimize the pose graph
-pg.optimize();
+pg2 = pg.optimize();
 
 % and then plot the optimized graph in a new figure
 figure; pg2.plot

demos/reedsshepp.mlx

@@ -55,34 +55,37 @@
 
 % Create the filter.  First we need to determine the initial covariance of the
 % vehicle, this is our uncertainty about its pose (x, y, theta)
-P0 = diag([0.005, 0.005, 0.001].^2);
+P0 = diag([0.005, 0.005, 0.001].^2)*1000;
+
 
 % Now we create an instance of the EKF filter class
 ekf = EKF(veh, V, P0, sensor, W, []);
 % and connect it to the vehicle and the sensor and give estimates of the vehicle
-% and sensor covariance (we never know this is practice).
+% and sensor covariance (we never know this in practice).
 
 % Now we will run the filter for 1000 time steps.  At each step the vehicle
 % moves, reports its odometry and the sensor measurements and the filter updates
 % its estimate of the vehicle's pose
-ekf.run(1000);
+ekf.run(500, 'plot');
 % all the results of the simulation are stored within the EKF object
 
-% First let's plot the map
-clf; map.plot()
+%% First let's plot the map
+map.plot()
 % and then overlay the path actually taken by the vehicle
 veh.plot_xy('b');
 % and then overlay the path estimated by the filter
 ekf.plot_xy('r');
 % which we see are pretty close
 
 % Now let's plot the error in estimating the pose
+figure
 ekf.plot_error()
 % and this is overlaid with the estimated covariance of the error.
 
 % Remember that the SLAM filter has not only estimated the robot's pose, it has
 % simultaneously estimated the positions of the landmarks as well.  How well did it
 % do at that task?  We will show the landmarks in the map again
+figure
 map.plot();
 % and this time overlay the estimated landmark (with a +) and the 95% confidence
 % bounds as green ellipses