I build robots that know where they are.

I’m building a stack that gives the robot a better-grounded estimate. A Monte Carlo localization filter with 450 particles recovers absolute position from four distance sensors. An extended Kalman filter fuses that with odometry, so motion stays smooth without trusting bad measurements. A chi-square gate, outlier rejection and rate-limited corrections keep one bad reading from yanking the motion controller. The opening film, Fig. 1 and Fig. 2 all show this filter at work.

The prototype compares the fused position with odometry-only runs over repeated autonomous paths, with contact and wheel slip. Next I plan to publish the implementation, the test method and the logs, so other VEX teams can reproduce the result or find where it breaks.

1if (!turnCorrectionSuppressed && !sensorsStale &&
2    correctionGeometryReady && correctionStable &&
3    correctionCloseToTrack &&
4    (threeSensorCorrectionReady || twoSensorCorrectionReady)) {
5  const float nisGate = threeSensorCorrectionReady
6      ? config.fusion.nisGate
7      : config.fusion.nisGate * kTwoSensorCorrectionNisScale;
8  if (nis <= nisGate) {
9    ekf.update(rawMeas.pose, rawMeas.covariance);
10    correctionAccepted = true;
11  }
12}

The work started with RealSense feasibility tests on a workstation, moved to a four-wheel Jetson robot, and ended on a humanoid. Most of it was the system around the algorithm: stereo geometry, sensor timing, image quality, mapping modes, and tests that someone else could repeat.

The robot carries FRAMOS stereo cameras built on Sony IMX900 sensors, an Xsens MTi-300 IMU and a Jetson. Before trusting any pose estimate I checked what the sensors were really delivering. The first finding was a driver default: an exposure setting of zero was being read as roughly 660 ms of exposure, which blurred every frame and slowed startup. A fixed 2 ms exposure raised the sharpness score from about 2.1 to 8.4.¹

Two things limited calibration that no solver can fix: the camera’s own image processing, and focus you can’t see. The driver sharpened and denoised every frame by default. That looks better to a person, and it moves corner positions by fractions of a pixel. I turned both off, then built a focus mode in Rerun: a per-lens sharpness meter with edge peaking and a center zoom, measured on full-resolution crops so that a change in stream bandwidth never moves the reading.

For geometry I moved the target onto a Dell P2722H monitor. Its panel is 1920 by 1080 pixels, and the greatest common divisor of 1920 and 1080 is 120. So a board of 120-pixel squares tiles the screen with nothing left over and no rounding between squares.² The board is 14 × 7 squares, which gives 13 × 6 inner corners, drawn by a small Electron tool I wrote. With OpenCV’s findChessboardCornersSB and its accuracy refinement, the stereo error fell from 27 px on the first solve to 0.137 px.

The last step was a solver setting. The standard pipeline blended the new camera intrinsics with older ones, 35% new, as a hedge from the days when the focal length wouldn’t hold still. By now the new values were the reliable ones, and blending pulled the focal length toward a stale number and raised the error to 0.342 px. Using the new intrinsics outright, which the log calls steer-1.0, gave 0.137.

A camera and an IMU can each be accurate and still disagree, because their clocks don’t match. The IMU’s messages were being stamped when they arrived at the computer, which put anywhere from 4.1 to 15.8 ms between samples that should have been exactly 10 ms apart. For calibration recordings I switched to the IMU’s hardware sample clock. Every interval then came out at 10.000 ms across 1,200 samples,³ and the camera–IMU alignment solved to a +2.0 ms offset with 80% inliers.

The robot hangs from two shoulder ropes and can’t move freely, so I wrote a 90-second excitation routine that produces linear acceleration anyway: vertical bounces into free fall and a catch, pitch and roll rocking through the lever arm, and fast reversals in yaw. A script checked the IMU statistics against pass and fail thresholds before any recording was accepted. With fusion on,⁴ the static jitter path fell from 63–75 cm to 11–18 cm.⁵

The robot also carries a RoboSense RS-E1 solid-state LiDAR, with a targetless LiDAR-to-camera calibration inherited from an earlier effort. Before comparing methods I fixed the cheapest real bug: the pipeline was still using stale camera intrinsics, with the principal point off by 54 pixels. Then I redid the calibration properly, using a 151-second slow-motion capture to fill in the sparse point cloud.

The answer was clear. Intensity-based targetless calibration can’t work on a sensor that never produces a dense, calibrated intensity image. By the criteria we had agreed on beforehand, I rolled the stack back to cuVSLAM with IMU fusion and wrote down why. ORB-SLAM3 kept running alongside as an open-source reference, so the final choice of platform rested on a comparison rather than a preference.

I packaged the stack into one-command mapping, localization and odometry modes, made saving the map part of shutdown, and versioned every calibration so the previous deployment is one command away. Intrinsics and camera–IMU extrinsics always ship together, because new intrinsics quietly invalidate the old extrinsics.

I left with a workflow as much as a result. It reproduces the calibration, flags a weak measurement early, and gets from a desk test to a running robot without hiding what is still uncertain. Most of what made the final number possible was staying with the problem through failed targets, timing jitter, image-quality limits and a change of platform.

1. Sharpness here is the variance of the Laplacian, a standard blur measure. Higher is sharper.
2. Squares of 24 mm would have needed 77.06 screen pixels each, so the grid lines would alternate between 77 and 78 pixels and bake a tenth of a pixel of error into the “ground truth.”
3. At runtime the system keeps arrival timestamps, because the hardware clock’s anchor wanders against the computer’s clock over long runs.
4. Fusion first trusted the IMU too much at its datasheet noise values and drifted 12–17 cm a minute. Telling the filter the IMU was seven times noisier stabilized it.
5. Jitter path is the total distance the estimate wanders while the robot sits still, added up step by step. It measures high-frequency noise.

Revision history