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…

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50

...
  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
    add_control_noise();

    // 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
    add_observation_noise();

    // 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.