Sensor fusion

Our robots know where they are from dead wheels: small unpowered tracking wheels with encoders that roll along the floor. When the engineer robot moved to new odometry pods with three of them, I rewrote the team's Kalman-filter odometry for them; I had taught myself Kalman filters from Kalman Filter from the Ground Up. The first version was one 8-state filter fusing the three wheels with the IMU. Its covariance between heading and the x and y motion turned out to be negligible, so I split it into three independent two-state filters, for x, y and heading, each updated by one measurement: every step now multiplies 2×2 matrices and divides by a number, instead of working through 8×8 matrices and inverting a 7×7 one, and the wheels alone turned out to be enough without the IMU. To tune it, I drove fixed paths (straight forward, straight sideways, spins and a square), logged the raw wheels, and replayed them offline to fit each pod's position, angle and wheel radius and the filter's noise, checking the result against the IMU's heading. Our target was under 10 cm of error over the engineer's path, the most our vision system could correct for, and the tuned odometry met it.

Before the base could navigate, it had to know where it was. The first source was optical flow: a sensor that watches the floor slide past. I brought it up over micro-ROS and fed it into ArduPilot's EKF alongside the IMU of the Pixhawk the base ran at the time. It only read sensibly within about 10 cm of the floor, so it sits low, lit by its own LED.

To tell whether it worked, I needed ground truth. The lab's motion capture tracks the robot more precisely than anything you could measure by hand, so I wrote the package that brings it into ROS: the EKF's estimate and the mocap pose land in one bag on one clock, and every run can be compared with the truth point by point. The first runs showed why that mattered: driving 5.5 m, the estimate stayed within 0.74 m of mocap, but spinning in place sent it 155 m off. Once teammates found the sensor's mounting missing from the config, I tuned the flow gain against mocap, run by run, until the estimate stayed within 5 mm of motion capture.

For localization and a map, the base carries two RPLIDAR A1 2D lidars, one at the front and one at the back. Bringing them up, with the team, was mostly fighting USB: the two units have identical USB IDs, so udev rules name each by the hub port it's plugged into, and they power up a few seconds apart so neither times out.

Mapping and Nav2 expect one scan, so I wrote a node that merges the two. The two scans never line up on their own: nothing guarantees the lidars spin at exactly the same rate or finish a revolution at the same moment, and each reaches the Thor with its own latency. So the node pairs scans by their timestamps (any two within 0.1 s), looks up each scan's transform into the robot's frame at that scan's own timestamp, resamples both to the finer of the two angular resolutions, and keeps the nearest return in each direction of one 360° scan, stamped halfway between the two and dropping hits on the robot itself. On top of it, I set up slam_toolbox and Nav2, which drove the robot point to point in simulation.