# Localization

The goals of this exercise are to

• implement a simple feature detector using the ultrasound receiver
• use range and bearing information to features in the environment to improve the robot’s position estimates
• get practical experience with error propagation

You need:

• a Sparki robot
• at least two PVC pipes of approximately 2 inch diameter and higher than 12cm (“beacons”)

## Overview

Preventing build-up of a robot’s position estimate requires additional information, such as range and/or bearing to known features in the environment. In this exercise, we will use Sparki’s ultrasound ranger to identify a set of beacons (PVC tubes) in the environment and calculating their relative range and bearing to Sparki. Fusing this information into an improved position estimate requires the following steps:

1. Action update: Calculating a position estimate of the robot using odometry and the error propagation law.
2. Perception update: Calculating a position estimate of the robot using the known location and measurements of range/bearing to 2 or 3 beacons.
3. Estimating the variance of the so-obtained pose using the variance of the ultrasound ranger and the error propagation law.
4. Calculating the new pose as a weighted sum of the robot’s previous estimate and the pose provided by the beacons using the inverse of its invariances as weights. (High variances will lead to this estimate becoming less influential.)

This lab will focus on step (1) and (3).

## Instructions

1. Implement a detector for the PVC pipes. Make sure the environment is clear of clutter so that the ultrasound sensor only sees the upright PVC pipes on the table. Use the following code to get started. Use #define NO_LCD to display the LCD and free up a lot of Sparki’s memory:
 #define NO_LCD
#include  <Sparki.h>; // include the sparki library
int angle=-30;
void setup() { }
void loop() {
sparki.servo(angle);
angle=angle+1;
if(angle>30){
angle=-30; Serial.println();
}
int cm = sparki.ping(); // measures the distance with Sparki's eyes
Serial.print(cm);
Serial.print(" ");
}


You can now inspect the ultrasound readings in the serial terminal. For example, sweeping from -30 to 30 degrees in 1 degree intervals yields the following readings:

First measurement: 72 64 39 39 39 39 40 40 39 39 39 38 19 19 19 18 18 18 18 18 18 18 18 18 18 17 18 18 18 18 18 18 18 19 18 19 56 57 57 62 63 62 62 62 62 62 61 61 61 61 61 61 62 63 64 69 69 70 70 72 71
Second measurement: 64 46 48 49 49 49 48 48 48 48 47 47 47 47 47 49 48 48 14 12 12 13 12 11 12 11 12 11 11 11 11 11 11 11 11 10 11 11 11 11 11 10 10 10 12 11 11 11 11 11 12 12 11 12 12 52 53 65 66 65 64
Third measurement: -1 4 48 48 48 48 48 48 48 48 48 49 49 49 6 6 5 5 4 4 4 4 4 4 4 4 3 4 4 4 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 4 3 3 3 4 4 4 4 5 4 5 5

corresponding to measurements at 18cm, 11cm and 3cm, respectively. Notice that the angle at which the pipe is discernible is quite large. It makes sense to store these values in an array and then look for values, that are feasible, e.g. by establishing a maximum distance that the beacon can be at. (The data shown here contain some clutter on the table.) One way of doing this is to look for changes that are larger than 15, e.g., to detect the presence of an object.

2. Experimentally estimate the variance of the ultrasound range sensor ($\sigma^2_r$) at different distances (10, 20, 30 and 40cm).
3.  Consider two beacons, one at position $(0,0)$ and one at position $(L,0)$. Measure ranges $r_1$ and $r_2$. You can use the following equations to calculate x and y:
$x=\left(\frac{L^2+r_1^2-r_2^2}{2L}\right)$
$y=\pm\sqrt{\left(r_1^2-\left(\frac{L^2+r_1^2-r_2^2}{2L}\right)^2\right)}$
Assume the variance of the ultrasound sensor to be $\sigma_r^2$. Calculate an expression for $\sigma_x^2$ and $\sigma_y^2$.

## Deliverables

1. Provide a brief write-up showing your data collected in (2) and your solution (equations) to (3).
2. What happens to the variance of x and y when L increases?
3. When would you rely on this position estimate over your odometry estimate? When would you rather trust your odometry?
4. Describe how you could estimate your bearing $\theta$ from your position estimate.