Bayesian filtering framework

2.3.1 Optimal Bayesian filtering

The state of the AUV at time t is a random vector denoted by X_t that usually includes the position and the speed of the AUV. The evolution of the AUV’s state is provided by the motion model. Usually, the motion model assumes that (X_k) is a Markov process entirely defined by the transition kernel p(xk|xk−1) and the distri-bution of the initial state X₀. The transition kernel is related to the measurements provided by the IMU or the DVL that predict the future state of the AUV given its past state. The state X_tis not directly observed but is known through measurements regularly gathered by the AUV. The measurements are, for example, the scan of a

rotating sonar or range measurements with respect to acoustic transponders. The measurement at time t is a random vector denoted by Y_t that only depends on X_t. The measurement model provides the conditional density p(y_t|x_t) that statistically links the observation Y_t to the state X_t. These assumptions are those of a hidden Markov model.

The objective in Bayesian filtering is to calculate the conditional density p(x_t|y_0:t) so that one has an estimate of the state X_t through the conditional expectation

E[X^t|Y_0:t] = Z

x_tp(x_t|y_0:t)dx_t (2.2) This conditional expectation is the estimate of X_t given Y_0:t that minimises the mean square error, namely

E[ |Xt− E[Xt|Y_0:t]|²] ≤ E[ |Xt− ψ(Y_0:t)|²] (2.3) for any other estimate ψ(Y_0:t) of X_t given the observations Y_0:t. The conditional den-sity p(x_t|y_0:t) may be recursively calculated from p(x_t−1|y_0:t−1) through the following exact prediction and correction steps

The normalisation constant c_t is unknown and given by 1/c_t=

Z

p(x_t|y_0:t)dx_t = Z

p(x_t|y_0:t−1)p(y_t|x_t)dx_t (2.6) Assuming that the motion and the measurement models are both linear and Gaus-sian, the distribution of X_t|Y_0:t is also Gaussian. The mean and the covariance matrix of X_t|Y_0:t may then be calculated using the recurrence relations of the Kalman filter [50, 51]. The calculation of the conditional expectation E[Xt|Y_0:t] is exact so that the Kalman filter is optimal, namely it provides the estimate of X_t given Y_0:t that minimises the mean square error, which was mentioned previously.

The motion and the measurement models of an AUV are seldom both linear and Gaussian. One has consequently to provide approximate and thereby suboptimal solutions to the non-linear non-Gaussian Bayesian filtering problem. Various approx-imation schemes have been put forward among which are the Monte Carlo and the

sum of Gaussian approximations. The former approximation is at the core of any particle filter and the latter is known as the Gaussian Sum Filter.

2.3.2 Kalman Filter

The Kalman Filter (KF) [50], [116] is a filter that can be derived directly by the optimal Bayesian filtering under the assumptions that the system is linear and the noise is Gaussian. Under this linear assumption the system can be described by

xt+1 = Ftxt+ wt are zero mean normally distributed. The key advantage of the Kalman Filter is that it represents the distributions in closed form, in terms of means and covariance matrix. The update of the Kalman filter can be carried out in the time of a matrix multiplication (O(n³), where n is the state dimension).

The iterative algorithm of the filter is the following:

• predict:

Unfortunately, in underwater robot domain, the evolution model, as well as the obser-vation model are non linear, and the noise cannot be considered Gaussian. However, for mild evolution laws and some specific problem, a non linear extension can be used: the Extended Kalman Filter (EKF) [116], in which the functions f and h can be non linear, but the covariance matrix is calculated in a local linearisation under the current state. The extended Kalman filter algorithm can be expressed as

• predict:

x⁰_t+1= f_tx_t

The key limitations in the use of extended Kalman filter lies in the strong as-sumptions that have to be done on the estimated system, namely: Gaussian noise, and linearisability. In most of the robotic systems used for localisation and SLAM the uncertainty is not expressible as a Gaussian distribution, being multi modal and non regularly shaped. When more modes are present in a distribution, dealing with multiple hypotheses is needed, while the Kalman Filter works on their mean. In these situations its use is prone to failure. Moreover, the linearisation of the system can introduce some systematic error in the estimate. Finally, some systems cannot be linearised (being their 1st order derivatives null), thus the Extended Kalman Filter cannot be applied. In these contexts a second order extension to the Kalman filter:

