Introduction

In many indoor environments it is difficult for robots to localize themselves via GPS. To aid in the localization process, fiducial markers such as April Tags can be used. Using these fiducial markers the robot can determine the distance it is away from a given fiducial, as well as the angle from the current direction it is pointing.

Environment

We again have a Robot.m file that defines the environment, including a map of the fiducial locations,

This map is also available in the Robot class as robot.m. In this array the columns correspond to each fiducial, and the rows correspond to the $x$ and $y$ coordinates of the fiducials respectively. Initially the location and orientation of the robot is unknown, except that it is somewhere between -3 and 3 meters in the $x$ and $y$ axes.

Robot Motion

The robot motion can be modeled with the following equation,

\[\begin{bmatrix} x_{i+1}\\ y_{i+1}\\ \theta_{i+1} \end{bmatrix}=\begin{bmatrix} x_i+\Delta t v\cos(\theta_i)\\ y_i+\Delta t v\sin(\theta_i)\\ \theta_i+\Delta t\omega \end{bmatrix}+w,\]

where $x_i$ and $y_i$ are the $(x,\ y)$ location of the robot at time $i$ and $\theta_i$ is the direction the robot is pointing, $\Delta t$ is the time step, and $w\sim\mathcal{N}(0, Q)$ with variance,

\[Q=\begin{bmatrix} 0.015^2&0&0\\ 0&0.015^2&0\\ 0&0&0.05^2 \end{bmatrix}.\]

The robot takes an input $u=\left[v\ \omega\right]^T$. The input applied to the robot can be accessed using the robot.input() function.

Robot Perception

The robot includes a sensor that can measure both the distance to these fiducials, as well as the angle to the fiducial. This sensor can only measure fiducials that are less than 2 meters away and within a 90 degree field of view. In other words, the robot can only measure the distance to fiducials which are between 45 degrees left and right of the direction the robot is pointing.

These measurements can be accessed using the robot.measurement() function which returns a $3\times n$ array,

\[\begin{bmatrix} s_1&s_2&&s_n\\ r_1&r_2&\cdots&r_n\\ \phi_1&\phi_2&&\phi_n \end{bmatrix},\]

where $s_i$ is the index of the fiducial, $r_i$ is the distance to the fiducial, $\phi_i$ is the angle to the fiducial, and $n$ is the number of fiducials measured. For these measurements the distance and angle measurements can be considered to have gaussian noise, $\mathcal{N}(0, 0.05^2)$. The distance between a fiducial and a given particle can be calculated using the 2 norm, and the angle to a fiducial from the robot can be calculated using,

\[\hat\phi_i=\mathrm{atan2}(m_{s_j,y} - y_i, m_{s_j,x} - x_i) - \theta_i,\]

Where $m_{s_j,x}$ and $m_{s_j,y}$ are the x and y locations of the $j$th measured fiducial, and $x_i$ and $y_i$ are the x and y locations of the $i$th particle. Using the measurements of the fiducials the location of the robot can be determined using a particle filter.

Robot Class API

The Robot class has a few other useful functions. Firstly, the robot.run() function returns true if your loop should continue and false if it should not. Next, the robot.inc(sleep) function increments the counter inside the Robot class so the next measurement and input can be accessed. The sleep argument specifies the number of seconds to wait, this helps by slowing down the animation to help visualizing the operation of the particle filter.

To visualize the operation of the particle filter two functions are available. First, the robot.plot_particles(X) function plots the robot location and orientation for each particle in the array X. Similarly, the robot.plot_mean(X, w) function plots the mean of all the particles X with weights w.

For your convenience two more functions are defined, robot.pdf(x, sigma), and, robot.resample(X, w). The robot.pdf(x, sigma) function returns the probability density function of a variable x given a zero mean and a standard deviation sigma. The robot.resample(X, w) function returns a new set of particles given the current set of particles X and the weights w.

The same robot.save_png(), robot.save_jpg(), and robot.save_pdf() functions are also included.

Suggested Steps

1. Initialize the Robot class.
2. Initialize an appropriate number of particles and weights.
3. Create a while loop which runs while robot.run() is true and contains robot.inc(sleep).
4. Inside your loop plot the particles and the mean.
5. Inside your loop get the robot input using robot.input() and update the particle locations using the robot motion model.
6. Inside your loop get the robot measurements using robot.measurement() and update the particle weights from this measurement.
7. Inside your loop normalize the weights.
8. Inside your loop resample the particles.
9. Experimentally determine the best number of particles to use.

Lab Report

When writing your lab report, discuss why the results are always the same or why not.