Robot Localization in R

The excellent “Artificial Intelligence for Robotics” class on Udacity starts with a Python example on teaching a robot to determine where it is, given sensor data. Assuming the robot starts with a map of the world, and can make some observations of it’s surroundings, a series of movements and successive observations can quickly narrow the location using probability distributions. Solving the problem of localization in this generic way opens a lot of possibilities for solving unrelated problems; e.g. a similar techniques might be used to build a database index of music, and match a song given a short segment.

This type of problem is a natural fit for R, given that R is built around statistical operations and lists. In this case I didn’t use much built-in functionality outside list operations, but clearly this process can be expressed concisely.

First comes set-up:

world 
The "world" variable is a set-up used in the course example; this is the map of the world, assumed to be circular. Probability estimates are established- error in sensor and movement is assumed. pHit and pHit are scaling factors used to describe the accuracy of sensor input. 
Next, we set up the measurement data, and helper data to generate a proper probability distribution, and to normalize a distribution:
measurements 
Then the sense function, which takes a measurement, and adjust the probabilities of where we are:
sense  p
[1] 0.2 0.2 0.2 0.2 0.2

> sense(p, "red")
[1] 0.1111111 0.3333333 0.3333333 0.1111111 0.1111111

You can see here that the "robot" has moved from knowing nothing (a constant distribution) to being somewhat confident that it is standing on one of the red squares. It can't be certain where yet, do to there being multiple red squares, as well as possible sensor error.
Next, we define a function to rotate the array, since the array is cyclic:
rotate  c(1:10)
 [1]  1  2  3  4  5  6  7  8  9 10
> rotate(c(1:10), 2)
 [1]  3  4  5  6  7  8  9 10  1  2
> rotate(c(1:10), 3)
 [1]  4  5  6  7  8  9 10  1  2  3

Define some constants for movement accuracy. pMove is the probability that if you move to the left one space, you'll be successful - pLeft and pRight are the chances of landing on either side.
#movement accuracy
pMove
The movement function is then very simple. This isn't normalized like "sense", because the movement probabilities add up to one, but if they didn't, this would also require normalization.
move 
In the example modeled below, the robot starts on the third square (red) and moves to the left several times. Each time it moves, it loses some certainty for it's location, but then gains far more certainty upon taking measurements.
> world
[1] "green" "red"   "red"   "green" "green"
> dist
[1] 0.2 0.2 0.2 0.2 0.2
> sense(dist, "red")
[1] 0.1111111 0.3333333 0.3333333 0.1111111 0.1111111
> move(sense(dist, "red"))
[1] 0.3111111 0.3111111 0.1333333 0.1111111 0.1333333
> sense(move(sense(dist, "red")), "red")
[1] 0.16470588 0.49411765 0.21176471 0.05882353 0.07058824
> move(sense(move(sense(dist, "red")), "red"))
[1] 0.43294118 0.22470588 0.07529412 0.07882353 0.18823529
> sense(move(sense(move(sense(dist, "red")), "red")), "green")
[1] 0.54117647 0.09362745 0.03137255 0.09852941 0.23529412
> move(sense(move(sense(move(sense(dist, "red")), "red")), "green"))
[1] 0.13215686 0.04431373 0.10549020 0.25220588 0.46583333

And finally, to combine this, the movements can all be done recursively. One note here is that you must remove elements explicitly, rather than successively, subsetting an array, as the recycling rule will keep adding elements when you try to remove the last entry, resulting in infinite recursion.
moveall  0) { 
    moveall(measurements[-c(1)], move(sense(p, measurements[1]))) 
  } else {
    p
  }; 
}
> measurements
[1] "red"   "red"   "green"
> moveall(measurements, dist)
[1] 0.13215686 0.04431373 0.10549020 0.25220588 0.46583333