feat: Multi-sensor pose fusion (Issue #595) #606

sl-perception · 2026-03-14T15:01:24-04:00

sl-perception commented

2026-03-14 15:01:24 -04:00

Summary

New package saltybot_pose_fusion — EKF fusing three sensor streams into a single authoritative PoseWithCovarianceStamped on /saltybot/pose/authoritative with TF2 map→base_link.

Sensor fusion architecture

Sensor	Topic	Rate	Role
Raw IMU	`/imu/data`	200 Hz	EKF predict step
UWB+IMU fused pose	`/saltybot/pose/fused_cov`	10 Hz	Absolute position+heading update
Visual odometry	`/saltybot/visual_odom`	30 Hz	Velocity-domain update (drift-free)

`pose_fusion_ekf.py` (pure Python, no ROS2)

State [x, y, θ, vx, vy, ω], 6-state EKF
predict_imu: body-frame accel+gyro, Jacobian F, process noise Q, running accel-bias estimator
update_uwb_position / update_uwb_heading: linear H, σ from message covariance or parameter override
update_vo_velocity: body-frame vx rotated to world via cos/sin(θ), updates [vx, vy, ω] state
Joseph-form covariance update for numerical stability
Dual dropout clocks (uwb_dropout_s, vo_dropout_s); velocity damping after 2s UWB dropout
Configurable sensor weights: sigma_uwb_pos_m, sigma_uwb_head_rad, sigma_vo_vel_m_s, sigma_vo_omega_r_s, sigma_imu_accel/gyro

`pose_fusion_node.py`

Output suppressed when uwb_dropout_s > uwb_dropout_max_s (10s default)
Throttled warnings on UWB / VO dropout
Status JSON on /saltybot/pose/status at 10 Hz (x, y, heading_deg, pos_uncertainty_m, dropout durations, healthy flag)
σ auto-extracted from incoming PoseWithCovarianceStamped.covariance (configurable)

Tests

13 unit tests in test/test_pose_fusion_ekf.py covering init, predict, UWB/VO updates, dropout reset, covariance shape/convergence, and sigma override correctness.

Closes #595

## Summary New package `saltybot_pose_fusion` — EKF fusing three sensor streams into a single authoritative `PoseWithCovarianceStamped` on `/saltybot/pose/authoritative` with TF2 `map→base_link`. ### Sensor fusion architecture | Sensor | Topic | Rate | Role | |--------|-------|------|------| | Raw IMU | `/imu/data` | 200 Hz | EKF predict step | | UWB+IMU fused pose | `/saltybot/pose/fused_cov` | 10 Hz | Absolute position+heading update | | Visual odometry | `/saltybot/visual_odom` | 30 Hz | Velocity-domain update (drift-free) | ### `pose_fusion_ekf.py` (pure Python, no ROS2) - State **[x, y, θ, vx, vy, ω]**, 6-state EKF - `predict_imu`: body-frame accel+gyro, Jacobian F, process noise Q, running accel-bias estimator - `update_uwb_position / update_uwb_heading`: linear H, σ from message covariance or parameter override - `update_vo_velocity`: body-frame vx rotated to world via cos/sin(θ), updates [vx, vy, ω] state - **Joseph-form** covariance update for numerical stability - Dual dropout clocks (uwb_dropout_s, vo_dropout_s); velocity damping after 2s UWB dropout - Configurable sensor weights: sigma_uwb_pos_m, sigma_uwb_head_rad, sigma_vo_vel_m_s, sigma_vo_omega_r_s, sigma_imu_accel/gyro ### `pose_fusion_node.py` - Output suppressed when uwb_dropout_s > uwb_dropout_max_s (10s default) - Throttled warnings on UWB / VO dropout - Status JSON on `/saltybot/pose/status` at 10 Hz (x, y, heading_deg, pos_uncertainty_m, dropout durations, healthy flag) - σ auto-extracted from incoming `PoseWithCovarianceStamped.covariance` (configurable) ### Tests 13 unit tests in `test/test_pose_fusion_ekf.py` covering init, predict, UWB/VO updates, dropout reset, covariance shape/convergence, and sigma override correctness. Closes #595

sl-perception added 1 commit 2026-03-14 15:01:25 -04:00

feat: Multi-sensor pose fusion node (Issue #595 ) c76d5b0dd7

New package saltybot_pose_fusion — EKF fusing UWB+IMU absolute pose,
visual odometry velocity, and raw IMU into a single authoritative pose.

pose_fusion_ekf.py (pure Python, no ROS2 deps):
  PoseFusionEKF — state [x, y, θ, vx, vy, ω], 6-state EKF.
  - predict_imu(ax_body, ay_body, omega, dt): body-frame IMU predict step
    with Jacobian F, bias-compensated accel, process noise Q.
  - update_uwb_position(x, y, sigma_m): absolute position measurement
    (H=[1,0,0,0,0,0; 0,1,0,0,0,0]) from UWB+IMU fused stream.
  - update_uwb_heading(heading_rad, sigma_rad): heading measurement.
  - update_vo_velocity(vx_body, omega, ...): VO velocity measurement —
    body-frame vx rotated to world via cos/sin(θ), updates [vx,vy,ω] state.
  - Joseph-form covariance update for numerical stability.
  - Dual dropout clocks: uwb_dropout_s, vo_dropout_s (reset on each update).
  - Velocity damping when uwb_dropout_s > 2s.
  - Sensor weight parameters: sigma_uwb_pos_m, sigma_uwb_head_rad,
    sigma_vo_vel_m_s, sigma_vo_omega_r_s, sigma_imu_accel/gyro,
    sigma_vel_drift, dropout_vel_damp.

pose_fusion_node.py (ROS2 node 'pose_fusion'):
  - Subscribes: /imu/data (Imu, 200Hz → predict), /saltybot/pose/fused_cov
    (PoseWithCovarianceStamped, 10Hz → position+heading update, σ extracted
    from message covariance when use_uwb_covariance=true), /saltybot/visual_odom
    (Odometry, 30Hz → velocity update, σ from twist covariance).
  - Publishes: /saltybot/pose/authoritative (PoseWithCovarianceStamped),
    /saltybot/pose/status (String JSON, 10Hz).
  - TF2: map→base_link broadcast at IMU rate.
  - Suppresses output when uwb_dropout_s > uwb_dropout_max_s (10s).
  - Warns (throttled) on UWB/VO dropout.

config/pose_fusion_params.yaml: sensor weights + dropout thresholds.
launch/pose_fusion.launch.py: single node launch with params_file arg.
test/test_pose_fusion_ekf.py: 13 unit tests — init, predict, UWB/VO
  updates, dropout reset, covariance shape/convergence, sigma override.

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

sl-jetson merged commit 2367e08140 into main

2026-03-14 15:54:48 -04:00

sl-jetson referenced this issue from a commit

2026-03-14 15:54:49 -04:00

Merge pull request 'feat: Multi-sensor pose fusion (Issue #595)' (#606) from sl-perception/issue-595-pose-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#606