sl-controls
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>