Implicit sampling is a recently developed variationally enhanced sampling method that guides its samples to regions of high probability, so that each sample carries information. Implicit sampling may thus improve the performance of algorithms that rely on Monte Carlo (MC) methods. Here the applicability and usefulness of implicit sampling for improving the performance of MC methods in estimation and control is explored, and implicit sampling based algorithms for stochastic optimal control, stochastic localization, and simultaneous localization and mapping (SLAM) are presented. The algorithms are tested in numerical experiments where it is found that fewer samples are required if implicit sampling is used, and that the overall runtimes of the algorithms are reduced.

# Implicit Sampling for Path Integral Control, Monte Carlo Localization, and SLAM OPEN ACCESS

**Matthias Morzfeld**

Contributed by the Dynamic Systems Division of ASME for publication in the JOURNAL OF DYNAMIC SYSTEMS, MEASUREMENT, AND CONTROL. Manuscript received September 13, 2013; final manuscript received November 6, 2014; published online January 27, 2015. Assoc. Editor: John B. Ferris.

*J. Dyn. Sys., Meas., Control*137(5), 051016 (Jan 27, 2015) (14 pages) Paper No: DS-13-1356; doi: 10.1115/1.4029064 History: Received September 13, 2013

Many problems in physics and engineering require that one produces samples of a random variable with a given probability density function (PDF) *p* [1,2]. If *p* is difficult to sample, one can sample an easier-to-sample PDF *p*_{0} (the importance function), and attach to each sample *X ^{j}*

^{ }∼

*p*

_{0},

*j*= 1,…,

*M*, the weight

*W*∝

^{j}*p*(

*X*)/

^{j}*p*

_{0}(

*X*) (capital letters denote realizations of random variables, and these realizations are indexed in superscript; subscript indices label discrete time). The weighted samples

^{j}*X*form an empirical estimate of the PDF

^{j}*p*and, under mild assumptions, the empirical estimate converges weakly to

*p*, i.e., expected values of a function

*h*with respect to

*p*can be approximated by weighted averages of the function values

*h*(

*X*). The difficulty is to find a “good” importance function

^{j}*p*

_{0}: if

*p*

_{0}is not a good approximation of

*p*, for example, if

*p*

_{0}is not large when

*p*is large, then many of the samples one proposes using

*p*

_{0}may have a small probability with respect to

*p*and, therefore, carry little information, and make sampling inefficient.

Implicit sampling is a general method for constructing useful importance functions [3,4]. The basic idea is to first locate the region of high probability with respect to *p* via numerical optimization, and then to map a reference variable to this region; this mapping is done by solving algebraic equations. While the cost of generating a sample with implicit sampling is larger than in many other MC schemes, implicit sampling can be efficient because the samples carry more information so that fewer samples are required. In Refs. [5–7], this tradeoff was examined in the context of geophysics, where it was found that implicit sampling indeed can be efficient, in particular when the dimension of the problem is large.

Here, implicit sampling is applied to estimation and control in robotics, and new algorithms for stochastic optimal control, MC localization (MCL), and SLAM are developed. Implementations and efficiencies of these algorithms are illustrated and explored with examples. In particular, it is investigated if implicit sampling, which requires fewer samples that are, however, more informative and more expensive, can be efficient compared to other sampling schemes that may require more samples, each of which is less informative, however cheaper to generate.

The optimal control of a stochastic control problem can be found by solving a stochastic Hamilton–Jacobi–Bellman (HJB) equation [8]. The dimension of the domain of this partial differential equation (PDE) equals the dimension of the state space of the control problem. Classical PDE solvers require a grid on the domain and, therefore, are impractical for control problems of moderate or large dimension. For a class of stochastic optimal control problems, one can use MC solvers instead of grid based PDE techniques and, since the MC approach avoids grids, it is in principle feasible to solve larger dimensional control problems within this class [9–12]. However, the sampling scheme must be chosen carefully or else MC based PDE solving will also fail (in the sense that unfeasibly many samples are required). It is shown in Sec. 3 how to apply implicit sampling to obtain a practical algorithm for stochastic optimal control that avoids many of the pitfalls one faces in MC based PDE solving. The method and algorithm are illustrated with the double slit problem (see Sec. 3.1 and Ref. [9]), which is a simple but vivid example of how things can go wrong, and how they can be fixed with implicit sampling. Using an inverse dynamics controller for a two degrees-of-freedom robotic arm as an example, it is also shown how to obtain a semi-analytic solution for a linear Gaussian problem via implicit sampling. Finally, it is indicated how implicit sampling and stochastic optimal control can help with being trapped in local minima of nonconvex optimization problems. An extension of the method presented here is also discussed in the conference paper [13].

In robotics, one often updates the predictions of a dynamic model of an autonomous robot with the output of the robot's sensors (e.g., radar or lidar scans). This problem is often called “localization,” and can be formulated as a sampling problem. Localization algorithms that rely on MC sampling for the computations are called MCL [14]. One can also learn the geometry and configuration of the map while simultaneously localizing the robot in it, which leads to the problem of “SLAM.” Efficient numerical solutions of the MCL and SLAM problems are a fundamental requirement for autonomous robots [15,16]. The various MCL and SLAM algorithms differ in their importance function *p*_{0}, and some algorithms have been shown to be inefficient due to a poor choice of *p*_{0} [15]. Here it is shown how to use implicit sampling to generate an efficient importance function for MCL and SLAM. The implicit sampling based MCL and SLAM algorithms are convergent, i.e., as the number of samples goes to infinity one obtains an empirical estimate of the true posterior even if the underlying dynamics are nonlinear (whereas many other SLAM algorithms require linearity for convergence [15]). The memory requirements of the implicit sampling based SLAM algorithm scale linearly with the number of features in the map. The efficiencies of the new MCL and SLAM algorithms are compared to the efficiencies of competing MCL and SLAM algorithms in numerical experiments with the data set [17].

