sl-uwb
|
7708c63698
|
feat: UWB-IMU EKF fusion for robust indoor localization (Issue #573)
EKF fusing UWB position (10Hz) with IMU accel+gyro (200Hz) for
SaltyBot indoor localization with UWB dropout resilience.
Package: saltybot_uwb_imu_fusion
- ekf_math.py: 6-state EKF [x,y,θ,vx,vy,ω], IMU predict + UWB update
- IMU as process input: body-frame accel rotated to world via heading
- Jacobian F for nonlinear rotation effect
- Process noise Q from continuous white-noise model
- UWB 2D position update, heading update from quaternion
- Accel bias estimation (low-pass)
- Velocity damping during UWB dropout (>2s threshold)
- ekf_node.py: ROS2 node subscribing to /imu/data (200Hz) + /saltybot/uwb/pose
or /uwb/bearing (10Hz)
- Publishes /saltybot/pose/fused (PoseStamped)
- Publishes /saltybot/pose/fused_cov (PoseWithCovarianceStamped)
- Broadcasts base_link → map TF2 at IMU rate
- Suppresses output after max_dead_reckoning_s without UWB
- 14/14 unit tests passing (predict, update, dropout, PD covariance)
- Launch: ros2 launch saltybot_uwb_imu_fusion uwb_imu_fusion.launch.py
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
|
2026-03-14 11:55:43 -04:00 |
|