Lidar + Odom Data Fusion
Hybrid localization achieving instant responsiveness with odom-first startup and seamless lidar correction
Context
Wally (wall-mounted system) uses RPLidar C1 to track a guide rail for precise positioning.
Constraints:
- Continuous lidar operation creates audible motor noise—unacceptable for consumer products
- Lidar motor lifespan shortens with continuous operation
- Lidar startup takes ~3 seconds (init + stabilization)
Goal: Achieve instant motion response without sacrificing positioning accuracy from lidar.
Core Problem
Waiting for lidar to become valid before starting motion adds 3-second latency—unacceptable for user experience.
Two issues:
- Startup latency: Lidar needs time to spin up, stabilize, and pass debounce checks
- Reliability tradeoff: Lidar-only approach fails when data is temporarily invalid (occlusion, etc.)
Key insight
Odom provides immediate position estimates (no startup delay), but drifts over time. Lidar provides absolute position but needs startup time.
Solution: Use odom for immediate response, correct drift with lidar when available.
Approach
1) Odom-first startup with background lidar init
When motion command received:
| Condition | Action |
|---|---|
| Y/angle already calibrated | Start immediately with odom |
| First motion (uncalibrated) | Wait for lidar to get initial calibration |
After first calibration, all subsequent motions start with odom—no lidar wait.
2) Seamless source switching
Input source changes based on guide_valid flag:
| Lidar Status | Position Source | Angle Source |
|---|---|---|
| Valid | Lidar direct | Lidar direct |
| Invalid | Odom + stored correction | Odom + stored correction |
Correction storage: When lidar is valid, update map_to_odom offsets (x, y, yaw). When invalid, apply stored corrections to odom.
3) Lidar lifecycle management (LidarFsm)
RPLidar’s provided node doesn’t support lifecycle control—implemented custom state machine:
WaitDevice → Init → Running ⇄ Idle
- Idle state: Motor stopped when not needed—eliminates audible noise
- Debounce: 2x count for initial detection, 1x for subsequent
- Lidar-agnostic: Designed to easily swap lidar hardware via handler callbacks
Tradeoffs & Limitations
- Initial motion with odom: Less accurate until lidar correction arrives (~3 seconds)
- Correction drift: If lidar stays invalid, stored correction becomes stale
Validation
Continuous field testing on deployed Wally units verified:
- Odom-first startup works reliably across power cycles
- Source switching is seamless (no visible position jumps)
- Idle state eliminates motor noise complaints
Results
| Metric | Before | After |
|---|---|---|
| Motion start latency | ~3 sec (wait for lidar) | 0 sec (odom-first) |
| Lidar correction arrival | - | ~3 sec average |
| Motor noise when idle | Continuous | Silent |
- Seamless transition: Source switches (odom ↔ lidar) without position discontinuity
- Lidar lifespan: Extended via Idle state management
Key Takeaway
Odom-first with lidar correction achieves both instant responsiveness and absolute accuracy:
- Start with odom immediately, correct with lidar when available
- Store corrections for fallback during lidar dropout
- LidarFsm manages lidar lifecycle (noise reduction + longevity)