FastSLAM - CodiMD

# FastSLAM ###### tags: `SLAM`, `perception-and-estimation` ###### Team: UG3 林育愷、UG3 王政元 [toc] * [[Official GitHub] HyphaROS MiniBot](https://github.com/Hypha-ROS/hypharos_minibot) * [[GitHub] kuoshih/hypharos_minibot](https://github.com/kuoshih/hypharos_minibot) ## FastSLAM * [FastSLAM: A Factored Solution to the Simultaneous Localization and Mapping Problem](http://robots.stanford.edu/papers/montemerlo.fastslam-tr.pdf) * [Robot Mapping Lecture note: FastSLAM – Feature-based SLAM with Particle Filters. Albert-Ludwigs-Universität Freiburg](http://ais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/slam10-fastslam.pdf) ### Abstract ![](https://i.imgur.com/OplsNYH.png) > Source: FastSLAM: A Factored Solution to the Simultaneous Localization and Mapping Problem * The robot poses $s_k$ evolve over time as a function of the robot controls $u_k$. * Each of the landmark measurements $z_k$ is a function of the position $\theta_k$ of the landmark measured and of the robot pose at the time the measurement was taken. From this diagram it is evident that **the SLAM problem exhibits important conditional independences**. *FastSLAM* decomposes the SLAM problem into a robot localization problem`, and a collection of landmark estimation problems that are conditioned on the robot pose estimation. *FastSLAM* uses a modified particle filter for estimating the posterior over robot paths. Each particle possesses $K$ Kalman filters that estimate the $K$ landmark locations conditioned on the path estimation. A native implementation of this idea leads to an algorithm that requires $O(MK)$ time, where $M$ is the number of particles in the particle filter and $K$ is the number of landmarks. *FastSLAM* develop a tree-based data structure that reduces the running time of *FastSLAM* to $O(M\log R)$, making it significantly faster than existing EKF-based SLAM (before 2002). It also can be extended to situations with unknowned data association and unknown number of landmarks. ### SLAM Problem Formulation #### Motion model $$ p(s_t | u_t, s_{t-1}) $$ #### Measurement model $$ p(z_t | s_t, \theta, n_t) $$ Simply to say, the SLAM problem is said to be $$ p(s^t, \theta | z^t, u^t, n^t). $$ The conditional independence property of the SLAM problem implies that the posterior can be factored as follows: $$ p(s^t|z^t, u^t, n^t)\prod_k p(\theta_k | s^t, z^t, u^t, n^t) $$ $$ \begin{aligned} p&(s^t, \theta | z^t, u^t, n^t) \\ &= p(s^t|z^t, u^t, n^t)\underbrace{\prod_k p(\theta_k | s^t, z^t, u^t, n^t)}_{\text{correction}} \end{aligned} $$ ### FastSLAM All individual landmark estimation problems are independent if one knew the robot's path and the correspondence variables. This conditional independence is the basis of the *FastSLAM* algorithm. The algorithm implements the path estimator $p(s^t | z^t, u^t, n^t)$ using a modified particle filter. The landmark pose estimators $p(\theta_k | s^t, z^t, u^t, n^t)$ are realized by Kalman filters, using separate filters for different landmarks. Since the landmark estimates are conditioned on the path estimate, each particle in the particle filter has its own, local landmark estimates. Thus, for $M$ particles and $K$ landmarks, there will be a total of $KM$ Kalman filters, each of dimension 2. #### Particle Filter Path Estimation *FastSLAM* employs a particle finter for estimating the path posterior $p(s^t|z^t,u^t,n^t)$ using a filter that is similar (but not identical) to the *Monte Carlo localization (MCL)*. *MCL algorithm* is an application of particle filter to the problem of robot pose estimation (localization). At each point in time $t$, both algorithms maintain a set of particles representing the posterior $p(s^t | z^t, u^t, n^t)$, denoted $S_t$. Each particle $s^{t, [m]}\in S_t$ represents a **guess** of the robot's path: $$ S_t = \{ s^{t,[m]} \}_m = \{ s^{[m]}_1, s^{[m]}_2, \cdots, s^{[m]}_t \}_m $$ where we use the superscript notation $^{[m]}$ to refer to the $m$-th particle in the set. The paritcle set $S_t$ is calculated incrementally, from the set $S_{t-1}$ at time $t-1$, a robot control $u_t$, and a measurement $z_t$. First, each particle $s^{t-1,[m]}$ in $S_{t-1}$ is used to generate a probabilistic guess of the robot's pose at time $t$: $$ s_t^{[m]} \sim p(s_t | u_t, s^{[m]}_{t-1}), $$ obtained by sampling from the probabilistic motion model. This estimate is then added to a temporary set of particles, along with the path $s^{t-1,[m]}$. Under the assumption that the set of particles in $S_{t-1}$ is distributed by $p(s^{t-1} | z^{t-1}, u^{t-1}, n^{t-1})$, the new particle is distributed according to $p(s^t | z^{t-1}, u^t, n^{t-1})$. This commonly referred to as the ***proposal distribution*** of the particle filtering. After generating $M$ particles in this way, the new set $S_t$ is obtained by sampling from the temporary particle set. Each particle $s^{t,[m]}$ is drawn (with replacement) with a probability proportional to a so-called ***importance factor*** $w^{[m]}_t$, which is calculated roughly as follows: $$ w^{[m]}_t = \frac{\text{target distribution}}{\text{proposal distribution}} = \frac{p(s^{t, [m]} | z^t, u^t, n^t)}{p(s^{t, [m]} | z^{t-1}, u^t, n^{t-1})} $$ #### Landmark Location Estimation *FastSLAM* represents the conditional landmark estimates $p(\theta| s^t, z^t, u^t, n^t)$ by the **extended Kalman filter (EKF)**. Specifically to say, the full posterior over paths and landmark positions in the *FastSLAM* is represented by the sample set $$ \mathcal{S}_t = \{ s^{t, [m]}, \mu_1^{t, [m]}, \Sigma_1^{t, [m]}, \cdots, \mu_K^{t, [m]}, \Sigma_K^{t, [m]} \}_m $$ Here $\mu_k^{t, [m]}\in\mathbb{R}^2$ and $\Sigma_k^{t, [m]}\in M(2, 2, \mathbb{R})$ are mean and covariance of the Gaussian representing the $k$-th landmark $\theta_k$, attached to the $m$-th particle. Its computation depends on whether or not $n_t = k$. That is, $$ p(\theta_k | s^t, z^t, u^t, n^t) = \left\{ \begin{array}{l} p(z^t | \theta_k. s^t, n^t) p(\theta_k | s^{t-1}, z^{t-1}, u^{t-1}, n^{t-1}) & \text{if } n_t = k,\\ p(\theta_k | s^{t-1}, z^{t-1}, u^{t-1}, n^{t-1}) & \text{else}. \end{array} \right. $$ #### Calculating the Importance Weights In short, with Bayes and Markov assumptions as well as the EKF (linearization) approximation, $$ w_t^{[m]} \approx \int p(z_t | \theta_{n_t}^{[m]}, s_{t}^{[m]}, n_t) p(\theta_{n_t}^{[m]})\;d\theta_{n_t}. $$ Assume that the distribution $p(n_t | \theta, s_t^{[m]})$ is uniform--a common assumption in SLAM. The **EKF** makes explicit the use of a linearized model as an approximation to the observation model $p(z_t | \theta, s_t^{[m]})$, and the resulting Gaussian posterior $p(\theta_{n_t}^{[m]})$. Finally, it turns out to be a closed form solution to the weighting function: $$ w_t^{[m]} \simeq |2\pi Q|^{-\frac{1}{2}}\exp\left(-\frac{1}{2}(z_t - \hat{z}^{[m]}_{n_t})^T Q^{-1} (z_t - \hat{z}^{[m]}_{n_t})\right), $$ where the $Q$ is the corresponding measurement covariance. #### Efficient Implementation (Balanced BST) -- A better data structure ![](https://i.imgur.com/oGD98kX.png) > Source: FastSLAM: A Factored Solution to the Simultaneous Localization and Mapping Problem ### Algorithm #### FastSLAM_1 1. **FastSLAM_1**($z_t$, $c_t$, $u_t$, $\mathcal{S}_{t-1}$): 2. **FOR** $k=1:N$ 3. Let $\left< x_{t-1}^{[k]}, \mu_1^{t-1, [k]}, \Sigma_1^{t-1, [k]}, \cdots \right>$ be paritle $k$ in $\mathcal{S}_{t-1}$. 4. $x_{t}^{[k]} \sim p(x_t| x_{t-1}^{[k]}, u_t)$ 5. $j = c_t$ 6. **IF** $j$ never seen before 7. $\mu_{j}^{t, [k]} = h^{-1}(z_t, x_{t}^{[k]})$ 8. $H = \frac{dh(\mu_{j}^{t, [k]}, x_{t}^{[k]})}{d(\mu, x)}$ 9. $\Sigma_{j}^{t, [k]} = H^{-1}Q_t (H^{-1})^T$ 10. $w^{[k]} = \rho_0$ 11. **ELSE** 12. $\hat{z}^{[k]} = h(\mu_{j}^{t-1, [k]}, x^{t, [k]})$ 13. $H = h'(\mu_{j}^{t-1, [k]}, x^{t, [k]})$ 14. $Q = H\Sigma_{j}^{t-1, [k]} H^T + Q_t$ 15. $K = \Sigma_{j}^{t-1, [k]}H^TQ^{-1}$ 16. $\mu_{j}^{t, [k]} = \mu_{j}^{t-1, [k]} + K(z_t - \hat{z}^{[k]})$ 17. $\Sigma_{j}^{t, [k]} = (I-KH)\Sigma_{j}^{t-1, [k]}$ 18. $w^{[k]} = |2\pi Q|^{-\frac{1}{2}}\exp\left\{ -\frac{1}{2}(z_t-\hat{z}^{[k]})^T Q^{-1} (z_t-\hat{z}^{[k]}) \right\}$ 18. **FOREACH** unobserved features **AS** $j^*$ 19. $\left< \mu_{j^*}^{t-1, [k]}, \Sigma_{j^*}^{t-1, [k]} \right> = \left< \mu_{j^*}^{t-1, [k]}, \Sigma_{j^*}^{t-1, [k]} \right>$ 20. **ENDFOR** 21. **ENDFOR** 22. $\mathcal{S}_t = \text{resample}\left(\left< x_{t}^{[k]}, \mu_1^{t, [k]}, \Sigma_1^{t, [k]}, \cdots \right>_{k=1,\cdots,N}\right)$ 23. **RETURN** $\mathcal{S}_t$ #### Resampling 1. **Resample**$\left(\left< x_{t}^{[k]}, \mu_1^{t, [k]}, \Sigma_1^{t, [k]}, \cdots \right>_{k=1,\cdots,N}\right)$: 2. Let $\{c_k\}_1^N$, where $c_1 = 0$ 3. Normalize $\{w^{[i]}\}$ 3. **FOR** $i=2:N$ 4. $c_i = c_{i-1} + w^{[i]}$ 5. **ENDFOR** 6. Let $i=1$ 7. Draw a starting point: $u_1\sim \text{unif}(0, N^{-1})$ 8. **FOR** $j=1:N$ 9. Move along the CDF: $u_j = u_1+N^{-1}(j-1)$ 10. **WHILE** $u_j > c_i$ 11. $i = i+1$ 12. **ENDWHILE** 13. Assign sample: $x_t^{[j]}=x_t^{[i]}$ 14. Assign weight: $w_t^{[j]}=N^{-1}$ 15. Assign parent: $i^j = i$ (?) 16. **ENDFOR** ## Implementation (C++) ### Parameters * Bound of sampling $$ (v, \omega) = (v_0, w_0) + \begin{bmatrix} 0.3 & 0.1 \\ 0.1 & 0.3 \end{bmatrix} \begin{bmatrix} n_v \\ n_\omega \end{bmatrix}, \quad n_v \sim \text{unif} (-v_0, v_0), n_\omega \sim \text{unif} (-\omega_0, \omega_0) $$ * Lidar covariance $$ \begin{bmatrix} 0.01 & 0 \\ 0 & 0.01 \end{bmatrix}^2 $$ ### Flowchart ```graphviz digraph SLAM { main [label="main.cpp"] io [label="robotIO.cpp"] slamcpp [label="slam.cpp"] map [label="gmapping.h"] slamh [label="slam.h"] eigen [label="Eigen/Dense"] rbt [label="redBlackTree.h"] ros [label="libros"] main -> slamcpp; main -> io; main -> map; slamcpp -> slamh; slamh -> eigen; slamh -> rbt; io -> eigen; io -> ros; } ```