Localization Algorithm

Localization Algorithm Development

We can not obtain very accurate estimates of our position through solely using the GPS readings alone. Therefore, we resort to a sensor fusion method to augment our accuracy.

The Algorithm

The algorithm has 3 stages:

• observing the current state of the bicycle from sensors
• updating the position from the GPS
• estimating and updating position from the current beliefs from speed and heading

Although the algorithm is mostly correct, it still experiences some trouble in determining the location of the bike. As of spring 2019, we have temporarily changed to using a phone as the primary method for determining the bicycle's location. After the position is estimated, the navigation algorithm uses this to calculate the front wheel angle and determine where to travel.

Stage 1

We update the speed, the bike heading, and the change in heading. Our sensor readings come from the GPS, rather than the the IMU. The bike heading angle of the bike is calculated as following: ${\dot {\psi }}={\frac {v\tan \delta }{l}}$, where $\psi$ is the bike heading (yaw), $\delta$ is the steer angle, and $l$ is the length of the wheelbase

Stage 2

The GPS updates our position. We believe that from our current GPS, the points are only on a grid. If the GPS coordinates have changed grid points, then we know that the coordinate has changed by a fixed amount, and we apply the following formula to obtain the new position estimates.

$x\leftarrow x+k(x'-x)$

Stage 3

The heading is updated by multiplying the time by the yaw. Then, we are able to update the bicycle's overall position estimate using the following:

$x\leftarrow x+\Delta t*v*\cos \psi$

$y\leftarrow x+\Delta t*v*\sin \psi$