In a previous article, I described how GPS can be fused with pure dead reckoning to achieve more accurate outdoor robot pose estimation using an Extended Kalman Filter (EKF). But what if you are navigating indoors, or outdoors with a poor GPS signal? What if you are in a war zone where GPS is jammed? Pure dead-reckoning becomes essentially useless in large outdoor environments once your robot has been running for an extended period.
Enter LiDAR. LiDAR sensors—in particular, 3D ones—are a fantastic source of positioning information. Generally, you can use them in one of two ways. If you have a 3D map generated with LiDAR (or can build one on the fly with SLAM), you can localize your robot by matching the features (corners, planes, and other shapes) sensed by the device with the map. This works great for indoor environments and small, well-structured outdoor scenarios. However, when navigating a large, unstructured outdoor space, creating and maintaining a massive map is often infeasible.
The better option here is to use scan-to-scan matching to produce LiDAR odometry. You can use registration algorithms like ICP (Iterative Closest Point) or NDT (Normal Distributions Transform) to compare the current LiDAR scan against the previous one. This plays beautifully with an EKF framework because the output of the scan matcher looks like a pose that mirrors the exact format of the state the filter is estimating. Because the scan matcher gives you a direct estimate of your pose (or a delta change in pose), the measurement model becomes linear and incredibly simple.
Since my implementation of the EKF is designed mostly for the outdoors, I will focus on the mapless version that produces LiDAR odometry (I hate maps anyway; I think robots should not rely on them). When you use LiDAR to produce a new source of odometry, you have two choices for how the filter fuses the data:
- Absolute Fusion: Treat the LiDAR odometry as an absolute source of positioning data, directly fusing the absolute pose output of the LiDAR algorithm.
- Differential Fusion: Treat the data differentially. Instead of fusing the pose directly, you derive velocities from it (how rapidly the pose is changing over time) and fuse those velocities into the filter.
The video below shows both approaches in action. The ground-truth pose is the black trajectory, while the orange trajectory is the EKF output using differential LiDAR data, and the blue plot is the result of absolute LiDAR fusion.
Notice how the blue line is a bit jerky (it “stutters” or jumps slightly). This happens because raw scan-to-scan matching can suffer from minor alignment noise from frame to frame. Since we are directly fusing it as an absolute position, the filter absorbs that noise directly—much like what happens when you fuse raw GPS fixes without heavy smoothing. The upside, however, is that because the underlying algorithm is so robust (hats off to the KISS-ICP crew!), the blue line stays tightly bound to the true trajectory without drifting away over time.
On the other hand, the orange trajectory uses derived velocities (differential data). As a result, the trajectory is beautifully smooth, though it suffers from typical odometry drift and slowly diverges from the ground truth.
Is it possible to fuse both the absolute and the differential aspects of the LiDAR data simultaneously? Absolutely, and it is something I will be exploring shortly.
Given the current output, which is more suitable for a real robot? For downstream tasks like path planning and trajectory tracking, smooth velocity estimates (like the orange line) are critical to prevent jerky control loops. However, to prevent eventual drift, integrating that absolute bounding box (like the blue line) is necessary. Finding the sweet spot between the two is where the magic happens.
Stay tuned for more updates on state estimation!
Leave a Reply