Chapter 3. BEARINGS-ONLY SLAM ALGORITHM
3.4 SLAM
3.4.1 Problem Definition
The simultaneous localization and mapping problem is commonly abbreviated as SLAM, and is also known as Concurrent Mapping and Localization. SLAM problem arise when the robot does not have access to a map of the environment, nor doest it know its own pose. All it is given are measurement z1:t and controls u1:t. In SLAM, the robot acquires a map of its environment while simultaneously localizing itself
1. Algorithm Particle_filter(χt−1,u zt, t):
2. χt =χt =0
3. for m=1 to M do(prediction) 4. sample xt[ ]m ∼ p(x u xt | t, t−1[ ]m ) 5. wt[ ]m = p(z xt | t[ ]m )
6. χt = +χt [xt[ ]m,wt[ ]m] 7. end for
8. for m=1 to M do
9. draw i with probability ∝wt[ ]i 10. add x to t[ ]i χt
11. end for 12. return χt
relative to this map. From a probabilistic perspective, there are two main forms of the SLAM problem, which are both of equal practical importance. The online SLAM estimates the posterior over the momentary pose along with the map.
1: 1:
( ,t | t, t)
p x m z u (3.18)
where xt is the pose at time t, m is the map. This problem is call online SLAM problem since it only involves the estimation of variables that persist at time t. The other category is called the full SLAM. In full SLAM, we calculate a posterior over the entire path x1:t along with the map, instead of just the current pose xt.
1: 1: 1:
( t, | t, t)
p x m z u (3.19)
The mathematical relation between (3.18) and (3.19) is shown below.
1: 1: 1: 1: 1: 1 2 1
( ,t | t, t) ( t, | t, t) t
p x m z u =∫∫ ∫p x m z u d dx x …dx− (3.20) In practice, calculating a full posterior like (3.19) is usually infeasible. Problems arise from the high dimensionality if the continuous parameter space, and the large number of discrete correspondence variables. Many state-of-the-art SLAM algorithms construct maps with tens of thousands of features, or more. Even under known correspondence, the posterior over those maps alone involves probability distributions over space with 105 or more dimensions. This is opposite to localization problems, in which posteriors were estimated over three-dimensional continuous spaces. Not to say in most application the correspondence are unknown. The number of possible assignments to the vector of all correspondence variables grows exponentially. Thus practical SLAM algorithms that can cope with the correspondence problem must rely on approximations.
3.4.2 FastSLAM
FastSLAM is a combinational algorithm that is using both the concept of particle filter and the extended Kalman filter. Particle filters are at the core of some of the most effective robotics algorithm. However the particle filter is not so applicable to the SLAM algorithm due to the course of dimensionality, it scales exponentially with the number of dimensions of the estimation problem. A straightforward implementation of particle filters for the SLAM problem would fail because of the large number of variables involved on describing a map.
In a SLAM problem with known correspondence, there is a conditional independence between any two disjoint set of features in the map, given the robot pose.
So we could estimate the location of all features independently of each other.
Dependencies on these estimates arise only through robot pose uncertainty. This structural characteristic makes it possible to apply Rao-Blackwellized Particle Filter (RB particle filter) to SLAM problem. RB particle filter uses particles to represent the posterior over some variables, along with parametric PDF to represent all other variables.
We use particle filter to estimate the robot path. Since the conditional independent characteristic holds, the mapping problem can be factored into many separate problems, one for each feature in the map. Each single map feature is estimated using a low-dimensional EKF. This is different from other SLAM algorithms that use single high-dimensional Gaussian to estimate the all features jointly.
The advantage of FastSLAM is that it could be implemented in time logarithmic in the number of features, so it’s computational efficient. Another key advantage of
FastSLAM is that the data association decisions can be made on per-particle basis. The ability to pursue multiple data associations simultaneously makes FastSLAM significantly more robust to data association problems than algorithms based on incremental maximum likelihood data association.
robot path feature 1 feature 2 feature N
Table 1. Particles in FastSLAM are composed of a path estimate and a set of estimators of individual feature locations with associated covariance Particles in the basic FastSLAM algorithm are of the form shown in Table 1. Each particle contains an estimated robot pose, denoted x[ ]1:kt , and a set of Kalman filters with mean μ and covariance j[ ]k Σj[ ]k , one for each feature mj in the map. Here [ ]k is the index of the particle. As usual, the total number of particles is denoted M . The basic step of the FastSLAM includes several steps. At each time step t , retrieve a pose xt−1[ ]k from the particle set, sample a new pose according to the distribution
[ ] [ ] Each new particle should have a new importance weight w[ ]k . The final step is resample, which is sampling the M particles with the probability proportional to
[ ]k
w
3.4.3 Unknown Data Association
The data association is also mentioned as correspondence in this thesis. In most of the Bayes filter process, the data association problem exists. Every time we take a measurement of the map, we don’t necessarily know which feature the measurement belongs to (usually we don’t have this piece of information). Generally the data association techniques are using argument such as maximum likelihood. They have only single data association per measurement for the entire filter, and once the association is incorrect, the update procedure fails and the filter diverges. The key advantage of using particle filters for SLAM is that each particle can rely on its own, local data association decision. Thus, the filter is actually sampling over possible data association decisions. Since there are multiple correspondence decisions, as long as a small subset of the particles is based on the correct data association, data association errors are not as fatal as in EKF approaches. Particles with wrong correspondence will possess inconsistent map, which decrease the weight of those particles, and hence they are more likely to be sampled out in the future resample step.