障碍物检测v3，改成了卡尔曼滤波完整版

src/obstacle_scanner/config/params.yaml

@@ -1,70 +1,81 @@
 obstacle_scanner:
   ros__parameters:
-    # Input LaserScan topic.
+    # 输入的 LaserScan 话题。
     scan_topic: "/scan"
 
-    # Frame id used on published /obstacles and debug images.
+    # 发布 /obstacles 和调试图像时使用的坐标系 ID。
-    frame_id: "laser_frame"
+    frame_id: "laser_link"
 
-    # Maximum Euclidean distance in meters between adjacent scan points before
+    # 相邻扫描点归入同一聚类时允许的最大欧氏距离，单位为米。
-    # starting a new cluster. The detector now uses scan order, so this is O(N).
+    # 检测器按扫描顺序处理，因此时间复杂度为 O(N)。
     cluster_gap: 0.1
 
-    # Drop clusters with more points than this before circle fitting. This keeps
+    # 圆拟合前丢弃点数超过此值的聚类。
-    # walls and large surfaces from becoming circular obstacles.
     max_cluster_points: 20
 
-    # Minimum accepted fitted radius in meters for 3+ point circle observations.
+    # 接受三点及以上观测的最小拟合半径，单位为米。
     radius_min: 0.03
 
-    # Maximum accepted fitted radius in meters for 3+ point circle observations.
+    # 接受三点及以上观测的最大拟合半径，单位为米。
-    radius_max: 0.05
+    radius_max: 0.06
 
-    # Merge the last and first scan-order clusters when their endpoint distance
+    # 当扫描序列首尾两个聚类相邻时，将它们合并。
-    # is below cluster_gap. Useful for near-360 degree scans; harmless otherwise
-    # unless scan edge points are physically close.
     merge_wrap: true
 
-    # Enable short-term laser-frame tracking. When false, only current-frame
+    # 为 true 时发布跟踪后的障碍物；为 false 时仅发布当前帧拟合结果。
-    # circle-fit observations are published.
     enable_tracking: true
 
-    # Number of successful updates before a track may be published.
+    # 确认一条轨迹所需的连续圆拟合命中次数。
-    track_confirm_hits: 1
+    track_confirm_fit_hits: 2
 
-    # Delete a track after this many consecutive missed frames.
+    # 确认一条轨迹所需的圆拟合与弦线观测总命中次数。
-    track_delete_misses: 2
+    track_confirm_total_hits: 3
 
-    # Maximum distance in meters for nearest-neighbor observation-to-track match.
+    # 已确认轨迹仍可发布预测结果的最大连续丢失帧数。
-    association_gate: 0.25
+    track_publish_misses: 4
 
-    # Extra tolerance in meters for accepting a two-point chord whose length is
+    # 已确认轨迹连续丢失达到此帧数后删除。
-    # close to the predicted track diameter.
+    track_delete_misses: 6
+
+    # 加速度过程噪声的 1σ 标准差，单位为米每二次方秒。
+    process_accel_noise: 3.0
+
+    # 初始速度不确定度的 1σ 标准差，单位为米每秒。
+    initial_velocity_stddev: 2.5
+
+    # 三点及以上圆拟合的位置噪声 1σ 标准差，单位为米。
+    fit_position_stddev: 0.02
+
+    # 两点弦线恢复的位置噪声 1σ 标准差，单位为米。
+    chord_position_stddev: 0.06
+
+    # 数据关联允许的最大马氏距离平方。
+    mahalanobis_gate: 9.21
+
+    # 数据关联附加的欧氏距离硬限制，单位为米。
+    max_association_distance: 0.35
+
+    # 位置不确定度超过此值时停止发布预测结果，单位为米。
+    max_position_stddev: 0.15
+
+    # 弦长接近预测直径时允许的额外容差，单位为米。
     chord_tolerance: 0.02
 
-    # Weight for center updates from 3+ point circle fitting. Higher values trust
+    # 扫描时间戳缺失或无效时使用的默认时间间隔，单位为秒。
-    # the current fit more.
+    nominal_dt: 0.0833333333
-    fit_update_alpha: 0.7
 
-    # Weight for center updates from 2-point chord recovery. Lower values keep
+    # 有效时间戳间隔的下限和上限，单位为秒。
-    # chord updates from pulling the track too aggressively.
+    min_dt: 0.02
-    chord_update_alpha: 0.3
+    max_dt: 0.20
 
-    # Enable debug publishers. This creates /processed_scan and, when debug_info
+    # 启用调试发布器；debug_info 是仅控制文本诊断的子开关。
-    # is true, /obstacle_scanner/debug_info.
+    debug: false
-    debug: true
-
-    # Publish compact JSON debug information on /obstacle_scanner/debug_info.
     debug_info: true
 
-    # Debug image width and height in pixels.
+    # 调试图像尺寸，以及每像素表示的米数。
     debug_image_size: 500
-
-    # Meters per pixel for the debug image.
     debug_resolution: 0.01
 
-    # Publish one debug image every N scan frames to reduce OpenCV drawing cost.
+    # 每处理 N 帧扫描发布一次调试图像或 JSON 诊断信息。
     debug_image_stride: 3
-
-    # Publish one JSON debug message every N scan frames.
     debug_info_stride: 1

src/obstacle_scanner/include/obstacle_scanner/tracking.hpp

@@ -2,7 +2,7 @@
 
 #include <Eigen/Dense>
 
-#include <cstddef>
+#include <limits>
 #include <string>
 #include <vector>
 
@@ -34,12 +34,21 @@ struct Observation
 
 struct TrackerConfig
 {
-  int track_confirm_hits{1};
+  int track_confirm_fit_hits{2};
-  int track_delete_misses{2};
+  int track_confirm_total_hits{3};
-  double association_gate{0.25};
+  int track_publish_misses{3};
+  int track_delete_misses{5};
+  double process_accel_noise{3.0};
+  double initial_velocity_stddev{2.5};
+  double fit_position_stddev{0.02};
+  double chord_position_stddev{0.06};
+  double mahalanobis_gate{9.21};
+  double max_association_distance{0.35};
+  double max_position_stddev{0.15};
   double chord_tolerance{0.02};
-  double fit_update_alpha{0.7};
+  double nominal_dt{1.0 / 12.0};
-  double chord_update_alpha{0.3};
+  double min_dt{0.02};
+  double max_dt{0.20};
 };
 
 struct TrackOutput
@@ -59,6 +68,14 @@ struct FrameDebug
   int chord_observations{0};
   int chord_updates{0};
   int chord_rejections{0};
+  int tracks_created{0};
+  int tracks_confirmed{0};
+  int tracks_deleted{0};
+  int fit_updates{0};
+  int association_rejections{0};
+  int predicted_published{0};
+  double mean_innovation{0.0};
+  double max_innovation{0.0};
   int active_tracks{0};
   int published_tracks{0};
 };
@@ -68,32 +85,47 @@ class ObstacleTracker
 public:
   explicit ObstacleTracker(const TrackerConfig & config = TrackerConfig());
 
-  FrameDebug update(const std::vector<Observation> & observations);
+  FrameDebug update(
+    const std::vector<Observation> & observations,
+    double stamp_seconds);
   std::vector<TrackOutput> confirmed_tracks() const;
   const std::vector<TrackOutput> & tracks_for_debug() const;
 
 private:
   struct Track
   {
-    int id{0};
+    Eigen::Vector4d state{Eigen::Vector4d::Zero()};
-    Eigen::Vector2d center{0.0, 0.0};
+    Eigen::Matrix4d covariance{Eigen::Matrix4d::Zero()};
     double radius{0.0};
-    int hit_count{0};
+    int id{0};
+    int fit_hits{0};
+    int total_hits{0};
     int missed_count{0};
-    UpdateSource last_source{UpdateSource::Predict};
+    bool confirmed{false};
     bool matched{false};
+    UpdateSource last_source{UpdateSource::Predict};
   };
 
