sl-controls
938c2a33d4
feat: Add Issue #216 - Odometry fusion node (complementary filter)
Fuses wheel, visual, and IMU odometry using complementary filtering. Publishes
fused /odom (nav_msgs/Odometry) and broadcasts odom→base_link TF at 50Hz.
Sensor Fusion Strategy:
- Wheel odometry: High-frequency accurate linear displacement (weight: 0.6)
- Visual odometry: Loop closure and long-term drift correction (weight: 0.3)
- IMU: High-frequency attitude and angular velocity (weight: 0.1)
Complementary Filter Architecture:
- Fast loop (IMU): High-frequency attitude updates, angular velocity
- Slow loop (Vision): Low-frequency position/orientation correction
- Integration: Velocity-based position updates with covariance weighting
- Dropout handling: Continues with available sources if sensors drop
Fusion Algorithm:
1. Extract velocities from wheel odometry (most reliable linear)
2. Apply IMU angular velocity (highest frequency rotation)
3. Update orientation from IMU with blending
4. Integrate velocities to position (wheel odometry frame)
5. Apply visual odometry drift correction (low-frequency)
6. Update covariances based on available measurements
7. Publish fused odometry with full covariance matrices
Published Topics:
- /odom (nav_msgs/Odometry) - Fused pose/twist with covariance
- /saltybot/odom_fusion_info (std_msgs/String) - JSON debug info
TF Broadcasts:
- odom→base_link - Position (x, y) and orientation (yaw)
Subscribed Topics:
- /saltybot/wheel_odom (nav_msgs/Odometry) - Wheel encoder odometry
- /rtab_map/odom (nav_msgs/Odometry) - Visual/SLAM odometry
- /imu/data (sensor_msgs/Imu) - IMU data
Package: saltybot_odom_fusion
Entry point: odom_fusion_node
Frequency: 50Hz (20ms cycle)
Features:
✓ Multi-source odometry fusion
✓ Complementary filtering with configurable weights
✓ Full covariance matrices for uncertainty tracking
✓ TF2 transform broadcasting
✓ Sensor dropout handling
✓ JSON telemetry with fusion status
✓ Configurable normalization of weights
Tests: 20+ unit tests covering:
- OdomState initialization and covariances
- Subscription handling for all three sensors
- Position integration from velocity
- Angular velocity updates
- Velocity blending from multiple sources
- Drift correction from visual odometry
- Covariance updates based on measurement availability
- Quaternion to Euler angle conversion
- Realistic fusion scenarios (straight line, circles, drift correction)
- Sensor dropout and recovery
Co-Authored-By: Claude Haiku 4.5 <noreply@anthropic.com>
2026-03-02 11:50:13 -05:00
..
2026-03-02 11:50:13 -05:00
2026-03-02 11:50:13 -05:00
2026-03-02 11:50:13 -05:00
2026-03-02 11:50:13 -05:00
2026-03-02 11:50:13 -05:00
2026-03-02 11:50:13 -05:00
2026-03-02 11:50:13 -05:00
2026-03-02 11:50:13 -05:00