• Introduction
• Software Setup
• Algorithm
• Lab Report
• Hints

Introduction

For this lab we will implement the Fast SLAM algorithm with known correspondence described on page 450 of Probabilistic Robotics. The algorithm you write will utilize the same data as in lab 3, the motion commands for the robot, and the distance and angle to fiducials. However, in this lab, the location of each fiducial is not known a priori, therefore, they will be mapped in real time.

Software Setup

The Robot.m class can be downloaded and used as in the same way as other labs.

Algorithm

The Robot.m class for this lab is very similar to the one used for lab 3. Therefore, you should again write a loop which runs until robot.run() no longer returns true. The robot command can still be accessed using the robot.input() function and the same motion model as lab 3 used, but with noise, $\mathcal{N}(0, Q)$, where,

Data Structure Initialization

While in lab 3, a simple array could be used for storing the particles, the data in this lab requires more structure for the code to be easily readable. The best way to do this is to use MATLAB cell arrays and structures. We will begin by defining the initial location and variance of each fiducial,

Sigma0 = 10000 * eye(2);
mu0 = zeros(2, 1);

These values can now be placed in a structure using,

mi0.mean = mu0;
mi0.variance = Sigma0;

Given that n equals the number of fiducials — 5 in our case — we can create a cell array of the location and variance of each fiducial using,

m0 = cell(1, n);
for i = 1:n
    m0{i} = mi0;
end

Finally, the full set of particles can be created using,

X = cell(1, N);
for i = 1:N
    X{i}.pose = x0;
    X{i}.map = m0;
end

where N is the number of particles, and $x_0=[-2.5,\,-2,\,0.25]^T$. This structure allows us to easily access or modify the data in the structure we are interested in. For example, X{j}.map{k}.mean specifies the estimated location of the $k^\text{th}$ fiducial in the $j^\mathrm{th}$ particle.

Map Update

Before updating the map, all the particle weights should be set to the same value such that they sum to one. After the detected fiducials are accessed using robot.measurement(), a for loop can be constructed to loop over the detected fiducials,

for i in 1:size(z, 2)
  % more code here
end

For each detected fiducial, the following should be done:

• Define a variable $\hat{z}=h(\mu_j,x)$ where the function $h(\mu_j,x)$ returns $[\hat{r},\,\hat{\phi}]^T$.
• Calculate a matrix $H=\frac{\partial}{\partial x}h(x,\mu_j)$. The entries in this matrix will be,

• Calculate a matrix $Q’=HP_jH^T+Q_t$. Where,

• Calculate a matrix $K=P_jH^TQ’^{-1}$.
• Calculate the error between the estimated and measured fiducial locations, $e=\hat{z}-z$
• Update the estimated location of the $j^\text{th}$ fiducial using $\mu_j=\mu_j+Ke$.
• Update the variance of the estimated location using $P_j=(I-KH)P_j$.
• The current weight can then be multiplied by the probability of the current measurement, $|2\pi Q|^{1/2}\text{exp}(-\frac{1}{2}e^TQe)$.

After the for loop, one final task must be completed. Normalize the weights so they sum to one using,

Lab Report

For your lab report,

1. present an overview of the method used including equations,
2. include a figure of the visualization with the estimated location of the fiducials.
3. explain how the algorithm is a combination of a particle filter and an extended Kalman filter.
4. provide a few reasons why simultaneous localization and mapping is difficult.

Hints

• $\frac{\partial\phi}{\partial \mu_{j,x}}=\frac{- \mu_{y} + y}{\left(\mu_{x} - x\right)^{2} + \left(\mu_{y} - y\right)^{2}}$
• $\frac{\partial\phi}{\partial \mu_{j,y}}=\frac{\mu_{x} - x}{\left(\mu_{x} - x\right)^{2} + \left(\mu_{y} - y\right)^{2}}$
• Sometimes the angular error (the second value in $e$ can) have a high value, but not because the error between the measured and estimated angle is large. This can happen, for example, when the estimated angle is just above zero, and the measured angle is approaching $2\pi$. In this case the two angles are almost identical, but a numerical comparison shows a large error. To fix this, we can re-scale the second element in $e$ using the code snippet below. This code modifies the second value in e such that it is always within the range of $[-\pi, \pi]$.

while abs(e(2)) > pi
    e(2) = e(2) - 2 * pi * sign(e(2));
end