# 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

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

is the distance between the wheels

is the distance traveled by the left wheel this tic

is the distance traveled by the right wheel this tic

is the radius of the circle the robot is turning on

is the angle the robot is turning around that circle

## Finding the Sector

First, we'll find the and that describe the sector traced out by the robot's path. Theta is easy, it's just radians. To see why this makes sense, try a few examples: 0 and 1, 1 and -1, and, to take a 90 degree (or 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 , or . Determining is a bit trickier. Something to note here is that if is 0, the sector is actually just a rectangle so is infinity. We ignore that case because it's easy enough to handle separately (the robot's just driving straight). The formula is

To derive this formula, start with the observation that the ratio between the distances is equal to the ratio between the radii . Think through this, especially with the sample cases above, and you'll see it makes sense. Then, solve for :

## Finding the Vector

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 , and call the angle between it and the X-axis . To find (the magnitude or length of ) and , we can draw an isosceles triangle with legs of length and a base of length , with a vertex angle of and therefore base angles of , as shown in the diagram. Knowing everything about this triangle, we can then see that because we take the angle between and the line connecting the robot's wheels last tic , and subtract the angle between that line and the X-axis , yielding the angle between and the X-axis. Finding is even easier because we can just use Law of Sines:

Now all we have to do is turn this vector into a change in Cartesian coordinates, which we can do by splitting into it component vectors. In order to find the vectors for the left and right wheels, replace with and respectively.