My series of tutorials on SLAM with the extended Kalman filter is finally done. At this point it’s a good idea to reflect on where we’re trying to go and what motivates us. My goal is to make a mobile robot that can autonomously map indoor spaces. I’m doing this mainly because it’s fascinating, but also to win a bet. Learning state estimation with EKFs was a great step toward that goal; it gave us an initial introduction to probabilistic state estimation. But we have a long way to go. We need to develop a SLAM system that’s more robust and better suited to our problem.
Mapping Methods
My eventual goal is to develop a SLAM system capable of accurately mapping an indoor environment. This gives us a couple of requirements. First, its map representation must be human readable and metrically accurate. A sparse collection of point landmarks, like we used in our EKF, is not sufficient for this. Some sort of dense map representation like an occupancy grid or signed distance function is required.
Example of an occupancy grid map build with RTAB-map. Black cells indicate occupied areas, white cells indicate unoccupied, and gray cells indicate uncertain areas.
Sensor Selection
Second, we need to select sensors suited to the task. Our sensors must be able to give us a metrically accurate picture of the local environment’s structure, not just the locations of discrete landmarks as in the UTIAS dataset. A 2D scanning LIDAR is ideal for taking dense measurements of the surrounding environment and they can be found relatively cheaply. Wheel encoders will also provide useful information on the robot’s motion between LIDAR measurements. Also, because we’re trying to build a globally consistent map, we need a topological understanding of the whole environment. That is, we need to understand how different areas in the environment are connected. This means we must be able to detect when the robot revisits a previously-visited location. This can be done using LIDAR data alone, but cameras using visual bag-of-words or deep place recognition could also be helpful here.
Visual depiction of loop closure using ORB-SLAM. Map on left is prior to loop closure with place association indicated by blue line. Map on right is after loop closure and reoptimization of the map and robot trajectory.
SLAM Formulation
Lastly, we need an estimation framework that can solve our problem quickly and efficiently. We want to estimate a global map of the environment and the robot’s full path through the environment. Filtering approaches are ill-suited to this task because they do not allow for re-estimation of previous poses. This makes it difficult to constrain drift in the robot’s state estimate. One would normally use loop closures to do this, but this is difficult with filtering since ideally the full map and robot trajectory should be re-estimated when a loop closure is detected. That is why we will be using a factor graph approach. A factor graph is a representation of the SLAM problem that visualizes the quantities to be estimated (the robot trajectory, map, etc) as nodes and sensor measurements as constraints between those nodes. The graph model can be translated into a single cost function that can be solved through nonlinear optimization. Best of all, this framework allows for the use of an arbitrary combination of sensors as long as we’re able to correctly incorporate the sensor models into the cost function.
SLAM factor graph example: Blue circles denote robot poses at consecutive time steps (x1, x2 , . . .), green circles denote landmark positions (l1 , l2 , . . .), red circle denotes the variable associated with the intrinsic calibration parameters (K). Factors are shown as black squares: the label “u” marks factors corresponding to odometry constraints, “v” marks factors corresponding to camera observations, “c” denotes loop closures, and “p” denotes prior factors. Taken from Cadena et al, 2016.
Moving Forward
So the plan is to design a graph SLAM system for indoor mapping that uses 2D scanning LIDAR and wheel encoders. It should be able to accurately map and estimate its position within its local environment, as well as perform reliable place recognition and loop closure. It should ideally be able to run in real time on a Raspberry Pi or similar single board computer, so careful attention must be paid to the sparsity of the problem’s factor graph. The graph should be as sparse as possible for efficient optimization. This means I will need to include mechanisms to avoid adding redundant information and to sparsify the problem as it proceeds.
I will first be testing my software on a publicly available dataset. The IRC dataset includes sensor streams from odometry and a 2D scanning LIDAR on a wheeled mobile robot as it traverses the Intel Research Center. It will allow us to test all the requirements on our algorithm including accurate mapping and place recognition. After we’re getting good results on the IRC dataset we can implement it on Colin!
and covariance matrix 
need to contain entries for the robot’s pose as well as the landmark locations. The big difference here is that we don’t know how many landmarks we’ll be tracking. This means we need to add new landmarks to the state as we find them.
, between each landmark and the measurement, and we instead search for the landmark association with the minimum Mahalanobis distance. This is just another way of doing maximum likelihood association. Note also we’re using the same 
matrix as in the ![\begin{align*} \text{for all}&\text{ measurements } z_t^i=[r_t^i\ \phi_t^i]^T \text{ do} \\ &\begin{bmatrix} \bar{\mu}_{N_t+1,x} \\ \bar{\mu}_{N_t+1,y} \end{bmatrix} = \begin{bmatrix} \bar{\mu}_{t,x} \\ \bar{\mu}_{t,y} \end{bmatrix} + r_t^i \begin{bmatrix} \cos(\phi_t^i+\bar{\mu}_{t,\theta}) \\ \sin(\phi_t^i+\bar{\mu}_{t,\theta}) \end{bmatrix} \\ \text{for}&\text{ all landmarks } k \text{ to } N_{t+1} \text{ do} \\ &q = (m_{k,x} - \bar{\mu}_{t,x})^2 + (m_{k,y} - \bar{\mu}_{t,y})^2 \\ &r = \sqrt{q} \\ &\hat{z}_t^k = \begin{bmatrix} \sqrt{q} \\ \text{atan2}(m_{k,y}-\bar{\mu}_{t,y},\ m_{k,x}-\bar{\mu}_{t,x})-\bar{\mu}_{t,\theta} \end{bmatrix} \\ &F_{x,k} = \begin{bmatrix} 1&0&0&0&\dots&0&0&\dots&0\\0&1&0&0&\dots&0&0&\dots&0\\0&0&1&0&\dots&0&0&\dots&0\\0&0&0&0&\dots&1&0&\dots&0\\0&0&0&0&\dots&0&1&\dots&0 \end{bmatrix} \\ &h_t^k = \begin{bmatrix} -\delta_{k,x}/r & -\delta_{k,y}/r & 0 & \delta_{k,x}/r & \delta_{k,y}/r \\ \delta_{k,y}/q & -\delta_{k,x}/q & -1 & -\delta_{k,y}/q & \delta_{k,x}/q \end{bmatrix} \\ &H_t^k=h_t^kF_{x,k}\\ &\Psi_k=H_t^k\bar{\Sigma}_t[H_t^k]^T + Q_t \\ &\pi_k=(z_t^i-\hat{z}_t^k)^T\Psi_k^{-1}(z_t^i-\hat{z}_t^k) \\ \text{end}&\text{for} \\ \pi_{N_t+1}&=\alpha \\ j(i) &= \text{argmin}_k\ \pi_k\\ N_t &= \text{max}\{N_t,j(i)\} \\ \text{endfor}& \end{align*}](https://i0.wp.com/andrewjkramer.net/wp-content/ql-cache/quicklatex.com-71b884a246c54720097fd6983f7a37dc_l3.png?resize=414%2C591&ssl=1 "Rendered by QuickLaTeX.com")
. We finally associate the measurement to the landmark with the lowest distance. If no landmark has a distance lower than 
. Now when doing SLAM the state vector has dimension 
where 
is the number of landmarks in our problem. In this formulation the coordinates of landmark 
(
and 
) are found at indices 
and 
of the state vector.
to 
where 
. 
matrix where the rightmost block is a 
is the motion update calculated from the control input at timestep 
(not the raw control input), 
is the motion Jacobian evaluated at timestep 
is a 
is the constant process noise covariance.
matrix. This time it will be 
, with the subscript 
signifying that it relates the pose 
to landmark 
matrix. The 
identity block occupying the lower two rows and starting at the index 
where 
is the measurement Jacobian. With only these augmentations we can change the localization algorithm with known correspondence to a SLAM algorithm with known correspondence. Not too complicated, right?![z_t^i = [r_t^i\ \phi_t^i\ s_t^i]^T](https://i0.wp.com/andrewjkramer.net/wp-content/ql-cache/quicklatex.com-2748ddc649f7f00566bd0e4f698c3cd3_l3.png?resize=114%2C20&ssl=1 "Rendered by QuickLaTeX.com")
where 
is the measurement to landmark 
at time 
. This vector contains the range 
, bearing 
, and, crucially, identity of the landmark being measured 
. This could be interpreted as “landmark 
is 
meters away, at a bearing of 
radians.”![z_t^i=[r_t^i\ \phi_t^i]^T](https://i0.wp.com/andrewjkramer.net/wp-content/ql-cache/quicklatex.com-5c2c2413b6a0bc6a72e04a501e2ac888_l3.png?resize=94%2C20&ssl=1 "Rendered by QuickLaTeX.com")
. So we are no longer directly telling the robot to which landmark each measurement corresponds. Without knowledge of this correspondence measurements are basically useless to our robot. Sure, it knows the location of a landmark relative to its own location. But without knowing where that landmark is in the world it can’t use that information to refine its estimate of its pose.
, map 
, past measurements 
, and past control inputs 
, which landmark has the highest probability of producing measurement 
?![\begin{align*} \text{for all}&\text{ measurements } z_t^i=[r_t^i\ \phi_t^i]^T \text{ do} \\ \text{for}&\text{ all landmarks } k \text{ in the map } m \text{ do} \\ &q = (m_{k,x} - \bar{\mu}_{t,x})^2 + (m_{k,y} - \bar{\mu}_{t,y})^2 \\ &\hat{z}_t^k = \begin{bmatrix} \sqrt{q} \\ \text{atan2}(m_{k,y}-\bar{\mu}_{t,y},\ m_{k,x}-\bar{\mu}_{t,x})-\bar{\mu}_{t,\theta} \end{bmatrix} \\ &H_t^k = \begin{bmatrix} -\frac{m_{k,x}-\bar{\mu}_{t,x}}{\sqrt{q}} & -\frac{m_{k,y}-\bar{\mu}_{t,y}}{\sqrt{q}} & 0 \\ \frac{m_{k,y}-\bar{\mu}_{t,y}}{q} & -\frac{m_{k,x}-\bar{\mu}_{t,x}}{q} & -1\end{bmatrix} \\ &S_t^k = H_t^k \bar{\Sigma}_t [H_t^k]^T + Q_t \\ \text{end}&\text{for} \\ j(i) &= \text{argmax}_k\ \text{det}(2\pi S_t^k)^{-\frac{1}{2}} \text{exp}\{-\frac{1}{2}(z_t^i-\hat{z}_t^k)[S_t^k]^{-1}(z_t^i-\hat{z}_t^k)^T\}\\ \text{endfor}& \end{align*}](https://i0.wp.com/andrewjkramer.net/wp-content/ql-cache/quicklatex.com-64e1846930d8c3fb71680afa0184f6e8_l3.png?resize=509%2C311&ssl=1 "Rendered by QuickLaTeX.com")
is the estimated correspondence 
and covariance of 
. The robot’s expected (or most likely) pose is at 
. Then it corrects for noise in the motion update using the robot’s sensor readings 
in order to estimate the state at time 
to time 
. This means it requires less memory and processor time to compute!
is the robot’s previous x position in meters
is the robot’s previous y position in meters
is the robot’s previous heading in radians
is the robot’s linear velocity in meters/sec
is the robot’s angular velocity in radians/sec
where 
where
is the correspondence between the measurement and landmarks
where 
where
is the x position of landmark 
is the y position of landmark 
is the ID of landmark 
, which is the likelihood of the measurements 
, which calculates a new pose based on the previous pose 
is small.
. In order to do this we need to multiply the covariance from the last timestep, 
. We also need to calculate the additional noise (or uncertainty) associated with the motion that occurred between 
through 
are noise parameters that, unfortunately, need to be hand-tuned for the particular robot being used. Also, 
is in control space and we need to map it into state space. To do this we need the Jacobian of the motion model with respect to the motion parameters 
. To get the complete updated covariance 
and it is calculated using the measurement model 
.
, 
, and 
are the x position, y position, and ID of landmark 
is a constant for all measurements. It is a 
diagonal matrix with noise parameters 
, 
, and 
. The sigmas represent the noise for landmark range, bearing, and noise, respectively. Like the alphas in equation (3), the sigmas must be hand-tuned in order to obtain good performance.
is then used to calculate the Kalman gain, 
. The Kalman gain determines how heavily the sensor information should be weighted when correcting the estimated new pose 
(calculated in equation (7) and the actual measurement 
and 
. The measurement probability is also calculated here but we are not making use of this value.
