Course:CPSC522/SLAM And Sensor Quality

SLAM and Sensor Quality

Principal Author: Bronson Bouchard
Collaborators:

Abstract

Simultaneous Localization and Mapping (SLAM) is a system used to estimate an agent's location in an unknown map and estimate the map of the world surrounding an agent simultaneously. This page will describe SLAM, with a focus on FastSLAM, and perform an experiment with varying sensor quality.

Builds on

This page is focused on FastSLAM which utilizes Particle Filtering and Kalman Filters

Content

Hypothesis

The simultaneous localization and mapping problem models noise in the sensors that perceive the world and noise in the movement within the world while determining the location of an agent. The experiment on this page will test if near perfect sensors perform worse than noisy sensors in a particle filter FastSLAM implementation.

Background

Simultaneous Localization and Mapping (SLAM) solves the problem of an agent, typically a robot or some device that moves in the world, which does not have a map of the world and needs to locate itself in the world. A common approach is to identify and keep track of landmarks which the robot can use to build a model of the world and place itself within the world. SLAM can be implemented in several ways but three of the most common are extended Kalman filters, graph-based SLAM, and particle based SLAM. This page will focus on particle based SLAM and particularly FastSLAM.

The FastSLAM algorithm is a particle approach to SLAM where a set of particles are maintained which provide a set of path samples. There are also a set of Gaussian distributions for each landmark. When a measurement is made, the location of each particle is updated by randomly sampling the known kinematic motion model. FastSLAM also computes the probability of the measurement that was made given the particle and an importance weight of the particle which is based off the measurement. A new set of particles is then resampled, where particles that have a high probability of the measurement persist.

The SLAM problem can be formally defined by:
$p(s^{t},\theta |z^{t},u^{t},n^{t})$
where s^t is the state of the robot or the pose, $\theta ^{t}$ is the set of landmarks, u^t is the robot control, and n^t is the variable identifying the landmark. The superscript t referring to the set of time steps from 1 to t.
The robot's pose is determined by the motion model:
$p(s_{t}|u_{t},s_{t-1})$
and the sensor measurements are determined by the measurement model:
$p(z_{t}|s_{t},\theta ,n_{t})$

Experiment

The hypothesis in this page is tested on a very simple implementation of FastSLAM in MatLab by Randolph Voorhies. The experiment consists of a simulated robot attempting to drive in a circle in a 2D environment. There are 6 land marks scattered around the robot which the robot can sense with noisy sensors. The robot attempts to drive in a circle but there noise in the movement which causes the robot to be in a slightly different position than intended. The robot can see landmarks that are 1.5 distance units away and has noise in the distance to a seen sensor, the angle to that sensor, and very minor noise in whether the landmark seen is correctly identified. The number of particles is set to 100 throughout the experiment.

In the first stage of the experiment, the noise was tested at 0.1 on the distance to the landmark, 0.01 on angle to the landmark, and 0.0001 noise on the identity. The noise was applied by sampling from a normal distribution then adding to the true measurement. A run of this first stage can be seen in Figure 1. All landmarks were estimated with reasonable accuracy and the robot knows where it is. The asterisk shapes are the land marks, the green circle is the robot, the small black circles are the locations that the robot estimated the landmarks to be, the red line is the path the robot has taken, the blue line extending out of the robot is where the robot senses the landmark, and the small red dots clouding around the robot are the particles estimating the trajectory of the robot.

Figure 1. Moderately noisy sensors

In the second stage of the experiment the sensors were moderately improved and the noise was reduced. The noise was tested at 0.001 on the distance to the landmark, 0.0001 on the angle to the landmark, and 0.0001 noise on the identity of the landmark. A run of this stage of the experiment can be seen in Figure 2. The robot located all landmarks and itself better than the first run.

Figure 2. Low noise sensors

In the third stage of the experiment the sensors were improved to be nearly perfect. The noise on the distance was set to 1E-16 on the distance, angle, and landmark identity, which is essentially immeasurable on the sensors. The robot was still able to see all the landmarks but the locations of each and the robot itself were much less accurate than the previous runs. In some runs, the robot was unable to locate some of the landmarks at all. Increasing the sensor quality/decreasing the noise continues to produce worse results. A run of this stage of the experiment can be seen in Figure 3.

Figure 3. Very low noise sensors

Another test was performed by increasing the noise to 0.5 on the distance, 0.05 on the angle, and keeping 0.0001 on the landmark identity. The robot is still able to locate the landmarks and itself reasonably well, as seen in Figure 4. But, as expected, increasing the noise to extreme values caused the robot to get more lost than with near perfect sensors.

Figure 4. High noise sensors

Experimentally increasing the number of particles in the extremely low noise settings was done as well but execution time increased drastically while testing.

Conclusion

In this experiment the effect of sensor quality on SLAM was tested. The results appear to show that in some cases more noise in sensors results in better localization and mapping. The reason for this could be that when sampling the particles there is such a small variance in the measurements that the probability of a measurement corresponding to a particle is too small and there are fewer particles that could match the measurement. The lack of sensor noise also doesn't appear to handle the inaccuracy in movement well. In cases where the robot could move with complete accuracy and sense with complete accuracy SLAM performed better.