added new filtration and fixed processing parameters

data_acq_and_processing/processing/processors/include/gpr_processor.hpp

@@ -1,5 +1,6 @@
 #pragma once
 
+#include "object_approach_filter.hpp"
 #include "processor_interface.hpp"
 
 namespace radar::processing {
@@ -13,6 +14,11 @@ class GprProcessor final : public ProcessorInterface {
         std::span<const ipc::PreprocessedCollection> previous_collections,
         const ProcessingLiveConfig& live_config
     ) -> ipc::ResultCollection override;
+
+  private:
+    // Cross-frame "approach" track filter. Persists across collections because the
+    // owning processor instance is long-lived (one per data_processor run).
+    ObjectApproachFilter approach_filter_{};
 };
 
 class LegacyGprProcessor final : public ProcessorInterface {

data_acq_and_processing/processing/processors/include/object_approach_filter.hpp

@@ -0,0 +1,172 @@
+#pragma once
+
+#include <algorithm>
+#include <cmath>
+#include <cstddef>
+#include <cstdint>
+#include <deque>
+#include <limits>
+#include <vector>
+
+namespace radar::processing {
+
+// Temporal "approach" filter for coherent-BP detections — a port of the objects-only
+// filter in Horns_motion_3libre's demonstrate notebook.
+//
+// It keeps only objects that persist as a *motion-consistent track* across at least
+// `min_frames` consecutive frames: as the radar moves, a real target reappears at a
+// predictable, shifting (x, z), whereas a one-frame noise spike forms no track and is
+// dropped. The expected per-frame change in range is `speed * dt * cos(look_angle)`,
+// where `dt` is the real interval between consecutive frames (so dropped frames and a
+// varying frame period are handled naturally).
+//
+// CAUSAL: unlike the offline notebook (which can look forward over the whole sequence),
+// this confirms a track by looking *backward* — an object is kept once it ends a track
+// of `min_frames` frames seen so far. A target therefore first appears after it has
+// persisted `min_frames` frames; the early frames of its track are not shown
+// retroactively.
+//
+// Stateless across pipeline restarts is approximated by breaking tracks across a large
+// inter-frame gap (`kMaxFrameGapSeconds`), so a stale history from a previous run cannot
+// spuriously confirm objects.
+//
+// IDEMPOTENT under reprocessing: the data_processor re-runs the last collection (or
+// replays the whole window) whenever live settings or the socket speed change, with no
+// new sweep. The history is therefore keyed by `frame_id` (the strictly increasing
+// collection id): the same id replaces its entry (reprocess of the current frame), a
+// smaller id rebuilds from scratch (a replay restart), so repeated reprocessing never
+// duplicates frames or falsely confirms a track.
+class ObjectApproachFilter {
+  public:
+    struct Point {
+        double x_m;
+        double z_m;
+    };
+
+    // Record `objects` as frame `frame_id` and return, per object, whether it is
+    // confirmed (ends a >= `min_frames` motion-consistent track). `frame_id` is the
+    // collection id (identity/order, survives reprocessing); `frame_time_seconds` is the
+    // frame's wall-clock timestamp (drives the inter-frame interval); `speed_m_s`/
+    // `look_angle_deg` are the live motion estimate. `min_frames <= 1` disables filtering.
+    [[nodiscard]] auto confirm(
+        const std::vector<Point>& objects,
+        std::uint64_t frame_id,
+        double frame_time_seconds,
+        double speed_m_s,
+        double look_angle_deg,
+        std::size_t min_frames
+    ) -> std::vector<bool> {
+        record_frame(Frame{frame_id, frame_time_seconds, objects});
+        const std::size_t history_depth = std::max<std::size_t>(min_frames, 1U);
+        while (history_.size() > history_depth) {
+            history_.pop_front();
+        }
+
+        std::vector<bool> confirmed(objects.size(), min_frames <= 1U);
+        if (min_frames <= 1U || history_.size() < min_frames) {
+            return confirmed;  // disabled, or not enough history yet to confirm anything
+        }
+
+        const double range_step_per_second = std::abs(speed_m_s) * std::cos(to_radians(look_angle_deg));
+        for (std::size_t object_index = 0U; object_index < objects.size(); ++object_index) {
+            confirmed[object_index] = has_backward_track(objects[object_index], min_frames, range_step_per_second);
+        }
+        return confirmed;
+    }
+
+    void reset() { history_.clear(); }
+
+  private:
+    struct Frame {
+        std::uint64_t id;
+        double time_seconds;
+        std::vector<Point> objects;
+    };
+
+    // Append a genuinely new frame, replace the current one on reprocessing (same id),
+    // or rebuild from scratch when the id steps backward (a replay restart). This keeps
+    // the history one entry per distinct collection no matter how often settings change.
+    void record_frame(Frame frame) {
+        if (history_.empty() || frame.id > history_.back().id) {
+            history_.push_back(std::move(frame));
+        } else if (frame.id == history_.back().id) {
+            history_.back() = std::move(frame);
+        } else {
+            history_.clear();
+            history_.push_back(std::move(frame));
+        }
+    }
+
+    // Fixed matching tolerances (Horns_motion notebook 0.2 block). Range decreases as the
+    // radar approaches the target, hence the negative Z sign.
+    static constexpr double kXToleranceM = 0.45;
+    static constexpr double kRangeToleranceFraction = 0.85;
+    static constexpr double kRangeToleranceFloorM = 0.12;
+    static constexpr double kRangeSign = -1.0;
+    static constexpr double kMaxFrameGapSeconds = 2.0;
+
+    [[nodiscard]] static auto to_radians(double degrees) -> double {
+        return degrees * (M_PI / 180.0);
+    }
+
+    // Walk back from the current object through the history, matching a motion-consistent
+    // predecessor in each earlier frame. Confirmed iff a full chain of `min_frames` frames
+    // (the current one plus `min_frames - 1` predecessors) is found.
+    [[nodiscard]] auto has_backward_track(
+        const Point& object,
+        std::size_t min_frames,
+        double range_step_per_second
+    ) const -> bool {
+        const std::size_t newest = history_.size() - 1U;
+        Point current = object;
+        for (std::size_t step = 1U; step < min_frames; ++step) {
+            const std::size_t earlier_index = newest - step;
+            const Frame& earlier = history_[earlier_index];
+            const double dt = history_[earlier_index + 1U].time_seconds - earlier.time_seconds;
+            if (!(dt > 0.0) || dt > kMaxFrameGapSeconds) {
+                return false;  // non-monotonic time, or a gap that breaks the track
+            }
+            const double expected_range_shift = range_step_per_second * dt;
+            const Point* predecessor = match_predecessor(current, earlier.objects, expected_range_shift);
+            if (predecessor == nullptr) {
+                return false;
+            }
+            current = *predecessor;
+        }
+        return true;
+    }
+
+    // The best earlier-frame object consistent with `object` having moved by one frame:
+    // its range was larger by `expected_range_shift` (radar since approached), within the
+    // cross-range and range tolerances. Returns nullptr when nothing matches.
+    [[nodiscard]] static auto match_predecessor(
+        const Point& object,
+        const std::vector<Point>& candidates,
+        double expected_range_shift
+    ) -> const Point* {
+        const double target_z = object.z_m - (kRangeSign * expected_range_shift);
+        const double range_tolerance =
+            std::max(kRangeToleranceFloorM, kRangeToleranceFraction * expected_range_shift);
+
+        const Point* best = nullptr;
+        double best_cost = std::numeric_limits<double>::infinity();
+        for (const Point& candidate : candidates) {
+            const double dx = std::abs(candidate.x_m - object.x_m);
+            const double dz = std::abs(candidate.z_m - target_z);
+            if (dx > kXToleranceM || dz > range_tolerance) {
+                continue;
+            }
+            const double cost = (dx / kXToleranceM) * (dx / kXToleranceM)
+                + (dz / range_tolerance) * (dz / range_tolerance);
+            if (cost < best_cost) {
+                best = &candidate;
+                best_cost = cost;
+            }
+        }
+        return best;
+    }
+
+    std::deque<Frame> history_{};  // recent frames, newest at the back; capped to min_frames
+};
+
+}  // namespace radar::processing