-  int find_nearest_unmatched_track(const Eigen::Vector2d & center) const;
+  void predict_tracks(double dt);
+  void update_track(
+    Track & track,
+    const Eigen::Vector2d & center,
+    double position_stddev,
+    UpdateSource source,
+    FrameDebug & debug,
+    double & innovation_sum,
+    int & innovation_count);
   bool chord_center_for_track(
     const Observation & observation,
     const Track & track,
     Eigen::Vector2d & center) const;
+  bool is_publishable(const Track & track) const;
   static TrackOutput to_output(const Track & track);
   void refresh_debug_outputs();
 
   TrackerConfig config_;
   int next_id_{1};
+  double last_stamp_seconds_{std::numeric_limits<double>::quiet_NaN()};
   std::vector<Track> tracks_;
   std::vector<TrackOutput> debug_outputs_;
 };

src/obstacle_scanner/src/obstacle_scanner_node.cpp

@@ -31,17 +31,26 @@ public:
     frame_id_ = declare_parameter<std::string>("frame_id", "laser_frame");
     detection_config_.cluster_gap = declare_parameter("cluster_gap", 0.1);
     detection_config_.max_cluster_points = declare_parameter("max_cluster_points", 20);
-    detection_config_.radius_min = declare_parameter("radius_min", 0.03);
+    detection_config_.radius_min = declare_parameter("radius_min", 0.02);
     detection_config_.radius_max = declare_parameter("radius_max", 0.05);
     detection_config_.merge_wrap = declare_parameter("merge_wrap", true);
 
     enable_tracking_ = declare_parameter("enable_tracking", true);
-    tracker_config_.track_confirm_hits = declare_parameter("track_confirm_hits", 1);
+    tracker_config_.track_confirm_fit_hits = declare_parameter("track_confirm_fit_hits", 2);
-    tracker_config_.track_delete_misses = declare_parameter("track_delete_misses", 2);
+    tracker_config_.track_confirm_total_hits = declare_parameter("track_confirm_total_hits", 3);
-    tracker_config_.association_gate = declare_parameter("association_gate", 0.25);
+    tracker_config_.track_publish_misses = declare_parameter("track_publish_misses", 3);
+    tracker_config_.track_delete_misses = declare_parameter("track_delete_misses", 5);
+    tracker_config_.process_accel_noise = declare_parameter("process_accel_noise", 3.0);
+    tracker_config_.initial_velocity_stddev = declare_parameter("initial_velocity_stddev", 2.5);
+    tracker_config_.fit_position_stddev = declare_parameter("fit_position_stddev", 0.02);
+    tracker_config_.chord_position_stddev = declare_parameter("chord_position_stddev", 0.06);
+    tracker_config_.mahalanobis_gate = declare_parameter("mahalanobis_gate", 9.21);
+    tracker_config_.max_association_distance = declare_parameter("max_association_distance", 0.35);
+    tracker_config_.max_position_stddev = declare_parameter("max_position_stddev", 0.15);
     tracker_config_.chord_tolerance = declare_parameter("chord_tolerance", 0.02);
-    tracker_config_.fit_update_alpha = declare_parameter("fit_update_alpha", 0.7);
+    tracker_config_.nominal_dt = declare_parameter("nominal_dt", 1.0 / 12.0);
-    tracker_config_.chord_update_alpha = declare_parameter("chord_update_alpha", 0.3);
+    tracker_config_.min_dt = declare_parameter("min_dt", 0.02);
+    tracker_config_.max_dt = declare_parameter("max_dt", 0.20);
     tracker_ = obstacle_scanner::ObstacleTracker(tracker_config_);
 
     debug_ = declare_parameter("debug", false);
