For an autonomous mobile robot, knowing "where am I right now" is the most basic function there is — it comes before mapping, before obstacle avoidance, before anything else. newbot uses LiDAR-Inertial Odometry (FAST-LIO) from a 3D LiDAR (Livox Mid-360) as its primary source of self-localization, and falls back to sensor fusion (an EKF) when no LiDAR is fitted.

ROS 2ROS 2

Image: Robot Operating System logo, Wikimedia Commons (CC BY-SA 4.0)

Why Keep Odometry Authority to Exactly One Source

The design keeps it so that self-position (/odom) and the TF (coordinate transform) representing the robot's pose are published by exactly one node, depending on the operating mode.

Having multiple nodes independently publish the same information (a dual source) makes it impossible to tell which one to trust the moment their values diverge, leaving mapping and navigation behavior undefined. Eliminating that ambiguity at the design stage is the whole point here. Since the upper-level map→odom transform is always handled by slam_toolbox, the TF tree forms one single chain: map → (slam_toolbox) → odom → (FAST-LIO or EKF) → base_link → (URDF) → each sensor frame.

Choosing FAST-LIO2

There are several algorithms to choose from for 3D LiDAR-Inertial Odometry, but since the Livox Mid-360 carries a 6-axis IMU (angular velocity and acceleration only, no magnetometer), FAST-LIO2 was chosen for its fit with a 6-axis IMU and its lighter dependency footprint. Something like LIO-SAM, which assumes a 9-axis IMU plus GTSAM, was judged a poor fit for this platform.

Getting it running required a patch: the stock implementation doesn't output twist (velocity), only pose, so a patch computes and fills in the velocity component, making it usable as a complete odometry message. FAST-LIO also estimates relative to body_frame (roughly the IMU itself), which is treated here as an approximation of base_link. Because the sensor is mounted directly above the chassis center, horizontal error from that approximation is effectively zero on flat ground.

Measured Drift Accuracy

A loop drive back to the starting point was used to measure drift as the gap between the start and end position.

Metric FAST-LIO (LiDAR) Wheel odometry
Horizontal loop-closure error ~1.0–1.3% of path length ~6.9–27.4% of path length

This accuracy holds even under conditions unfavorable to LiDAR — driving dominated by in-place turns, where slip has more effect. The gap against wheel odometry alone is clear in the numbers. On the other hand, about 0.2 m of drift remains in the vertical (height) axis, an open problem — likely because indoor driving in a small area, dominated by turning in place, offers too few vertical features to constrain the Z axis. Since 2D path planning projects onto the horizontal plane anyway, the practical impact is thought to be small, but a fix using the EKF's 2D-constrained mode to pin Z/roll/pitch to zero downstream is under consideration.

An EKF Misconfiguration Found During a Desk Review

A desk review before deployment on real hardware turned up one bug hiding in the EKF configuration. The LiDAR driver in use (livox_ros_driver2) never sets the orientation (absolute pose) field on IMU messages at all — it only publishes angular velocity and acceleration. But the original ekf.yaml had the yaw component of that orientation set to true as a fusion input. That meant the EKF was treating a value that was never actually there (in practice, zero-filled) as an absolute pose, which left its yaw estimate effectively pinned and broken. The fix was to fuse angular velocity (vyaw) only, along with a review of the gravity-removal flag to match the driving environment. This bug was one that reading the code alone was enough to catch — but it's also the kind that only shows up as a symptom once the hardware is actually running, which made a solid case for the value of desk reviews.

A Design Weakness: No Redundancy

In the current TF tree, only FAST-LIO can publish odom→base_link, so the moment LiDAR loses function, self-position is lost instantly. In real driving tests, LiDAR has dropped out multiple times (up to 5 in a single day) over the course of a single day's run.

Investigating the Root Cause of LiDAR Dropouts

Beyond just recovering with a restart each time it drops, the root cause is also under active investigation. Checking the error counter on the wired Ethernet link used to talk to the LiDAR turned up only 2 errors out of roughly 2.8 million packets — the communication link itself doesn't appear to be the problem. On the other hand, the time to recover from a dropout runs 5–16 seconds, which lines up closely with how long the LiDAR unit itself takes to reboot. Putting those two facts together points toward a brief power interruption at the LiDAR unit itself, rather than the communication path, as the likely cause. What settles it for certain is missing, though: the voltage-divider circuit meant to monitor the power rail isn't wired up yet, so there's no direct way to confirm whether a voltage drop is actually happening. Adding the auto-recovery mechanism let driving continue despite the dropouts, but that only treats the symptom rather than curing the cause — measuring the power system directly remains an open item on the list.

What runs deeper is how recovery actually works. There's already a mechanism to detect a LiDAR dropout and auto-recover (a watchdog force-kills the FAST-LIO process, and launch's respawn feature recreates it), but restarting FAST-LIO always re-initializes self-position from the origin. That makes the odom frame's origin jump, and slam_toolbox — which publishes map→odom — has no way to know about that jump, so its correspondence with the map breaks. In other words, the current auto-recovery only gets as far as "odometry publishing resumes" — Nav2 just keeps operating, trusting a new (in fact wrong) self-position. Driving may appear to continue uninterrupted, but that's not the same as recovering to a safe state.

To address this, one option under consideration is folding scan-matching-based re-localization against the existing map into the recovery sequence (slam_toolbox's localization mode, or a mechanism that restarts FAST-LIO with a given initial pose). There's also a broader direction under consideration: representing self-position confidence not as a binary "correct / broken" but with an intermediate state — "confidence has just degraded right after a LiDAR dropout" — so Nav2 can switch to more conservative behavior: slowing down, refusing new goals, waiting in place.

The Limits of Wheel Encoder Resolution

Since wheel odometry serves as the fallback for whenever LiDAR isn't usable, the quality of the rotary encoders themselves also had to be confronted through direct measurement. The current firmware counts only the rising edge of the A phase (1x multiplication), determining direction from the state of the B phase. Measurement confirmed this approach has distinct weaknesses at both low and high speed.

This error shows up as apparent rotation during straight-line driving (since wheel odometry computes angular velocity from the difference in left/right wheel distance, an over-counting wheel gets misread as turning when the robot is actually going straight). In one measurement, over a 1.2 m straight run, FAST-LIO reported a 6.5° change in heading, while the wheel-odometry calculation implied a full 80° of rotation. The EKF's wheel-odometry fusion setting (odom0) currently pulls in both yaw and vyaw, but that config is exclusive to the no-LiDAR mode and isn't active under the normal FAST-LIO configuration, so no real harm has come of it yet. That said, if this path is ever meant to be used more actively as a LiDAR-dropout fallback, 4x multiplication (CHANGE interrupt on the A phase, XORed with the B phase) — or making use of the ESP32's hardware PCNT peripheral — for better resolution and noise resistance becomes a precondition.

References

#FAST-LIO #ROS2