Another approach to deal with uncertainty is that of Bayes filtering. The general idea is to estimate a hidden state variablex of a dynamical system from observations that can be made. The underly- ing model for Bayes filters are Hidden Markov Models (HMM), which in this case can be seen as simple Bayesian nets. These models enjoy the Markov property as it is assumed that the particular statextat a timet of the system only depends on xt−1, that is,p(xt|x0. . . , xt−1) = p(xt|xt−1).
Further, the observation made at timet is only dependent on the state xtand is conditionally
independent of all other states, i.e.p(ot|x0, . . . , xt) = p(ot|xt).
The key idea of Bayes filtering is to estimate a probability density over the state space condi- tioned on the observation made. It is an estimate as the true state is unknown (hidden). The belief Bel(xt) of the system being in state xtis defined by the posterior
Bel(xt) = P (xt|o1, . . . , ot)
A Bayes filter consists of the following two steps:
1. Prediction step: At each time step a prediction of the state is made: Bel−(xt) =
Z
p(xt|xt−1) · Bel(xt−1)dxt−1.
3.2. BAYES FILTERING FOR ROBOT LOCALIZATION 37
2. Correction step: Each time a new observation is made it is used to correct the predicted belief
Bel(xt) = η · p(ot|xt) · Bel−(xt).
The likelihood of an observationotassumed that the system is in statextis expressed by
the perceptual modelp(ot|xt). η is a normalizing constant which ensures that the posterior
over the entire state space sums up to one.
Bayes filter have a wide application range. In our context it is used mainly for localizing a robot in its environment (Chapter 5). The observations then can be seen as data coming from the robot’s sensors. The state variablext = (x, y, θ)Trepresents the pose of the robot, with(x, y)
being the position, andθ the orientation of the robot. When localizing a robot with a Bayes filter, Bel(x0) is initialized with prior knowledge about the position. Often, the initial pose in unknown
andBel(x0) is initialized with a uniform probability distribution, as all positions are equally likely.
To localize a robot with no information about the initial position, is called global localization. If the robot has a high belief about its position the process of localization is called local localization or position tracking.
In the following we briefly introduce two Bayes filters which we will use in Chapter 6.
3.2.1 Kalman Filter
The Kalman filter (Kalman 1960) is a linear Bayes filter with Gaussian distribution. The state variablext∈ IRnand the observationsot∈ IRmare formulated by the equations
xt= Axt−1+ But−1+ wt−1 and ot= Hxt+ vt.
Then × n matrix A is the transition model stating the relationship of the state variable x between two time steps. With the termB · u an additional control input can be given. w and v are random variables and are adding noise tox and o. They are assumed to be independent of each other and normally distributed withp(w) ∼ N(0, Q) and p(v) ∼ N(0, R). Q is the process noise covariance andR the measurement noise covariance matrix.1 The matrixH relates observable
components of the measurement and the state.
Letµ−t ∈ IRnbe the prior, andµt∈ IRnthe posterior state estimate at stept, i.e. µ−t = Ex−t
andµt= E [xt]. The prior and posterior state estimation error can be defined as e−t ≡ xt− µ−t
andet≡ xt− µt. The respective error covariances areΣ−t = E
h e−
te−tT
i
andΣt= EeteTt. The
posterior state estimateµtis calculated byµt= µ−tK(ot− Hµ−t), where the difference ot− Hµ−t
is called innovation and reflects the difference between the predicted measurementHµ− t and the
actual measurementot.K is called Kalman gain and is calculated by
Kt= Σ−tHT(HΣ−tHT+ R)−1=
Σ− tHT
HΣ−tHT+ R
.
The Kalman gain is needed to calculate the posterior error covariance from the prior one (in the correction step of the filter below). The idea is to minimize the mean-square error of a state given a
state prediction of the previous step and new observations, i.e.E(xt− µt)2 is to be minimized.
This is equivalent to minimizing the posterior error covarianceΣt; the above formula is the result
of minimizingΣt. The derivations are given for example in (Thrun et al. 2005). Given these
equations, one can establish the both steps of a Bayes filter mathematically.
1. Prediction step: In the prediction step the a posteriori state estimate together with its error covariance is calculated:
µ−t = Aµt−1+ But−1
Σ−
t = AΣt−1AT+ Q
,
whereQ is the process noise covariance. It means that the a posteriori prediction error plus the noise which is inherent to the whole process forms the next a priori error.
2. Correction step: The just calculated a priori error is minimized and with it the new state estimate is established. Finally, we correct the a posteriori error needed for the next iteration of the filter
Kt= Σ−tHT(HΣ−tHT+ R)−1
µt= µ−t + Kt(ot− Hµ−t)
Σt= (I − KtH)Σ−t
In the notation given in the introduction the respective density functions arep(xt|xt−1) =
N (Axt−1, Q), p(ot|xt) = N (Hxt, R), and p(xt−1|o0, . . . , ot−1) = N (µt−1, Σt−1), with N de-
noting the normal distributed probability density function.
The Kalman filter has a variety of applications. Each linear stochastic process with Gaussian noise where the state of the system is only indirectly given through observations can be modeled by Kalman filters. Kalman filters are optimal estimators. Extensions to deal with non-linear models also exist (e.g. (Maybeck 1990)). Regarding the localization of a mobile robot one can say that Kalman filters can only be used for position tracking. The reason is that the belief of the position is represented by a uni-modal Gaussian and the filter is thus unable to represent multiple hypotheses. An extension regarding this issue is Multiple Hypothesis Tracking (MHT) (Bar-Shalom and Li 1995) where for each single hypothesis a separate Kalman filter is used.
3.2.2 Particle Filter
Next, we briefly describe an approach to robot localization using Monte Carlo methods. For the localization problem, these methods extend the so-called Markov localization. Markov localiza- tion uses a grid-based state representation of the posehx, y, θi of the robot. The environment map is represented as a 3D occupancy grid (Moravec and Elfes 1985) where cells which are occupied have a high occupancy value and free cells are assigned the occupancy value zero. The localization of the robot is pursued relative to this map.
3.2. BAYES FILTERING FOR ROBOT LOCALIZATION 39
The Markov localization method is a recursive Bayes filter. The initial belief about the robot’s pose is represented by a uniform distribution over the whole state space for the case of global lo- calization, or by a Gaussian with a certain small mean and variance around a given initial position. The recursive update equations are:
1. Prediction step: Bel−(x t) = Z p(xt|xt−1, at−1, ot−1, . . . , a0, o0)p(xt−1|at−1, . . . , o0)dxt−1 = Z p(xt|xt−1, at−1)p(xt−1|at−1, . . . , o0)dxt−1 = Z p(xt|xt−1, at−1)Bel(xt−1)dxt−1
The derivation follows again from the Markov assumption. Theoiare as above the observa-
tions the robots made, i.e. the sensor inputs, theaiare odometry updates. These are sensor
values for the wheel encoder of the robot and represent the actions (drive commands) of the robot.
2. Correction step: In the correction step the Markov assumption is applied again. It is as- sumed that the measurements only depend on the current position of the robot.
Bel(xt) = η · p(ot|xt, at−1, ot−1, . . . , a0, o0) · Bel−(xt)
= η · p(ot|xt) · Bel−(xt)
The belief distribution is updated by a discretized approximation of Bel(xt) according to xt.
Each grid cell in the occupancy grid must be touched for the update. Though extensions which only update the most important regions at the current time step exist, there are more efficient methods for approximating the belief distribution. Using sampling techniques for this approximation is far more efficient. In the following we present the particle filter following the notation given in (Thrun et al. 2000). Bel(x) is represented by set of weighted samples
Bel(x) ≈ {x(i), w(i)} i=1,...,m,
wherex(i)is one out ofm samples of the random variable x and represents one hypothesis, a
pose in the case of localization. The importance factorw(i)assigns a weight to this hypothesis,
the sum of all weights is to be one, i.e. Pm
i=0w(i) = 1. One Monte Carlo method is the SIS
(sequential importance sampling) filter. It is a bootstrap filter which means that from an initial guess after several iterations the filter converges towards the real distribution. It is also called bootstrap filtering, particle filter, or condensation algorithm. The recursive update is performed by the following steps.
1. Prediction step: A number of samplesx(i)t−1from Bel(xt−1) and samples x(i)t according
top(xt|xt−1, at−1) with i = 1, . . . , m are drawn. The tuples hx(i)t , x (i)
t−1i are distributed
according to Bel−(xt). Bel−(xt) restated for a sample pair hx(i)t , x (i) t−1i is then Bel−(x(i)t ) = p(x (i) t |x (i) t−1, at−1)Bel (x(i)t−1)
The particleshx(i)t , x(i)t−1i do not reflect the target distribution Bel(x) at time t. The error is
corrected in the next step.
2. Correction step: Each sample is now weighted according towˆ(i)= p(o
t|x(i)t ). The distri-
butionp(ot|xt) is the sensor model. In the localization context, it is easy to calculate the
probability of making a sensor readingotgiven the posext. A commonly used method to es-
timate the probability is to use ray-tracing methods on the environment map given. For each single proximity measurement a probability is calculated with aid of the ray-tracing method. The whole distribution forj single measurements is calculated by p(o|x) =Q
jp(oj|x). ˆw
represents the target distribution except for a factor:
w(i)t = ηp(ot|x(i)t )p(x (i) t |x (i) t−1, at−1)Bel (x(i)t−1) p(x(i)t |x (i) t−1, at−1)Bel (x(i)t−1) =ηp(ot|t (i) t )Bel−(x (i) t ) Bel−(x(i)t ) = η ˆwt(i).
This means thatw = η · ˆw, the weights ˆw gained from the sensor model and the approxi- mationw of the target distribution are proportional to each other. Normalizing ˆw yield the correctly distributed values to approximate Bel(xt)
Bel(x(i)t ) = η · ˆw(i)· Bel−(x (i) t ).
The sampling process is repeatedm times resulting in a set of m weighted samples x(i)t .
Initially, thew0(i)are set to1/m.
Monte Carlo methods for localizing a robot are very successful. Many different approaches and refinements exist for the class of Monte Carlo localization algorithms, like dual sampling, or mixture MCL. For a thorough discussion we refer to (Thrun 2006; Montemerlo et al. 2006).