Here is my simulation of the Simultaneous Localisation and Mapping (SLAM) technique, as applied to an autonomous vehicle navigating through waypoints across a feature-rich terrain. The code was written in the Processing language and utilises the Processing.js library. The original Matlab version was written by Tim Bailey and Juan Nieto, both from the Australian Centre for Field Robotics (ACFR).

New waypoints can be added (in nearest-distance order) by clicking the left mouse button. Existing waypoints can be moved around by holding the left mouse button down in their vicinity and then dragging them, or deleted by clicking with the right mouse button in their vicinity.

The simulator configuration, along with various parameters of the vehicle and sensors are set in at the top of the pde file. Also defined is a set of landmarks and vehicle waypoints for the desired vehicle path. The simulator makes use of my matrix class, which can be found in its own pde file. In an attempt to optimise the code a little, I replaced many of the original matrix operation functions with in-place versions.

The following is a snippet of code showing the basic SLAM operations that take place...

...
void update() {
/*
Update the vehicle true & estimated pose and covariance.
*/
// compute the steering control
compute_steering();

// perform loops: if final waypoint reached, go back to first
if (iwp == -1) {
iwp = 0;
if (iloop > 1) {
iloop -= 1;
} else {
noLoop();
}
}
// compute the vehicle response
vehicle_model();

// add random noise to nominal control values

// EKF predict step
predict();

dtsum += dt;
if (dtsum < DT_OBSERVE)
return;
dtsum = 0.0;

// compute exact observation for each visible landmark
get_observations();

// add random noise to observations

// maintain the feature/observation lookup table
data_associate_known();

// EKF update step
batch_update();

// augment the state and covariance with any new observations
augment();

// add the estimated pose to the vehicle's path history
store_data();
}
...


A final note on performance: The simulation runs around 3x faster in Google Chrome than it does in Firefox! I haven't tested either beta version yet.