Robot Localizer Redesign

A filter can't fix a mistake we made before we handed it the data.

This is what I'm on right now at Thunderbots. Started as an IMU calibration ticket, turned into a full redesign.

Context I was fixing IMU calibration and kept noticing the IMU was breaking stuff way beyond calibration. We weren't fusing IMU $\omega$ and vision properly and it was wrecking our position prediction.

One of the members on the team measured the localizer last sprint and got about 1-2cm off at low speed, 2-3cm at medium. That's the part that made me feel like smth wrong is going on because it scales with speed.

Our prediction step has delta T hardcoded to 0.015s. So at 1 m/s the filter predicts we moved 1.5cm. At 2 m/s, 3cm. Those are basically exactly the numbers he measured.

This shows that our robot localizer is just predicting. The measurements we got from wheel encoders and IMUs are not integrated well enough. More testing will be done to show this.

The localizer has three signals coming in, and they don't agree on frame or on time:

Signal	Frame	Property
$[x,y,\theta {]}_{g,t-\Delta }^T$ from vision	global	accurate, but ~15ms late
$[v_x,v_y{]}_{r,t}^T$ from the wheels	robot	real time, no drift correction
$[\omega ,\alpha {]}_{r,t}^T$ from the IMU	robot, rotation invariant	real time, good for heading rate

What we actually want out is $[x,y,\theta {]}_{g,t}^T$ , accurate, right now.

Problem

Right now it's a Linear Kalman Filter in the global frame, fusing delayed vision, IMU and wheel odometry. I found six structural issues and most of them come back to one thing.

We rotate wheel velocities into the global frame before they ever reach the filter. The wheels give us $[v_x,v_y{]}_{r,t}^T$ in the robot frame, so upstream of the filter we apply a rotation

$${\left[\begin{array}{c}{v}_x\\ v_y\end{array}\right]}_{g,t}=R(\hat{\theta }){\left[\begin{array}{c}{v}_x\\ v_y\end{array}\right]}_{r,t},R(\theta )=\left[\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right]$$

and then hand the result to the filter, which integrates it as $x_g+v_{x,g}\Delta t$ .

The problem is the $\hat{\theta }$ in there. It is our current heading estimate, which may be stale and has not been corrected yet this step. Because the rotation happens outside the filter, an error $\delta \theta$ in that estimate becomes a velocity error $\frac{\partial R}{\partial \theta }v\delta \theta$ that is now baked into the measurement itself. The correction step has no way to touch it. It sees a velocity, not a heading mistake.

The rest of the list:

• Rewind duration is hardcoded. Vision comes in late so we rewind, apply the measurement back at that point in time, then replay forward. But packet latency varies, and honestly the constant we picked isn't even the right constant.
• No real control input. The filter can't predict where the robot is being told to go. We're using acceleration as a stand-in and it's noisy and it's not what we want to model anyway.
• History grows forever during a vision blackout. No packets means nothing trims the buffer, so it just runs off.
• Filter is untuned. $Q$ and $R$ aren't derived from the actual robot model or from real measurement noise, and the process model isn't using the correct relative-motion formula, $a_A=a_B+\alpha \times r_{A/B}+{\omega }^2{r}_{A/B}$ .
• Matrix formatting doesn't match the rest of the codebase. Annoying to read next to the ball filter.

Plan

Two changes, and they only work together. LKF to EKF, and global frame to local (robot) frame.

We have to convert frames either way. It's only a question of where we do it.

Putting the state in the robot frame fixes the root cause, because the wheels are already giving us $[v_x,v_y{]}_{r,t}^T$ . Nothing needs converting on the wheel path at all. It flips the problem around: now it's vision, which is natively global, that has to come into the local frame to be used as a measurement, so we apply $R(\theta {)}^{-1}=R(-\theta )$ to the vision fix instead.

The difference is that this rotation now sits inside the filter. Which is exactly why an LKF stops working. A Kalman filter assumes the observation model is linear, $z=Hx+v$ , so it can be written as a matrix that doesn't depend on the state. Ours can't be, because $\theta$ is part of $x$ and it's multiplying the position terms:

$$h(x)=R(-\theta )\left[\begin{array}{c}x\\ y\end{array}\right]$$

So we linearise. The EKF replaces $H$ with the Jacobian $H_k={\frac{\partial h}{\partial x}|}_{{\hat{x}}_k}$ , evaluated at the current estimate, and everything downstream carries on as normal.

The other thing we get is uncertainty done properly. Heading uncertainty ${\Sigma }_{\theta }$ lives in the state covariance, and because $\theta$ now appears in $H_k$ , it propagates into positional uncertainty through $H_{k}P{H}_k^T$ during correction. Under the LKF that coupling didn't exist at all. The rotation used a point estimate of $\theta$ and threw away everything we knew about how wrong it might be.

Fix	Resolves
Measure rewind time instead of hardcoding it	Hardcoded rewind
Convert vision into local frame, inside the filter	Conversion order
Add a real control input	Missing control
Circular buffer for the history	Unbounded growth
Unfreeze velocity during the update step	Knock-on of conversion order
Tune with the correct car model and real measurement noise	Untuned filter
Match covariance formatting to the ball filter	Consistency

Velocity is frozen during updates right now because updates were messing up the velocity estimate. I'd argue that's just issue 2 showing up somewhere else, not something we should be working around. Building it will tell me if I'm right.

Current State

I'm confirming where the control input should come from before I start implementing. The best team in the world, TigersMannheim runs an EKF with a slightly different structure to incorperate IMU and wheel data. I think it will be really cool to incorperate smth new, the rewind KF, to see if it can work well. Credits to our old Software Lead Andrew for the rewind idea!