I’m working on swerve odometry, and I need a double check on my math/logic.

Using ether’s paper linked here:

https://www.chiefdelphi.com/uploads/default/original/3X/8/c/8c0451987d09519712780ce18ce6755c21a0acc0.pdf

And figure 4b, we know for wheel 1 that:

V1x=Vx+w*L/2

Vx is the X velocity of the center of the robot

w is the angular velocity of the robot

V1x is wheel velocity in the X direction

L is the wheel base length.

Now, since we know that we’d be measuring each of those quantities over the same time-slice (ignoring differences between the encoder reporting and the gyro reporting, which may be non-trivial), that:

X1x_previous + delta_X1x=Xx_previous + delta_Xx + Theta * L / 2

X1x_previous is the previous position of the wheel

delta_X1x is the change in the X direction of the wheel, calculated with wheel encoder and module + robot angle

Xx_Previous is the previous X position of the robot

delta_Xx is the change in robot’s X position

Theta is the gyro angle

Assuming I’m right so far, we can then calculate the current X position of the robot

Xx_previous+delta_Xx.

So,

Robot X Position = Xx_previous+delta_Xx = X1x_previous + delta_X1x - Theta * L/2

The entire right side of this equation are knowns from the encoders, module angles, and robot gyro. We will have similar formulas for each wheel. In a perfect world they’ll all give the same answer, but in reality they’ll all have different answers. So we get an over-determined system of 4 equations for 1 variable.

X=answer1

X=answer2

X=answer3

X=answer4

And the sum of squares using matrix math really just ends up being the average of the 4 values.

I feel like I’m doing something wrong here. It doesn’t seem to match up with what I think Jared is saying in this thread:

Little help?