# Pose Estimation

From Team 449 Wiki

This page details the derivation of the algorithm we use to draw profiles. It finds the next position of a 2-sided tank drive given the past position and the distance traveled by each wheel since that position. It assumes velocity was constant on that interval. In this description I'll be referring to each interval as a "tic."

## Explanation

In each tic, the robot can be doing one of three things: driving in a straight line, turning, or standing still. If it's standing still, the next position is the same as the current position. If it's driving in a straight line, we just move its position in the direction it's facing by the measured distance moved. However, if it's turning, things get a bit more complicated. The robot is turning some angle around some point. We know that point must be in a line with the two wheels, but we don't know how far away it is (it can be between the wheels). The point it's turning around is known as the Instantaneous Center of Curvature, or ICC.

## Variables

Diagram with labels showing the variables

$\phi$ is the robot's previous heading (i.e. what angle it was pointed at at the beginning of the tic)

$T$ is the distance between the wheels

$D_{L}$ is the distance traveled by the left wheel this tic

$D_{R}$ is the distance traveled by the right wheel this tic

$r$ is the radius of the circle the robot is turning on

$\theta$ is the angle the robot is turning around that circle

## Finding the Sector

First, we'll find the $r$ and $\theta$ that describe the sector traced out by the robot's path. Theta is easy, it's just $\frac{D_{L}-D_{R}}{T}$ radians. To see why this makes sense, try a few examples: 0 and 1, 1 and -1, and, to take a 90 degree (or $\frac{\pi}{2}$ radian) pivot turn, the right side moves 0 and the left side moves a quarter of the circumference of the circle with a radius of $T$, or $\frac{2T\pi}{4}$. Determining $r$ is a bit trickier. Something to note here is that if $\theta$ is 0, the sector is actually just a rectangle so $r$ is infinity. We ignore that case because it's easy enough to handle separately (the robot's just driving straight). The formula is

$r=\frac{T}{2} \cdot \frac{D_{L}+D_{R}}{D_{L}-D_{R}}$

To derive this formula, start with the observation that the ratio between the distances $(\frac{D_{L}}{D_{R}})$ is equal to the ratio between the radii $(\frac{r+\frac{T}{2}}{r-\frac{T}{2}})$. Think through this, especially with the sample cases above, and you'll see it makes sense. Then, solve for $r$:

$\frac{D_{L}}{D_{R}}=\frac{r+\frac{T}{2}}{r-\frac{T}{2}}$
$\frac{D_{L}}{D_{R}} \cdot (r-\frac{T}{2}) = r+\frac{T}{2}$
$\frac{D_{L}}{D_{R}} \cdot r - \frac{D_{L}}{D_{R}} \cdot \frac{T}{2} = r+\frac{T}{2}$
$\frac{D_{L}}{D_{R}} \cdot r - r = \frac{T}{2} + \frac{D_{L}}{D_{R}} \cdot \frac{T}{2}$
$r(\frac{D_{L}}{D_{R}} - 1) = \frac{T}{2}(\frac{D_{L}}{D_{R}} + 1)$
$r = \frac{T}{2} \cdot \frac{\frac{D_{L}}{D_{R}} + 1}{\frac{D_{L}}{D_{R}} - 1}$
$r = \frac{T}{2} \cdot \frac{\frac{D_{L} + D_{R}}{D_{R}}}{\frac{D_{L} - D_{R}}{D_{R}}}$
$r=\frac{T}{2} \cdot \frac{D_{L}+D_{R}}{D_{L}-D_{R}}$

## Finding the Vector

A diagram of the sector with the vector (blue) overlaid

Next, we want to turn the sector we found into a change in Cartesian coordinate values. To do this, we'll draw a vector pointing from the center of the where the robot was last tic to where it is now. We'll call this vector $\vec{a}$, and call the angle between it and the X-axis $\alpha$. To find $\left\|\vec{a}\right\|$ (the magnitude or length of $\vec{a}$) and $\alpha$, we can draw an isosceles triangle with legs of length $r$ and a base of length $\left\|\vec{a}\right\|$, with a vertex angle of $\theta$ and therefore base angles of $\frac{\pi-\theta}{2}$, as shown in the diagram. Knowing everything about this triangle, we can then see that $\alpha=\frac{\pi-\theta}{2}-(\frac{\pi}{2}-\phi)$ because we take the angle between $\vec{a}$ and the line connecting the robot's wheels last tic $(\frac{\pi-\theta}{2})$, and subtract the angle between that line and the X-axis $(\frac{\pi}{2}-\phi)$, yielding the angle between $\vec{a}$ and the X-axis. Finding $\left\|\vec{a}\right\|$ is even easier because we can just use Law of Sines:

$\frac{r}{\sin(\frac{\pi-\theta}{2})}=\frac{\left\|\vec{a}\right\|}{\sin \theta}$
$\left\|\vec{a}\right\|=\sin \theta \cdot \frac{r}{\sin(\frac{\pi-\theta}{2})}$

Now all we have to do is turn this vector into a change in Cartesian coordinates, which we can do by splitting $\vec{a}$ into it component vectors. In order to find the vectors for the left and right wheels, replace $r$ with $r+\frac{T}{2}$ and $r-\frac{T}{2}$ respectively.