sl-controls created pull request seb/saltylab-firmware#100

sl-controls
Sun, 01 Mar 2026 23:31:13 -0500

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

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