Background

In my free time, I mentor FIRST Robotics, where part of my job is teaching advanced controls theory. It turns out it's moderately hard to get the average highschooler to have fun doing math, so I decided a more visual self-paced format to absorb information would (hopefully) prove fruitful. This is the first installment in a series of incredibly niche concepts, but the problem solving and the concepts behind them are universally applicable.

on to the lecture,,,

how to tell if your robot is lying

Based on Team 1690 Orbit's Software Session

the problem with trusting your wheels

Ordinarily, we fuse readings from various sensors on the robot to calculate a representation of the robot's position in space. This is usally a combination of a locally-derived position based on the robot's motor readings, and an objectively-derived position based on vision. A significant factor to the locally calculated (dead-reckoned) odometry is the wheel's encoder values. By seeing how much the wheels have turned, and multiplying that by pi and the wheel's diameter, we can calculate the linear displacement contributed at any moment, and sum over these to attain a position. This is mostly fine, with a caveat.

Wheel encoders measure velocity by counting how fast the wheel spins. But if a wheel is not maintaining constant and perfect traction with the floor, the encoder reports more or less motion than is actually happening. Feed that bad data into odometry, and the local position estimate drifts.

The question is: how do we determine when this is happening?

the redundancy insight

A swerve drive has 4 independent modules, each reporting:

speed — how fast the wheel is spinning (m/s)
angle — which direction the wheel is pointing (radians)

That's 8 measurements total.

Robot motion only has 3 degrees of freedom:

$v_x$ — forward/backward velocity
$v_y$ — left/right velocity
$\omega$ — rotational velocity

We have 8 numbers describing something that only needs 3. This system is overdetermined.

the rigid body constraint

Before we can detect lies, we need to understand what truth looks like.

A swerve robot is a rigid body. Every point moves uniformly, no stretching or deformation.

When a rigid body moves, any point's velocity is the sum of two components:

$\vec{v}_{\text{point}} = \vec{v}_{\text{translation}} + \vec{v}_{\text{rotation}}$

The translational component is the velocity of the body's center of mass

Translation is identical for every point on a rigid body. If the robot's center moves at $(v_x, v_y)$ , then every point on the robot — including all four wheel modules — shares that same translational velocity.

This is the foundation of the entire algorithm.

Pure translation: all four modules have identical velocity vectors.

the geometry of rotation

Rotation is different. When a rigid body spins around its center, points farther from the center move faster. A point at position $\vec{r} = (x, y)$ relative to the center has rotational velocity:

$\vec{v}_{\text{rot}} = \omega \times \vec{r}$

In 2D, this cross product simplifies beautifully. If we define $\omega$ as a scalar (positive = counterclockwise), then:

$\vec{v}_{\text{rot}} = \omega \cdot (-y, x)$

Why $(-y, x)$ ? This is the vector perpendicular to $\vec{r}$ , pointing in the direction of rotation. You can verify: if a point is at $(1, 0)$ — directly to the right of center — and we rotate counterclockwise, it should move upward. Indeed: $(-0, 1) = (0, 1)$ .

The magnitude follows directly:

$|\vec{v}_{\text{rot}}| = \omega \cdot \sqrt{x^2 + y^2} = \omega \cdot r$

Points farther from center move faster. This is why the rotational component differs for each module based on its position.

Pure rotation: each module's velocity is perpendicular to its radius, with magnitude proportional to distance from center.

superposition

Practical motion ona swerve combines both. Each module's measured velocity is:

$\vec{v}_{\text{measured}}^{(i)} = \vec{v}_{\text{trans}} + \vec{v}_{\text{rot}}^{(i)}$

For module $i$ at position $(x_i, y_i)$ :

$\vec{v}_{\text{measured}}^{(i)} = (v_x, v_y) + \omega \cdot (-y_i, x_i)$

Expanding into components:

$v_{\text{measured},x}^{(i)} = v_x - \omega \cdot y_i$ $v_{\text{measured},y}^{(i)} = v_y + \omega \cdot x_i$

The measured velocities differ between modules, but the underlying translation is the same for all of them. The differences come entirely from the rotational component.

Combined motion: measured velocities (blue) differ, but translational components (orange) are identical.

the inverse problem

Now we can frame the detection algorithm. We have 4 modules reporting measured velocities. We want to:

Estimate the robot's motion $(v_x, v_y, \omega)$
Extract each module's implied translational component
Check if they agree

Forward kinematics: from measurements to motion

Each module gives us two equations (x and y components). With 4 modules, that's 8 equations for 3 unknowns. In matrix form:

$\begin{bmatrix} v_{1x} \\ v_{1y} \\ v_{2x} \\ v_{2y} \\ v_{3x} \\ v_{3y} \\ v_{4x} \\ v_{4y} \end{bmatrix} = \begin{bmatrix} 1 & 0 & -y_1 \\ 0 & 1 & x_1 \\ 1 & 0 & -y_2 \\ 0 & 1 & x_2 \\ 1 & 0 & -y_3 \\ 0 & 1 & x_3 \\ 1 & 0 & -y_4 \\ 0 & 1 & x_4 \end{bmatrix} \begin{bmatrix} v_x \\ v_y \\ \omega \end{bmatrix}$