The efficiency of MC sampling depends on how well the importance function *p*_{0} approximates the target PDF *p*. In particular, *p*_{0} must be large where *p* is large, or the samples one produces using *p*_{0} are unlikely with respect to *p*. Implicit sampling is a general MC sampling technique that constructs an importance function that is large where *p* is large by mapping a reference variable to the region where *p* is large [3–7]. Here implicit sampling is described in general terms; below implementations of implicit sampling are described in the context of applications.

The region where *p* is large is the region where −log(*p*) is small (the logarithm is used here because in applications *p* often involves exponential functions). The region where *p* is large can thus be located via minimization of the function

If *F* is convex, the minimizer *μ* = argmin *F* (i.e., the mode of *p*) is the most likely value, and high-probability samples are in its neighborhood. One can obtain samples in this region by solving the stochastic algebraic equation

where $\phi =minF,\xi ~N(0,Im)$ is an *m*-dimensional Gaussian reference variable and where *T* denotes a transpose and *I _{m}* is the identity matrix of order

*m*; here, and for the remainder of this paper, $N(a,B)$ is short-hand notation for a Gaussian PDF with mean

*a*and covariance matrix

*B*. Note that the right-hand side of Eq. (1) is likely to be small because

*ξ*is close to the origin, which implies that the left-hand side is small, which in turn means that the solution of Eq. (1), i.e., the sample, is close to the mode and, thus, in the high-probability region.

The weights of the samples are proportional to the absolute value of the Jacobian determinant of the map that connects the sample *X ^{j}* to the reference variable

*ξ*

In practice, the weights are usually normalized so that their sum is one. Various ways of constructing a map from *ξ* to *x* have been reported in Refs. [4,7] two of which are summarized below. In summary, implicit sampling requires: (1) minimizing *F* = −log *p*(*x*) and (2) solving Eq. (1). This two-step approach makes efficient use of the available computational resources: the minimization identifies the high probability region and the samples are focused to lie in this region, so that (almost) all samples carry information. This is in contrast to other sampling schemes where an importance function is chosen ad hoc, which often means that many of the samples carry little or no information; the computations used for generating uninformative samples are wasted.

Finally, the assumption that *F* is convex can be relaxed. Implicit sampling can be used without modification if *F* is merely *U*-shaped, i.e., the target PDF, *p*, is unimodal. Multimodal target PDFs can be sampled, e.g., by using mixture models, for which one approximates each mode using the above recipe (see Refs. [4,7,18] for more details).

To generate samples, one solves Eq. (1) with a one-to-one and onto mapping. There are many choices for such a mapping, and one is to solve Eq. (1) in a random direction, i.e., one puts

where *L* is a Cholesky factor of the Hessian of *F* at the minimum, $\eta j=\xi j/(\xi j)T\xi j$ is a vector which is uniformly distributed on the *m*-dimensional hypersphere, and where *λ ^{j}* is a scalar. One then computes

*λ*by substituting Eq. (2) into Eq. (1). This approach, called “random map,” requires solving a scalar equation to generate a sample [7]. Moreover, the Jacobian of this map is easy to evaluate with the formula (see Ref. [7] for a derivation)

^{j}In numerical implementations of this method, calculating ∇*F* may require repeated evaluations of *F*, e.g., if the gradient is approximated via finite differences. This can be avoided by noting that

where *ρ* = *ξ*^{T}*ξ*, so that the Jacobian becomes

The scalar derivative *dλ*/*dρ* can be evaluated using finite differences with a few evaluations of *F* (the precise number of function evaluations depends on the finite difference scheme one chooses).

Another path to solving Eq. (1) is to replace it with an approximate quadratic equation

is the Taylor expansion of order two of *F* about its minimizer *μ*, and where *H* is the Hessian of *F* at the minimum. A solution of Eq. (5) is

where *L* is the Cholesky factor of *H* = *LL*^{T}. The weights are

The finite horizon stochastic optimal control problem considered here is as follows: find a control *u* (a *p* dimensional vector function of the state *x*) that minimizes the cost function

where *x* is an *m*-dimensional vector (the state), *t*_{0} ≤ *t* ≤ *t*_{f} is time, *t*_{f} is the final time (or the horizon), Φ is a scalar function that describes the “final cost,” *R* is a *p* × *p* symmetric positive definite matrix that specifies the control cost, and *V* is a scalar function which describes the “running cost” (which is also called “potential”); the expectation is taken over trajectories of the stochastic differential equation (SDE)

starting at *x*(*t*_{0}) = *x*_{0}, where *f* is a smooth *m*-dimensional vector function which describes the dynamics, and where *G* and *Q* are *m* × *p* and *p* × *r* matrices that describe how the uncertainty and controls are distributed within the system; *W* is Brownian motion (see, e.g., Ref. [8] for more details about stochastic optimal control; I closely follow Ref. [9] in the presentation of path integral control).

and satisfies the stochastic HJB equation [8]

linearizes the stochastic HJB equation and one obtains

with final condition *ψ*(*x*, *t*_{f}) = exp(−Φ(*x*(*t*_{f}))/*γ*) and with the optimal control

see Refs. [9,19]. Thus, one can compute the optimal control by solving the HJB equation. Numerical PDE solvers typically require a grid on the domain of the PDE; however, the domain has the dimension of the state space of the control problem. Thus, grid based numerical PDE techniques only apply to control problems of a low dimension (or else the computational requirements become excessive).

For larger dimensional problems, one can use stochastic PDE solvers which do not require a grid. In particular, one can solve the adjoint equation

forward in time (instead of solving Eq. (9) backward in time) with the Feynman–Kac formula (see, e.g., Ref. [1])

starting at *y*(*τ* = *t*) = *x*(*t*). This is the path integral formulation of stochastic optimal control [9–12]. Note that the class of problems that can be tackled with path integral control is rather general, since the assumptions of: (1) a quadratic control cost and (2) the condition on the noise and the control cost in Eq. (8) are not restrictive. In particular, the dynamics *f*(*x*, *t*) and the potential *V*(*x*, *t*) in Eq. (7) can be nonlinear.

To evaluate the Feynman–Kac formula numerically, one approximates the infinite dimensional integral in Eq. (11) by a finite dimensional one. For example, one can discretize the integral on a regular grid in time with constant time step Δ*t*, so that

where *y*_{1} = *x*(*t*), *n* = (*t*_{f} − *t*)/Δ*t*, *τ _{i}* =

*t*+

*iΔt*,

*i*= 0, 1,…,

*n*, and where

*p*is the PDF of the discretized trajectory

*y*

_{1},…,

*y*. Here the trapezoidal rule is used to discretize the integral of the potential; however, other choices are also possible [20]. The SDE (12) implies that the increments Δ

_{n}*y*=

_{i}*y*−

_{i}*y*

_{i}_{−1}are independent Gaussians, e.g., for a forward Euler discretization [21] of Eq. (12)

where Σ = *γGR*^{−1}*G*^{T}^{ }= *GQQ*^{T}*G*^{T}, so that

Thus,

where

Note that this *F* depends on how one discretizes the integral of the potential *V*, and the SDE (12); other discretization schemes will lead to different *F*s.

For a given discretization, i.e., for a given *F*, MC sampling can be applied to compute the expectation in Eq. (13). For example, one can evaluate

for discretized trajectories of Eq. (12), followed by averaging. However, this method can fail if the potential *V* has deep wells [9–11]. In this case, many trajectories of Eq. (12) can end up where *V* is large and, thus, contribute little to the approximation of the expected value *ψ*. For efficient sampling, one needs a method that guides the samples to remain where the potential is small.

This guiding of the samples can be achieved via implicit sampling. Recall that in implicit sampling one first locates the region of high probability by minimizing *F* = −log(*p*(*x*)), where *p*(*x*) is the PDF of the random variable one is interested in. The trajectories we wish to sample have the joint PDF (14), so that, in order to apply implicit sampling to path integral control, one needs to minimize *F* in Eq. (15). With this *F*, one solves the algebraic equation (1) with a one-to-one and onto map (2). The integral in Eq. (13) becomes

where the expectation is over the reference variable *ξ* and where *∂x*/*∂ξ* is the Jacobian of the map from *x* to *ξ* in Eq. (2). Combining Eq. (16) with the expression (3) for the Jacobian gives

The expected value is now straightforward to compute via MC, i.e., sampling a the reference variable *ξ* to obtain *M* samples *ξ ^{j}*,

*j*= 1,…,

*M*, computing, for each one,

*λ*(

*ξ*) and the gradient of

^{j}*F*, followed by averaging. In numerical implementations, it may be more efficient to use the equivalent expression (3) for the Jacobian (which avoids computing the gradient of

*F*). In this case, one obtains

Once *ψ*(*x*, *t*) is computed, we can compute the optimal control from Eq. (10), e.g., via finite differences. It is important to note that this defines the optimal control at time *t*, and that the computations have to be repeated at the next time step to compute *ψ*(*x*(*t* + *dt*), *t* + *dt*). The use of implicit sampling in stochastic optimal control is illustrated with three examples.

The double slit problem is a simple example that illustrates the pitfalls one must avoid when using MC sampling for path integral control [9]. The problem is as follows. Suppose you observe a somehow confused person walking (randomly) toward a wall with two doors, and your job is to guide this person through either one of the two doors. The person and you are modeled by the controlled dynamics

the final cost is quadratic, Φ = *x*(*t*_{f})^{2}/2, and the scalar *R* > 0, that defines the cost of the control, is given; the “wall” is modeled by the potential

for given *a* < *b* < *c* < *d* and *t*_{1} > 0. The stochastic HJB equation of the double slit problem is

One can attempt to solve this equation using standard MC sampling as follows. Solve the SDE

starting at *y*(*τ* = *t*) = *x*(*t*) repeatedly, e.g., using a uniform grid in time and the forward Euler scheme [21]

where Δ*w ^{n}* are independent Gaussians with mean 0 and variance 1. For each trajectory {

*y*

_{1},…,

*y*}, evaluate

_{n}With a high probability, the trajectories will hit the potential wall at *t*_{1} and, thus, *G* is infinite, so that the contribution of a trajectory to the expected value *ψ*(*x*, *t*) in Eq. (11) is zero with a high probability. The situation is illustrated with a simulation using the parameters of Table 1. The results are shown in the left panel of Fig. 1. One observes that 5000 unguided random walks from Eq. (17), all starting at *x*(0), hit the potential wall, and, thus, score *G* = *∞*. All 5000 samples thus carry no probability and do not contribute to the approximation of *ψ*. Even when *O*(10^{5}) samples are considered, it is unlikely that sufficiently many make it past the potential wall [9]. We conclude that this method is unfeasible for this problem, since the number of samples required is extremely large due to the deep wells in the potential.

Implicit sampling can be applied here to fix these problems. In particular, one finds from the Feynman–Kac formula that

which one can compute with implicit sampling in two parts. For *t* ≥ *t*_{1}, i.e., after one has passed the potential wall, the probability of each path is Gaussian with variance *s* = *t*_{f} − *t*, so that

whose minimizer and minimum are $\mu =argmin\u2003F=y\gamma /(s+\gamma ),\phi =minF=y2/(s+\gamma )$. Since *F* is quadratic, $F=\phi +H(y-\mu 2)/2$, where *H* = (*s* + *γ*)/(*sγ*) is the Hessian of *F* at the minimum. The algebraic equation (1) can thus be solved by the linear map

