I’m working on swerve odometry, and I need a double check on my math/logic.
Using ether’s paper linked here:
And figure 4b, we know for wheel 1 that:
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
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.
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: