MCL Motion and Sensing Models

The first step in MCL involves updating particle positions with respect to our motion model:
Given the current and previous poses reported by odometric data, we extract the distance and angle at which the robot should have moved. We then scale the distance by 1 ± p for some percent error p. The angles are randomized within a constant tolerance, c, of their values. By trial and error we found that values of 50% and .5 degrees, for p and c respectively, works quite well for the simulated roomba with a simulated noise factor of 2.5%.

The next step involves comparing robot sensory data with what each particle would sense from its pose in the given map. We used the following cases for assigning probabilities:

1. Robot bumps left and right
---> If the particle bumps left and right, it gets our maximum probability value: 0.9
---> If the particle bumps only on one side, we find the minimum angle, a, that it would need to rotate in order to register both left and right bumps, and assign a probability of (1-a/pi)*0.9.
---> If the particle is not touching any walls, we take a range reading, d, from its pose, and assign a probability of (1/(d+1))*0.9

2. Robot bumps on either left or right, but not both
---> If the particle bumps on the same side and not both, it gets probability 0.9
---> If the particle bumps on both sides, it gets 0.75*0.9
---> If the particle bumps on the opposite side, it gets 0.5*0.9
---> If the particle is not touching any walls, we take a range reading, d, from its position, at a 45 degree angle left or right, as appropriate, of its heading and assign a probability of (1/(d+1))*0.9

3. Robot is not bumping
---> If the particle is not bumping, it gets 0.9
---> If it is bumping, it gets 0.9 / N, where N is the number of particles.