# 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

For the sake of these calculations, we treat the tank-drive robot as two wheels along an axis (corresponding to the center of the robot). During each tic, we assume the robot can be doing one of three things: driving in a straight line, turning along some circle, or standing still. The approximation of circular movement is convenient; it is easy to calculate and (assuming constant velocity) accurately accounts for robot movement with errors on the order of O(x^3).

If the robot is 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

${\displaystyle \phi }$ is the robot's previous heading (i.e. what angle it was pointed at at the beginning of the tic). Note that for the purpose of this article, all angles will be positive counterclockwise from the x-axis. If you are implementing this on a system that follows the opposite convention, simply swap "right" and "left" for all the math that follows.

${\displaystyle T}$ is the distance between the wheels

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

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

${\displaystyle r}$ is the radius of the circle the robot is turning on

${\displaystyle \theta }$ is the angle the robot is turning around that circle

## Finding the Sector

First, we'll find the ${\displaystyle r}$ and ${\displaystyle \theta }$ that describe the sector traced out by the robot's path. First, we can find theta. The radius of the circle which the left side is following is ${\displaystyle r-{\frac {T}{2}}}$, and the radius of the circle the right side is following is ${\displaystyle r+{\frac {T}{2}}}$. So:

${\displaystyle D_{L}=(r-{\frac {T}{2}})\theta }$
${\displaystyle D_{R}=(r+{\frac {T}{2}})\theta }$
${\displaystyle D_{R}=\left((r-{\frac {T}{2}})+T\right)\theta }$
${\displaystyle =(r-{\frac {T}{2}})\theta +T\theta }$
${\displaystyle =D_{L}+T\theta }$
${\displaystyle D_{R}-D_{L}=T\theta }$
${\displaystyle {\frac {D_{R}-D_{L}}{T}}=\theta }$

Determining ${\displaystyle r}$ is a bit trickier. Something to note here is that if ${\displaystyle \theta }$ is 0, the sector is actually just a rectangle so ${\displaystyle r}$ is infinity. We ignore that case because it's easy enough to handle separately (the robot's just driving straight). The formula is

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

To derive this formula, start with the observation that the ratio between the distances ${\displaystyle ({\frac {D_{R}}{D_{L}}})}$ is equal to the ratio between the radii ${\displaystyle ({\frac {r+{\frac {T}{2}}}{r-{\frac {T}{2}}}})}$. To get this, simply divide the second equation in the previous derivation by the first. Then, solve for ${\displaystyle r}$:

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

## 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 ${\displaystyle {\vec {a}}}$, and call the angle between it and the X-axis ${\displaystyle \alpha }$. To find ${\displaystyle \left\|{\vec {a}}\right\|}$ (the magnitude or length of ${\displaystyle {\vec {a}}}$) and ${\displaystyle \alpha }$, we can draw an isosceles triangle with legs of length ${\displaystyle r}$ and a base of length ${\displaystyle \left\|{\vec {a}}\right\|}$, with a vertex angle of ${\displaystyle \theta }$ and therefore base angles of ${\displaystyle {\frac {\pi -\theta }{2}}}$, as shown in the diagram. Knowing everything about this triangle, we can then see that ${\displaystyle \alpha =({\frac {\pi }{2}}+\phi )-({\frac {\pi -\theta }{2}})=\phi +{\frac {\theta }{2}}}$ because we take the angle between ${\displaystyle {\vec {a}}}$ and the line connecting the robot's wheels last tic ${\displaystyle ({\frac {\pi -\theta }{2}})}$, and subtract this from the angle between that line and the X-axis ${\displaystyle ({\frac {\pi }{2}}+\phi )}$, yielding the angle between ${\displaystyle {\vec {a}}}$ and the X-axis. Note that this is equivalent to simply taking the average of the current and previous robot headings.

Finding ${\displaystyle \left\|{\vec {a}}\right\|}$ is even easier because we can just use Law of Sines:

${\displaystyle {\frac {r}{\sin({\frac {\pi -\theta }{2}})}}={\frac {\left\|{\vec {a}}\right\|}{\sin \theta }}}$
${\displaystyle \left\|{\vec {a}}\right\|=r\cdot {\frac {\sin \theta }{\sin({\frac {\pi -\theta }{2}})}}=r\cdot {\frac {\sin {\theta }}{\cos {\frac {\theta }{2}}}}=2\cdot r\cdot \sin {\frac {\theta }{2}}}$ (note that this can also be seen directly by bisecting ${\displaystyle \theta }$ and considering the resulting right triangle)

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

## Practical Implementation

In practice, encoder measurements are not perfectly reliable indicators of robot movement: wheels can slip, such as during hard acceleration or a collision, de-coupling the measured wheel movement from the actual movement of the robot. For this reason, pose estimation works best when the robot is moving in a smooth, well-controlled manner, such as during autonomous (especially when following motion profiles that have been chosen to limit slip-causing factors such as jerk and acceleration).

Even under relatively ideal conditions, however, tank drives inevitably experience some degree of slippage due to "wheel scrub" during turns. In practical terms, this leads to the calculated value of ${\displaystyle \theta }$ ending up somewhat larger than actual angular change of the robot.

A "first-order" correction for this problem can be achieved by substituting an inflated, "effective" value of ${\displaystyle T}$ for the actual geometrical value, thus operating under the assumption that the effect of wheel scrub is to decrease ${\displaystyle \theta }$ by some constant factor relative to the theoretical value. In order to determine a suitable "effective" value of ${\displaystyle T}$, rather than measuring the robot, one can run a motion-profile that spins the robot in place with each side of the drive moving a known distance, and measure the resulting angular change; ${\displaystyle T}$ can then be back-calculated from the equations above. Repeating the test for a variety of distances and averaging the results may yield a better value.

While this correction works reasonably well given slow, well-controlled conditions, it is still highly imperfect. A better solution involves the use of a gyroscope to directly measure ${\displaystyle \theta }$. The simplest implementation is to simply neglect to calculate ${\displaystyle \theta }$ from the encoders, and instead directly use the measured value from the gyro. The encoders are thus used only to determine the value of ${\displaystyle r}$; assuming that wheel scrub contributes only to angular error, and not to translational error, we instead calculate ${\displaystyle r={\frac {D_{R}+D_{L}}{2\theta }}}$. Note that no value of ${\displaystyle T}$ is needed at all for this method.

More complex solutions may involve fusing gyro and encoder estimates for ${\displaystyle \theta }$ in more rigorous ways, such as through a Kalman filter.