""" MIMO GPR — локализация через пересечение эллипсов ================================================== Физика в двух словах: Пик A-скана пары (Tx_i, Rx_j) на задержке τ означает: |Tx → объект| + |объект → Rx| = v · τ Это уравнение эллипса. Истинный отражатель лежит на пересечении всех 16 эллипсов (по одному на пару). Алгоритм: 1. S(f) → IFFT → 16 A-сканов 2. Поиск пиков: SNR = пик / медиана > порог 3. Для каждого пика → мягкий эллипс в аккумуляторе (с компенсацией геометрического и углового затухания) 4. CLEAN: найти максимум → убрать его эллипсы → повторить О параметре SHELL_SIGMA: Аккумулятор — это «мягкое голосование». Каждый эллипс добавляет не единицу, а гауссово-взвешенный вклад: w = exp(−δ²/2σ²), где δ = |R_Tx + R_Rx − v·τ| SHELL_SIGMA — ширина этой гауссовой оболочки. Слишком широко → ghost-цели не подавляются. Слишком узко → вклад падает до нуля из-за дискретности сетки. Оптимум: ~ 0.4 × δZ, где δZ = v/(2B) — разрешение по глубине. О score: score = количество пар (из 16), чей эллипс проходит через данную точку с невязкой δ < 3σ. Принимает целые значения от 0 до 16. Максимальный score у истинного объекта = 16 (все пары согласны). Ghost-цели имеют меньший score, т.к. согласуются только с частью пар. О компенсации затухания: При генерации S(f) сигнал ослаблен: geo(i,j) = 1/(R_Tx · R_Rx) — геометрическое ослабление pat(i,j) = cos²(θ_Tx)·cos²(θ_Rx) — диаграмма направленности Без компенсации глубокий/угловой отражатель будет недооценён. Компенсация: делим вес каждого пика на ожидаемое затухание в точке z_apparent, вычисленное для данной пары антенн. Геометрия: плоскость XZ (X — вдоль антенн, Z — глубина). """ """ MIMO GPR — локализация через пересечение эллипсов ================================================== Версия для реальных данных """ import numpy as np import matplotlib.pyplot as plt from matplotlib.lines import Line2D from scipy.signal import find_peaks from scipy.ndimage import gaussian_filter, label from pathlib import Path from dataclasses import dataclass, field from typing import Dict, List, Tuple ### Изменяемые параметры ====== INPUT_IDX = [0,1,2,3] OUTPUT_IDX = [0,3] MIN_DEPTH = 3.0 # [м] пропустить прямую волну MAX_DEPTH = 20.0 COMP_POWER = 0.22 # степень компенсации затухания # Обрезка по частоте F_START = 28*1e8 # Нижняя частота F_STOP = 60*1e8 # Верхняя частота # Параметры скорости в Motion Config SPEED_M_S = 0.48 # Скорость м/с LOOK_ANGLE_DEG = 2.0 # Угол наклона радара отн-но горизонтали (град) # Вычитание среднего фона (background removal) # True → вычитать среднее по всем снимкам в папке (убирает прямую волну и статичные отражения) # False → использовать данные как есть BG_SUBTRACT = True BG_PATH = Path('10m_cyl_motion_21sec_0.48msec_27032026_2.8-6ghz_751/preprocessed') DATA_PATH = Path('10m_cyl_motion_21sec_0.48msec_27032026_2.8-6ghz_751/preprocessed/0002_id3_ns26032413993535') # <-- УКАЖИТЕ ПУТЬ # =============================== @dataclass class MotionConfig: """ Конфигурация движения для одного кадра из 8 пар. speed_m_s: Линейная скорость движения радара. look_angle_deg: Угол между направлением движения и осью дальности Z. Если движение почти "вдоль дальности", ставьте угол близкий к 0°. Тогда dz = v_move * dt * cos(angle) ≈ v_move * dt. sweep_time_s: Время прохода по всем частотам для одной пары. switch_time_s: Время переключения между соседними парами. pair_order_phys: Реальный порядок измерения в физических индексах: [(tx_phys_1, rx_phys_1), (tx_phys_2, rx_phys_2), ...] reference_mode: Относительно какого момента считаем dt: - 'frame_center' : середина всего цикла по 8 парам - 'first_pair' : центр первой пары direction_sign: Знак движения по оси дальности. +1 -> более поздние пары выглядят глубже -1 -> более поздние пары выглядят ближе """ speed_m_s: float = 0.50 look_angle_deg: float = 0.0 sweep_time_s: float = 0.040 switch_time_s: float = 0.005 pair_order_phys: List[Tuple[int, int]] = field(default_factory=lambda: [ (tx_phys, rx_phys) for tx_phys in sorted(OUTPUT_IDX) for rx_phys in sorted(INPUT_IDX) ]) reference_mode: str = 'frame_center' direction_sign: float = +1.0 # Конфигурация движения MOTION_CONFIG = MotionConfig( speed_m_s=SPEED_M_S, look_angle_deg=LOOK_ANGLE_DEG, sweep_time_s=0.15, # ref 0.15 switch_time_s=1e-5, pair_order_phys=[ (0, 0), (0, 1), (0, 2), (0, 3), # Этот порядок текущий, возможно в будущем что-то поменяется (3, 0), (3, 1), (3, 2), (3, 3), ], reference_mode='frame_center', direction_sign=+1.0, ) ### Список параметров и констант использующиеся в коде: ### MODE = 'point' # Один из 2х режимов `point` or `extended` eps_r = 1.0 # Диэлектрическая проницаемость среды v = 3e8 / np.sqrt(eps_r) # скорость света в среде # ══════════════════════════════════════════════════════ # КООРДИНАТЫ АНТЕНН — УКАЖИТЕ РЕАЛЬНЫЕ ЗНАЧЕНИЯ! # ══════════════════════════════════════════════════════ # Координаты вдоль оси X на поверхности (z = 0), в метрах # Физические координаты антенн по их реальным индексам TX_POSITIONS = {0: 90.5*0.01, # Tx с индексом 0 → x = +0.9 м 3: -90.5*0.01} # Tx с индексом 3 → x = -0.9 м RX_POSITIONS = {0: -18*0.01, 1: 48.5*0.01, 2: -49.0*0.01, 3: 18.5*0.01} # Массивы позиций в порядке возрастания физических индексов # (нужны для сетки аккумулятора и графиков) x_tx = np.array([TX_POSITIONS[i] for i in sorted(TX_POSITIONS)]) x_rx = np.array([RX_POSITIONS[j] for j in sorted(RX_POSITIONS)]) # Параметры алгоритма SNR_THRESH = 4.5 # минимальный SNR пика SNR_COMP_MAX = 25.0 # Верхний порог для компенсированного значения SNR # Параметр: максимальное число объектов для поиска MAX_OBJECTS = 15 # <-- настройте под вашу задачу # Границы сетки аккумулятора x_min, x_max = x_tx.min() - 2.0, x_tx.max() + 2.0 # [м] z_min, z_max = 0.2, MAX_DEPTH # <-- глубина [м] # ══════════════════════════════════════════════════════ # 1. ЗАГРУЗКА РЕАЛЬНЫХ ДАННЫХ # ══════════════════════════════════════════════════════ def load_mimo_data(data_path, input_idx, output_idx): data_path = Path(data_path) s21_data = {} freq_data = {} for f in data_path.glob("i*_o*_s21.npy"): name = f.stem parts = name.split('_') i_tx_phys = int(parts[1][1:]) i_rx_phys = int(parts[0][1:]) if i_tx_phys not in output_idx or i_rx_phys not in input_idx: continue # Переводим физический индекс → порядковый (0,1,2,...) i_tx = sorted(output_idx).index(i_tx_phys) i_rx = sorted(input_idx).index(i_rx_phys) s21_data[(i_tx, i_rx)] = np.load(f) freq_file = data_path / f"i{i_rx_phys}_o{i_tx_phys}_freq.npy" freq_data[(i_tx, i_rx)] = np.load(freq_file) tx_indices = sorted(set(k[0] for k in s21_data)) rx_indices = sorted(set(k[1] for k in s21_data)) n_tx = len(tx_indices) n_rx = len(rx_indices) print(f"Загружено пар: {len(s21_data)}") print(f"Передатчиков: {n_tx}, Приёмников: {n_rx}") return s21_data, freq_data, n_tx, n_rx # Загрузка данных s21_data, freq_data, N_tx, N_rx = load_mimo_data(DATA_PATH, INPUT_IDX, OUTPUT_IDX) N_pairs = len(s21_data) # ══════════════════════════════════════════════════════ # 1б. ВЫЧИСЛЕНИЕ СРЕДНЕГО ФОНА ПО ВСЕМ СНИМКАМ # ══════════════════════════════════════════════════════ def compute_background(bg_path, input_idx, output_idx): """ Для каждой пары (i_tx, i_rx) усредняем S21 по всем снимкам в папке. Возвращает: bg : dict[(i_tx, i_rx)] → np.array (complex), усреднённый S21 """ bg_path = Path(bg_path) snapshots = sorted(bg_path.glob("*/")) # каждый подкаталог — один снимок snapshots = [s for s in snapshots if s.is_dir()] if len(snapshots) == 0: print("⚠️ Снимков для фона не найдено, BG_SUBTRACT отключён.") return None print(f"Вычисление фона по {len(snapshots)} снимкам...", end=" ", flush=True) # Накопитель: для каждой пары суммируем S21 bg_sum = {} bg_count = {} for snap_dir in snapshots: for f in snap_dir.glob("i*_o*_s21.npy"): name = f.stem parts = name.split('_') i_tx_phys = int(parts[1][1:]) i_rx_phys = int(parts[0][1:]) if i_tx_phys not in output_idx or i_rx_phys not in input_idx: continue i_tx = sorted(output_idx).index(i_tx_phys) i_rx = sorted(input_idx).index(i_rx_phys) key = (i_tx, i_rx) s21 = np.load(f) if key not in bg_sum: bg_sum[key] = np.zeros_like(s21, dtype=complex) bg_count[key] = 0 bg_sum[key] += s21 bg_count[key] += 1 bg = {key: bg_sum[key] / bg_count[key] for key in bg_sum} print(f"готово. Пар: {len(bg)}, снимков на пару: " f"{list(bg_count.values())[0] if bg_count else 0}") return bg if BG_SUBTRACT: background = compute_background(BG_PATH, INPUT_IDX, OUTPUT_IDX) if background is None: BG_SUBTRACT = False # автоматически выключаем если нет данных else: background = None print("BG_SUBTRACT = False, вычитание фона отключено.") # Проверка частот (берём из первой пары как референс) first_key = list(freq_data.keys())[0] freqs = freq_data[first_key] # Проверим что частоты одинаковые для всех пар for key, freq in freq_data.items(): if not np.allclose(freq, freqs): print(f"⚠️ Частоты для пары {key} отличаются!") mask_freq = (freqs >= F_START) & (freqs <= F_STOP) freqs = freqs[mask_freq] f_min, f_max = freqs[0], freqs[-1] BW = f_max - f_min N_f = len(freqs) # ══════════════════════════════════════════════════════ # 2. ПАРАМЕТРЫ СИСТЕМЫ # ══════════════════════════════════════════════════════ # Проверка соответствия координатов антенн assert len(x_tx) == N_tx, f"x_tx должен содержать {N_tx} элементов" assert len(x_rx) == N_rx, f"x_rx должен содержать {N_rx} элементов" # SHELL_SIGMA — ширина гауссовой оболочки SHELL_SIGMA = v / BW * 0.5 # [м] # Сетка аккумулятора x_grid = np.linspace(x_min, x_max, 300) z_grid = np.linspace(z_min, z_max, 300) XX, ZZ = np.meshgrid(x_grid, z_grid) # Расстояния от сетки до каждой антенны R_tx_grid = {i: np.sqrt((XX - x_tx[i])**2 + ZZ**2) for i in range(N_tx)} R_rx_grid = {j: np.sqrt((XX - x_rx[j])**2 + ZZ**2) for j in range(N_rx)} # ══════════════════════════════════════════════════════ # 3. ВЫЧИСЛЕНИЕ A-СКАНОВ ИЗ РЕАЛЬНЫХ S21 # ══════════════════════════════════════════════════════ def compute_ascan(s21, freq, f_start, f_stop, window=True): """ S21(f) → IFFT → A-скан с правильным частотным сдвигом. Проблема наивного подхода (buf[:n] = s21): IFFT считает, что спектр начинается с 0 Гц. Реальные данные начинаются с f[0] > 0, поэтому нулевая задержка смещается и в A-скане появляются биения. Правильный подход — сдвиг спектра: Шаг частотной сетки df вычисляется из данных. Индекс первой частоты: k0 = round(f[0] / df). Данные кладутся в H[k0 : k0+n], а не в H[0 : n]. Тогда IFFT корректно восстанавливает временной сигнал с нулевой задержкой в t=0. Размер FFT: Минимум для покрытия всего диапазона [0, f[-1]]: min_len = 2 * (k0 + n - 1) Округляем вверх до степени двойки для скорости FFT. """ mask_freq_ = (freq >= f_start) & (freq <= f_stop) freq = freq[mask_freq_] s21 = s21[mask_freq_] n = len(freq) if n < 2: raise ValueError("Слишком мало частотных точек") # Шаг частотной сетки df = (freq[-1] - freq[0]) / (n - 1) if df <= 0: raise ValueError("Частоты не возрастают") # Индекс первой частоты в полной сетке от 0 до f[-1] k0 = int(np.round(freq[0] / df)) # Минимальный размер FFT, округлённый до степени двойки min_len = 2 * (k0 + n - 1) n_fft = 1 << int(np.ceil(np.log2(min_len))) # Временна́я ось — пересчитываем из нового n_fft dt = 1.0 / (n_fft * df) t_sec = np.arange(n_fft, dtype=float) * dt # Оконная функция (подавление боковых лепестков IFFT) s = s21 * np.hanning(n) if window else s21.copy() # Спектр со сдвигом: данные на своём месте в частотной сетке H = np.zeros(n_fft, dtype=np.complex128) H[k0 : k0 + n] = s y = np.abs(np.fft.ifft(H)) return t_sec[:y.size], y[:y.size] print("Вычисление A-сканов из реальных данных...", end=" ", flush=True) A = {} # A[(i,j)] — амплитудный A-скан T_h = {} # T_h[(i,j)] — временна́я ось для этой пары [с] Z_h = {} # Z_h[(i,j)] — ось глубины [м] for (i, j), s21 in s21_data.items(): s21_proc = s21.copy() # Вычитание фона в частотной области if BG_SUBTRACT and background is not None and (i, j) in background: s21_proc = s21_proc - background[(i, j)] # Примечание: вычитаем до обрезки по частоте и до окна — # фон вычисляется из полных (необрезанных) данных, # поэтому вычитание корректно в полном частотном диапазоне. t_pair, a_pair = compute_ascan(s21_proc, freq_data[(i, j)], f_start=F_START, f_stop=F_STOP) T_h[(i, j)] = t_pair Z_h[(i, j)] = t_pair * v / 2 A[(i, j)] = a_pair bg_label = "с вычитанием фона" if BG_SUBTRACT else "без вычитания фона" print(f"готово ({bg_label}).") # Общая ось z для визуализации и поиска пиков # (берём максимальный диапазон по всем парам) z_h = Z_h[list(Z_h.keys())[0]] # все пары дают одинаковую ось, если freq совпадают t_h = T_h[list(T_h.keys())[0]] # ══════════════════════════════════════════════════════ # 4. ДЕТЕКТИРОВАНИЕ ПИКОВ # ══════════════════════════════════════════════════════ def attenuation_at_depth(i_tx, i_rx, z_app): """ Ожидаемое ослабление geo·pattern для точки прямо под виртуальным центром пары на глубине z_app. Используется для компенсации: реальный SNR пика делится на это значение, чтобы вес глубокого/углового объекта не занижался. """ xc = (x_tx[i_tx] + x_rx[i_rx]) / 2.0 # виртуальный центр Rtx = np.sqrt((xc - x_tx[i_tx])**2 + z_app**2) Rrx = np.sqrt((xc - x_rx[i_rx])**2 + z_app**2) geo = 1.0 / (Rtx * Rrx + 1e-12) pat = (z_app / (Rtx + 1e-12))**2 * (z_app / (Rrx + 1e-12))**2 return geo * pat + 1e-30 # +ε чтобы не делить на ноль def find_peaks_snr(i_tx, i_rx, SNR_COMP_MAX = SNR_COMP_MAX): """ Поиск пиков A-скана. Возвращает список dict: z_app — кажущаяся глубина [м] tau — задержка [с] snr_raw — SNR без компенсации = пик / медиана snr_comp— SNR с компенсацией ослабления (используется в аккумуляторе) """ ascan = A[(i_tx, i_rx)] z_h_ij = Z_h[(i_tx, i_rx)] t_h_ij = T_h[(i_tx, i_rx)] i_min = np.searchsorted(z_h_ij, MIN_DEPTH) i_max = np.searchsorted(z_h_ij, MAX_DEPTH) noise = np.median(ascan[i_min:i_max]) # Добавил чтобы было удобно считать SNR отнсительно выбранной области min_dist = max(4, int(v / (2*BW) / (z_h_ij[1] - z_h_ij[0]) * 0.7)) idx, _ = find_peaks(ascan[i_min:i_max], height=noise * SNR_THRESH, distance=min_dist) idx += i_min result = [] for p in idx: z_app = float(z_h_ij[p]) snr_raw = float(ascan[p] / noise) atten = attenuation_at_depth(i_tx, i_rx, z_app) atten_norm = atten / attenuation_at_depth(i_tx, i_rx, 3.0) # Референсная глубина - 3м snr_comp = snr_raw / (atten_norm ** COMP_POWER + 1e-12) snr_comp = min(snr_comp, SNR_COMP_MAX) # ← clipping result.append({'z_app': z_app, 'tau': float(t_h_ij[p]), 'snr_raw': snr_raw, 'snr_comp': snr_comp}) return result peaks = {(i, j): find_peaks_snr(i, j) for i in range(N_tx) for j in range(N_rx)} # ══════════════════════════════════════════════════════ # 6. ПОИСК ОБЪЕКТОВ # ══════════════════════════════════════════════════════ def find_centroid(acc_s, iz, ix, rpz, rpx): """ Взвешенный центроид аккумулятора в окрестности (iz, ix). """ NZ, NX = acc_s.shape iz0 = max(0, iz - rpz); iz1 = min(NZ, iz + rpz) ix0 = max(0, ix - rpx); ix1 = min(NX, ix + rpx) patch = acc_s[iz0:iz1, ix0:ix1].copy() W = patch.sum() if W <= 0: return x_grid[ix], z_grid[iz] rows = np.arange(iz0, iz1)[:, None] * np.ones(patch.shape) cols = np.ones(patch.shape) * np.arange(ix0, ix1)[None, :] iz_c = int(round(np.clip((rows * patch).sum() / W, 0, NZ-1))) ix_c = int(round(np.clip((cols * patch).sum() / W, 0, NX-1))) return x_grid[ix_c], z_grid[iz_c] # ══════════════════════════════════════════════════════ # 8. MOTION-AWARE FIRST-ORDER CORRECTION # ══════════════════════════════════════════════════════ """ Первый блок для движения без изменения продакшн-пайплайна выше. Идея: 1. Для каждой пары (Tx, Rx) задаём время центра её измерения. 2. По известной скорости и углу получаем сдвиг по дальности dz. 3. Переводим dz в поправку по задержке dtau. 4. Для уже найденных пиков формируем motion-corrected версию: tau_corr, z_corr Это first-order модель: считаем, что вся пара измерена в момент времени t_center. Если позже окажется, что смещение за один sweep пары уже заметно, следующим шагом надо будет делать per-frequency коррекцию до IFFT. """ def _build_phys_to_logical_maps(output_idx, input_idx): tx_map = {phys: k for k, phys in enumerate(sorted(output_idx))} rx_map = {phys: k for k, phys in enumerate(sorted(input_idx))} return tx_map, rx_map def compute_pair_timestamps(config: MotionConfig, output_idx=OUTPUT_IDX, input_idx=INPUT_IDX) -> Tuple[Dict[Tuple[int, int], Dict], List[Dict]]: """ Для каждой пары возвращает: t_start, t_center, dt_ref, dz_motion, dtau_motion Ключи словаря pair_timestamps — логические индексы (i_tx, i_rx), совместимые с существующим словарём peaks. """ tx_phys_to_log, rx_phys_to_log = _build_phys_to_logical_maps(output_idx, input_idx) rows: List[Dict] = [] t_cursor = 0.0 for order_idx, (tx_phys, rx_phys) in enumerate(config.pair_order_phys): if tx_phys not in tx_phys_to_log: raise ValueError(f"Tx {tx_phys} отсутствует в OUTPUT_IDX={output_idx}") if rx_phys not in rx_phys_to_log: raise ValueError(f"Rx {rx_phys} отсутствует в INPUT_IDX={input_idx}") i_tx = tx_phys_to_log[tx_phys] i_rx = rx_phys_to_log[rx_phys] t_start = t_cursor t_center = t_start + 0.5 * config.sweep_time_s t_stop = t_start + config.sweep_time_s rows.append({ 'order_idx': order_idx, 'tx_phys': tx_phys, 'rx_phys': rx_phys, 'i_tx': i_tx, 'i_rx': i_rx, 't_start_s': t_start, 't_center_s': t_center, 't_stop_s': t_stop, }) t_cursor = t_stop + config.switch_time_s if not rows: return {}, [] if config.reference_mode == 'frame_center': t_ref = 0.5 * (rows[0]['t_center_s'] + rows[-1]['t_center_s']) elif config.reference_mode == 'first_pair': t_ref = rows[0]['t_center_s'] else: raise ValueError("reference_mode must be 'frame_center' or 'first_pair'") cos_theta = np.cos(np.radians(config.look_angle_deg)) pair_timestamps: Dict[Tuple[int, int], Dict] = {} for row in rows: dt_ref = row['t_center_s'] - t_ref dz_motion = config.direction_sign * config.speed_m_s * dt_ref * cos_theta dtau_motion = 2.0 * dz_motion / v row['dt_ref_s'] = dt_ref row['dz_motion_m'] = dz_motion row['dtau_motion_s'] = dtau_motion pair_timestamps[(row['i_tx'], row['i_rx'])] = row.copy() return pair_timestamps, rows def build_corrected_peaks(peaks_in: Dict[Tuple[int, int], List[Dict]], pair_timestamps: Dict[Tuple[int, int], Dict]) -> Dict[Tuple[int, int], List[Dict]]: """ Формирует словарь corrected_peaks с motion-aware поправками. Для каждого пика добавляет: tau_raw, z_app_raw tau_corr, z_corr dz_motion, dtau_motion Важно: z_corr = v * tau_corr / 2 потому что ось z_app в текущем пайплайне — это apparent depth. """ corrected = {} for key, peak_list in peaks_in.items(): if key not in pair_timestamps: raise KeyError(f"Нет временной информации для пары {key}") info = pair_timestamps[key] dz_motion = info['dz_motion_m'] dtau_motion = info['dtau_motion_s'] corrected_list = [] for pk in peak_list: tau_raw = float(pk['tau']) z_raw = float(pk['z_app']) # dz_motion here is defined as a correction to the apparent depth itself: # z_corr = z_raw + dz_motion. # Therefore tau must be corrected with the same sign. tau_corr = tau_raw + dtau_motion z_corr = 0.5 * v * tau_corr pk_corr = dict(pk) pk_corr.update({ 'tau_raw': tau_raw, 'z_app_raw': z_raw, 'tau_corr': tau_corr, 'z_corr': z_corr, 'dz_motion': dz_motion, 'dtau_motion': dtau_motion, }) corrected_list.append(pk_corr) corrected[key] = corrected_list return corrected pair_timestamps, pair_timing_rows = compute_pair_timestamps(MOTION_CONFIG) corrected_peaks = build_corrected_peaks(peaks, pair_timestamps) # ══════════════════════════════════════════════════════ # 9. MOTION-AWARE IMAGE BUILD FROM CORRECTED PEAKS # ══════════════════════════════════════════════════════ """ Эта ячейка строит motion-aware картинку, используя corrected_peaks из блока выше. Что меняется относительно статического продакшн-пайплайна: - в аккумуляторе используется tau_corr вместо tau - в apparent-depth логике CLEAN используется z_corr вместо z_app - score считается по corrected пикам Исходные A-сканы остаются теми же, но на графике ниже можно показывать уже motion-corrected положения пиков. """ def _peak_in_work_depth(pk): return MIN_DEPTH <= pk['z_corr'] <= MAX_DEPTH def build_accumulator_motion(corrected_peaks_in, exclude_z_ranges): acc = np.zeros_like(XX) for i in range(N_tx): for j in range(N_rx): for pk in corrected_peaks_in[(i, j)]: if not _peak_in_work_depth(pk): continue if any(lo <= pk['z_corr'] <= hi for lo, hi in exclude_z_ranges): continue r_total = v * pk['tau_corr'] residual = R_tx_grid[i] + R_rx_grid[j] - r_total shell = np.exp(-0.5 * (residual / SHELL_SIGMA)**2) acc += shell * pk['snr_comp'] return acc def count_agreeing_ellipses_motion(x_est, z_est, corrected_peaks_in, exclude_z_ranges): count = 0 for i in range(N_tx): for j in range(N_rx): for pk in corrected_peaks_in[(i, j)]: if not _peak_in_work_depth(pk): continue if any(lo <= pk['z_corr'] <= hi for lo, hi in exclude_z_ranges): continue Rt = np.sqrt((x_est - x_tx[i])**2 + z_est**2) Rr = np.sqrt((x_est - x_rx[j])**2 + z_est**2) if abs(Rt + Rr - v * pk['tau_corr']) < SHELL_SIGMA * 6: count += 1 break return count def clean_find_motion(corrected_peaks_in, n_search=10, suppress_r_cm=7, thresh_frac=0.05): dx = x_grid[1] - x_grid[0] dz = z_grid[1] - z_grid[0] rpx = int(suppress_r_cm / 100 / dx) rpz = int(suppress_r_cm / 100 / dz) excl_z = [] found_motion = [] acc_initial = build_accumulator_motion(corrected_peaks_in, []) for step in range(n_search): acc = build_accumulator_motion(corrected_peaks_in, excl_z) acc_s = gaussian_filter(acc, sigma=3) if acc_s.max() < thresh_frac * acc_initial.max(): break iz, ix = np.unravel_index(acc_s.argmax(), acc_s.shape) x_est, z_est = find_centroid(acc_s, iz, ix, rpz, rpx) score = count_agreeing_ellipses_motion(x_est, z_est, corrected_peaks_in, excl_z) found_motion.append({'x': x_est, 'z': z_est, 'score': score}) matched = [pk['z_corr'] for i in range(N_tx) for j in range(N_rx) for pk in corrected_peaks_in[(i, j)] if _peak_in_work_depth(pk) and not any(lo <= pk['z_corr'] <= hi for lo, hi in excl_z) and abs(np.sqrt((x_est - x_tx[i])**2 + z_est**2) + np.sqrt((x_est - x_rx[j])**2 + z_est**2) - v * pk['tau_corr']) < SHELL_SIGMA * 3] if matched: margin = SHELL_SIGMA * 1.0 excl_z.append((min(matched) - margin, max(matched) + margin)) return found_motion, acc_initial if MODE == 'point': found_motion, accum_motion = clean_find_motion(corrected_peaks, n_search=MAX_OBJECTS) # ─── График 2: motion-aware карта накопления ─────────────────────── fig, ax = plt.subplots(figsize=(12, 7)) acc_motion_s = gaussian_filter(accum_motion, sigma=3) im = ax.imshow( acc_motion_s, extent=[x_grid[0]*100, x_grid[-1]*100, z_grid[-1]*100, z_grid[0]*100], aspect='auto', origin='upper', cmap='hot', vmin=acc_motion_s.max()*0.45, vmax=acc_motion_s.max()*0.95, ) plt.colorbar(im, ax=ax, label='Накопленный вес (motion-aware)') ax.plot(x_tx*100, np.zeros(N_tx), 'r^', ms=10, label='Tx', zorder=5) ax.plot(x_rx*100, np.zeros(N_rx), 'bv', ms=10, label='Rx', zorder=5) for obj in found_motion: lbl = f"score={obj['score']}/{N_pairs}" ax.plot(obj['x']*100, obj['z']*100, 'wD', ms=9, zorder=11, markeredgecolor='black', mew=1.2) ax.annotate(lbl, (obj['x']*100, obj['z']*100), textcoords='offset points', xytext=(6, 4), fontsize=8, color='white', bbox=dict(boxstyle='round,pad=0.2', fc='black', alpha=0.5)) ax.plot([], [], 'wD', ms=9, markeredgecolor='k', mew=1.2, label='Найденные объекты') ax.set_xlabel('X [см]') ax.set_ylabel('Глубина Z [см]') ax.set_title('Motion-aware карта накопления эллипсов') ax.set_xlim(x_grid[0]*100, x_grid[-1]*100) ax.set_ylim(z_grid[-1]*100, z_grid[0]*100) ax.legend(loc='lower right', fontsize=9) ax.grid(alpha=0.25) ax.invert_yaxis() plt.tight_layout()