Or more compactly: $\vec{m} = A \vec{s}$ , where $\vec{m}$ is measurements, $A$ is the kinematics matrix, and $\vec{s} = (v_x, v_y, \omega)$ is the chassis state.

This is overdetermined — 8 equations, 3 unknowns. We solve it with least squares regression:

$\vec{s} = (A^T A)^{-1} A^T \vec{m}$

This finds the chassis state that best explains all 4 module measurements, minimizing squared error. We have to use least squares, instead of solving the system directly, because only one solution precisely satisfies our system of equations, but due to sensor noise and real-world factors, we will basically never set up this problem to line up in such a fashion. So we must find the closest thing instead.

ChassisSpeeds speeds = kinematics.toChassisSpeeds(moduleStates);
double omega = speeds.omegaRadiansPerSecond;

Inverse kinematics: from motion to expected rotation

Now we compute what each module's velocity would be if the robot was only rotating (no translation):

$\vec{v}_{\text{rot}}^{(i)} = \omega \cdot (-y_i, x_i)$

SwerveModuleState[] rotationalOnly = kinematics.toSwerveModuleStates(
    new ChassisSpeeds(0, 0, omega)
);

Extraction: isolating translation

If $\vec{v}_{\text{measured}} = \vec{v}_{\text{trans}} + \vec{v}_{\text{rot}}$ , then $\vec{v}_{\text{trans}} = \vec{v}_{\text{measured}} - \vec{v}_{\text{rot}}$ .

For each module, subtract the rotational component to get the implied translational velocity:

$\vec{v}_{\text{trans}}^{(i)} = \vec{v}_{\text{measured}}^{(i)} - \vec{v}_{\text{rot}}^{(i)}$

Translation2d measured = new Translation2d(measuredState.speed, measuredState.angle);
Translation2d rotational = new Translation2d(rotationalState.speed, rotationalState.angle);
Translation2d translational = measured.minus(rotational);

Decomposition: after subtracting rotation (purple) from measured (blue), all four translational components (orange) should match.

the consistency check

Now we can finally do something useful. In a perfect world with no skidding:

$|\vec{v}_{\text{trans}}^{(1)}| = |\vec{v}_{\text{trans}}^{(2)}| = |\vec{v}_{\text{trans}}^{(3)}| = |\vec{v}_{\text{trans}}^{(4)}|$

Why? Because translation is the same for all points on a rigid body. After removing rotation, what's left must be identical. If the extracted translations disagree, the measurements are inconsistent.

We quantify disagreement with the skid ratio:

$\text{skidRatio} = \frac{\max_i |\vec{v}_{\text{trans}}^{(i)}|}{\min_i |\vec{v}_{\text{trans}}^{(i)}|}$

Skid Ratio	Meaning
1.0	All 4 points are moving uniformly
> 1	something funny is happening

what skidding looks like

There are two kinds of wheel slip, and they produce opposite encoder errors:

Wheel spin — the wheel loses traction and spins freely. The encoder reports more velocity than the robot is actually achieving. Less common in FRC, but happens on worn carpet or when accelerating aggressively.

Wheel drag — the wheel locks up or drags across the surface. The robot is still moving, but the wheel isn't rotating as fast as it should. The encoder reports less velocity than the robot's actual motion. This is the classic scenario where you suddenly brake, and your wheels indeterminately skid across the carpet while momentum carries you forward.

Both cases cause the robot to yaw toward the affected wheel. With wheel drag, FR locks up and creates more resistance, so the robot pivots around that corner — like slamming one brake on a car. With wheel spin, FR provides less thrust, so the left side overpowers it.

The detection algorithm catches both due to only caring about magnitude of disagreement rather than direction

Wheel drag: FR locks up and reports low velocity (short red vector) while the robot curves toward the dragging wheel. The extracted translations disagree.

edge cases

Division by zero

When the robot is stationary, all translational magnitudes approach zero. The ratio becomes $\frac{0}{0}$ , which is undefined.

if (minTranslational < 1e-4) {
    return 1.0;  // Can't skid if you're not moving
}

Pure rotation

When the robot spins in place with no translation, all extracted translations should be zero. In practice, sensor noise gives small random values. The ratio might be $\frac{0.01}{0.005} = 2.0$ , which looks like skidding.

But notice: the magnitudes are tiny. We can either threshold on absolute magnitude (ignore small translations) or accept that the ratio is unstable when translation is near-zero. In practice, pure rotation without any translation is rare during actual driving.

why this works

this solution is actually quite elegant; four wheels reporting to three degrees of freedom means we have more information than strictly necessary. This allows us to do internal consistency checking, and is actually a very common theme across engineering! if you have a 4 legged robot for example, you're overconstrained by one leg, so you can easily tell if your robot is slipping or falling.

sandbox

if you just wanna play around with the simulation made for this article

references

Team 1690 Orbit Software Session — Original explanation
WPILib Swerve Kinematics
254 Cheesy Poofs