some fixes and improvements

data_acq_and_processing/processing/data_processor/include/processing_live_config.hpp

@@ -18,6 +18,14 @@ enum class LegacyGprMode {
     Extended,
 };
 
+enum class LegacyGprReferenceMode {
+    // t_ref = midpoint between the first and last event centers.
+    FrameCenter,
+    // t_ref = center of the first event. Useful when motion offsets should be
+    // accumulated from frame start, e.g. for tagging frames by their head time.
+    FirstTxEvent,
+};
+
 struct ProcessingLiveConfig {
     std::string processor_mode = "pass_through";
     std::string pass_through_channel = "s21";
@@ -42,6 +50,15 @@ struct ProcessingLiveConfig {
     std::string gpr_score_mode = "combined";
     float gpr_speed_m_s = 0.0F;
     float gpr_look_angle_deg = 0.0F;
+    // Motion model parameters for the legacy GPR pipeline. The direction sign
+    // selects which way later Tx-events appear deeper (+1) or shallower (-1).
+    // Intra-sweep phase correction compensates the motion that happens *inside*
+    // one Tx-sweep before the IFFT — it is independent of the per-pair coarse
+    // tau shift and can be disabled without affecting the rest of the pipeline.
+    // The reference mode picks the anchor used to compute dt_ref per event.
+    float gpr_direction_sign = 1.0F;
+    bool gpr_apply_freq_phase_correction = true;
+    LegacyGprReferenceMode gpr_reference_mode = LegacyGprReferenceMode::FrameCenter;
     float gpr_snr_thresh = 4.5F;
     float gpr_snr_comp_max = 25.0F;
     float gpr_start_freq_mhz = 3000.0F;

data_acq_and_processing/processing/data_processor/src/processing_live_config.cpp

@@ -41,6 +41,18 @@ using Json = nlohmann::json;
     throw std::runtime_error(field_name + " must be one of: point, extended");
 }
 
+[[nodiscard]] auto parse_legacy_gpr_reference_mode(
+    const std::string& value, const std::string& field_name
+) -> LegacyGprReferenceMode {
+    if (value == "frame_center") {
+        return LegacyGprReferenceMode::FrameCenter;
+    }
+    if (value == "first_tx_event") {
+        return LegacyGprReferenceMode::FirstTxEvent;
+    }
+    throw std::runtime_error(field_name + " must be one of: frame_center, first_tx_event");
+}
+
 [[nodiscard]] auto parse_gpr_score_mode(const std::string& value, const std::string& field_name) -> std::string {
     if (value == "peak" || value == "combined") {
         return value;
@@ -266,6 +278,25 @@ void apply_legacy_gpr_algorithm_alias(ProcessingLiveConfig& config, const std::s
         }
         config.gpr_look_angle_deg = static_cast<float>(found->get<double>());
     }
+    if (const auto found = root.find("gpr_direction_sign"); found != root.end()) {
+        if (!found->is_number()) {
+            throw std::runtime_error("processing.gpr_direction_sign must be number");
+        }
+        config.gpr_direction_sign = static_cast<float>(found->get<double>());
+    }
+    if (const auto found = root.find("gpr_apply_freq_phase_correction"); found != root.end()) {
+        if (!found->is_boolean()) {
+            throw std::runtime_error("processing.gpr_apply_freq_phase_correction must be bool");
+        }
+        config.gpr_apply_freq_phase_correction = found->get<bool>();
+    }
+    if (const auto found = root.find("gpr_reference_mode"); found != root.end()) {
+        if (!found->is_string()) {
+            throw std::runtime_error("processing.gpr_reference_mode must be string");
+        }
+        config.gpr_reference_mode =
+            parse_legacy_gpr_reference_mode(found->get<std::string>(), "processing.gpr_reference_mode");
+    }
     if (const auto found = root.find("gpr_snr_thresh"); found != root.end()) {
         if (!found->is_number()) {
             throw std::runtime_error("processing.gpr_snr_thresh must be number");

data_acq_and_processing/processing/processors/src/gpr_legacy_processor.ipp

@@ -1,3 +1,37 @@
+// Legacy MIMO GPR — ellipse-intersection localizer with motion compensation.
+//
+// This translation unit is included from `gpr_processor.cpp` *after*
+// `gpr_backprojection_processor.ipp`, which defines the shared building blocks
+// (kPi, SelectedTrace, GeometrySelection, distance_3d, fft_inplace, ...). Do
+// not include this file directly.
+//
+// Pipeline overview (mirrors the Python reference Ellips_motion_remake_2.py):
+//   1.  Pre-process each pair: optional background subtraction (already done in
+//       collect_selected_traces); optional intra-sweep phase correction of S21
+//       before the IFFT — compensates radar displacement that happens *inside*
+//       one sweep where the frequencies are stepped linearly in time.
+//   2.  IFFT each pair → A-scan; find peaks above an SNR threshold within the
+//       depth gate; record both raw and attenuation-compensated SNR.
+//   3.  Apply coarse per-event motion correction so every peak carries a
+//       motion-corrected (tau_corr, z_corr) on top of the apparent values.
+//   4.  CLEAN-style iterative ellipse intersection: build a soft Gaussian-shell
+//       accumulator, take the strongest pixel, count agreeing pairs, suppress
+//       its depth band, repeat.
+//   5.  Optional extended-mode region detection for diffuse reflectors.
+//
+// Sweep-event model — radar topology matters:
+//   * Matrix radars (`librevna_multi`, `sn9000`) fire one Tx at a time and
+//     receive on all Rx channels in parallel. A run of 8 traces is just 2
+//     Tx-events; all (tx_k, *) pairs share one timestamp.
+//   * Sequential radars (single librevna with switches, kamil_adc, k209…)
+//     measure each pair separately. A run of 8 traces is 8 separate sweeps;
+//     every pair has its own timestamp.
+// Both cases reduce to: there are N events in the frame, each event lasts
+// `event_duration_s = (capture_end_ns - capture_start_ns) / N`. What changes
+// is how event indices are assigned to pairs — by Tx for matrix radars, by
+// trace run-order for sequential ones. `event_duration_s` is derived from
+// the collection metadata, never from a live-config knob.
+
 constexpr double kLegacyGridZMinM = 0.20;
 constexpr double kLegacySmoothSigma = 3.0;
 constexpr double kLegacyCleanSuppressRadiusM = 0.07;
@@ -5,6 +39,7 @@ constexpr double kLegacyCleanThresholdFrac = 0.05;
 constexpr std::size_t kLegacyMaxObjects = 15U;
 constexpr double kLegacyExtendedThresholdFrac = 0.75;
 constexpr double kLegacyExtendedMinAreaCm2 = 2.0;
+constexpr double kLegacyAttenuationReferenceDepthM = 3.0;
 
 struct LegacyAscanResult {
     std::vector<double> time_s{};
@@ -36,8 +71,26 @@ struct LegacyRegionRecord {
     std::vector<float> mask{};
 };
 
-struct LegacyPairTiming {
-    double dtau_motion_s = 0.0;
+// Per-pair timing snapshot. For matrix radars, all (tx_k, *) pairs share the
+// same row; for sequential radars every pair has a distinct row. Indexing by
+// pair keeps the rest of the pipeline ignorant of the radar topology.
+struct LegacyEventTiming {
+    std::size_t event_index = 0U;     // 0-based order of the event in the frame
+    double t_start_s = 0.0;           // start of this event sweep relative to frame
+    double t_center_s = 0.0;          // center of this event sweep
+    double dt_ref_s = 0.0;            // t_center_s - t_frame_ref_s
+    double dz_motion_m = 0.0;         // direction_sign * speed * dt_ref * cos(theta)
+    double dtau_motion_s = 0.0;       // 2 * dz_motion_m / velocity
+};
+
+struct LegacyMotionTiming {
+    double event_duration_s = 0.0;    // (capture_end - capture_start) / num_events
+    double cos_look_angle = 1.0;
+    double direction_sign = 1.0;
+    double speed_m_s = 0.0;
+    bool   apply_intra_sweep_phase = false;
+    bool   parallel_rx_per_tx_event = false;
+    std::unordered_map<PairKey, LegacyEventTiming> by_pair{};
 };
 
 enum class LegacyPeakDomain {
@@ -90,20 +143,31 @@ enum class LegacyPeakDomain {
     return selected;
 }
 
+// Expected geo*pattern attenuation for a target directly under the virtual
+// pair center at depth `z_app`. Uses full 3D antenna coordinates so this
+// generalizes to non-coplanar antenna layouts; boresight is taken along +Z.
 [[nodiscard]] auto legacy_attenuation_at_depth(
     std::size_t tx_index,
     std::size_t rx_index,
     double z_app,
-    const std::vector<double>& x_tx,
-    const std::vector<double>& x_rx
+    const GeometrySelection& selection,
+    double imaging_plane_y_m
 ) -> double {
-    const double x_center = 0.5 * (x_tx[tx_index] + x_rx[rx_index]);
-    const double r_tx = std::hypot(x_center - x_tx[tx_index], z_app);
-    const double r_rx = std::hypot(x_center - x_rx[rx_index], z_app);
+    const double x_center = 0.5 * (selection.x_tx[tx_index] + selection.x_rx[rx_index]);
+    const double r_tx = distance_3d(
+        x_center - selection.x_tx[tx_index],
+        imaging_plane_y_m - selection.y_tx[tx_index],
+        z_app - selection.z_tx[tx_index]
+    );
+    const double r_rx = distance_3d(
+        x_center - selection.x_rx[rx_index],
+        imaging_plane_y_m - selection.y_rx[rx_index],
+        z_app - selection.z_rx[rx_index]
+    );
+    const double cos_tx = (z_app - selection.z_tx[tx_index]) / (r_tx + 1e-12);
+    const double cos_rx = (z_app - selection.z_rx[rx_index]) / (r_rx + 1e-12);
     const double geo = 1.0 / ((r_tx * r_rx) + 1e-12);
-    const double pattern =
-        std::pow(z_app / (r_tx + 1e-12), 2.0) *
-        std::pow(z_app / (r_rx + 1e-12), 2.0);
+    const double pattern = (cos_tx * cos_tx) * (cos_rx * cos_rx);
     return (geo * pattern) + 1e-30;
 }
 
@@ -203,67 +267,193 @@ enum class LegacyPeakDomain {
     return false;
 }
 
-[[nodiscard]] auto build_legacy_motion_timing_by_pair(
-    const std::vector<SelectedTrace>& traces,
-    std::size_t total_combo_count,
+// True when `z_value` should participate in the ellipse vote — both the static
+// depth gate ([min, max]) and the per-step CLEAN-suppression bands must allow it.
+// Mirrors Python's `_peak_in_work_depth` + the per-step `excl_z` filter.
+[[nodiscard]] auto is_legacy_depth_active(
+    double z_value,
+    double min_depth_m,
+    double max_depth_m,
+    const std::vector<std::pair<double, double>>& excluded_ranges
+) -> bool {
+    if (z_value < min_depth_m || z_value > max_depth_m) {
+        return false;
+    }
+    return !is_legacy_depth_excluded(z_value, excluded_ranges);
+}
+
+[[nodiscard]] auto is_matrix_radar_model(const std::string& model) -> bool {
+    return model == "librevna_multi" || model == "sn9000";
+}
+
+// Assign an `event_index` to every selected pair. The mapping depends on the
+// radar topology:
+//   * Matrix radar — all (tx_k, *) pairs share one event, ordered by the Tx's
+//     first appearance in run order. So 8 traces with 2 Tx's give 2 events.
+//   * Sequential radar — every pair is its own event, ordered by run order.
+//     So 8 traces give 8 events.
+[[nodiscard]] auto assign_event_indices(
+    const std::vector<SelectedTrace>& selected_traces,
+    bool matrix_radar
+) -> std::pair<std::unordered_map<PairKey, std::size_t>, std::size_t> {
+    std::unordered_map<PairKey, std::size_t> event_index_by_pair{};
+    event_index_by_pair.reserve(selected_traces.size());
+
+    if (matrix_radar) {
+        std::unordered_map<std::uint32_t, std::size_t> event_by_tx{};
+        std::size_t next_event = 0U;
+        for (const auto& trace : selected_traces) {
+            const auto [event_it, inserted] = event_by_tx.try_emplace(trace.tx_local_index, next_event);
+            if (inserted) {
+                ++next_event;
+            }
+            event_index_by_pair[make_pair_key(trace.tx_local_index, trace.rx_local_index)] = event_it->second;
+        }
+        return {std::move(event_index_by_pair), next_event};
+    }
+
+    // Sequential mode: rank traces by their run_order so the event index is a
+    // dense 0..N-1 sequence regardless of any holes in run_order.
+    std::vector<std::pair<std::size_t, PairKey>> ordered{};
+    ordered.reserve(selected_traces.size());
+    for (const auto& trace : selected_traces) {
+        ordered.emplace_back(trace.run_order, make_pair_key(trace.tx_local_index, trace.rx_local_index));
+    }
+    std::sort(ordered.begin(), ordered.end(),
+              [](const auto& lhs, const auto& rhs) { return lhs.first < rhs.first; });
+    for (std::size_t event_index = 0U; event_index < ordered.size(); ++event_index) {
+        event_index_by_pair[ordered[event_index].second] = event_index;
+    }
+    return {std::move(event_index_by_pair), ordered.size()};
+}
+
+// Build per-pair motion timing. `event_duration_s` is derived from collection
+// metadata as `(capture_end_ns - capture_start_ns) / num_events` — it is the
+// duration of one sweep event in the frame, never a live-config knob. If the
+// motion model is disabled (speed = 0 and phase correction off), the function
+// still returns one row per pair so downstream code can index uniformly.
+[[nodiscard]] auto compute_legacy_motion_timing(
+    const std::vector<SelectedTrace>& selected_traces,
+    bool matrix_radar,
     std::uint64_t capture_start_ns,
     std::uint64_t capture_end_ns,
     const ProcessingLiveConfig& live_config,
     double velocity_mps
-) -> std::unordered_map<PairKey, LegacyPairTiming> {
-    std::unordered_map<PairKey, LegacyPairTiming> timing_by_pair{};
-    timing_by_pair.reserve(traces.size());
+) -> LegacyMotionTiming {
+    LegacyMotionTiming timing{};
+    timing.speed_m_s = static_cast<double>(live_config.gpr_speed_m_s);
+    timing.direction_sign = static_cast<double>(live_config.gpr_direction_sign);
+    timing.cos_look_angle = std::cos((static_cast<double>(live_config.gpr_look_angle_deg) * kPi) / 180.0);
+    timing.apply_intra_sweep_phase = live_config.gpr_apply_freq_phase_correction;
+    timing.parallel_rx_per_tx_event = matrix_radar;
 
-    const double speed_mps = static_cast<double>(live_config.gpr_speed_m_s);
-    if (!(std::abs(speed_mps) > 1e-12)) {
-        for (const auto& trace : traces) {
-            timing_by_pair.emplace(make_pair_key(trace.tx_local_index, trace.rx_local_index), LegacyPairTiming{});
+    if (selected_traces.empty()) {
+        return timing;
+    }
+
+    auto [event_index_by_pair, num_events] = assign_event_indices(selected_traces, matrix_radar);
+    if (num_events == 0U) {
+        return timing;
+    }
+
+    const bool speed_meaningful = std::abs(timing.speed_m_s) > 1e-12;
+    const bool model_active = speed_meaningful || timing.apply_intra_sweep_phase;
+
+    // No motion and no phase correction — populate with zeroed rows and bail.
+    if (!model_active) {
+        for (const auto& [pair_key, event_index] : event_index_by_pair) {
+            timing.by_pair.emplace(pair_key, LegacyEventTiming{.event_index = event_index});
         }
-        return timing_by_pair;
+        return timing;
     }
 
-    if (total_combo_count == 0U) {
-        throw std::runtime_error("Legacy GPR requires at least one run combo");
-    }
     if (capture_end_ns <= capture_start_ns) {
         throw std::runtime_error(
-            "Legacy GPR requires valid capture_start_ns/capture_end_ns metadata when speed is non-zero"
+            "Legacy GPR motion model requires valid capture_start_ns/capture_end_ns metadata"
         );
     }
 
-    const double capture_span_s = static_cast<double>(capture_end_ns - capture_start_ns) * 1e-9;
-    const double slot_duration_s = capture_span_s / static_cast<double>(total_combo_count);
-    if (!(slot_duration_s > 0.0)) {
-        throw std::runtime_error("Legacy GPR requires positive collection capture span when speed is non-zero");
+    const double total_span_s = static_cast<double>(capture_end_ns - capture_start_ns) * 1e-9;
+    const double event_duration_s = total_span_s / static_cast<double>(num_events);
+    if (!(event_duration_s > 0.0)) {
+        throw std::runtime_error("Legacy GPR motion model requires positive per-event duration");
+    }
+    timing.event_duration_s = event_duration_s;
+
+    // Reference anchor for dt_ref: either the midpoint between the first and
+    // last event centers (frame_center) or just the first event center
+    // (first_tx_event). Matches Python's `MOTION_CONFIG.reference_mode`.
+    const double first_center_s = 0.5 * event_duration_s;
+    const double last_center_s = (static_cast<double>(num_events) - 0.5) * event_duration_s;
+    const double t_ref_s = live_config.gpr_reference_mode == LegacyGprReferenceMode::FirstTxEvent
+        ? first_center_s
+        : 0.5 * (first_center_s + last_center_s);
+
+    const double motion_factor = speed_meaningful
+        ? timing.direction_sign * timing.speed_m_s * timing.cos_look_angle
+        : 0.0;
+
+    for (const auto& [pair_key, event_index] : event_index_by_pair) {
+        LegacyEventTiming row{};
+        row.event_index = event_index;
+        row.t_start_s = static_cast<double>(event_index) * event_duration_s;
+        row.t_center_s = row.t_start_s + (0.5 * event_duration_s);
+        row.dt_ref_s = row.t_center_s - t_ref_s;
+        row.dz_motion_m = motion_factor * row.dt_ref_s;
+        row.dtau_motion_s = (2.0 * row.dz_motion_m) / velocity_mps;
+        timing.by_pair.emplace(pair_key, row);
     }
 
-    const double t_ref_s = 0.5 * capture_span_s;
-    const double cos_theta = std::cos((static_cast<double>(live_config.gpr_look_angle_deg) * kPi) / 180.0);
-    for (const auto& trace : traces) {
-        const double t_center_s = (static_cast<double>(trace.run_order) + 0.5) * slot_duration_s;
-        const double dz_motion_m = speed_mps * (t_center_s - t_ref_s) * cos_theta;
-        timing_by_pair.emplace(
-            make_pair_key(trace.tx_local_index, trace.rx_local_index),
-            LegacyPairTiming{.dtau_motion_s = (2.0 * dz_motion_m) / velocity_mps}
-        );
+    return timing;
+}
+
+// Compensate for the radar moving while a single sweep is being recorded.
+// Frequencies inside one sweep are stepped linearly in time, so each frequency
+// is sampled from a slightly different antenna position. The correction shifts
+// each frequency's phase back to the event center; after that the IFFT
+// produces an A-scan as if the whole sweep were captured at one position.
+void apply_intra_sweep_phase_correction(
+    SelectedTrace& trace,
+    const LegacyEventTiming& timing,
+    const LegacyMotionTiming& motion,
+    double velocity_mps
+) {
+    if (!motion.apply_intra_sweep_phase || !(motion.event_duration_s > 0.0)) {
+        return;
+    }
+    if (!(std::abs(motion.speed_m_s) > 1e-12)) {
+        return;  // No motion → zero phase shift, no-op.
+    }
+    const std::size_t point_count = trace.frequency_hz.size();
+    if (point_count < 2U || trace.s21.size() != point_count) {
+        return;
     }
 
-    return timing_by_pair;
+    const double dt_freq_s = motion.event_duration_s / static_cast<double>(point_count - 1U);
+    const double motion_factor = motion.direction_sign * motion.speed_m_s * motion.cos_look_angle;
+
+    for (std::size_t index = 0U; index < point_count; ++index) {
+        const double t_abs_s = timing.t_start_s + (static_cast<double>(index) * dt_freq_s);
+        const double dt_intra_s = t_abs_s - timing.t_center_s;
+        const double delta_path_m = 2.0 * motion_factor * dt_intra_s;
+        const double phi = (2.0 * kPi * trace.frequency_hz[index] * delta_path_m) / velocity_mps;
+        trace.s21[index] *= std::polar(1.0, phi);
+    }
 }
 
 void apply_legacy_motion_correction(
     std::unordered_map<PairKey, std::vector<LegacyPeakRecord>>& peaks_by_pair,
-    const std::unordered_map<PairKey, LegacyPairTiming>& timing_by_pair,
+    const LegacyMotionTiming& motion,
     double velocity_mps
 ) {
     for (auto& [key, peaks] : peaks_by_pair) {
-        const auto timing_it = timing_by_pair.find(key);
-        if (timing_it == timing_by_pair.end()) {
-            throw std::runtime_error("Missing motion timing for selected legacy GPR combo");
+        const auto timing_it = motion.by_pair.find(key);
+        if (timing_it == motion.by_pair.end()) {
+            throw std::runtime_error("Missing motion timing for selected legacy GPR pair");
         }
-
+        const double dtau_motion_s = timing_it->second.dtau_motion_s;
         for (auto& peak : peaks) {
-            peak.tau_corr = peak.tau + timing_it->second.dtau_motion_s;
+            peak.tau_corr = peak.tau + dtau_motion_s;
             peak.z_corr = 0.5 * velocity_mps * peak.tau_corr;
         }
     }
@@ -275,6 +465,8 @@ void apply_legacy_motion_correction(
     const std::vector<std::pair<double, double>>& exclude_ranges,
     double velocity_mps,
     double shell_sigma_m,
+    double min_depth_m,
+    double max_depth_m,
     const std::vector<double>& x_tx,
     const std::vector<double>& x_rx,
     LegacyPeakDomain domain
@@ -298,7 +490,8 @@ void apply_legacy_motion_correction(
             const auto& tx_grid = grid.tx_distance_grids[tx_index];
             const auto& rx_grid = grid.rx_distance_grids[rx_index];
             for (const auto& peak : peak_it->second) {
-                if (is_legacy_depth_excluded(legacy_peak_depth_for_domain(peak, domain), exclude_ranges)) {
+                const double depth = legacy_peak_depth_for_domain(peak, domain);
+                if (!is_legacy_depth_active(depth, min_depth_m, max_depth_m, exclude_ranges)) {
                     continue;
                 }
 
@@ -324,6 +517,8 @@ void apply_legacy_motion_correction(
     const std::vector<double>& x_rx,
     double velocity_mps,
     double shell_sigma_m,
+    double min_depth_m,
+    double max_depth_m,
     LegacyPeakDomain domain
 ) -> double {
     std::size_t count = 0U;
@@ -337,7 +532,8 @@ void apply_legacy_motion_correction(
             }
 
             for (const auto& peak : peak_it->second) {
-                if (is_legacy_depth_excluded(legacy_peak_depth_for_domain(peak, domain), exclude_ranges)) {
+                const double depth = legacy_peak_depth_for_domain(peak, domain);
+                if (!is_legacy_depth_active(depth, min_depth_m, max_depth_m, exclude_ranges)) {
                     continue;
                 }
 
@@ -405,22 +601,26 @@ void apply_legacy_motion_correction(
     const std::vector<double>& x_rx,
     double velocity_mps,
     double shell_sigma_m,
+    double min_depth_m,
+    double max_depth_m,
     LegacyPeakDomain domain
 ) -> std::pair<std::vector<LegacyPointRecord>, std::vector<double>> {
     std::vector<LegacyPointRecord> found{};
-    const auto accumulator =
-        build_legacy_accumulator(grid, peaks_by_pair, {}, velocity_mps, shell_sigma_m, x_tx, x_rx, domain);
+    const auto accumulator = build_legacy_accumulator(
+        grid, peaks_by_pair, {}, velocity_mps, shell_sigma_m, min_depth_m, max_depth_m, x_tx, x_rx, domain
+    );
     const double initial_max = max_value(accumulator);
     if (!(initial_max > 0.0) || grid.x_grid.size() < 2U || grid.z_grid.size() < 2U) {
         return {found, gaussian_filter_2d(accumulator, grid.x_grid.size(), grid.z_grid.size(), kLegacySmoothSigma)};
     }
 
+    // Suppression radius rounds down to mirror Python's `int(0.07 / dx)`.
     const double dx = grid.x_grid[1] - grid.x_grid[0];
     const double dz = grid.z_grid[1] - grid.z_grid[0];
     const auto radius_x =
-        static_cast<std::size_t>(std::max(1.0, std::round(kLegacyCleanSuppressRadiusM / std::max(dx, 1e-6))));
+        static_cast<std::size_t>(std::max(1.0, std::floor(kLegacyCleanSuppressRadiusM / std::max(dx, 1e-6))));
     const auto radius_z =
-        static_cast<std::size_t>(std::max(1.0, std::round(kLegacyCleanSuppressRadiusM / std::max(dz, 1e-6))));
+        static_cast<std::size_t>(std::max(1.0, std::floor(kLegacyCleanSuppressRadiusM / std::max(dz, 1e-6))));
 
     std::vector<std::pair<double, double>> excluded_ranges{};
     for (std::size_t step = 0U; step < kLegacyMaxObjects; ++step) {
@@ -430,6 +630,8 @@ void apply_legacy_motion_correction(
             excluded_ranges,
             velocity_mps,
             shell_sigma_m,
+            min_depth_m,
+            max_depth_m,
             x_tx,
             x_rx,
             domain
@@ -469,11 +671,16 @@ void apply_legacy_motion_correction(
                     x_rx,
                     velocity_mps,
                     shell_sigma_m,
+                    min_depth_m,
+                    max_depth_m,
                     domain
                 ),
             }
         );
 
+        // Collect depths of all peaks consistent with the just-detected point;
+        // they form the next exclusion band so subsequent CLEAN steps cannot
+        // re-pick the same target.
         std::vector<double> matched_depths{};
         for (std::size_t tx_index = 0U; tx_index < x_tx.size(); ++tx_index) {
             for (std::size_t rx_index = 0U; rx_index < x_rx.size(); ++rx_index) {
@@ -484,14 +691,15 @@ void apply_legacy_motion_correction(
                     continue;
                 }
                 for (const auto& peak : peak_it->second) {
-                    if (is_legacy_depth_excluded(legacy_peak_depth_for_domain(peak, domain), excluded_ranges)) {
+                    const double depth = legacy_peak_depth_for_domain(peak, domain);
+                    if (!is_legacy_depth_active(depth, min_depth_m, max_depth_m, excluded_ranges)) {
                         continue;
                     }
                     const double r_tx = std::hypot(x_est - x_tx[tx_index], z_est);
                     const double r_rx = std::hypot(x_est - x_rx[rx_index], z_est);
                     if (std::abs((r_tx + r_rx) - (velocity_mps * legacy_peak_tau_for_domain(peak, domain))) <
                         shell_sigma_m * 3.0) {
-                        matched_depths.push_back(legacy_peak_depth_for_domain(peak, domain));
+                        matched_depths.push_back(depth);
                     }
                 }
             }
@@ -512,7 +720,9 @@ void apply_legacy_motion_correction(
     const std::vector<double>& x_tx,
     const std::vector<double>& x_rx,
     double velocity_mps,
-    double shell_sigma_m
+    double shell_sigma_m,
+    double min_depth_m,
+    double max_depth_m
 ) -> std::pair<std::vector<LegacyRegionRecord>, std::vector<double>> {
     std::vector<LegacyRegionRecord> regions{};
     const auto accumulator = build_legacy_accumulator(
@@ -521,6 +731,8 @@ void apply_legacy_motion_correction(
         {},
         velocity_mps,
         shell_sigma_m,
+        min_depth_m,
+        max_depth_m,
         x_tx,
         x_rx,
         LegacyPeakDomain::Apparent
@@ -622,6 +834,8 @@ void apply_legacy_motion_correction(
                 x_rx,
                 velocity_mps,
                 shell_sigma_m,
+                min_depth_m,
+                max_depth_m,
                 LegacyPeakDomain::Apparent
             );
             region.pixel_count = static_cast<double>(component.size());
@@ -649,7 +863,7 @@ void apply_legacy_motion_correction(
 
     validate_collection_trace_order(run_config, collection);
     const auto background_mean = build_background_mean(previous_collections, selection, live_config);
-    const auto selected_traces = collect_selected_traces(collection, selection, background_mean);
+    auto selected_traces = collect_selected_traces(collection, selection, background_mean);
     if (selected_traces.empty()) {
         return results;
     }
@@ -663,6 +877,32 @@ void apply_legacy_motion_correction(
     if (!(max_depth_m > min_depth_m)) {
         return results;
     }
+    const double imaging_plane_y_m = static_cast<double>(live_config.gpr_imaging_plane_y_m);
+
+    // Motion timing is computed once per collection. Matrix radars get one
+    // event per Tx (parallel Rx); sequential radars get one event per pair.
+    const bool matrix_radar = is_matrix_radar_model(run_config.radar.model);
+    const auto motion_timing = compute_legacy_motion_timing(
+        selected_traces,
+        matrix_radar,
+        collection.capture_start_ns,
+        collection.capture_end_ns,
+        live_config,
+        velocity_mps
+    );
+
+    // Intra-sweep phase correction (frequency-domain) — happens BEFORE the IFFT
+    // because it modifies the S21 spectrum that compute_legacy_ascan consumes.
+    if (motion_timing.apply_intra_sweep_phase) {
+        for (auto& trace : selected_traces) {
+            const auto pair_key = make_pair_key(trace.tx_local_index, trace.rx_local_index);
+            const auto timing_it = motion_timing.by_pair.find(pair_key);
+            if (timing_it == motion_timing.by_pair.end()) {
+                continue;
+            }
+            apply_intra_sweep_phase_correction(trace, timing_it->second, motion_timing, velocity_mps);
+        }
+    }
 
     std::unordered_map<PairKey, LegacyAscanResult> ascans_by_pair{};
     double bandwidth_hz = 0.0;
@@ -678,12 +918,7 @@ void apply_legacy_motion_correction(
         return results;
     }
 
-    const auto grid = build_grid(
-        selection,
-        max_depth_m,
-        kLegacyGridZMinM,
-        static_cast<double>(live_config.gpr_imaging_plane_y_m)
-    );
+    const auto grid = build_grid(selection, max_depth_m, kLegacyGridZMinM, imaging_plane_y_m);
     if (grid.x_grid.empty() || grid.z_grid.empty()) {
         return results;
     }
@@ -724,15 +959,17 @@ void apply_legacy_motion_correction(
             const auto peak_indices =
                 find_legacy_peak_indices(ascan.amplitude, min_index, max_index, noise * snr_thresh, min_distance);
 
+            const double attenuation_ref = legacy_attenuation_at_depth(
+                tx_index, rx_index, kLegacyAttenuationReferenceDepthM, selection, imaging_plane_y_m
+            );
+
             auto& peaks = peaks_by_pair[key];
             peaks.reserve(peak_indices.size());
             for (const auto peak_index : peak_indices) {
                 const double z_app = ascan.depth_m[peak_index];
                 const double snr_raw = ascan.amplitude[peak_index] / std::max(noise, 1e-12);
                 const double attenuation =
-                    legacy_attenuation_at_depth(tx_index, rx_index, z_app, selection.x_tx, selection.x_rx);
-                const double attenuation_ref =
-                    legacy_attenuation_at_depth(tx_index, rx_index, 3.0, selection.x_tx, selection.x_rx);
+                    legacy_attenuation_at_depth(tx_index, rx_index, z_app, selection, imaging_plane_y_m);
                 const double snr_comp = std::min(
                     snr_raw / (std::pow(attenuation / attenuation_ref, comp_power) + 1e-12),
                     snr_comp_max
@@ -762,7 +999,9 @@ void apply_legacy_motion_correction(
             selection.x_tx,
             selection.x_rx,
             velocity_mps,
-            shell_sigma_m
+            shell_sigma_m,
+            min_depth_m,
+            max_depth_m
         );
         results.collection_payloads.push_back(
             build_image_payload("gpr_accumulator", grid.x_grid, grid.z_grid, smoothed_accumulator)
@@ -793,15 +1032,10 @@ void apply_legacy_motion_correction(
         return results;
     }
 
-    const auto motion_timing_by_pair = build_legacy_motion_timing_by_pair(
-        selected_traces,
-        run_config.run_combos.size(),
-        collection.capture_start_ns,
-        collection.capture_end_ns,
-        live_config,
-        velocity_mps
-    );
-    apply_legacy_motion_correction(peaks_by_pair, motion_timing_by_pair, velocity_mps);
+    // Apply coarse per-Tx-event motion correction to the peak set produced
+    // above. After this step every peak carries both apparent and motion-
+    // corrected (tau, depth) values; the CLEAN search uses the corrected domain.
+    apply_legacy_motion_correction(peaks_by_pair, motion_timing, velocity_mps);
 
     const auto [points, smoothed_accumulator] = clean_legacy_find_points(
         grid,
@@ -810,6 +1044,8 @@ void apply_legacy_motion_correction(
         selection.x_rx,
         velocity_mps,
         shell_sigma_m,
+        min_depth_m,
+        max_depth_m,
         LegacyPeakDomain::Corrected
     );
     results.collection_payloads.push_back(

data_acq_and_processing/sweep_orchestrator/device_drivers/interfaces/radar_driver.hpp

@@ -34,6 +34,19 @@ class RadarDriver {
     virtual void close() = 0;
     /** @brief Acquire one sweep containing the forward traces exposed by the driver. */
     [[nodiscard]] virtual auto acquire_sweep() -> SweepTrace = 0;
+
+    /**
+     * @brief Announce which switch combo the next `acquire_sweep` belongs to.
+     *
+     * Real radars are agnostic to this because the switch state itself decides
+     * what they see. Mock drivers use it to synthesise per-combo variation so
+     * downstream plots show eight distinct traces for an eight-combo run
+     * instead of eight identical curves stacked on top of each other.
+     *
+     * Default implementation is a no-op so production drivers do not need to
+     * override.
+     */
+    virtual void set_active_combo(const ipc::ComboKey& /*combo*/) {}
 };
 
 }  // namespace radar::drivers

data_acq_and_processing/sweep_orchestrator/device_drivers/radar/librevna/librevna_minimal_driver_lifecycle.cpp

@@ -126,6 +126,10 @@ void LibreVnaMinimalDriver::close() {
     is_open_ = false;
 }
 
+void LibreVnaMinimalDriver::set_active_combo(const ipc::ComboKey& combo) {
+    active_combo_ = combo;
+}
+
 auto LibreVnaMinimalDriver::acquire_sweep() -> SweepTrace {
     if (!is_open_) {
         throw std::runtime_error("Radar driver is not open");
@@ -189,6 +193,17 @@ auto LibreVnaMinimalDriver::acquire_mock() -> SweepTrace {
     const float range_drift_m =
         0.01F * std::sin(0.07F * static_cast<float>(sweep_index_));
 
+    // Combo-dependent variation. Without this the mock returns near-identical
+    // S21 for every (input, output) combo and an eight-combo pass-through plot
+    // collapses into a single visible trace. The factors below are arbitrary
+    // but chosen small enough that the overall response stays in a reasonable
+    // band and large enough that each pair is visually distinct.
+    const auto input_pos = static_cast<float>(active_combo_.input_pos);
+    const auto output_pos = static_cast<float>(active_combo_.output_pos);
+    const float combo_amplitude_gain = 0.55F + 0.08F * input_pos + 0.05F * output_pos;
+    const float combo_phase_offset = 0.4F * input_pos + 0.9F * output_pos;
+    const float combo_range_offset_m = 0.05F * input_pos + 0.12F * output_pos;
+
     // Deterministic-per-sweep noise so two consecutive frames look distinct
     // but the test stays reproducible for any given sweep index.
     std::mt19937 noise_engine(
@@ -205,7 +220,7 @@ auto LibreVnaMinimalDriver::acquire_mock() -> SweepTrace {
         std::complex<float> s11_total{0.0F, 0.0F};
 
         for (const auto& target : kMockTargets) {
-            const float range_m = target.range_m + range_drift_m;
+            const float range_m = target.range_m + range_drift_m + combo_range_offset_m;
             // Round-trip phase: 2π·f·(2R/v).
             const float round_trip_phase =
                 2.0F * detail::kPi * frequency_hz * (2.0F * range_m / kGroundVelocityMps);
@@ -216,8 +231,8 @@ auto LibreVnaMinimalDriver::acquire_mock() -> SweepTrace {
                 std::exp(-kAttenuationCoeffPerMeterAtRefHz * range_m * frequency_scale);
 
             const std::complex<float> contribution = std::polar<float>(
-                target.reflection_magnitude * spreading * attenuation,
-                -round_trip_phase
+                target.reflection_magnitude * spreading * attenuation * combo_amplitude_gain,
+                -round_trip_phase + combo_phase_offset
             );
             s21_total += contribution;
             s11_total += kS11CrossCouplingFactor * contribution;

data_acq_and_processing/sweep_orchestrator/device_drivers/radar/librevna_minimal_driver.hpp

@@ -43,6 +43,7 @@ class LibreVnaMinimalDriver final : public RadarDriver {
     void open() override;
     void close() override;
     [[nodiscard]] auto acquire_sweep() -> SweepTrace override;
+    void set_active_combo(const ipc::ComboKey& combo) override;
 
   private:
     /**
@@ -92,6 +93,10 @@ class LibreVnaMinimalDriver final : public RadarDriver {
     LibreVnaMinimalDriverSettings settings_{};
     bool is_open_ = false;
     std::uint64_t sweep_index_ = 0;
+    // Latest combo announced by the orchestrator. Used by the mock backend to
+    // give each (input, output) pair a slightly different reflectivity profile
+    // so a multi-combo run does not render as eight identical traces.
+    ipc::ComboKey active_combo_{};
 
     libusb_context* usb_context_ = nullptr;
     libusb_device_handle* usb_handle_ = nullptr;

data_acq_and_processing/sweep_orchestrator/src/sweep_orchestrator.cpp

@@ -157,6 +157,9 @@ auto SweepOrchestrator::acquire_one_collection(
         input_switch_driver_.switch_to(combo.input_pos);
         sleep_if_needed_ms(config_.runtime.settling_ms);
 
+        // Production drivers ignore this; mock drivers use it to give every
+        // (input, output) pair its own synthetic response.
+        radar_driver_.set_active_combo(combo);
         auto sweep = radar_driver_.acquire_sweep();
         validate_sweep(sweep);