the Unscented Kalman Filter (UKF) has been proposed in [112]. While the UKF in general behaves better than the Kalman filter, the hypotheses of Gaussian noise is still required to hold. Nevertheless, the Kalman Filter is one of the most used tools in localisation and SLAM, due to its simplicity. Moreover, when the underlying hy-potheses hold, it exhibits a strong convergence rate if compared with other filtering techniques [112].

2.3.3 Particle filters

Any particle filter relies on a Monte Carlo approximation of the conditional density p(x_t|y_0:t) using a finite set of N points ξ_tⁱ in the state space called particles. The

and where δ_ξⁱ

t denotes the usual Dirac function, namely δ_ξⁱ

t(x) =

(1 if x = ξⁱ_t 0 otherwise.

(2.13)

Formula (2.12) may be interpreted in the following way: the denser the particles in a region of the state space and the higher their weights, the higher the probability that the state lies in this region. Assuming that one knows how to sample from p(x_t|y_0:t), the Monte Carlo approximation becomes The previous assumption is unrealistic since it implies that the Bayesian filtering problem is solved. Any particle filter rely on the importance sampling principle which provides a mechanism to build a Monte Carlo approximation of p(x_t|y_0:t).

Suppose one would like to sample from a distribution whose density f is of the form f (x) = c r(x)g(x) where

1. g is the density of a distribution from which it is easy to sample and which is called the proposal distribution;

2. r is a weighting function easy to evaluate;

3. c =R f (x)dx is an unknown normalisation constant.

The importance sampling principle provides the following approximation for f .

f (x) ' the particles being generated using another distribution than f .

The first particle filter ever proposed was the bootstrap filter introduced in [37] but the most commonly used particle filter is the sampling with importance resampling (SIR) filter. Other kinds of particle filters such as the auxiliary particle filter or the kernel filter have been introduced in order to improve the performance of the SIR algorithm. The interested reader can found an overview of the existing sequential Monte Carlo methods for Bayesian filtering in [16, 3].

The SIR filter intends to reproduce the optimal prediction and correction steps of the optimal Bayesian filter. The three steps of iteration t ≥ 1 of the SIR algorithm are as follows.

1. Selection: Generate τ_tⁱ ∼ (w_t−1¹ , . . . , w^N_t−1) 2. Propagation: Generate ξ_tⁱ ∼ p(x_t|ξ_t−1^τti ) 3. Correction: Set w_tⁱ ∝ p(y_t|ξ_tⁱ)

In the SIR particle filter, the propagation step of iteration t uses the transition kernel to simulate the new set of particles (ξ_t¹, . . . , ξ_t^N), as in the bootstrap filter. However, only the most likely particles at time t − 1 are selected to generate the particles at time t. The selection is made according to the weights since the higher a weight, the higher the corresponding particle is in adequacy with the observations. This selection step makes the SIR particle filter more efficient than the bootstrap filter.

2.3.4 Gaussian Sum Filter

Particle filters rely on a Monte Carlo approximation of the distribution of X_t|Y_0:t. They require a large number of particles so that the approximation is accurate enough and thereby have a high computational cost. A long time before the emergence of particle filters, an approximation scheme based on a mixture of Gaussian distributions was suggested in [2]. This Gaussian sum approximation was shown to perform better than the Extended Kalman Filter while being compatible with the computational capabilities of that time. The conditional density p(x_t|y_0:t) is approximated by

p(x_t|y_0:t) '

where x 7→ Γ(x ; m, P ) denotes the probability density function of a Gaussian multi-variate distribution of mean m and covariance matrix P . The individual means and covariance matrices mⁱ_t and P_tⁱ are updated according to the recurrence relations of the Extended Kalman Filter. The interested reader can found a complete description of the Gaussian Sum Filter in [2].

2.3.5 Bayesian approach to the SLAM problem

The Bayesian filtering framework is suitable to estimate the state Xt of an AUV.

The Simultaneous Localisation and Mapping (SLAM) problem also requires the esti-mation of the state X_t along with the estimation of the map of the environment. In the SLAM problem considered in this section, the environment is composed of acous-tic transponders whose positions are to be found. The position of each transponder is denoted by B_i and B denotes the set of all the transponders.

The Bayesian approach to the SLAM problem requires the calculation of the