feat: UWB-IMU EKF fusion for robust indoor localization (Issue #573) #581

sl-jetson · 2026-03-14T11:56:06-04:00

sl-jetson commented

2026-03-14 11:56:06 -04:00

Summary

EKF sensor fusion of UWB position (10 Hz) + IMU accel/gyro (200 Hz) for SaltyBot indoor localization with dropout resilience.

Package: `saltybot_uwb_imu_fusion`

State vector: [x, y, θ, vx, vy, ω] (6-DOF ground robot)

ekf_math.py — pure math, no ROS2 deps:

IMU predict: body accel rotated to world via heading, Jacobian F
Process noise Q: continuous white-noise model (accel + gyro + vel drift)
UWB 2D position update (H = eye rows x,y)
Accel bias low-pass, velocity damping after 2s dropout

ekf_node.py — ROS2:

/imu/data (200 Hz) → predict at full IMU rate
/saltybot/uwb/pose or /uwb/bearing (10 Hz) → EKF update
Publishes /saltybot/pose/fused (PoseStamped) + /saltybot/pose/fused_cov
TF2 base_link → map broadcast
Output suppressed after max_dead_reckoning_s without UWB

14/14 unit tests passing

ros2 launch saltybot_uwb_imu_fusion uwb_imu_fusion.launch.py

Closes #573

## Summary EKF sensor fusion of UWB position (10 Hz) + IMU accel/gyro (200 Hz) for SaltyBot indoor localization with dropout resilience. ### Package: `saltybot_uwb_imu_fusion` **State vector:** `[x, y, θ, vx, vy, ω]` (6-DOF ground robot) **ekf_math.py** — pure math, no ROS2 deps: - IMU predict: body accel rotated to world via heading, Jacobian F - Process noise Q: continuous white-noise model (accel + gyro + vel drift) - UWB 2D position update (H = eye rows x,y) - Accel bias low-pass, velocity damping after 2s dropout **ekf_node.py** — ROS2: - /imu/data (200 Hz) → predict at full IMU rate - /saltybot/uwb/pose or /uwb/bearing (10 Hz) → EKF update - Publishes /saltybot/pose/fused (PoseStamped) + /saltybot/pose/fused_cov - TF2 base_link → map broadcast - Output suppressed after max_dead_reckoning_s without UWB **14/14 unit tests passing** ```bash ros2 launch saltybot_uwb_imu_fusion uwb_imu_fusion.launch.py ``` Closes #573

sl-jetson added 1 commit 2026-03-14 11:56:06 -04:00

feat: UWB-IMU EKF fusion for robust indoor localization (Issue #573 ) 7708c63698

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>

sl-jetson merged commit b09017c949 into main

2026-03-14 12:14:06 -04:00

sl-jetson referenced this issue from a commit

2026-03-14 12:14:07 -04:00

Merge pull request 'feat: UWB-IMU EKF fusion for robust indoor localization (Issue #573)' (#581) from sl-uwb/issue-573-uwb-imu-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#581