A little help with Swerve Odometry and Matrix Math

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:

Little help?

Sorry, I didn’t look through the math. However, the LabVIEW trajectory library has a swerve drive kinematics and odometry module. This is a port of the WPILIB classes. Perhaps either of these can help. (robot sample 5 is a swerve drive. The odometry works okay, but the simulation and control are a work in progress…) You can find the source here http://github.com/jsimpso81

1 Like

I will definitely be taking a look at that once I get home. I’d like to compare my fairly “simple” method above to whatever magic they cooked up.

The way I think about it is: for each wheel contact point, there are two ways to determine the velocity (Vx[i], Vy[i] for i=0,1,2,3):

  1. From the steering angle and velocity of the pod (under a no-slip assumption). Call this V[i]_from_odometry.
  2. As a rigid transformation of the robot center’s velocity (Vx_c, Vy_c, Vtheta_c) accounting for the relative position of the pod from the center. Call this V[i]_from_robot_center.

In a perfect world, these would be equal, so V[i]_from_odometry - V[i]_from_robot_center = 0.

In practice (due to slip or measurement noise), these two equation are seldom equal for all four pods, so we solve for the least-square solutions (which try to minimize the sum of squared errors for each equation).

Under this formulation, your 8 equations are:

  • Vx[i]_from_odometry - Vx[i]_from_robot_center = 0 (for i=0,1,2,3)
  • Vy[i]_from_odometry - Vy[i]_from_robot_center = 0 (for i=0,1,2,3)

The three unknowns are (Vx_c, Vy_c, Vtheta_c), though if you use the gyro for Vtheta_c you can potentially eliminate that one.


And here’s what that looks like more formally:

(Screenshots are from subsection 12.4.3 of https://file.tavsys.net/control/controls-engineering-in-frc.pdf. Look there for all the steps in between the two.)

where r_1 through r_4 are the 2D vectors from the center of rotation of the robot to the swerve module location (note that the center of rotation need not be the center of the robot). This is the method used by WPILib’s SwerveDriveKinematics class, and thus WPILib’s SwerveDriveOdometry class.


Whoa, this PDF is an amazing resource!!! I can’t wait to dive into it and give it a full read.


I think it is an amazing resource as well. The one thing I wish it had in many cases is more examples of the theory in use (with actual numbers). I’ve shown it to several people and the application of the theory is over their heads. While the title says ‘for high schoolers’, I’d venture a guess that there are a whole lot of high schools out there that don’t have mentors / teachers fluent enough to apply the information in a practical manner.

That said, it’s the best resource of it’s kind that I’ve ever seen.


The main thing stopping that before has been the sheer amount of boilerplate required to implement it all in Java or C++. Now that WPILib has simulation support, I can start including code examples and logging/plot generation into the book’s build process. I’ve just been busy with WPILib and 3512’s software stuff. My FRC team made lockdown an eternal build season. :eyes:

Making it consumable by high schoolers has been the goal, but I’ve wanted to do that by gradually bringing the reader up to the appropriate level of rigor rather than dumbing things down. Often, that amount of detail and nuance is needed when troubleshooting the implementation. I’ll be the first to admit the book hasn’t reached the ideal in its tagline yet.

Looking back on my controls education, being able to go from a couple equations to an implementation of those equations was one of the more useful skills I picked up, but it was a byproduct of the projects we did. Worked examples will help emphasize that connection.

1 Like

I’m not even really talking about in a particular formal language. Presenting pseudocode, or even just showing practical problems with actual numbers would go a long way towards helping those high school folks apply the concepts.