For *t* < *t*_{1}, one splits the integration into two parts, first going from time *t* to *t*_{1} with probability $p(y1,t1|y,t)$, and then from *t*_{1} onward to *t*_{f} with probability $p(z,tf|y1)=N(y1,s),s=tf-t1$

Implicit sampling uses the information about the potential which is infinite at *t*_{1} except for the two slits, so that $p(y1,t1|y,t)=0$ outside the slits and Gaussian with mean *y* and variance $s\u2227=t1-t$ otherwise. Since only the slits need to be explored with samples, the integration can be carried out over the slits

The evaluation of the above integral using the same strategy as above for *t* ≥ *t*_{1} is tedious, but straightforward. Note that the implicit sampling strategy here is the key to solving this problem, because it locates the wells in the potential.

The above analytic solution is compared to a numerical implementation of implicit sampling, for which the paths are discretized with a constant time step Δ*t*. Here the numerical integration is also split up into two parts. For *t* ≥ *t*_{1}, the function *F* is quadratic because the probabilities are Gaussian and the potential has no effect. Thus,

where *n* = (*t*_{f} − *t*)/Δ*t* and *y _{i}* =

*y*(

*t*+

*iΔt*),

*i*= 1,…,

*n*. Minimizing

*F*is straightforward, and the algebraic equation (1) can be solved with a linear map

where *y* = (*y*_{0},…,*y _{n}*) is a

*n*-dimensional vector,

*μ*is the minimizer of

*F*and

*ξ*is an

*n*-dimensional vector whose elements are independent standard normal variates;

*L*is a Cholesky factor of the Hessian at the minimum. Since the Jacobian of this linear map is constant, Eq. (16) gives

where *y* again is a vector whose elements are the discretized path and where *μ* is the location of a local minimum of the constraint problem and *L* is a Cholesky factor of the unconstrained Hessian at a minimum. Equation (16) becomes

The expected value of the Jacobian (1/det *L*) over the slits can be computed by MC as follows. Generate *M* samples *ξ ^{j}*,

*j*= 1,…,

*M*, and, for each one, compute the corresponding trajectory and check if it hits the potential wall. The integral is 1/det(

*L*) times the ratio of the number of trajectories that pass through the wall and

*M*.

The right panel of Fig. 1 shows 50 trajectories one obtains with this approximation at *t* = Δ*t*. Note that most of the trajectories pass through the slit, i.e., most of the samples carry a significant probability, score a small *F*, and, thus, contribute to the approximation of the expected value *ψ*(*x*, *t*). These “guiding” effects make it possible to solve the problem with *O*(10) samples, while the standard MC scheme fails even with *O*(10^{5}) samples [9]. The Laplace guided strategy presented in Ref. [9] (which uses similar ingredients as implicit sampling and is also related to the optimal nudging constructions in Refs. [24,25]) requires about 100 samples.

The numerical approximation obtained with implicit sampling (50 samples) is compared to the analytical solution in Fig. 2, where the optimal control and the trajectory under this control are shown. One observes that the numerical approximation of the optimal control is quite close to the control, one obtains from the analytical solution (right panel of Fig. 2) and, hence, the controlled trajectory one obtains with implicit sampling is also close to the one computed from the analytical solution (left panel of Fig. 2). This statement can be made more precise by solving the control problem repeatedly (each solution is a random event) and averaging.

The double slit problem is solved 500 times and the error of the numerical approximation is recorded for each run. The Euclidean norm of the difference of the analytical solution and the approximation via implicit sampling is used to measure an error, in particular in *x* (the trajectory under optimal control) and *u* (the optimal control). The number of samples of implicit sampling is varied to study the convergence of the algorithm. The results are shown in Table 2 where the mean and standard deviation of the Euclidean norm of the error in *x* and *u*, respectively, scaled by the mean of the norms of *x* and *u,* respectively, are listed. These numbers indicate the errors one should expect in a typical run. The errors are relatively small when 50 samples are used. The errors do not dramatically decrease for larger sample numbers, which indicates that the algorithm has converged. The small variance of the errors indicates that similar errors occur in each run, so that one concludes that implicit sampling is reliable.

The error in the numerical implementation of implicit sampling is mostly due to neglecting one of the local minima of *F*. In numerical tests, only a slight improvement was observed when both minima were used for sampling; however, the computations are twice as fast if one considers only the smaller of the two minima.

Stochastic optimal control of the two degrees-of-freedom robotic arm shown in Fig. 3 is considered. The controller is an “inverse dynamics controller” that linearizes the system. This example demonstrates how implicit sampling based path integral control works in linear problems. Specifically, a semi-analytical solution is derived for which a numerical optimization is required; however, the expected value of the implicit sampling solution in Eq. (16) can be evaluated explicitly (i.e., no numerical sampling is needed). Moreover, only some of the dynamic equations of the first-order formulation of the dynamics are driven by noise, so that the dimension of the stochastic subspace is less than the actual dimension of the problem. Implicit sampling can exploit these features, and this example demonstrates how. The algorithm is tested in numerical experiments in which false parameters are given to the algorithm to demonstrate the robustness of the path integral controller to model error.

The goal is to compute two independent torques *τ*_{1} and *τ*_{2} that drive the arm to a desired position *θ*^{*} (described by the two-angles *θ*_{1}, *θ*_{2}, the degrees-of-freedom). Neglecting energy dissipation, and assuming the robot is mounted on a horizontal table (no effects of gravity), the equation of motion is

where *θ* = (*θ*_{1}, *θ*_{2}) is a two-dimensional vector and where dots denote derivatives with respect to time; the 2 × 2 matrices

