## ME465 Lab 3

#### 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,

Fiducial | x | y |
---|---|---|

1 | 2 | -0.3 |

2 | 0.6 | 1.1 |

3 | 1.1 | -1.8 |

4 | -0.4 | 2.6 |

5 | -0.5 | -1 |

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,

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

- Initialize the
`Robot`

class. - Initialize an appropriate number of particles and weights.
- Create a
`while`

loop which runs while`robot.run()`

is`true`

and contains`robot.inc(sleep)`

. - Inside your loop plot the particles and the mean.
- Inside your loop get the robot input using
`robot.input()`

and update the particle locations using the robot motion model. - Inside your loop get the robot measurements using
`robot.measurement()`

and update the particle weights from this measurement. - Inside your loop normalize the weights.
- Inside your loop resample the particles.
- 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.