yiliao2026/docs/superpowers/plans/2026-07-20-lightweight-kalman-tracker-plan.md

Lightweight Kalman Obstacle Tracker Implementation Plan

For agentic workers: REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (- [ ]) syntax for tracking.

Goal: Replace the laser-frame EMA tracker with a fixed-size constant-velocity Kalman tracker that remains accurate at 2 m/s, suppresses one-frame false detections, and bridges short sparse-scan dropouts.

Architecture: ObstacleTracker owns one [x,y,vx,vy] state and 4x4 covariance per track. Every scan predicts all tracks from the LaserScan timestamp, greedily associates fit observations and then chord observations using Mahalanobis plus Euclidean gates, performs source-specific Kalman updates, and applies separate confirmation, publication, and deletion lifetimes.

Tech Stack: ROS2 Humble, C++17, Eigen fixed-size matrices, ament_cmake_gtest, colcon with --symlink-install.

---

File Map

• Modify src/obstacle_scanner/include/obstacle_scanner/tracking.hpp: Kalman configuration, track state, timestamped API, debug counters.
• Modify src/obstacle_scanner/src/tracking.cpp: prediction, gating, association, correction, lifecycle.
• Modify src/obstacle_scanner/test/test_tracking.cpp: test-first behavioral coverage.
• Modify src/obstacle_scanner/src/obstacle_scanner_node.cpp: timestamp, parameters, debug JSON.
• Modify src/obstacle_scanner/config/params.yaml: parameter migration while preserving the user's radius_min: 0.02.
• Do not change detection behavior, messages, launch files, TF, or navigation packages.

Task 1: Define Timestamped Lifecycle Behavior

Files:

• Modify: src/obstacle_scanner/test/test_tracking.cpp

• Modify: src/obstacle_scanner/include/obstacle_scanner/tracking.hpp

• Modify: src/obstacle_scanner/src/tracking.cpp

• Step 1: Write failing tentative and confirmation tests

Use this exact configuration and timestamped calls:

TrackerConfig test_config()
{
  TrackerConfig config;
  config.track_confirm_fit_hits = 2;
  config.track_confirm_total_hits = 3;
  config.track_publish_misses = 3;
  config.track_delete_misses = 5;
  config.process_accel_noise = 3.0;
  config.initial_velocity_stddev = 2.5;
  config.fit_position_stddev = 0.02;
  config.chord_position_stddev = 0.06;
  config.mahalanobis_gate = 9.21;
  config.max_association_distance = 0.35;
  config.max_position_stddev = 0.15;
  config.chord_tolerance = 0.02;
  config.nominal_dt = 1.0 / 12.0;
  config.min_dt = 0.02;
  config.max_dt = 0.20;
  return config;
}

Assert one fit at t=0 creates one active track but publishes none. Assert a second fit at t=1/12 confirms one published track. Check tracks_created and tracks_confirmed.

• Step 2: Verify RED

Run the Release symlink build. Expected: compilation fails because the new config fields, update(observations, stamp_seconds), and debug counters do not exist.

• Step 3: Add the minimal API and lifecycle

Replace obsolete EMA fields with the fields above. Change the API to:

FrameDebug update(
  const std::vector<Observation> & observations,
  double stamp_seconds);

Define each track with Eigen::Vector4d state, Eigen::Matrix4d covariance, radius, id, fit hits, total hits, misses, confirmation, match flag, and source. Add tracker-level last timestamp initialized to NaN. Implement only creation and two-fit confirmation.

• Step 4: Verify GREEN

Run ./build/obstacle_scanner/test_tracking. Expected: new lifecycle tests pass.

• Step 5: Commit

git add src/obstacle_scanner/include/obstacle_scanner/tracking.hpp   src/obstacle_scanner/src/tracking.cpp   src/obstacle_scanner/test/test_tracking.cpp
git commit -m "refactor: define Kalman track lifecycle"

Task 2: Implement Prediction And Fit Correction

Files:

• Modify: src/obstacle_scanner/test/test_tracking.cpp

• Modify: src/obstacle_scanner/src/tracking.cpp

• Step 1: Write the failing 2 m/s test

Feed fits at x positions 1.0, 1.1666667, and 1.3333333 at 12 Hz, followed by one empty frame. Assert one confirmed predicted track and center_x within 0.08 m of 1.5.

• Step 2: Verify RED

Run ./build/obstacle_scanner/test_tracking --gtest_filter='*TracksTwoMetersPerSecond*'. Expected: failure because velocity is not estimated.

• Step 3: Implement fixed-size prediction

Use:

Eigen::Matrix4d transition = Eigen::Matrix4d::Identity();
transition(0, 2) = dt;
transition(1, 3) = dt;
Eigen::Matrix<double, 4, 2> gain;
gain << 0.5 * dt * dt, 0.0,
        0.0, 0.5 * dt * dt,
        dt, 0.0,
        0.0, dt;
const Eigen::Matrix4d process_noise =
  std::pow(config_.process_accel_noise, 2) * gain * gain.transpose();

Predict state = F * state and P = F * P * F.transpose() + Q. Use nominal_dt for invalid or non-positive deltas and clamp valid deltas to [min_dt,max_dt].

• Step 4: Implement fit correction

Use a 2x4 position matrix, fit_position_stddev^2 * I, an LDLT solve for the 2x2 innovation covariance, and Joseph covariance update:

P = (I - K * H) * P * (I - K * H).transpose() + K * R * K.transpose();

