sl-controls fa0162fadc feat(social): multi-modal tracking fusion — UWB+camera Kalman filter (Issue #92) ...

New packages:
  saltybot_social_msgs   — FusedTarget.msg custom message
  saltybot_social_tracking — 4-state Kalman fusion node

saltybot_social_tracking/tracking_fusion_node.py
  Subscribes to /uwb/target (PoseStamped, ~10 Hz) and /person/target
  (PoseStamped, ~30 Hz) and publishes /social/tracking/fused_target
  (FusedTarget) at 20 Hz.

  Source arbitration:
    • "fused"     — both UWB and camera are fresh; confidence-weighted blend
    • "uwb"       — UWB fresh, camera stale
    • "camera"    — camera fresh, UWB stale
    • "predicted" — all sources stale; KF coasts for up to predict_timeout (3 s)

  Kalman filter (kalman_tracker.py):
    State [x, y, vx, vy] with discrete Wiener acceleration noise model
    (process_noise=3.0 m/s²) sized for EUC speeds (20-30 km/h, ≈5.5-8.3 m/s).
    Separate UWB (0.20 m) and camera (0.12 m) measurement noise.
    Velocity estimate converges after ~3 s of 10 Hz UWB measurements.

  Confidence model (source_arbiter.py):
    Per-source confidence = quality × max(0, 1 - age/timeout).
    Composite confidence accounts for KF positional uncertainty and
    is capped at 0.4 during dead-reckoning ("predicted") mode.

Tests: 58/58 pass (no ROS2 runtime required).

Note: saltybot_social_msgs here adds FusedTarget.msg; PR #98
(Issue #84) adds PersonalityState.msg + QueryMood.srv to the same
package. The maintainer should squash-merge #98 first and rebase
this branch on top of it before merging to avoid the package.xml
conflict.

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

2026-03-01 23:59:10 -05:00

..

src

feat(social): multi-modal tracking fusion — UWB+camera Kalman filter (Issue #92)

2026-03-01 23:59:10 -05:00