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

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

    // add random noise to nominal control values

    // EKF predict step

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

    // compute exact observation for each visible landmark

    // add random noise to observations

    // maintain the feature/observation lookup table

    // EKF update step

    // augment the state and covariance with any new observations

    // add the estimated pose to the vehicle's path history

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.