• Step 5: Verify and commit

Run all tracking tests, then commit tracking.cpp and its test with message feat: add constant velocity Kalman tracking.

Task 3: Add Fit And Chord Association

Files:

• Modify: src/obstacle_scanner/test/test_tracking.cpp

• Modify: src/obstacle_scanner/src/tracking.cpp

• Step 1: Write failing association tests

Add independent tests proving:

two fits 0.1667 m apart -> one confirmed track
a fit 0.50 m away -> new tentative track, no duplicate published track
one fit plus two valid chords -> one confirmed track
a chord beyond 0.35 m -> no update and one association rejection

Preserve the existing far-side candidate geometry assertion.

• Step 2: Verify RED

Run the association and chord tests. Expected: Mahalanobis, hard-distance, and fit-plus-two-chords cases fail.

• Step 3: Implement candidate assignment

For each source pass, construct candidates containing observation index, track index, source-specific center, squared Mahalanobis distance, and Euclidean distance. Accept only:

mahalanobis_squared <= config_.mahalanobis_gate &&
euclidean_distance <= config_.max_association_distance

Sort by Mahalanobis score and greedily accept pairs whose observation and track are unused. Process fits before chords. Unmatched fits create tentative tracks; unmatched chords are discarded and counted. Chords use chord_position_stddev and never update radius.

• Step 4: Verify GREEN

Run all tracking tests. Expected: association, geometry, confirmation, and prediction tests pass.

• Step 5: Commit

Commit tracker and test changes with message feat: gate fit and chord associations.

Task 4: Bound Publication And Deletion

Files:

• Modify: src/obstacle_scanner/test/test_tracking.cpp

• Modify: src/obstacle_scanner/src/tracking.cpp

• Step 1: Write failing boundary tests

After two fits confirm a track, assert misses 1-3 publish prediction, miss 4 remains internal but is not published, and miss 5 deletes it. Add tests for uncertainty stopping publication early and tentative deletion on its first miss.

• Step 2: Verify RED

Run ./build/obstacle_scanner/test_tracking --gtest_filter='*Miss*:*Uncertainty*:*Tentative*'. Expected: lifecycle boundaries fail.

• Step 3: Implement separate predicates

Delete tentative tracks at one miss and confirmed tracks at track_delete_misses. Publish only confirmed tracks satisfying:

missed_count <= config_.track_publish_misses &&
std::sqrt(std::max(covariance(0, 0), covariance(1, 1))) <=
  config_.max_position_stddev

Populate created, confirmed, deleted, fit/chord update, rejection, predicted publication, mean innovation, and maximum innovation debug fields.

• Step 4: Verify GREEN

Run both test_tracking and test_detection. Expected: zero failed tests.

• Step 5: Commit

Commit tracker and tests with message feat: bound predicted obstacle lifetime.

Task 5: Integrate ROS Parameters And Diagnostics

Files:

• Modify: src/obstacle_scanner/src/obstacle_scanner_node.cpp

• Modify: src/obstacle_scanner/config/params.yaml

• Step 1: Verify the node API is RED

Run the Release build after the timestamped tracker API exists. Expected: node compilation fails at the old one-argument update call.

• Step 2: Wire timestamps and parameters

Declare every new config field. Convert the scan timestamp using:

const double stamp_seconds = rclcpp::Time(msg->header.stamp).seconds();
tracker_debug = tracker_.update(observations, stamp_seconds);

Remove obsolete track_confirm_hits, association_gate, fit_update_alpha, and chord_update_alpha.

• Step 3: Expand debug JSON

Add exact keys tracks_created, tracks_confirmed, tracks_deleted, fit_updates, chord_updates, association_rejections, predicted_published, mean_innovation, and max_innovation. Preserve existing keys.

• Step 4: Migrate YAML carefully

Preserve radius_min: 0.02. Add every Kalman and lifecycle parameter with units and behavior comments. Keep debug as master and debug_info as text-only sub-switch.

• Step 5: Build and commit

Run the Release symlink build. Expected: one package finishes with exit code zero. Commit only scanner node and YAML with message feat: configure lightweight Kalman tracker.

Task 6: Full Verification And Smoke Test

Files:

• Verify only; do not alter unrelated workspace files.

• Step 1: Fresh build

source /opt/ros/humble/setup.bash
cd /home/sunrise/yiliao_ws
colcon build --symlink-install --packages-select obstacle_scanner   --cmake-args -DCMAKE_BUILD_TYPE=Release

Expected: exit code zero.

• Step 2: Fresh tests

colcon test --packages-select obstacle_scanner --event-handlers console_direct+
colcon test-result --test-result-base build/obstacle_scanner/test_results --verbose

Expected: zero current failures. Check timestamps and run a checker directly if stale XML appears.

• Step 3: Isolated ROS smoke node

On domain 22, run a second node named obstacle_scanner_kalman_smoke, remap /obstacles, /processed_scan, and debug info to smoke-test topics, and set image stride to a very large value. Do not stop or rename the user's existing scanner.

• Step 4: Observe for five seconds

Verify obstacle and debug topics publish near 12 Hz, JSON is valid, processing remains far below the 83 ms scan period, and track counts stay bounded. Stop only the smoke node.

• Step 5: Final scoped review

Run git status --short and inspect only src/obstacle_scanner changes. Confirm radius_min: 0.02 and all unrelated user modifications remain intact. Report build, tests, smoke evidence, and residual tuning risk.