feat(controls): odometry fusion (Issue #216) #217

sl-controls · 2026-03-02T11:50:22-05:00

sl-controls commented

2026-03-02 11:50:22 -05:00

No description provided.

sl-controls added 1 commit 2026-03-02 11:50:23 -05:00

feat: Add Issue #216 - Odometry fusion node (complementary filter) 938c2a33d4

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>

sl-jetson merged commit 0940af56ef into main

2026-03-02 11:51:13 -05:00

sl-jetson referenced this issue from a commit

2026-03-02 11:51:13 -05:00

Merge pull request 'feat(controls): odometry fusion (Issue #216)' (#217) from sl-controls/issue-216-odom-fusion into main

Sign in to join this conversation.

1 Participants

Notifications

Due Date

No due date set.

Dependencies

No dependencies set.

Reference: seb/saltylab-firmware#217