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