Probabilistic State Representation

we describe the laser range and bearing sensors as well as the acoustic camera models that we employ later in the thesis.

2.2 Probabilistic State Representation

The fundamental objective in online SLAM implementations is to maintain, over time, an estimate for the current pose and map, xt and M, which comprise the latent state, based upon the available observables, z^t and u^t. The stochastic nature of the vehicle motion and sensor data complicates the estimation and, in particular, the coupling between navigation and mapping that is inherent to SLAM. Many successful SLAM algorithms address these issues by formulating the problem in a probabilistic manner, using generative methods to track the joint distribution over the robot pose and map.

Probabilistic interpretations ofSLAMrepresent the problem in terms of the tuple, S_t = hx_t, M, z^t, u^t, T, Oi. The formulations model the robot pose as a stochastic process and the map as a random vector. The random vectors xt and M denote the latent pose and map states.² Similarly, the vehicle motion (odometry, velocity) data and map observations are particular realizations of the random variables, z_t and u_t that constitutes the evidence. Probabilistic SLAM algorithms consider the entire time history of measurements, z^t = {z1, z2, . . . , zt}, and u^t= {u1, u2, . . . , ut}.

The probabilistic state space model specifies a transition function, measurement likelihood model, and a prior over the state and observations. The state transition function, T : xt× u_t+1× x_t+1 → [0, 1], is a stochastic model for the dynamic behavior of the robot pose in response to control inputs. Typical SLAM algorithms assume that the map is static, i.e. that p (Mt+1) = p (Mt) = p (M), and that only the robot pose is dynamic [127]. Note that, while we adopt this assumption in the thesis, it is not always valid, since environments often contain dynamic elements (e.g. chairs within an office, people). Probabilistic algorithms that assume a static world are robust to some change [128], yet, as we discussed in Section 1.1.3, the ability to deal with dynamic environments remains an open problem in SLAM.

We model the vehicle dynamics according to a first-order Markov process of the form, T : p (x_t+1 | x_t, x_t−1, . . . , x₀, u_t+1) = p (x_t+1 | x_t, u_t+1), where x_t is the current state and ut the control³ [128]. Given the previous pose and current control input, the current pose is conditionally independent of the map and the historical data, i.e. p (x_t+1| x_t, M, z^t, u^t+1) = p (x_t+1| x_t, u_t+1). The distribution captures the uncer-tainty in the vehicle dynamics model along with the noise that corrupts the odometry data, which we describe in (2.4).

The perceptual model, O : xt×M×zt→ [0, 1], is a sensor-specific stochastic repre-sentation for the physical properties underlying the formation of measurements (2.6).

2We use fonts without serifs to denote random variables (e.g. x) and fonts with serifs to represent particular instantiations of the random variable (e.g. x). The same applies to bold symbols that represent vectors.

3This assumption is not restrictive as higher-order Markov models can easily be represented as first-order Markov by defining a new state.

A generative model, p (zt| xt, M) specifies the dependency of sensor measurements on the robot pose and the observed map elements. The distribution explicitly models noise in the data along with inaccuracies in the physical models.

Finally, the prior specifies the initial probability density over the state. In the context ofSLAM, which starts with an empty map, the prior is over the initial robot pose, p (x0).

2.2.1 Bayesian Filtering

This thesis treats SLAM as a filtering problem whereby we track the joint dis-tribution over the state based upon current and historical observation data, i.e.

z^t = {z1, z2, . . . , zt} and u^t = {u1, u2, . . . , ut}. The goal is then to maintain the conditional density

p¡xt, M | z^t, u^t¢

(2.7) as it evolves over time. With each subsequent time step, data arrives in the form of motion control inputs and relative measurements of the environment. The filter recursively updates the distribution to reflect these new observations, {ut+1, zt+1}.

This update follows from the application of Bayes’ rule, p¡xt, M | z^t, u^t¢ ^ut+1, zt+1

−−−−−−→ p¡xt+1, M | z^t+1, u^t+1¢ p¡xt+1, M | z^t+1, u^t+1¢ = η p (zt+1| xt+1, M)

· Z

p (xt+1 | x_t, ut+1) · p¡xt, M | z^t, u^t¢ dxt. (2.8) The term, η, is a normalizing constant that ensures the new distribution is a valid probability function. The recursive update (2.8) assumes a first-order Markov model for the motion model and represents the measurements as temporally independent given the state, i.e. p (zt+1| xt+1, M, z^t, u^t+1) = p (zt+1 | xt+1, M).

The Bayesian filter (2.8) accomplishes two core updates to the distribution: time prediction and measurement update. The time prediction step updates the distribu-tion to reflect the vehicle’s modistribu-tion from time t to time t + 1,

p¡xt, M | z^t, u^t¢ ^ut+1

−−→ p¡xt+1, M | z^t, u^t+1¢

(2.9) per the state transition function, T . It is useful to break the time prediction com-ponent into two sub-processes: state augmentation and roll-up. State augmentation first adds the new pose to the state according to the vehicle motion model based upon odometry data and control inputs.

p¡x_t, M | z^t, u^t¢ −→ p ¡x_t+1, x_t, M | z^t, u^t+1¢

(2.10) A roll-up process follows augmentation and marginalizes away the previous pose,

2.2. Probabilistic State Representation 37

effectively transferring information from the prior to the new distribution,⁴ p¡xt+1, M | z^t, u^t+1¢ =

Z

p¡xt+1, xt, M | z^t, u^t+1¢ dxt

= Z

p (xt+1| xt, ut+1) · p¡xt, M | z^t, u^t¢ dxt. (2.11) We summarize two two-step interpretation of time prediction in (2.12).

After projecting the dynamics forward in time, the filter incorporates new mea-surement data into the distribution. The meamea-surement update step is a Bayesian update to the distribution in (2.12b), based upon the generative model of the obser-vation likelihood. The result is the posterior distribution at time t + 1, which results from conditioning the state upon the measurement data, zt+1, revealed in (2.13).

Algorithm 2.1 (Bayesian SLAM Filter)

p¡xt+1, M | z^t+1, u^t+1¢ ∝ p (zt+1 | xt+1, M) Z

p (xt+1| xt, ut+1) p¡xt, M | z^t, u^t¢dxt

1) Time Prediction:

p¡xt, M | z^t, u^t¢ ^ut+1

−−→ p¡xt+1, M | z^t, u^t+1¢

Augmentation:

p¡x_t, M | z^t, u^t¢ ^ut+1

−−→ p¡x_t+1, x_t, M | z^t, u^t+1¢

(2.12a)

Roll-up:

p¡xt+1, xt, M | z^t, u^t+1¢ −−→ p ¡xt+1, M | z^t, u^t+1¢

(2.12b) 2) Update:

p¡xt+1, M | z^t, u^t+1¢ ^zt+1

−−→ p¡xt+1, M | z^t+1, u^t+1¢

(2.13)

Algorithm 2.1 summarizes the principle Bayesian filter steps. These components form the basis of the many different algorithms that address the SLAM estimation problem [128].

4Note that smoothing SLAMfilters, also known as delayed-state filters, explicitly maintain the robot pose history and do not marginalize over poses.