@@ -98,7 +107,8 @@ private:
     std::vector<obstacle_scanner::TrackOutput> output_tracks;
     std::vector<obstacle_scanner::TrackOutput> debug_tracks;
     if (enable_tracking_) {
-      tracker_debug = tracker_.update(observations);
+      const double stamp_seconds = rclcpp::Time(msg->header.stamp).seconds();
+      tracker_debug = tracker_.update(observations, stamp_seconds);
       output_tracks = tracker_.confirmed_tracks();
       debug_tracks = tracker_.tracks_for_debug();
     } else {
@@ -275,6 +285,14 @@ private:
     out << "\"chord\":" << detection_debug.chord_observations << ",";
     out << "\"chord_updates\":" << tracker_debug.chord_updates << ",";
     out << "\"chord_rejections\":" << tracker_debug.chord_rejections << ",";
+    out << "\"tracks_created\":" << tracker_debug.tracks_created << ",";
+    out << "\"tracks_confirmed\":" << tracker_debug.tracks_confirmed << ",";
+    out << "\"tracks_deleted\":" << tracker_debug.tracks_deleted << ",";
+    out << "\"fit_updates\":" << tracker_debug.fit_updates << ",";
+    out << "\"association_rejections\":" << tracker_debug.association_rejections << ",";
+    out << "\"predicted_published\":" << tracker_debug.predicted_published << ",";
+    out << "\"mean_innovation\":" << tracker_debug.mean_innovation << ",";
+    out << "\"max_innovation\":" << tracker_debug.max_innovation << ",";
     out << "\"tracks\":" << tracker_debug.active_tracks << ",";
     out << "\"published\":" << published_count << ",";
     out << "\"ms\":" << processing_ms << ",";

src/obstacle_scanner/src/obstacle_scanner_node.cpp.orig

@@ -0,0 +1,321 @@
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/image.hpp>
+#include <sensor_msgs/msg/laser_scan.hpp>
+#include <std_msgs/msg/string.hpp>
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/opencv.hpp>
+#include <Eigen/Dense>
+
+#include "obstacle_scanner/detection.hpp"
+#include "obstacle_scanner/msg/obstacle.hpp"
+#include "obstacle_scanner/msg/obstacle_array.hpp"
+#include "obstacle_scanner/tracking.hpp"
+
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <cstdio>
+#include <sstream>
+#include <string>
+#include <vector>
+
+class ObstacleScannerNode : public rclcpp::Node
+{
+public:
+  ObstacleScannerNode()
+  : rclcpp::Node("obstacle_scanner")
+  {
+    using std::placeholders::_1;
+
+    scan_topic_ = declare_parameter<std::string>("scan_topic", "/scan");
+    frame_id_ = declare_parameter<std::string>("frame_id", "laser_frame");
+    detection_config_.cluster_gap = declare_parameter("cluster_gap", 0.1);
+    detection_config_.max_cluster_points = declare_parameter("max_cluster_points", 20);
+    detection_config_.radius_min = declare_parameter("radius_min", 0.03);
+    detection_config_.radius_max = declare_parameter("radius_max", 0.05);
+    detection_config_.merge_wrap = declare_parameter("merge_wrap", true);
+
+    enable_tracking_ = declare_parameter("enable_tracking", true);
+    tracker_config_.track_confirm_fit_hits = declare_parameter(" track_confirm_fit_hits, 2);
+ tracker_config_.track_confirm_total_hits = declare_parameter(	rack_confirm_total_hits, 3);
+ tracker_config_.track_publish_misses = declare_parameter(	rack_publish_misses, 3);
+    tracker_config_.track_delete_misses = declare_parameter("track_delete_misses", 2);
+    tracker_config_.chord_tolerance = declare_parameter("chord_tolerance", 0.02);
+    tracker_config_.nominal_dt = declare_parameter(" nominal_dt, 1.0 / 12.0);
+ tracker_config_.min_dt = declare_parameter(min_dt, 0.02);
+ tracker_config_.max_dt = declare_parameter(max_dt, 0.20);
+    tracker_ = obstacle_scanner::ObstacleTracker(tracker_config_);
+
+    debug_ = declare_parameter("debug", false);
+    debug_info_ = declare_parameter("debug_info", true);
+    debug_image_size_ = declare_parameter("debug_image_size", 500);
+    debug_resolution_ = declare_parameter("debug_resolution", 0.01);
+    debug_image_stride_ = std::max(
+      1, static_cast<int>(declare_parameter<int>("debug_image_stride", 3)));
+    debug_info_stride_ = std::max(
+      1, static_cast<int>(declare_parameter<int>("debug_info_stride", 1)));
+
+    obstacles_pub_ = create_publisher<obstacle_scanner::msg::ObstacleArray>(
+      "/obstacles", 10);
+
+    if (debug_) {
+      debug_pub_ = create_publisher<sensor_msgs::msg::Image>(
+        "/processed_scan", 10);
+      if (debug_info_) {
+        debug_info_pub_ = create_publisher<std_msgs::msg::String>(
+          "/obstacle_scanner/debug_info", 10);
+      }
+    }
+
+    scan_sub_ = create_subscription<sensor_msgs::msg::LaserScan>(
+      scan_topic_, rclcpp::SensorDataQoS(),
+      std::bind(&ObstacleScannerNode::scan_callback, this, _1));
+
+    RCLCPP_INFO(get_logger(), "ObstacleScannerNode started");
+  }
+
+private:
+  void scan_callback(const sensor_msgs::msg::LaserScan::ConstSharedPtr & msg)
+  {
+    const auto start_time = std::chrono::steady_clock::now();
+    frame_count_++;
+
+    std::vector<Eigen::Vector2d> points;
+    points.reserve(msg->ranges.size());
+
+    for (std::size_t i = 0; i < msg->ranges.size(); ++i) {
+      const double range = msg->ranges[i];
+      if (!std::isfinite(range) || range <= 0.0) {
+        continue;
+      }
+      const double angle = msg->angle_min + static_cast<double>(i) * msg->angle_increment;
+      points.emplace_back(range * std::cos(angle), range * std::sin(angle));
+    }
+
+    obstacle_scanner::DetectionDebug detection_debug;
+    const auto observations = obstacle_scanner::detect_observations(
+      points, detection_config_, detection_debug);
+
+    obstacle_scanner::FrameDebug tracker_debug;
+    std::vector<obstacle_scanner::TrackOutput> output_tracks;
+    std::vector<obstacle_scanner::TrackOutput> debug_tracks;
+    if (enable_tracking_) {
+      const double stamp_seconds = rclcpp::Time(msg->header.stamp).seconds();
+      tracker_debug = tracker_.update(observations, stamp_seconds);
+      output_tracks = tracker_.confirmed_tracks();
+      debug_tracks = tracker_.tracks_for_debug();
+    } else {
+      output_tracks = tracks_from_fit_observations(observations);
+      debug_tracks = output_tracks;
+      tracker_debug.published_tracks = static_cast<int>(output_tracks.size());
+      tracker_debug.active_tracks = tracker_debug.published_tracks;
+    }
+
+    const double processing_ms = elapsed_ms(start_time);
+
+    RCLCPP_DEBUG(
+      get_logger(),
+      "points=%zu clusters=%d fit=%d chord=%d tracks=%d published=%zu ms=%.3f",
+      points.size(), detection_debug.total_clusters,
+      detection_debug.fit_observations, detection_debug.chord_observations,
+      tracker_debug.active_tracks, output_tracks.size(), processing_ms);
+
+    auto out_msg = obstacle_scanner::msg::ObstacleArray();
+    out_msg.header.stamp = msg->header.stamp;
+    out_msg.header.frame_id = frame_id_;
+    for (const auto & track : output_tracks) {
+      obstacle_scanner::msg::Obstacle obstacle;
+      obstacle.center_x = track.center_x;
+      obstacle.center_y = track.center_y;
+      obstacle.radius = track.radius;
+      out_msg.obstacles.push_back(obstacle);
+    }
+    obstacles_pub_->publish(out_msg);
+
+    if (debug_) {
+      if (debug_pub_ && frame_count_ % static_cast<std::uint64_t>(debug_image_stride_) == 0) {
+        auto img_msg = render_debug_image(
+          points, debug_tracks, detection_debug, tracker_debug,
+          processing_ms, msg->header.stamp);
+        debug_pub_->publish(*img_msg);
+      }
+      if (
+        debug_info_pub_ &&
+        frame_count_ % static_cast<std::uint64_t>(debug_info_stride_) == 0)
+      {
+        std_msgs::msg::String debug_msg;
+        debug_msg.data = build_debug_json(
+          points.size(), detection_debug, tracker_debug,
+          debug_tracks, output_tracks.size(), processing_ms);
+        debug_info_pub_->publish(debug_msg);
+      }
+    }
+  }
+
+  static double elapsed_ms(const std::chrono::steady_clock::time_point & start_time)
+  {
+    const auto end_time = std::chrono::steady_clock::now();
+    return std::chrono::duration<double, std::milli>(end_time - start_time).count();
+  }
+
+  static std::vector<obstacle_scanner::TrackOutput> tracks_from_fit_observations(
+    const std::vector<obstacle_scanner::Observation> & observations)
+  {
+    std::vector<obstacle_scanner::TrackOutput> tracks;
+    int id = 1;
+    for (const auto & observation : observations) {
+      if (observation.type != obstacle_scanner::ObservationType::CircleFit) {
+        continue;
+      }
+      obstacle_scanner::TrackOutput track;
+      track.id = id++;
+      track.center_x = observation.center.x();
+      track.center_y = observation.center.y();
+      track.radius = observation.radius;
+      track.hit_count = 1;
+      track.missed_count = 0;
+      track.last_source = obstacle_scanner::UpdateSource::Fit;
+      tracks.push_back(track);
+    }
+    return tracks;
+  }
+
+  sensor_msgs::msg::Image::SharedPtr render_debug_image(
+    const std::vector<Eigen::Vector2d> & points,
+    const std::vector<obstacle_scanner::TrackOutput> & tracks,
+    const obstacle_scanner::DetectionDebug & detection_debug,
+    const obstacle_scanner::FrameDebug & tracker_debug,
+    double processing_ms,
+    const builtin_interfaces::msg::Time & stamp)
+  {
+    const int size = debug_image_size_;
+    const double res = debug_resolution_;
+    const int origin = size / 2;
+
+    cv::Mat img(size, size, CV_8UC3, cv::Scalar(255, 255, 255));
+
+    for (const auto & pt : points) {
+      const int col = static_cast<int>(std::round(origin + pt.x() / res));
+      const int row = static_cast<int>(std::round(origin - pt.y() / res));
+      if (col >= 0 && col < size && row >= 0 && row < size) {
+        cv::circle(img, cv::Point(col, row), 1, cv::Scalar(0, 0, 255), cv::FILLED);
+      }
+    }
+
+    cv::rectangle(
+      img, cv::Point(origin - 2, origin - 2),
+      cv::Point(origin + 2, origin + 2),
+      cv::Scalar(0, 255, 255), cv::FILLED);
+
+    for (const auto & track : tracks) {
+      draw_track(img, track, origin, res);
+    }
+
+    char summary[160];
+    std::snprintf(
+      summary, sizeof(summary),
+      "pts=%zu cls=%d fit=%d chord=%d trk=%d pub=%d ms=%.1f",
+      points.size(), detection_debug.total_clusters,
+      detection_debug.fit_observations, detection_debug.chord_observations,
+      tracker_debug.active_tracks, tracker_debug.published_tracks, processing_ms);
+    cv::putText(
+      img, summary, cv::Point(8, size - 12),
+      cv::FONT_HERSHEY_SIMPLEX, 0.32, cv::Scalar(40, 40, 40), 1);
+
+    auto image_msg = cv_bridge::CvImage(
+      std_msgs::msg::Header(), "bgr8", img).toImageMsg();
+    image_msg->header.stamp = stamp;
+    image_msg->header.frame_id = frame_id_;
+    return image_msg;
+  }
+
+  static void draw_track(
+    cv::Mat & img,
+    const obstacle_scanner::TrackOutput & track,
+    int origin,
+    double res)
+  {
+    const int col = static_cast<int>(std::round(origin + track.center_x / res));
+    const int row = static_cast<int>(std::round(origin - track.center_y / res));
+    const int radius_px = std::max(1, static_cast<int>(std::round(track.radius / res)));
+
+    cv::Scalar color(160, 160, 160);
+    int line_type = cv::LINE_8;
+    if (track.last_source == obstacle_scanner::UpdateSource::Fit) {
+      color = cv::Scalar(0, 180, 0);
+    } else if (track.last_source == obstacle_scanner::UpdateSource::Chord) {
+      color = cv::Scalar(255, 0, 0);
+    } else {
+      line_type = cv::LINE_4;
+    }
+
+    cv::circle(img, cv::Point(col, row), radius_px, color, 1, line_type);
+    cv::circle(img, cv::Point(col, row), 2, color, cv::FILLED);
+
+    char label[48];
+    std::snprintf(
+      label, sizeof(label), "#%d %s h%d/m%d",
+      track.id, obstacle_scanner::update_source_name(track.last_source).c_str(),
+      track.hit_count, track.missed_count);
+    cv::putText(
+      img, label, cv::Point(col + radius_px + 2, row),
+      cv::FONT_HERSHEY_SIMPLEX, 0.3, color, 1);
+  }
+
+  static std::string build_debug_json(
+    std::size_t point_count,
+    const obstacle_scanner::DetectionDebug & detection_debug,
+    const obstacle_scanner::FrameDebug & tracker_debug,
+    const std::vector<obstacle_scanner::TrackOutput> & tracks,
+    std::size_t published_count,
+    double processing_ms)
+  {
+    std::ostringstream out;
+    out << "{";
+    out << "\"points\":" << point_count << ",";
+    out << "\"clusters\":" << detection_debug.total_clusters << ",";
+    out << "\"fit\":" << detection_debug.fit_observations << ",";
+    out << "\"chord\":" << detection_debug.chord_observations << ",";
+    out << "\"chord_updates\":" << tracker_debug.chord_updates << ",";
+    out << "\"chord_rejections\":" << tracker_debug.chord_rejections << ",";
+    out << "\"tracks\":" << tracker_debug.active_tracks << ",";
+    out << "\"published\":" << published_count << ",";
+    out << "\"ms\":" << processing_ms << ",";
+    out << "\"sources\":[";
+    for (std::size_t i = 0; i < tracks.size(); ++i) {
+      if (i > 0) {
+        out << ",";
+      }
+      out << "\"" << obstacle_scanner::update_source_name(tracks[i].last_source) << "\"";
+    }
+    out << "]}";
+    return out.str();
+  }
+
+  std::string scan_topic_;
+  std::string frame_id_;
+  obstacle_scanner::DetectionConfig detection_config_;
+  obstacle_scanner::TrackerConfig tracker_config_;
+  obstacle_scanner::ObstacleTracker tracker_;
+  bool enable_tracking_{true};
+  bool debug_{false};
+  bool debug_info_{true};
+  int debug_image_size_{500};
+  double debug_resolution_{0.01};
+  int debug_image_stride_{3};
+  int debug_info_stride_{1};
+  std::uint64_t frame_count_{0};
+
+  rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
+  rclcpp::Publisher<obstacle_scanner::msg::ObstacleArray>::SharedPtr obstacles_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr debug_pub_;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr debug_info_pub_;
+};
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<ObstacleScannerNode>());
+  rclcpp::shutdown();
+  return 0;
+}

src/obstacle_scanner/src/obstacle_scanner_node.cpp.rej

@@ -0,0 +1,17 @@
+--- src/obstacle_scanner/src/obstacle_scanner_node.cpp
++++ src/obstacle_scanner/src/obstacle_scanner_node.cpp
+@@ -275,6 +275,14 @@
+     out << "\"chord_updates\":" << tracker_debug.chord_updates << ",";
+     out << "\"chord_rejections\":" << tracker_debug.chord_rejections << ",";
++    out << "\"tracks_created\":" << tracker_debug.tracks_created << ",";
++    out << "\"tracks_confirmed\":" << tracker_debug.tracks_confirmed << ",";
++    out << "\"tracks_deleted\":" << tracker_debug.tracks_deleted << ",";
++    out << "\"fit_updates\":" << tracker_debug.fit_updates << ",";
++    out << "\"association_rejections\":" << tracker_debug.association_rejections << ",";
++    out << "\"predicted_published\":" << tracker_debug.predicted_published << ",";
++    out << "\"mean_innovation\":" << tracker_debug.mean_innovation << ",";
++    out << "\"max_innovation\":" << tracker_debug.max_innovation << ",";
+     out << "\"tracks\":" << tracker_debug.active_tracks << ",";
+ 
+ 
+ 

src/obstacle_scanner/src/tracking.cpp

@@ -2,125 +2,417 @@
 
 #include <algorithm>
 #include <cmath>
-#include <limits>
+#include <stdexcept>
+#include <vector>
 
 namespace obstacle_scanner
 {
+namespace
+{
+
+struct AssociationCandidate
+{
+  std::size_t observation_index{0};
+  std::size_t track_index{0};
+  Eigen::Vector2d center{Eigen::Vector2d::Zero()};
+  double mahalanobis_squared{0.0};
+  double euclidean_distance{0.0};
+  bool geometry_valid{true};
+};
+
+Eigen::Matrix<double, 2, 4> position_matrix()
+{
+  Eigen::Matrix<double, 2, 4> matrix = Eigen::Matrix<double, 2, 4>::Zero();
+  matrix(0, 0) = 1.0;
+  matrix(1, 1) = 1.0;
+  return matrix;
+}
+
+bool innovation_for_track(
+  const Eigen::Vector4d & state,
+  const Eigen::Matrix4d & covariance,
+  const Eigen::Vector2d & center,
+  double position_stddev,
+  double & mahalanobis_squared,
+  double & euclidean_distance)
+{
+  const auto h = position_matrix();
+  const Eigen::Vector2d innovation = center - h * state;
+  const Eigen::Matrix2d measurement_noise =
+    position_stddev * position_stddev * Eigen::Matrix2d::Identity();
+  const Eigen::Matrix2d innovation_covariance =
+    h * covariance * h.transpose() + measurement_noise;
+  Eigen::LDLT<Eigen::Matrix2d> ldlt(innovation_covariance);
+  if (ldlt.info() != Eigen::Success) {
+    return false;
+  }
+  const Eigen::Vector2d solved = ldlt.solve(innovation);
+  if (!solved.allFinite()) {
+    return false;
+  }
+  mahalanobis_squared = std::max(0.0, innovation.dot(solved));
+  euclidean_distance = innovation.norm();
+  return std::isfinite(mahalanobis_squared) && std::isfinite(euclidean_distance);
+}
+
+void validate_config(const TrackerConfig & config)
+{
+  const auto finite_positive = [](double value) {
+      return std::isfinite(value) && value > 0.0;
+    };
+  const auto finite_nonnegative = [](double value) {
+      return std::isfinite(value) && value >= 0.0;
+    };
+
+  const bool valid =
+    config.track_confirm_fit_hits > 0 &&
+    config.track_confirm_total_hits > 0 &&
+    config.track_publish_misses >= 0 &&
+    config.track_delete_misses > config.track_publish_misses &&
+    finite_nonnegative(config.process_accel_noise) &&
+    finite_positive(config.initial_velocity_stddev) &&
+    finite_positive(config.fit_position_stddev) &&
+    finite_positive(config.chord_position_stddev) &&
+    finite_positive(config.mahalanobis_gate) &&
+    finite_positive(config.max_association_distance) &&
+    finite_positive(config.max_position_stddev) &&
+    finite_nonnegative(config.chord_tolerance) &&
+    finite_positive(config.nominal_dt) &&
+    finite_positive(config.min_dt) &&
+    finite_positive(config.max_dt) &&
+    config.min_dt <= config.nominal_dt &&
+    config.nominal_dt <= config.max_dt;
+  if (!valid) {
+    throw std::invalid_argument("invalid obstacle tracker configuration");
+  }
+}
+
+}  // namespace
 
 ObstacleTracker::ObstacleTracker(const TrackerConfig & config)
 : config_(config)
 {
+  validate_config(config_);
 }
 
-FrameDebug ObstacleTracker::update(const std::vector<Observation> & observations)
+FrameDebug ObstacleTracker::update(
+  const std::vector<Observation> & observations,
+  double stamp_seconds)
 {
   FrameDebug debug;
+  debug.fit_observations = static_cast<int>(std::count_if(
+      observations.begin(), observations.end(),
+      [](const Observation & observation) {
+        return observation.type == ObservationType::CircleFit;
+      }));
+  debug.chord_observations = static_cast<int>(std::count_if(
+      observations.begin(), observations.end(),
+      [](const Observation & observation) {
+        return observation.type == ObservationType::TwoPointChord;
+      }));
+
+  if (!std::isfinite(stamp_seconds)) {
+    stamp_seconds = std::isfinite(last_stamp_seconds_) ?
+      last_stamp_seconds_ + config_.nominal_dt : 0.0;
+  }
+  double dt = config_.nominal_dt;
+  if (std::isfinite(last_stamp_seconds_)) {
+    const double raw_dt = stamp_seconds - last_stamp_seconds_;
+    if (std::isfinite(raw_dt) && raw_dt > 0.0) {
+      dt = std::clamp(raw_dt, config_.min_dt, config_.max_dt);
+    }
+  }
+  last_stamp_seconds_ = stamp_seconds;
+
   for (auto & track : tracks_) {
     track.matched = false;
   }
+  predict_tracks(dt);
 
-  for (const auto & observation : observations) {
+  double innovation_sum = 0.0;
+  int innovation_count = 0;
+  std::vector<bool> observation_matched(observations.size(), false);
+
+  std::vector<AssociationCandidate> fit_candidates;
+  fit_candidates.reserve(observations.size() * tracks_.size());
+  for (std::size_t observation_index = 0;
+    observation_index < observations.size(); ++observation_index)
+  {
+    const auto & observation = observations[observation_index];
     if (observation.type != ObservationType::CircleFit) {
       continue;
     }
-    debug.fit_observations++;
+    for (std::size_t track_index = 0; track_index < tracks_.size(); ++track_index) {
+      const auto & track = tracks_[track_index];
+      double mahalanobis_squared = 0.0;
+      double euclidean_distance = 0.0;
+      if (innovation_for_track(
+          track.state, track.covariance, observation.center,
+          config_.fit_position_stddev, mahalanobis_squared, euclidean_distance))
+      {
+        fit_candidates.push_back({
+          observation_index, track_index, observation.center,
+          mahalanobis_squared, euclidean_distance, true});
+      }
+    }
+  }
+  std::sort(
+    fit_candidates.begin(), fit_candidates.end(),
+    [](const AssociationCandidate & lhs, const AssociationCandidate & rhs) {
+      return lhs.mahalanobis_squared < rhs.mahalanobis_squared;
+    });
 
-    const int track_index = find_nearest_unmatched_track(observation.center);
+  for (const auto & candidate : fit_candidates) {
-    if (track_index >= 0) {
+    if (observation_matched[candidate.observation_index] ||
-      auto & track = tracks_[static_cast<std::size_t>(track_index)];
+      tracks_[candidate.track_index].matched)
-      const double alpha = config_.fit_update_alpha;
+    {
-      track.center = (1.0 - alpha) * track.center + alpha * observation.center;
-      track.radius = 0.8 * track.radius + 0.2 * observation.radius;
-      track.hit_count++;
-      track.missed_count = 0;
-      track.last_source = UpdateSource::Fit;
-      track.matched = true;
       continue;
     }
-
+    if (candidate.mahalanobis_squared > config_.mahalanobis_gate ||
-    Track track;
+      candidate.euclidean_distance > config_.max_association_distance)
-    track.id = next_id_++;
+    {
-    track.center = observation.center;
+      continue;
+    }
+    auto & track = tracks_[candidate.track_index];
+    const auto & observation = observations[candidate.observation_index];
+    track.fit_hits++;
+    track.total_hits++;
+    update_track(
+      track, candidate.center, config_.fit_position_stddev, UpdateSource::Fit,
+      debug, innovation_sum, innovation_count);
     track.radius = observation.radius;
-    track.hit_count = 1;
+    observation_matched[candidate.observation_index] = true;
-    track.missed_count = 0;
+    debug.fit_updates++;
-    track.last_source = UpdateSource::Fit;
-    track.matched = true;
-    tracks_.push_back(track);
   }
 
-  for (const auto & observation : observations) {
+  for (std::size_t observation_index = 0;
+    observation_index < observations.size(); ++observation_index)
+  {
+    const auto & observation = observations[observation_index];
+    if (observation.type != ObservationType::CircleFit ||
+      observation_matched[observation_index])
+    {
+      continue;
+    }
+    if (!tracks_.empty()) {
+      debug.association_rejections++;
+    }
+    Track track;
+    track.id = next_id_++;
+    track.state.head<2>() = observation.center;
+    track.covariance.diagonal() <<
+      config_.fit_position_stddev * config_.fit_position_stddev,
+      config_.fit_position_stddev * config_.fit_position_stddev,
+      config_.initial_velocity_stddev * config_.initial_velocity_stddev,
+      config_.initial_velocity_stddev * config_.initial_velocity_stddev;
+    track.radius = observation.radius;
+    track.fit_hits = 1;
+    track.total_hits = 1;
+    track.matched = true;
+    track.last_source = UpdateSource::Fit;
+    track.confirmed =
+      track.fit_hits >= config_.track_confirm_fit_hits ||
+      track.total_hits >= config_.track_confirm_total_hits;
+    tracks_.push_back(track);
+    observation_matched[observation_index] = true;
+    debug.tracks_created++;
+    debug.fit_updates++;
+    if (track.confirmed) {
+      debug.tracks_confirmed++;
+    }
+  }
+
+  std::vector<AssociationCandidate> chord_candidates;
+  chord_candidates.reserve(observations.size() * tracks_.size());
+  std::vector<bool> chord_geometry_valid(observations.size(), false);
+  for (std::size_t observation_index = 0;
+    observation_index < observations.size(); ++observation_index)
+  {
+    const auto & observation = observations[observation_index];
     if (observation.type != ObservationType::TwoPointChord) {
       continue;
     }
-    debug.chord_observations++;
+    for (std::size_t track_index = 0; track_index < tracks_.size(); ++track_index) {
-
+      const auto & track = tracks_[track_index];
-    int best_index = -1;
-    double best_distance = std::numeric_limits<double>::infinity();
-    Eigen::Vector2d best_center{0.0, 0.0};
-    bool had_geometry_rejection = false;
-
-    for (std::size_t i = 0; i < tracks_.size(); ++i) {
-      const auto & track = tracks_[i];
       if (track.matched) {
         continue;
       }
-
+      Eigen::Vector2d center = Eigen::Vector2d::Zero();
-      Eigen::Vector2d chord_center{0.0, 0.0};
+      if (!chord_center_for_track(observation, track, center)) {
-      if (!chord_center_for_track(observation, track, chord_center)) {
-        had_geometry_rejection = true;
         continue;
       }
-
+      chord_geometry_valid[observation_index] = true;
-      const double distance = (chord_center - track.center).norm();
+      double mahalanobis_squared = 0.0;
-      if (distance < config_.association_gate && distance < best_distance) {
+      double euclidean_distance = 0.0;
-        best_index = static_cast<int>(i);
+      if (innovation_for_track(
-        best_distance = distance;
+          track.state, track.covariance, center,
-        best_center = chord_center;
+          config_.chord_position_stddev, mahalanobis_squared, euclidean_distance))
+      {
+        chord_candidates.push_back({
+          observation_index, track_index, center,
+          mahalanobis_squared, euclidean_distance, true});
       }
     }
+  }
+  std::sort(
+    chord_candidates.begin(), chord_candidates.end(),
+    [](const AssociationCandidate & lhs, const AssociationCandidate & rhs) {
+      return lhs.mahalanobis_squared < rhs.mahalanobis_squared;
+    });
 
-    if (best_index >= 0) {
+  std::vector<bool> chord_matched(observations.size(), false);
-      auto & track = tracks_[static_cast<std::size_t>(best_index)];
+  for (const auto & candidate : chord_candidates) {
-      const double alpha = config_.chord_update_alpha;
+    if (chord_matched[candidate.observation_index] ||
-      track.center = (1.0 - alpha) * track.center + alpha * best_center;
+      tracks_[candidate.track_index].matched)
-      track.hit_count++;
+    {
-      track.missed_count = 0;
+      continue;
-      track.last_source = UpdateSource::Chord;
+    }
-      track.matched = true;
+    if (candidate.mahalanobis_squared > config_.mahalanobis_gate ||
+      candidate.euclidean_distance > config_.max_association_distance)
+    {
+      continue;
+    }
+    auto & track = tracks_[candidate.track_index];
+    track.total_hits++;
+    update_track(
+      track, candidate.center, config_.chord_position_stddev, UpdateSource::Chord,
+      debug, innovation_sum, innovation_count);
+    chord_matched[candidate.observation_index] = true;
     debug.chord_updates++;
-    } else if (had_geometry_rejection) {
+  }
+
+  for (std::size_t observation_index = 0;
+    observation_index < observations.size(); ++observation_index)
+  {
+    const auto & observation = observations[observation_index];
+    if (observation.type != ObservationType::TwoPointChord ||
+      chord_matched[observation_index])
+    {
+      continue;
+    }
+    bool has_in_gate_candidate = false;
+    for (const auto & candidate : chord_candidates) {
+      if (candidate.observation_index == observation_index &&
+        candidate.mahalanobis_squared <= config_.mahalanobis_gate &&
+        candidate.euclidean_distance <= config_.max_association_distance)
+      {
+        has_in_gate_candidate = true;
+        break;
+      }
+    }
+    if (chord_geometry_valid[observation_index] || has_in_gate_candidate) {
+      debug.association_rejections++;
+    } else if (!tracks_.empty()) {
       debug.chord_rejections++;
     }
   }
 
   for (auto & track : tracks_) {
     if (track.matched) {
+      track.missed_count = 0;
       continue;
     }
     track.missed_count++;
     track.last_source = UpdateSource::Predict;
   }
 
+  const auto old_size = tracks_.size();
   tracks_.erase(
     std::remove_if(
       tracks_.begin(), tracks_.end(),
       [&](const Track & track) {
-        return track.missed_count >= config_.track_delete_misses;
+        return (!track.confirmed && track.missed_count >= 1) ||
+               (track.confirmed && track.missed_count >= config_.track_delete_misses);
       }),
     tracks_.end());
+  debug.tracks_deleted = static_cast<int>(old_size - tracks_.size());
 
   refresh_debug_outputs();
   debug.active_tracks = static_cast<int>(tracks_.size());
   debug.published_tracks = static_cast<int>(confirmed_tracks().size());
+  debug.predicted_published = static_cast<int>(std::count_if(
+      tracks_.begin(), tracks_.end(),
+      [&](const Track & track) {
+        return !track.matched && is_publishable(track);
+      }));
+  if (innovation_count > 0) {
+    debug.mean_innovation = innovation_sum / static_cast<double>(innovation_count);
+  }
   return debug;
 }
 
+void ObstacleTracker::predict_tracks(double dt)
+{
+  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();
+
+  for (auto & track : tracks_) {
+    track.state = transition * track.state;
+    track.covariance =
+      transition * track.covariance * transition.transpose() + process_noise;
+    track.covariance =
+      0.5 * (track.covariance + track.covariance.transpose());
+  }
+}
+
+void ObstacleTracker::update_track(
+  Track & track,
+  const Eigen::Vector2d & center,
+  double position_stddev,
+  UpdateSource source,
+  FrameDebug & debug,
+  double & innovation_sum,
+  int & innovation_count)
+{
+  const auto h = position_matrix();
+  const Eigen::Matrix2d measurement_noise =
+    position_stddev * position_stddev * Eigen::Matrix2d::Identity();
+  const Eigen::Vector2d innovation = center - h * track.state;
+  const Eigen::Matrix2d innovation_covariance =
+    h * track.covariance * h.transpose() + measurement_noise;
+  Eigen::LDLT<Eigen::Matrix2d> ldlt(innovation_covariance);
+  if (ldlt.info() != Eigen::Success) {
+    return;
+  }
+  const Eigen::Matrix<double, 4, 2> kalman_gain =
+    track.covariance * h.transpose() * ldlt.solve(Eigen::Matrix2d::Identity());
+  track.state += kalman_gain * innovation;
+  const Eigen::Matrix4d identity = Eigen::Matrix4d::Identity();
+  const Eigen::Matrix4d residual_projection = identity - kalman_gain * h;
+  track.covariance =
+    residual_projection * track.covariance * residual_projection.transpose() +
+    kalman_gain * measurement_noise * kalman_gain.transpose();
+  track.covariance =
+    0.5 * (track.covariance + track.covariance.transpose());
+  track.missed_count = 0;
+  track.last_source = source;
+  track.matched = true;
+  innovation_sum += innovation.norm();
+  innovation_count++;
+  debug.max_innovation = std::max(debug.max_innovation, innovation.norm());
+  if (
+    !track.confirmed &&
+    (track.fit_hits >= config_.track_confirm_fit_hits ||
+    track.total_hits >= config_.track_confirm_total_hits))
+  {
+    track.confirmed = true;
+    debug.tracks_confirmed++;
+  }
+}
+
 std::vector<TrackOutput> ObstacleTracker::confirmed_tracks() const
 {
   std::vector<TrackOutput> outputs;
   for (const auto & track : tracks_) {
-    if (track.hit_count >= config_.track_confirm_hits) {
+    if (is_publishable(track)) {
       outputs.push_back(to_output(track));
     }
   }
@@ -132,24 +424,6 @@ const std::vector<TrackOutput> & ObstacleTracker::tracks_for_debug() const
   return debug_outputs_;
 }
 
-int ObstacleTracker::find_nearest_unmatched_track(const Eigen::Vector2d & center) const
-{
-  int best_index = -1;
-  double best_distance = std::numeric_limits<double>::infinity();
-  for (std::size_t i = 0; i < tracks_.size(); ++i) {
-    const auto & track = tracks_[i];
-    if (track.matched) {
-      continue;
-    }
-    const double distance = (center - track.center).norm();
-    if (distance < config_.association_gate && distance < best_distance) {
-      best_index = static_cast<int>(i);
-      best_distance = distance;
-    }
-  }
-  return best_index;
-}
-
 bool ObstacleTracker::chord_center_for_track(
   const Observation & observation,
   const Track & track,
@@ -165,8 +439,7 @@ bool ObstacleTracker::chord_center_for_track(
   }
 
   const double half_chord = 0.5 * chord_length;
-  const double radius_squared = track.radius * track.radius;
+  const double h_squared = track.radius * track.radius - half_chord * half_chord;
-  const double h_squared = radius_squared - half_chord * half_chord;
   if (h_squared < 0.0) {
     return false;
   }
@@ -181,14 +454,24 @@ bool ObstacleTracker::chord_center_for_track(
   return true;
 }
 
+bool ObstacleTracker::is_publishable(const Track & track) const
+{
+  const double position_stddev = std::sqrt(std::max(
+      track.covariance(0, 0), track.covariance(1, 1)));
+  return track.confirmed &&
+         track.missed_count <= config_.track_publish_misses &&
+         std::isfinite(position_stddev) &&
+         position_stddev <= config_.max_position_stddev;
+}
+
 TrackOutput ObstacleTracker::to_output(const Track & track)
 {
   TrackOutput output;
   output.id = track.id;
-  output.center_x = track.center.x();
+  output.center_x = track.state.x();
-  output.center_y = track.center.y();
+  output.center_y = track.state.y();
   output.radius = track.radius;
-  output.hit_count = track.hit_count;
+  output.hit_count = track.total_hits;
   output.missed_count = track.missed_count;
   output.last_source = track.last_source;
   return output;

src/obstacle_scanner/test/test_tracking.cpp

@@ -3,7 +3,8 @@
 #include <gtest/gtest.h>
 
 #include <cmath>
-#include <vector>
+#include <limits>
+#include <stdexcept>
 
 namespace obstacle_scanner
 {
@@ -13,104 +14,279 @@ namespace
 TrackerConfig test_config()
 {
   TrackerConfig config;
-  config.track_confirm_hits = 1;
+  config.track_confirm_fit_hits = 2;
-  config.track_delete_misses = 2;
+  config.track_confirm_total_hits = 3;
-  config.association_gate = 0.25;
+  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.fit_update_alpha = 0.7;
+  config.nominal_dt = 1.0 / 12.0;
-  config.chord_update_alpha = 0.3;
+  config.min_dt = 0.02;
+  config.max_dt = 0.20;
   return config;
 }
 
-TEST(ObstacleTrackerTest, CircleFitCreatesConfirmedTrack)
+Observation fit(double x, double y = 0.0, double radius = 0.04)
+{
+  Observation observation;
+  observation.type = ObservationType::CircleFit;
+  observation.center = Eigen::Vector2d(x, y);
+  observation.radius = radius;
+  observation.cluster_size = 4;
+  return observation;
+}
+
+Observation chord(double x, double half_height = 0.02)
+{
+  Observation observation;
+  observation.type = ObservationType::TwoPointChord;
+  observation.p1 = Eigen::Vector2d(x, -half_height);
+  observation.p2 = Eigen::Vector2d(x, half_height);
+  observation.cluster_size = 2;
+  return observation;
+}
+
+void confirm_track(ObstacleTracker & tracker, double x = 1.0)
+{
+  tracker.update({fit(x)}, 0.0);
+  tracker.update({fit(x)}, 1.0 / 12.0);
+}
+
+void seed_moving_track(ObstacleTracker & tracker)
+{
+  tracker.update({fit(1.0)}, 0.0);
+  tracker.update({fit(1.1666667)}, 1.0 / 12.0);
+  tracker.update({fit(1.3333333)}, 2.0 / 12.0);
+}
+
+TEST(ObstacleTrackerTest, FirstFitIsTentativeAndSecondFitConfirms)
 {
   ObstacleTracker tracker(test_config());
-  Observation obs;
-  obs.type = ObservationType::CircleFit;
-  obs.center = Eigen::Vector2d(1.0, 0.2);
-  obs.radius = 0.04;
-  obs.cluster_size = 4;
 
-  FrameDebug debug = tracker.update({obs});
+  const FrameDebug first = tracker.update({fit(1.0, 0.2)}, 0.0);
+  EXPECT_TRUE(tracker.confirmed_tracks().empty());
+  EXPECT_EQ(first.tracks_created, 1);
+  EXPECT_EQ(first.tracks_confirmed, 0);
+  EXPECT_EQ(first.fit_updates, 1);
+  ASSERT_EQ(tracker.tracks_for_debug().size(), 1u);
+  EXPECT_EQ(tracker.tracks_for_debug()[0].hit_count, 1);
+
+  const FrameDebug second = tracker.update({fit(1.0, 0.2)}, 1.0 / 12.0);
   const auto tracks = tracker.confirmed_tracks();
-
   ASSERT_EQ(tracks.size(), 1u);
-  EXPECT_EQ(debug.fit_observations, 1);
+  EXPECT_EQ(second.tracks_confirmed, 1);
-  EXPECT_EQ(debug.published_tracks, 1);
+  EXPECT_EQ(second.fit_updates, 1);
+  EXPECT_EQ(tracks[0].hit_count, 2);
+  EXPECT_EQ(tracks[0].last_source, UpdateSource::Fit);
   EXPECT_NEAR(tracks[0].center_x, 1.0, 1e-9);
   EXPECT_NEAR(tracks[0].center_y, 0.2, 1e-9);
-  EXPECT_NEAR(tracks[0].radius, 0.04, 1e-9);
-  EXPECT_EQ(tracks[0].last_source, UpdateSource::Fit);
 }
 
-TEST(ObstacleTrackerTest, TwoPointChordUpdatesExistingTrack)
+TEST(ObstacleTrackerTest, TracksTwoMetersPerSecondAcrossOneEmptyFrame)
 {
   ObstacleTracker tracker(test_config());
-  Observation fit;
+  tracker.update({fit(1.0)}, 0.0);
-  fit.type = ObservationType::CircleFit;
+  tracker.update({fit(1.1666667)}, 1.0 / 12.0);
-  fit.center = Eigen::Vector2d(1.0, 0.0);
+  tracker.update({fit(1.3333333)}, 2.0 / 12.0);
-  fit.radius = 0.05;
-  fit.cluster_size = 4;
-  tracker.update({fit});
 
-  Observation chord;
+  const FrameDebug debug = tracker.update({}, 3.0 / 12.0);
-  chord.type = ObservationType::TwoPointChord;
-  chord.p1 = Eigen::Vector2d(0.985, -0.02);
-  chord.p2 = Eigen::Vector2d(0.985, 0.02);
-  chord.cluster_size = 2;
-
-  FrameDebug debug = tracker.update({chord});
   const auto tracks = tracker.confirmed_tracks();
-
   ASSERT_EQ(tracks.size(), 1u);
-  EXPECT_EQ(debug.chord_observations, 1);
+  EXPECT_EQ(debug.predicted_published, 1);
-  EXPECT_EQ(debug.chord_updates, 1);
-  EXPECT_EQ(debug.chord_rejections, 0);
-  EXPECT_EQ(tracks[0].last_source, UpdateSource::Chord);
-  EXPECT_GT(tracks[0].center_x, 1.0);
-  EXPECT_NEAR(tracks[0].center_y, 0.0, 1e-6);
-}
-
-TEST(ObstacleTrackerTest, TwoPointChordLongerThanDiameterIsRejected)
-{
-  ObstacleTracker tracker(test_config());
-  Observation fit;
-  fit.type = ObservationType::CircleFit;
-  fit.center = Eigen::Vector2d(1.0, 0.0);
-  fit.radius = 0.04;
-  fit.cluster_size = 4;
-  tracker.update({fit});
-
-  Observation chord;
-  chord.type = ObservationType::TwoPointChord;
-  chord.p1 = Eigen::Vector2d(1.0, -0.08);
-  chord.p2 = Eigen::Vector2d(1.0, 0.08);
-  chord.cluster_size = 2;
-
-  FrameDebug debug = tracker.update({chord});
-  const auto tracks = tracker.confirmed_tracks();
-
-  ASSERT_EQ(tracks.size(), 1u);
-  EXPECT_EQ(debug.chord_updates, 0);
-  EXPECT_EQ(debug.chord_rejections, 1);
   EXPECT_EQ(tracks[0].last_source, UpdateSource::Predict);
+  EXPECT_NEAR(tracks[0].center_x, 1.5, 0.08);
 }
 
-TEST(ObstacleTrackerTest, StaleTracksAreDeleted)
+TEST(ObstacleTrackerTest, TwoFitsSeparatedBySixteenCentimetersShareOneTrack)
 {
   ObstacleTracker tracker(test_config());
-  Observation fit;
-  fit.type = ObservationType::CircleFit;
-  fit.center = Eigen::Vector2d(1.0, 0.0);
-  fit.radius = 0.04;
-  fit.cluster_size = 4;
-  tracker.update({fit});
 
-  tracker.update({});
+  tracker.update({fit(1.0)}, 0.0);
-  EXPECT_EQ(tracker.confirmed_tracks().size(), 1u);
+  const FrameDebug debug = tracker.update({fit(1.1666667)}, 1.0 / 12.0);
 
-  tracker.update({});
+  EXPECT_EQ(debug.tracks_created, 0);
-  EXPECT_TRUE(tracker.confirmed_tracks().empty());
+  ASSERT_EQ(tracker.tracks_for_debug().size(), 1u);
+  EXPECT_EQ(tracker.tracks_for_debug()[0].hit_count, 2);
+}
+
+TEST(ObstacleTrackerTest, FarFitStartsTentativeTrackWithoutDuplicatePublication)
+{
+  ObstacleTracker tracker(test_config());
+  confirm_track(tracker);
+
+  const FrameDebug debug = tracker.update({fit(1.5)}, 2.0 / 12.0);
+  ASSERT_EQ(tracker.confirmed_tracks().size(), 1u);
+  ASSERT_EQ(tracker.tracks_for_debug().size(), 2u);
+  EXPECT_EQ(debug.tracks_created, 1);
+  EXPECT_EQ(debug.association_rejections, 1);
+}
+
+TEST(ObstacleTrackerTest, OneFitAndTwoChordsConfirmOneTrack)
+{
+  ObstacleTracker tracker(test_config());
+
+  tracker.update({fit(1.0)}, 0.0);
+  const FrameDebug first_chord = tracker.update({chord(0.985)}, 1.0 / 12.0);
+  const FrameDebug second_chord = tracker.update({chord(0.985)}, 2.0 / 12.0);
+
+  ASSERT_EQ(tracker.confirmed_tracks().size(), 1u);
+  EXPECT_EQ(first_chord.chord_updates, 1);
+  EXPECT_EQ(second_chord.chord_updates, 1);
+  EXPECT_EQ(second_chord.tracks_confirmed, 1);
+}
+
+TEST(ObstacleTrackerTest, ChordBeyondAssociationDistanceIsRejected)
+{
+  ObstacleTracker tracker(test_config());
+  confirm_track(tracker);
+
+  const FrameDebug debug = tracker.update({chord(1.5, 0.01)}, 2.0 / 12.0);
+  ASSERT_EQ(tracker.tracks_for_debug().size(), 1u);
+  EXPECT_EQ(debug.chord_updates, 0);
+  EXPECT_EQ(debug.chord_rejections, 0);
+  EXPECT_EQ(debug.association_rejections, 1);
+}
+
+TEST(ObstacleTrackerTest, ValidChordUsesFarSideCandidate)
+{
+  ObstacleTracker tracker(test_config());
+  tracker.update({fit(1.0, 0.0, 0.05)}, 0.0);
+
+  const FrameDebug debug = tracker.update({chord(0.985)}, 1.0 / 12.0);
+  ASSERT_EQ(debug.chord_updates, 1);
+  ASSERT_EQ(tracker.tracks_for_debug().size(), 1u);
+  EXPECT_GT(tracker.tracks_for_debug()[0].center_x, 1.0);
+}
+
+TEST(ObstacleTrackerTest, ConfirmedTrackPublishesThroughThirdMissOnly)
+{
+  ObstacleTracker tracker(test_config());
+  confirm_track(tracker);
+
+  EXPECT_EQ(tracker.update({}, 2.0 / 12.0).published_tracks, 1);
+  EXPECT_EQ(tracker.update({}, 3.0 / 12.0).published_tracks, 1);
+  EXPECT_EQ(tracker.update({}, 4.0 / 12.0).published_tracks, 1);
+  EXPECT_EQ(tracker.update({}, 5.0 / 12.0).published_tracks, 0);
+  EXPECT_EQ(tracker.tracks_for_debug().size(), 1u);
+  EXPECT_EQ(tracker.update({}, 6.0 / 12.0).tracks_deleted, 1);
+  EXPECT_TRUE(tracker.tracks_for_debug().empty());
+}
+
+TEST(ObstacleTrackerTest, LargePositionUncertaintyStopsPublication)
+{
+  TrackerConfig config = test_config();
+  config.process_accel_noise = 10.0;
+  config.max_position_stddev = 0.02;
+  ObstacleTracker tracker(config);
+  confirm_track(tracker);
+
+  const FrameDebug debug = tracker.update({}, 2.0 / 12.0);
+  EXPECT_EQ(debug.published_tracks, 0);
+  EXPECT_EQ(tracker.tracks_for_debug().size(), 1u);
+}
+
+TEST(ObstacleTrackerTest, TentativeTrackIsDeletedOnFirstMiss)
+{
+  ObstacleTracker tracker(test_config());
+  tracker.update({fit(1.0)}, 0.0);
+
+  const FrameDebug debug = tracker.update({}, 1.0 / 12.0);
+  EXPECT_EQ(debug.tracks_deleted, 1);
+  EXPECT_TRUE(tracker.tracks_for_debug().empty());
+}
+
+TEST(ObstacleTrackerTest, InvalidTimestampBoundsAreRejected)
+{
+  TrackerConfig config = test_config();
+  config.min_dt = 0.20;
+  config.max_dt = 0.02;
+
+  EXPECT_THROW(ObstacleTracker tracker(config), std::invalid_argument);
+}
+
+TEST(ObstacleTrackerTest, DuplicateAndInvalidTimestampsUseNominalDelta)
+{
+  ObstacleTracker reference(test_config());
+  ObstacleTracker duplicate(test_config());
+  ObstacleTracker invalid(test_config());
+  seed_moving_track(reference);
+  seed_moving_track(duplicate);
+  seed_moving_track(invalid);
+
+  reference.update({}, 3.0 / 12.0);
+  duplicate.update({}, 2.0 / 12.0);
+  invalid.update({}, std::numeric_limits<double>::quiet_NaN());
+
+  ASSERT_EQ(reference.tracks_for_debug().size(), 1u);
+  ASSERT_EQ(duplicate.tracks_for_debug().size(), 1u);
+  ASSERT_EQ(invalid.tracks_for_debug().size(), 1u);
+  EXPECT_NEAR(
+    duplicate.tracks_for_debug()[0].center_x,
+    reference.tracks_for_debug()[0].center_x, 1e-9);
+  EXPECT_NEAR(
+    invalid.tracks_for_debug()[0].center_x,
+    reference.tracks_for_debug()[0].center_x, 1e-9);
+}
+
+TEST(ObstacleTrackerTest, ValidTimestampDeltaIsClampedToConfiguredBounds)
+{
+  ObstacleTracker lower_reference(test_config());
+  ObstacleTracker lower_clamped(test_config());
+  ObstacleTracker upper_reference(test_config());
+  ObstacleTracker upper_clamped(test_config());
+  seed_moving_track(lower_reference);
+  seed_moving_track(lower_clamped);
+  seed_moving_track(upper_reference);
+  seed_moving_track(upper_clamped);
+  const double last_stamp = 2.0 / 12.0;
+
+  lower_reference.update({}, last_stamp + test_config().min_dt);
+  lower_clamped.update({}, last_stamp + 0.001);
+  upper_reference.update({}, last_stamp + test_config().max_dt);
+  upper_clamped.update({}, last_stamp + 1.0);
+
+  EXPECT_NEAR(
+    lower_clamped.tracks_for_debug()[0].center_x,
+    lower_reference.tracks_for_debug()[0].center_x, 1e-9);
+  EXPECT_NEAR(
+    upper_clamped.tracks_for_debug()[0].center_x,
+    upper_reference.tracks_for_debug()[0].center_x, 1e-9);
+}
+
+TEST(ObstacleTrackerTest, GreedyAssociationAssignsTwoFitsToTwoTracks)
+{
+  ObstacleTracker tracker(test_config());
+  tracker.update({fit(1.0), fit(2.0)}, 0.0);
+
+  const FrameDebug debug = tracker.update(
+    {fit(2.05), fit(1.05)}, 1.0 / 12.0);
+
+  EXPECT_EQ(debug.tracks_created, 0);
+  EXPECT_EQ(debug.fit_updates, 2);
+  ASSERT_EQ(tracker.confirmed_tracks().size(), 2u);
+  ASSERT_EQ(tracker.tracks_for_debug().size(), 2u);
+  EXPECT_EQ(tracker.tracks_for_debug()[0].hit_count, 2);
+  EXPECT_EQ(tracker.tracks_for_debug()[1].hit_count, 2);
+}
+
+TEST(ObstacleTrackerTest, FitAssociationTakesPriorityOverChordInSameFrame)
+{
+  ObstacleTracker tracker(test_config());
+  tracker.update({fit(1.0, 0.0, 0.05)}, 0.0);
+
+  const FrameDebug debug = tracker.update(
+    {chord(0.985), fit(1.01, 0.0, 0.05)}, 1.0 / 12.0);
+
+  EXPECT_EQ(debug.fit_updates, 1);
+  EXPECT_EQ(debug.chord_updates, 0);
+  ASSERT_EQ(tracker.confirmed_tracks().size(), 1u);
+  EXPECT_EQ(tracker.confirmed_tracks()[0].last_source, UpdateSource::Fit);
 }
 
 }  // namespace