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

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₀ (the importance function), and attach to each sample X^j∼ p₀, j = 1,…, M, the weight W^j ∝ p(X^j)/p₀(X^j) (capital letters denote realizations of random variables, and these realizations are indexed in superscript; subscript indices label discrete time). The weighted samples X^j form an empirical estimate of the PDF 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^j). The difficulty is to find a “good” importance function p₀: if p₀ is not a good approximation of p, for example, if p₀ is not large when p is large, then many of the samples one proposes using p₀ 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₀, and some algorithms have been shown to be inefficient due to a poor choice of p₀ [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₀ approximates the target PDF p. In particular, p₀ must be large where p is large, or the samples one produces using p₀ 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.

where $φ=minF,ξ~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.

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).

where ∇F is the gradient of F with respect to x (an m-dimensional row vector).

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).

and account for the error one makes by solving Eq. (5) instead of Eq. (1) (see Ref. [4] for more details).

starting at x(t₀) = x₀, 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).

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).

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.

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 Fs.

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.

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.

where $γ=rσ$ satisfies Eq. (8).

With a high probability, the trajectories will hit the potential wall at t₁ 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⁵) 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.

The evaluation of the above integral using the same strategy as above for t ≥ t₁ 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.

where $φ$ is the minimum of F.

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⁵) 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.

define the dynamics and depend on the lengths of the arms l₁, l₂ and the loads m₁, m₂ (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 $τ=C(θ,θ·)+M(θ)u$ to derive the linear system

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.

where y_i, z_i are two-dimensional vectors whose elements are the discretized angular velocities (⁠$θ·$⁠) and the discretized angles (θ); 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 $θ·=θ·$⁠. While the noise propagates via the coupling to all variables, the PDFs that define the path in Eq. (11) are in terms of y_ns only.

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₁ and m₂ 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.4, m₂ = 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.

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.

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₁ = 2.5, x₂ = −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].

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.

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.

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.

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_j is parametrized by the location of the sample at time n − 1, $Xn-1j$⁠, the control u_n and the datum z_n; the variables of these functions are the location of the robot at 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.

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

where $fnj=Xn-1j+R(Xn-1j,un)δ$ and where $zni$ denotes the measurement for the ith landmark at time n. These F^js 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.

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:n 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]).

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² memory requirements which make it infeasible for realistic N.

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.

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.

and samples ${Xnj,θ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²) 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.

A kinematic model of the robot is described in Ref. [46] and shown for convenience in Fig. 8. The robot is controlled by the speed v_c and the steering angle α. It can be shown that the derivatives of the position and orientation of the axle of the robot are