Background

I took Udacity's CS-373 class - Programming a Robotic Car - in March 2012. The class was taught by Sebastian Thrun, one of the most successful roboticists in many years, whose autonomous cars placed 1st and 2nd in the DARPA's Grand Challenge and Urban Challenge, respectively.

The student of this course had access to a very active forum. Here I reproduce a post about SLAM that many people found useful, in the hope that new CS-373 Udacity students also find it useful.

SLAM stands for Simultaneous Localization and Mapping. It is a technique that allows a robot to use repeated observations of landmarks to locate himself in a map of the environment built as it explores its surroundings.

In Graph-SLAM, the version of SLAM taught in CS-373, the robot motions and measurements are written as a set of linear equations ξ = Ω X. The matrices Ω and ξ encode three type of constraints:

  • Absolute Position
  • Relative Position
  • Relative Measurement of a landmark

The only known position of the robot is given by the Absolute Position constraint, commonly assigned to the initial position of the robot, i.e., x0, although I believe it could be assigned to any one position. Suppose that the robot moves to a new location by an amount d0→1. Since x0 is an absolute reference, this would mean that the location of x1 is also absolute.

The problem is that there is an error associated with the motion and thus, we really do not know when the robot ended up at. Our best guess, though, is that the position x1 is about

x1 = x0 + d0→1

We can form a chain of relative motions of this kind, each related to the next one by

xi+1 = xi + di→i+1

The observation of the landmarks from different locations xi lead to similar constraints. A landmark Lj observed from position xi at a distance k is described as

Lj = xi + k

in which, again, we have that the relationship is only an approximation because of the errors in both the location of the robot and the measurement.

Finally, depending on our confidence on a particular constraint we can add weight to an equation, leading to a solution in which the error for that particular equation is reduced more than the others, e.g.,

W*(Lj = xi + k)

or

W*Lj = W*xi + W*k

We assign a large weigh to an equation (i.e., W > 1) when we believe that that particular equation has a small error (low variance) and should count more towards finding the solution of the system. Likewise, an equation associated with a large error (high variance) should be assigned a low weight (0 < W < 1).

The ξ vector encodes the value of the constraint while the Ω matrix encodes the locations that are involved in the constraint.

Once the ξ and Ω matrices are set up, we can solve for the vector X to obtain the absolute locations of the robot, i.e., the actual path that it followed, and the location of the landmarks, all with respect to the location set with the absolute position constraint.



The setup

graph of robot motions and measurements

Distances are shown in blue, weights are shown in red.

The constrains shown on the graph are:

  • Initial position: x0 = -3
  • Relative measurement: L = x0+10
  • Relative pose: x1 = x0+5
  • Relative measurement: L = x1+5
  • Relative pose: x2 = x1+3
  • Relative measurement: L = x2+1. Weight = 5

In this example taken straight from the course, the robot is moving along a line, on which the landmark L is also located. The robot moves twice, taking a measurement from every location:

  • It starts at x=-3, from which it observes the landmark at a distance of +10. This means that the landmark is close to -3+10=7.

  • Then the robot moves +5 from which it observes the landmark at +5. This means that the robot should be close to -3+5= +2 and the landmark should be close to 2+5=7, which matches the observation from the first location.

  • Finally, the robot moves +3 from which it observes the landmark at +1. This means that the robot is at location 2+3 = 5 and the landmark should be close to 5+1=6, which does not match the observations from the first two locations.

This last measurement, though, has a weight of 5 indicating that it is a particularly good measurement (i.e., it has a low variance of 1/W = 0.2).



The Ω matrix

To build the Ω matrix, we could start by finding its diagonal. Each element of the diagonal is the sum of the weights of the incoming and outgoing edges to the particular node:

diagonal of the omaga matrix

Then we can find the off-diagonal elements: for each edge i, j it is the negative of the weight between node i and node j:

off-diagonal of the omega matrix

Thus we have the Omega matrix:

final omega matrix

Also, an easy way to verify that the Ω matrix is correct without drawing the graph is to add all the elements of a row or column. In the case of the initial position, the sum should add up to the weight of the initial position constraint. In the example, when adding the elements of the x0 row in Ω we get

3 - 1 + 0 - 1 = 1

For every other constraint, either relative pose or landmark, the sum should be 0. In the example, when adding the elements of the row of x2 in Ω we get

0 - 1 + 6 - 5 = 0

The ξ vector

The value of each element of ξ that corresponds to a node is the sum of the weighted incoming edges to the node minus the sum of the weighted outgoing edges:

solution to puzzle 1

Comment on the solution

This example was given in class but, although the derivation of the equations is correct, the setup of the equations had an error. Let's see:

Solving ξ = ΩX gives us

X = [x0 x1, x2, L]T = [-3.000, 2.179, 5.714, 6.821]T

i.e., the landmark is at 6.821 instead of 7, like the first two observations indicated. Also, the robot did not move +5 and +3 as commanded but made significant errors.

This result is a consequence of assigning a large weight to the last measurement. By assigning it a weight of 5 we indicated that the variance of the measurement was 1/w = 0.2, i.e., a very good measurement, when in reality, it was the opposite.

If the last observation had been weighted as bad instead of good, i.e., if it had had a variance of 5 and a weight of W = 1/variance = 0.2, then the results would have been

X = [-3.000, 2.050, 5.200, 6.950]T

Now everything fits. The robot moved as commanded with smaller errors than before and all the observations are now consistent.

Taking the case to the extreme and assigning a weight of 0.0001 to the last measurement, indicating that the measurement was lousy and the equation should be given no value when solving the system, we would have gotten:

X = [-3.000, 2.000, 5.000, 7.000]T

i.e., the expected result for an error-free scenario.