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

sl-controls · 2026-03-01T23:31:07-05:00

sl-controls commented

2026-03-01 23:31:07 -05:00

Summary

New package saltybot_social_tracking: 4-state Kalman filter (x, y, vx, vy) fusing /uwb/target (PoseStamped, ~10 Hz) and /person/target (PoseStamped, ~30 Hz) into /social/tracking/fused_target (FusedTarget) at 20 Hz
New package saltybot_social_msgs: FusedTarget.msg — position, velocity, bearing, range, confidence, active_source, tag_id
Source arbiter: 4-state handoff — fused → uwb → camera → predicted — with linear staleness decay
Predictive coasting: KF dead-reckoning for up to predict_timeout (default 3 s) when all sources are lost
EUC-speed tuning: process_noise=3.0 m/s² sized for 20–30 km/h acceleration transients; velocity estimate converges in ~3 s

Files

jetson/ros2_ws/src/
├── saltybot_social_msgs/
│   ├── CMakeLists.txt
│   ├── package.xml
│   └── msg/FusedTarget.msg
└── saltybot_social_tracking/
    ├── saltybot_social_tracking/
    │   ├── kalman_tracker.py       # pure 4-state KF, no ROS2 dep
    │   ├── source_arbiter.py       # confidence scoring + source selection
    │   └── tracking_fusion_node.py # ROS2 node
    ├── config/tracking_params.yaml
    ├── launch/tracking.launch.py
    └── test/test_tracking_fusion.py  # 58/58 pass

Test plan

58 unit tests pass (pytest test/test_tracking_fusion.py) with no ROS2 runtime
KF velocity tracking test: converges to 8 m/s (EUC speed) within 0.6 m/s after 6 s warm-up
Signal loss: KF coasts 1 s without diverging
UWB→camera handoff: position continues progressing
Confidence degrades during coast
Source selection transitions verified

Merge note

Both this PR and PR #98 (Issue #84) add saltybot_social_msgs. Recommend merging PR #98 first then rebasing this branch on top of it — the combined package will have FusedTarget.msg + PersonalityState.msg + QueryMood.srv.

🤖 Generated with Claude Code

## Summary - **New package `saltybot_social_tracking`**: 4-state Kalman filter (x, y, vx, vy) fusing `/uwb/target` (PoseStamped, ~10 Hz) and `/person/target` (PoseStamped, ~30 Hz) into `/social/tracking/fused_target` (FusedTarget) at 20 Hz - **New package `saltybot_social_msgs`**: `FusedTarget.msg` — position, velocity, bearing, range, confidence, active_source, tag_id - **Source arbiter**: 4-state handoff — `fused` → `uwb` → `camera` → `predicted` — with linear staleness decay - **Predictive coasting**: KF dead-reckoning for up to `predict_timeout` (default 3 s) when all sources are lost - **EUC-speed tuning**: `process_noise=3.0 m/s²` sized for 20–30 km/h acceleration transients; velocity estimate converges in ~3 s ## Files ``` jetson/ros2_ws/src/ ├── saltybot_social_msgs/ │ ├── CMakeLists.txt │ ├── package.xml │ └── msg/FusedTarget.msg └── saltybot_social_tracking/ ├── saltybot_social_tracking/ │ ├── kalman_tracker.py # pure 4-state KF, no ROS2 dep │ ├── source_arbiter.py # confidence scoring + source selection │ └── tracking_fusion_node.py # ROS2 node ├── config/tracking_params.yaml ├── launch/tracking.launch.py └── test/test_tracking_fusion.py # 58/58 pass ``` ## Test plan - [x] 58 unit tests pass (`pytest test/test_tracking_fusion.py`) with no ROS2 runtime - [x] KF velocity tracking test: converges to 8 m/s (EUC speed) within 0.6 m/s after 6 s warm-up - [x] Signal loss: KF coasts 1 s without diverging - [x] UWB→camera handoff: position continues progressing - [x] Confidence degrades during coast - [x] Source selection transitions verified ## Merge note Both this PR and PR #98 (Issue #84) add `saltybot_social_msgs`. Recommend merging PR #98 first then rebasing this branch on top of it — the combined package will have `FusedTarget.msg` + `PersonalityState.msg` + `QueryMood.srv`. 🤖 Generated with [Claude Code](https://claude.com/claude-code)

sl-controls added 1 commit 2026-03-01 23:31:11 -05:00

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

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>

sl-webui force-pushed sl-controls/tracking-fusion from 3310a858de to 0821845210

2026-03-01 23:33:40 -05:00

Compare

sl-webui force-pushed sl-controls/tracking-fusion from 0821845210 to 84c8b6a0ae

2026-03-01 23:56:24 -05:00

Compare

sl-webui force-pushed sl-controls/tracking-fusion from 84c8b6a0ae to fa0162fadc

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

Compare

sl-jetson merged commit 79d9a1daa1 into main

2026-03-02 07:00:57 -05:00

sl-jetson referenced this issue from a commit

2026-03-02 07:00:58 -05:00

Merge pull request 'feat(social): multi-modal tracking fusion — UWB+camera Kalman filter (Issue #92)' (#100) from sl-controls/tracking-fusion into main

Sign in to join this conversation.

2 Participants

Notifications

Due Date

No due date set.

Dependencies

No dependencies set.

Reference: seb/saltylab-firmware#100