define the dynamics and depend on the lengths of the arms *l*_{1}, *l*_{2} and the loads *m*_{1}, *m*_{2} (see, e.g., Ref. [26] for more details about this model and Table 3 for the parameters used in simulations). One can compute the torques by inverting the dynamics and choosing $\tau =C(\theta ,\theta \xb7)+M(\theta )u$ to derive the linear system

where 0 denotes the matrix whose elements are all zero (of appropriate dimensions), and *I* is the 2 × 2 identity matrix; *u* is a two-dimensional vector of controls which must be computed. To make the control robust to modeling errors, one can add noise and solve the stochastic optimal control problem

where $x=(\theta ,\theta \xb7)$ is a four-dimensional vector and where the matrices *A*, *G,* and *Q* can be read from Eq. (17), and where *W* is a two-dimensional Brownian motion. The final cost is chosen as

so that the robotic arm stops at *t*_{f} > 0 at the desired position *θ*^{*}. This linear problem can be solved with linear quadratic Gaussian control [8]; however, here a semi-analytical solution is obtained via implicit sampling.

As before, the time is discretized using a constant time step Δ*t* = (*t*_{f} − *t*)/*n*. The discretized dynamics are

where *y _{i}*,

*z*are two-dimensional vectors whose elements are the discretized angular velocities ($\theta \xb7$) and the discretized angles (

_{i}*θ*); note that only one of the above equations is driven by noise (Δ

*B*), since there is no reason to inject noise into the (trivial) equation $\theta \xb7=\theta \xb7$. While the noise propagates via the coupling to all variables, the PDFs that define the path in Eq. (11) are in terms of

*y*s only.

_{n}The function *F* in implicit sampling is thus a function of *y* only

where *γ* = *r* is chosen to satisfy Eq. (8). Because *F* is quadratic the minimization is straightforward, and the algebraic equation (1) can be solved with a linear map whose Jacobian is constant and given by the determinant of a Cholesky factor *L* of the Hessian of *F* at the minimum (see above). Thus, Eq. (16) becomes

There is no need for generating samples, since the expected value is computed explicitly (i.e., by evaluating the integral analytically).

The robustness of the implicit sampling based path integral control is tested in numerical experiments. To simulate that the “real” robotic arm behaves differently from the numerical model that the path integral controller uses to find a control, one can give the controller false information about the parameters of the numerical model. Here the parameters *m*_{1} and *m*_{2} are perturbed values of the true parameters simulating that the robotic arm picked up a payload (of unknown weight). Thus, the controller works with values *m*_{1} = 1.4, *m*_{2} = 1, whereas the “true” robotic arm has parameters as in Table 3. The results of a simulation are shown in Fig. 4 where state trajectories and controls are shown for a truly optimal controller (i.e., one who has access to the exact model parameters), and for the path integral controller which uses false model parameters. One observes that the path integral controller can perform the desired task (moving the arm to a new position) and that its control and the resulting state trajectories closely follow those that are generated by a truly optimal controller. This example thus indicates that the path integral controller can perform reliably and quickly in applications where model error may be an issue.

One can setup a conventional optimization problem, i.e., find min *f*(*x*) for a given smooth function *f*, as a stochastic optimal control problem as follows: find a control *u* to minimize the cost function

The idea is to make use of the stochastic component to explore valleys in the function *f*. The parameters one can tune are the noise level *σ*, the final time *t*_{f}, and the control cost *R*.

Implicit sampling for this control problem requires minimization of the function

where the discretization is done using a constant time step Δ*t* = *t*_{f}/*n* as before. Note that the control approach to this problem inserts a quadratic term through which the space is (randomly) explored.

which is a popular test of the performance of optimization algorithms because of its many local minima [27]. The parameters are *R* = 0.01, *σ* = 0.01, Δ*t* = 1, *t*_{f} = 20, and the minimization is initialized at (−1, −4). Figure 5 shows the iterations obtained with implicit sampling and 50 samples, and, for comparison, the iterations of Newton's method.

In this example, the stochastic approach is successful and finds a much lower minimum than Newton's method. However, since each run of the stochastic approach is random, one may find another local minimum in another run. This could be useful when one needs to explore valleys or if one suspects the existence of other local minima.

To test the reliability of the stochastic control approach, 70 experiments were performed, each starting at the same initial condition. The iterations of five such experiments are shown in Table 4. All 70 runs ended up close to the local minimum around *x*_{1} = 2.5, *x*_{2} = −2.5. The average value of *F* is an approximation to *C* in Eq. (18) and, with 70 experiments was found to be *C* ≈ 1.75, which corresponds to a much lower value of *f* than the value $fminNewton=178.34$, that one obtains with Newton's method.

Consider a mobile robot that navigates autonomously and, as it moves, collects noisy measurements about its motion as well as scans of its environment. If the scans reveal locations of features that are known to the robot, i.e., if the robot has a map of its environment, then it can localize itself on this map. This problem is known as localization. When the robot has no map of its environment, then it must construct a map while localizing itself on it, leading to the problem of SLAM [28–30]. Efficient solutions to the localization and SLAM problems are a fundamental requirement for autonomous robots, which must move through unknown environments where no global positioning data (GPS) are available, for example indoors, in abandoned mines, underwater, or on far-away planets [16,31,32]. Application of implicit sampling to localization and SLAM is the subject of Secs. 4.1 and 4.2, where the algorithms will also be tested using experimental data obtained by Nebot and colleagues at the University of Sydney [17].

The localization problem can be formulated as follows. Information about the initial state of the robot is available in the form of a PDF *p*(*x*_{0}), where *x*_{0} is an *m*-dimensional vector whose elements are the state variables (e.g., position and velocity of the robot). A probabilistic motion model defines the PDF

where *n* = 1, 2,… is discrete time and where *u _{n}* is a

*p*-dimensional vector of known “controls,” e.g., the odometry. A data equation describes the robot's sensors and defines the PDF

where *z _{n}* is a

*k*-dimensional vector whose elements are the data (

*k*≤

*m*) and where Θ is the map. For example, one can use the “landmark model,” for which the map describes the coordinates of a collection of obstacles, called “landmarks;” the data are measurements of range and bearing of the position of the robot relative to a landmark. The landmark model and range and bearing data will be used in the applications below, but the algorithms described are more generally applicable.

The motion and sensor model jointly define the conditional PDF $p(x0:n|z0:n,u0:n,\Theta )$, where the short-hand notation $x0:n={x0,x1,\u2026,xn}$ is used to denote a sequence of vectors. By using Bayes' rule repeatedly, one can derive the recursive formula

Approximations of this PDF are used to localize the robot. For example, methods based on the Kalman filter [33] (KF) construct a Gaussian approximation which is often problematic because of nonlinearities in the model and data. Moreover, KF-based localization algorithms can diverge, for example, in multimodal scenarios (i.e., if the data are ambiguous) or during relocalization after system failure [15]. The basic idea of MCL is to relax the Gaussian assumptions required by KF, and to apply importance sampling to the localization problem [14]. The method proceeds as follows.

Given *M* weighted samples ${X0:n-1j,Wn-1j},j=1,\u2026,M$, which form an empirical estimate of the PDF $p(x0:n-1|z0:n-1,u0:n-1,\Theta )$ at time *n* − 1, one updates each particle to time *t _{n}* given the datum

*z*. In standard MCL [14], this is done by choosing the importance function

_{n}that is, the robot location is predicted with the motion model and then this prediction, $Xnj$, is weighted by

to assess how well it fits the data. However, this choice of importance function can be inefficient, especially if the motion model is noisy and the data are accurate (as is often the case [16]). The reason is that many of the samples generated by the model are unlikely with respect to the data and the computations used to generate these samples are wasted.

Implicit sampling can be used to speed up the computations. This requires in particular that implicit sampling, as described in Sec. 2, is applied to the *M* functions

One needs *M* functions *F ^{j}*,

*j*= 1,…,

*M*, one per sample, because the recursive problem formulation requires a factorization of the importance function (compare to Secs. 2 and 3, where only one function was needed). Here, each function

*F*is parametrized by the location of the sample at time

_{j}*n*− 1, $Xn-1j$, the control

*u*and the datum

_{n}*z*; the variables of these functions are the location of the robot at

_{n}*t*,

_{n}*x*.

_{n}For MCL with implicit sampling, each function *F ^{j}* must be minimized, e.g., using Newton's method. After the minimization, one can generate samples by solving the stochastic equation (1) with the techniques described in Sec. 2. Using information from derivatives in sampling for MCL has also been considered in Refs. [34,35].

The University Car Park data set [17] is used to demonstrate the efficiency of MCL with implicit sampling. The scenario is as follows. A robot is moving around a parking lot and steering and speed data are collected via a wheel encoder on the rear left wheel and a velocity encoder. An outdoor laser (SICK LMS 221) is mounted on the front bull-bar and is directed forward and horizontal (see Ref. [36] for more information on the hardware). The laser returns measurements of relative range and bearing to different features. Speed and steering data are the controls of a kinematic model of the robot and the laser data are used to update this model's predictions about the state of the robot.

The motion model is the forward Euler discretization of the continuous time two-dimensional-model in Ref. [36]

where *R*(*x _{m}*,

*u*) is a three-dimensional vector function,

_{n}*δ*is the time step, and (Σ

_{1})

*is a given covariance matrix. The data equation is*

_{n}where *h* is a two-dimensional vector function that maps the position of the robot to relative range and bearing and where Σ_{2} is a 2 × 2 diagonal matrix (see Ref. [36] and the Appendix for the various model parameters).

With the above equations for motion model and data, the functions *F ^{j}* of implicit sampling in Eq. (21) become

where $fnj=Xn-1j+R(Xn-1j,un)\delta $ and where $zni$ denotes the measurement for the *i*th landmark at time *n*. These *F ^{j}*s are minimized with Newton's method and samples are generated by solving a quadratic equation as explained in Sec. 2. The gradient and Hessian in Newton's method were computed analytically (see the Appendix). If uneven weights were observed, a “resampling” was done using Algorithm 2 in Ref. [37]. Resampling replaces samples with a small weight with samples with a larger weight without introducing significant bias. If no laser data are available, all samples evolve according to the model equation (19).

The results of MCL with implicit sampling are shown in the left panel of Fig. 6. The “ground truth” shown here is the result of an MCL run with GPS data (these GPS data however are not used in implicit sampling); the large dots are the positions of the landmarks. The reconstruction by the MCL algorithm (dashed line) is an approximation of the conditional mean, which is obtained by averaging the samples. One observes that MCL with implicit sampling gives accurate estimates whenever laser data are available. After periods during which no laser data were available, one can observe a strong “pull” toward the true state trajectory, as soon as data become available, as is indicated by the jumps in the trajectory, e.g., around *x* = 0, *y* = 0.

The efficiency of MCL with implicit sampling is studied by running the algorithm with 10, 20, 40, 80, 100, and 150 samples, i.e., with increasing computational cost, and comparing the reconstruction of the MCL with the true path. The error is measured by the Euclidean norm of the difference between the “ground truth” (see above) and the reconstruction by the MCL algorithm (scaled by the Euclidean norm of the ground truth). The standard MCL method was also applied with 10, 20, 40, 80, 100, 150, 250, and 300 samples. The results are shown in Table 5.

It is clear that the both MCL algorithms converge to the same error, since both algorithms converge to the conditional mean as the number of samples (and hence, the computational cost) goes to infinity. However the convergence rate of MCL with implicit sampling is faster, which can be seen from the right panel of Fig. 6, where the computing time is plotted as a function of the error. In this figure, the area between the lines corresponds to the improvement of implicit sampling over the standard method, and it is clear that implicit sampling is more efficient than the standard method. The improvement is particularly pronounced if a high accuracy is required, in which case MCL with implicit sampling can be orders of magnitudes faster than the standard method.

In SLAM, one is given the probabilistic motion model (19) and a data equation (20), and one needs to estimate the position of the robot as well as the configuration and geometry of the map; the conditional PDF of interest is thus

where the map, Θ, is a variable (not a given parameter as in MCL); the variable *η*_{0:}* _{n}* is the “data association,” i.e., it maps the data to the known features, or creates a new feature if the data are incompatible with current features [38,39]. Here it is assumed that the data association is done by another algorithm, so that

*η*

_{0:}

*in Eq. (23) can be assumed to be given (in the numerical implementation in Sec. 4.2.1, a maximum-likelihood algorithm is used [15]).*

_{n}Note that the SLAM state-vector is different from the state of the localization problem: it is the number of variables needed to describe the robot's position, *x*, and the coordinates of the features of the map, Θ. If the map contains many features (which is typically the case), then the state dimension is large and this makes KF based SLAM prohibitively expensive. The reason is that KF SLAM requires dense matrix operations on matrices of size *N* (where *N* is the number of features), due to correlations between the robot's position and the features [38]. KF SLAM thus has *N*^{2} memory requirements which make it infeasible for realistic *N*.

MC approaches to SLAM reduce the computational cost by exploring conditional independencies [40]. In particular, the various features of the map conditioned on the robot path are independent, which implies the factorization

where *θ ^{k}*,

*k*= 1,…,

*N*are the features of the map [15,41]. This factorization makes it possible to update one feature at a time and thus reduces memory requirements. Moreover, “online” SLAM will be considered here, i.e., the map and the robot's position are constructed recursively as data become available, using

where *θ _{n}* is the feature observed at time

*n*. Alternatively, one can wait and collect all the data, and then assimilate this large data set in one sweep; such a “smoothing” approach is related to graph based SLAM (see, e.g., Ref. [42]) but will not be discussed further in this paper.

For online probabilistic SLAM, one assumes that *M* weighted samples ${X0:n-1j,\Theta j,Wn-1j}$ approximate

at time *n* − 1 and then updates the samples to time *n* by applying importance sampling to

Here, it is assumed that only one feature is observed at a time (which is realistic [15]); however, the extension to observing multiple features simultaneously is straightforward.

Various importance functions have been considered and tested in the literature. For example, the fastSLAM algorithm [43] was shown to be problematic in many cases due to the poor distribution of its samples [15,44]. The reason is that the samples are generated by the motion model and, thus, are not informed by the data. As a result, the samples are often unlikely with respect to the data. The fastSLAM 2.0 algorithm [15,44] ameliorates this problem by making use of an importance function that depends on the data. The algorithm was shown to outperform fastSLAM and was proven to converge (as the number of samples goes to infinity) to the true SLAM posterior for linear models. The convergence for nonlinear models is not well understood (because of the use of extended KFs (EKFs) to track and construct the map). Here, a new SLAM algorithm with implicit sampling is described. The algorithm converges for nonlinear models at a computational cost that is comparable to the cost of fastSLAM 2.0.

Online SLAM with implicit sampling requires that implicit sampling is applied to the *M* functions *F ^{j}* (one per sample), whose variables are the position at time

*t*,

_{n}*x*, and the location of the feature

_{n}*θ*, observed at time

_{n}*t*

_{n}the position of the *j*th sample at time *t _{n}*

_{−1}is a parameter. As in MCL, one needs

*M*functions here due to the recursive problem formulation. The minimization problems of implicit sampling are

and samples ${Xnj,\theta nj}$ are generated by solving the stochastic equation (1). One can use the same technique for solving these equations as described in Sec. 2. Moreover, if the feature *θ _{n}* is already known, the SLAM algorithm reduces to the MCL algorithm with implicit sampling described above.

Note that the memory requirements of SLAM with implicit sampling are linear in the number of features, because the algorithm makes use of the conditional independence of the features given the robot path, so that, at each step, only one feature needs to be considered. Moreover, SLAM with implicit sampling converges to the true SLAM posterior (under the usual assumptions about the supports of the importance function and target PDF [45]) as the number of samples goes to infinity; this convergence is independent of linearity assumptions about the model and data equations. Convergence for fastSLAM and fastSLAM 2.0, however can only be proven for linear Gaussian models [15].

The applicability and efficiency of SLAM with implicit sampling is demonstrated with numerical experiments that mimic the University Car Park data set [17]. Here speed and steering data of Ref. [17] are used, however the laser data are replaced by synthetic data, because the data in Ref. [17] are too sparse (in time) for successful online SLAM (even EKF SLAM, often viewed as a benchmark, does not give satisfactory results).

In these synthetic data experiments, a virtual laser scanner returns noisy range and bearing measurements of features in a half-circle with 15 m radius. If the laser detects more than one feature, one is selected at random and returned to the SLAM algorithm. Since each synthetic data set is a random event, 50 synthetic data sets are used to compute the average errors for various SLAM algorithms. These errors are defined as the norm of the difference of the ground truth and the conditional mean computed with a SLAM algorithm. The performances of EKF SLAM, fastSLAM, fastSLAM 2.0, and SLAM with implicit sampling are compared using these synthetic data sets. All SLAM methods make use of the same maximum likelihood algorithm for the data association. A Newton method was implemented for the optimization in implicit sampling, and the quadratic approximation of *F* was used to solve the algebraic equation (1). A typical outcome of a numerical experiment is shown in the left panel of Fig. 7, where the true path and true positions of the landmarks as well as their reconstructions via SLAM with implicit sampling (100 samples) are plotted.

The results of 50 numerical experiments with fastSLAM, fastSLAM 2.0, and SLAM with implicit sampling are shown in Table 6. It was observed in this example that EKF SLAM gives an average error of 3.89% at an average computation time of 0.43 ms and, thus, is computationally efficient and accurate. However, computational efficiency disappears in scenarios with larger maps due to the *O*(*N*^{2}) memory requirements (*N* is the number of features in the map). The error of fastSLAM 2.0 (whose memory requirements scale linearly with *N*) decreases with the number of samples, and it seems as if the fastSLAM 2.0 solution converges (as the number of samples and, thus, the computation time increases) to the EKF solution. This is intuitive because fastSLAM 2.0 uses EKFs to track the map. The fastSLAM algorithm, on the other hand, converges to an error that is larger than in EKF SLAM or fastSLAM 2.0. SLAM with implicit sampling converges to an error that is smaller than in EKF SLAM. The reason is as follows: implicit sampling requires no linearity assumptions and, therefore, the empirical estimate it produces converges (as the number of samples goes to infinity) to the true SLAM posterior.

Moreover, the convergence rate of SLAM with implicit sampling is faster than for any other SLAM method, due to the data informed importance function. Thus, the approximation of the conditional mean (the minimum mean square error estimator) one obtains with implicit sampling is accurate even if the number of samples is relatively small. As a result, SLAM with implicit sampling is more efficient than the other SLAM algorithms considered here. This is illustrated in the right panel of Fig. 7, where the computing time is shown as a function of the average error. As in MCL (see above), the area between the various curves indicates the improvement in efficiency. Here implicit sampling is the most efficient method, giving a high accuracy (small error) at a small computational cost. Moreover, SLAM with implicit sampling can achieve an accuracy which is higher than the accuracy of all other methods (including EKF SLAM), because it is a convergent algorithm.

Implicit sampling is a MC scheme that localizes the high-probability region of the sample space via numerical optimization, and then produces samples in this region by solving algebraic equations with a stochastic right-hand side. The computational cost per sample in implicit sampling is larger than in many other MC sampling schemes that randomly explore the sample space. However, the minimization directs the computational power toward the relevant region of the sample space so that implicit sampling often requires fewer samples than other MC methods. There is a tradeoff between the cost-per-sample and the number of samples and this tradeoff was studied in this paper in the context of three applications: MCL and probabilistic online SLAM in robotics, as well as path integral control.

In path integral control one solves the stochastic HJB equation with a MC solver. This has the advantage that no grid on the state space is needed and the method is therefore (in principle) applicable to control problems of relatively large dimension. Implicit sampling was applied in this context to speed up the MC calculations. The applicability of this approach was demonstrated on two test problems. Path integral control was also used to find local minima in nonconvex optimization problems.

An implementation of implicit sampling for MCL was tested and it was found that implicit sampling performs better than standard MCL (in terms of computing time and accuracy), especially if the data are accurate and the motion model is noisy (which is the case typically encountered in practice [16]). Similarly, implicit sampling for SLAM outperformed standard algorithms (fastSLAM and fastSLAM 2.0). Under mild assumptions, but for nonlinear models, SLAM with implicit sampling converges to the true SLAM posterior as the number of samples goes to infinity, at a computational cost that is linear in the number of features of the map.

I thank Professor Alexandre J. Chorin of UC Berkeley for many interesting technical discussions and for bringing path integral control to my attention. I thank Dr. Robert Saye of Lawrence Berkeley National Laboratory for help with proofreading this manuscript. This material is based upon work supported by the U.S. Department of Energy, Office of Science, Office of Advanced Scientific Computing Research, Applied Mathematics program under Contract No. DE-AC02005CH11231, and by the National Science Foundation under Grant No. DMS-1217065.

The velocity *v _{l}*, however, is measured at the rear left wheel and, thus, must be translated to the axle

The position of the laser can be obtained from the position of the axle by using

so that the forward model (without noise) is

where *x* = (*x*, *y*, *β*)^{T} and *u* = (*v _{c}*,

*α*)

^{T}. The uncertainty in the model is due to errors in the inputs and in the model [46]

where *W _{u}* and

*W*are two-, respectively, three-dimensional vectors whose components are independent Brownian motions,

_{x}*P*and

*Q*are real 2 × 2, respectively, 3 × 3 matrices and

*u*(

*t*) is the unperturbed control signal. The above equations are linearized in $u\u2227$ to obtain

and the stochastic forward Euler method [21] with time step *δ* is used to discretize the equations

The data are measurements of range and bearing of the features in the map *θ* relative to the robot and are obtained by the laser. Only “high intensity points” from the data set [17] are used. The laser is modeled by the data equation

where Σ_{2} is a 2 × 2 diagonal matrix whose elements are *σ*_{1} = 0.05 and *σ*_{2} = 0.05*π*/180 and

where $x\u2227jn$ is the *j*th element of the vector $x\u2227n$, the “true” robot position, and $m1j,m2j$ are the *x*- and *y*-coordinates of the *j*th element of a landmark; the norm $\Vert \xb7\Vert $ is the Euclidean norm.

The gradient and Hessian of *F _{j}* in Eq. (22) are

where the elements of the 3 × 3 matrices $H\u2227i$ are

## References

## Figures

Random walks for the double slit problem. Left: standard MC sampling with 5000 unguided random walks, all of which hit the potential wall. Right: 50 guided random walks obtained via implicit sampling, only four of which hit the potential wall.

Comparison of the analytical solution and its approximation by implicit sampling with 50 samples. Left: the trajectory under optimal control (solid) and its numerical approximation (dashed). Right: the optimal control (solid) and its numerical approximation (dashed).