awe/radar_system
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

some fixes and improvements

Browse Source
This commit is contained in:
Ayzen
2026-05-28 14:33:12 +03:00
parent 83a934f251
commit eacea436a4
29 changed files with 2114 additions and 424 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Ellips_motion_remake_2.py
+956
View File
@@ -0,0 +1,956 @@
"""
MIMO GPR — локализация через пересечение эллипсов
==================================================
Физика в двух словах:
Пик A-скана пары (Tx_i, Rx_j) на задержке τ означает:
|Tx → объект| + |объект → Rx| = v · τ
Это уравнение эллипса. Истинный отражатель лежит на
пересечении всех Tx/Rx-эллипсов (по одному на измеренную пару).
Алгоритм:
1. S(f) → IFFT → A-сканы всех Tx/Rx-пар
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 = количество пар, чей эллипс проходит
через данную точку с невязкой δ < 3σ.
Принимает целые значения от 0 до N_pairs.
Максимальный score у истинного объекта = N_pairs (все пары согласны).
Ghost-цели имеют меньший score, т.к. согласуются только
с частью пар.
О компенсации затухания:
При генерации S(f) сигнал ослаблен:
geo(i,j) = 1/(R_Tx · R_Rx) — геометрическое ослабление
pat(i,j) = cos²(θ_Tx)·cos²(θ_Rx) — диаграмма направленности
Без компенсации глубокий/угловой отражатель будет недооценён.
Компенсация: делим вес каждого пика на ожидаемое затухание
в точке z_apparent, вычисленное для данной пары антенн.
Геометрия: карта строится в плоскости XZ при y=ELLIPS_PLANE_Y,
но бистатические дальности считаются до 3D-координат антенн.
"""
"""
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
# ══════════════════════════════════════════════════════
# 0.1 ИЗМЕНЯЕМЫЕ ПАРАМЕТРЫ
# ══════════════════════════════════════════════════════
INPUT_IDX = [0, 1, 2, 3]
OUTPUT_IDX = [0, 1]
# Частотный диапазон и глубинный gate.
F_START = 40 * 1e8
F_STOP = 60 * 1e8
MIN_DEPTH = 3.0
MAX_DEPTH = 14.5
# Движение радара между Tx-событиями.
# Новая схема измерения: один Tx излучает, все 4 Rx принимают одновременно;
# затем излучает следующий Tx. Поэтому motion correction имеет 2 временные точки,
# а не 8 последовательных точек для отдельных Tx/Rx-пар.
SPEED_M_S = 1.11
LOOK_ANGLE_DEG = 12.0 # угол между направлением движения и осью дальности Z
TX_SWEEP_TIME_S = 0.072 # время одного sweep для одного Tx-события
TX_SWITCH_TIME_S = 1e-5
APPLY_FREQ_PHASE_CORRECTION = True # True: корректировать движение внутри sweep до IFFT
# Данные и вычитание среднего фона.
# True: вычитать среднее по всем снимкам в папке, убирая прямую волну и статику.
# False: использовать данные как есть.
BG_SUBTRACT = True
BG_PATH = Path('/Users/ivan_root/Downloads/Telegram_dwnld/moving_26052026/20260526_10-6000_751_50k_triplet_fast_1/preprocessed')
DATA_PATH = Path('/Users/ivan_root/Downloads/Telegram_dwnld/moving_26052026/20260526_10-6000_751_50k_triplet_fast_1/preprocessed/0046_id47_ns3315537839930')
# Параметры поиска пиков и CLEAN по карте эллипсов.
MODE = 'point' # 'point' или 'extended'
SNR_THRESH = 3.8 # минимальный SNR пика A-скана
SNR_COMP_MAX = 25.0 # верхний предел компенсированного SNR
COMP_POWER = 0.22 # степень компенсации геометрического/углового затухания
MAX_OBJECTS = 15
# ══════════════════════════════════════════════════════
# 0.2 КОНФИГИ АНТЕНН И ДВИЖЕНИЯ
# ══════════════════════════════════════════════════════
# Физические координаты антенн по их реальным индексам, [м].
# Формат: x, (x, y) или (x, y, z). X - поперечная ось, Z - наклонная
# дальность вдоль оси радара, Y - привязанная к радару вертикальная ось.
# Карта строится в плоскости y=ELLIPS_PLANE_Y, но расстояния Tx/Rx считаются
# в полном 3D. Это даёт 2D проекцию бистатических эллипсов без 3D volume search.
ELLIPS_PLANE_Y = 0.0
TX_POSITIONS = {
0: (-75.0 * 0.01, 0.0, 0.0),
1: ( 75.0 * 0.01, 0.0, 0.0),
}
RX_POSITIONS = {
0: ( 19.0 * 0.01, 0.0, 0.0),
1: ( 45.0 * 0.01, 0.0, 0.0),
2: (-45.0 * 0.01, 0.0, 0.0),
3: (-19.0 * 0.01, 0.0, 0.0),
}
@dataclass
class MotionConfig:
"""
Конфигурация движения для кадра с параллельным приёмом по Rx.
speed_m_s:
Линейная скорость радара во время съёмки кадра.
look_angle_deg:
Угол между направлением движения и осью дальности Z.
Если движение почти вдоль дальности, ставьте угол близкий к 0°.
tx_sweep_time_s:
Время sweep для одного Tx-события. Все Rx этого Tx имеют один timestamp.
tx_switch_time_s:
Время переключения между соседними Tx-событиями.
pair_order_phys:
Порядок пар в физических индексах, как они записаны в данных:
[(tx_phys_1, rx_phys_1), (tx_phys_2, rx_phys_2), ...]
При расчёте движения пары с одинаковым tx_phys считаются одновременными.
reference_mode:
Относительно какого момента считаем dt:
- 'frame_center' : середина между первым и последним Tx-событием;
- 'first_tx_event' : центр первого Tx-события.
direction_sign:
Знак движения по оси дальности.
+1 -> более поздние Tx-события выглядят глубже;
-1 -> более поздние Tx-события выглядят ближе.
apply_freq_phase_correction:
True -> до IFFT компенсировать движение внутри одного Tx-sweep,
потому что частоты измеряются последовательно снизу вверх.
"""
speed_m_s: float = 0.0
look_angle_deg: float = 0.0
tx_sweep_time_s: float = 0.15
tx_switch_time_s: float = 1e-5
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
apply_freq_phase_correction: bool = True
MOTION_CONFIG = MotionConfig(
speed_m_s=SPEED_M_S,
look_angle_deg=LOOK_ANGLE_DEG,
tx_sweep_time_s=TX_SWEEP_TIME_S,
tx_switch_time_s=TX_SWITCH_TIME_S,
pair_order_phys=[
(0, 0), (0, 1), (0, 2), (0, 3),
(1, 0), (1, 1), (1, 2), (1, 3),
],
reference_mode='frame_center',
direction_sign=+1.0,
apply_freq_phase_correction=APPLY_FREQ_PHASE_CORRECTION,
)
# ══════════════════════════════════════════════════════
# 0.3 НЕИЗМЕНЯЕМЫЕ ПАРАМЕТРЫ (ЛУЧШЕ НЕ ТРОГАТЬ)
# ══════════════════════════════════════════════════════
eps_r = 1.0
v = 3e8 / np.sqrt(eps_r)
def positions_to_xyz(position_dict):
coords = []
for idx in sorted(position_dict):
pos = np.asarray(position_dict[idx], dtype=float)
if pos.ndim == 0:
pos = np.array([float(pos), 0.0, 0.0], dtype=float)
elif pos.shape == (2,):
pos = np.array([pos[0], pos[1], 0.0], dtype=float)
elif pos.shape != (3,):
raise ValueError(f'Позиция антенны {idx} должна быть x, (x,y) или (x,y,z), получено {pos}')
coords.append(pos)
return np.vstack(coords)
tx_xyz = positions_to_xyz(TX_POSITIONS)
rx_xyz = positions_to_xyz(RX_POSITIONS)
x_tx, y_tx, z_tx = tx_xyz.T
x_rx, y_rx, z_rx = rx_xyz.T
# Границы сетки аккумулятора в плоскости y=ELLIPS_PLANE_Y.
x_ant = np.concatenate([x_tx, x_rx])
x_min, x_max = x_ant.min() - 2.0, x_ant.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)
def range_to_tx(i_tx, X, Z, yy=ELLIPS_PLANE_Y):
return np.sqrt((X - x_tx[i_tx])**2 + (yy - y_tx[i_tx])**2 + (Z - z_tx[i_tx])**2)
def range_to_rx(i_rx, X, Z, yy=ELLIPS_PLANE_Y):
return np.sqrt((X - x_rx[i_rx])**2 + (yy - y_rx[i_rx])**2 + (Z - z_rx[i_rx])**2)
def bistatic_ranges(i_tx, i_rx, X, Z, yy=ELLIPS_PLANE_Y):
Rtx = range_to_tx(i_tx, X, Z, yy=yy)
Rrx = range_to_rx(i_rx, X, Z, yy=yy)
return Rtx, Rrx
def antenna_boresight_cos_z(R, z_ant, Z):
# В локальных координатах радара все антенны смотрят вдоль +Z.
return (Z - z_ant) / (R + 1e-12)
# Расстояния от 2D-сетки до 3D-координат антенн.
# Это не 3D поиск: мы строим карту только в плоскости y=ELLIPS_PLANE_Y,
# но каждая точка карты получает корректную бистатическую дальность в 3D.
R_tx_grid = {i: range_to_tx(i, XX, ZZ) for i in range(N_tx)}
R_rx_grid = {j: range_to_rx(j, XX, ZZ) 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]
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 _tx_event_order_from_pairs(pair_order_phys):
"""Возвращает порядок Tx-событий по первому появлению tx_phys в pair_order_phys."""
tx_events = []
for tx_phys, _ in pair_order_phys:
if tx_phys not in tx_events:
tx_events.append(tx_phys)
return tx_events
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 Tx-события и грубый pair shift.
Пары с одинаковым tx_phys получают одинаковое время, потому что Rx принимают
параллельно. t_start/t_center также используются для частотной phase correction
внутри sweep до IFFT.
"""
tx_phys_to_log, rx_phys_to_log = _build_phys_to_logical_maps(output_idx, input_idx)
expected_pairs = {(tx, rx) for tx in output_idx for rx in input_idx}
observed_pairs = set(config.pair_order_phys)
missing_pairs = expected_pairs - observed_pairs
extra_pairs = observed_pairs - expected_pairs
if missing_pairs:
raise ValueError(f"pair_order_phys не содержит пары: {sorted(missing_pairs)}")
if extra_pairs:
raise ValueError(f"pair_order_phys содержит лишние пары: {sorted(extra_pairs)}")
if len(config.pair_order_phys) != len(observed_pairs):
raise ValueError("pair_order_phys содержит повторяющиеся пары")
tx_event_order = _tx_event_order_from_pairs(config.pair_order_phys)
tx_event_timing = {}
for event_idx, tx_phys in enumerate(tx_event_order):
if tx_phys not in tx_phys_to_log:
raise ValueError(f"Tx {tx_phys} отсутствует в OUTPUT_IDX={output_idx}")
t_start = event_idx * (config.tx_sweep_time_s + config.tx_switch_time_s)
t_center = t_start + 0.5 * config.tx_sweep_time_s
t_stop = t_start + config.tx_sweep_time_s
tx_event_timing[tx_phys] = {
'tx_event_idx': event_idx,
't_start_s': t_start,
't_center_s': t_center,
't_stop_s': t_stop,
}
if not tx_event_order:
return {}, []
first_center = tx_event_timing[tx_event_order[0]]['t_center_s']
last_center = tx_event_timing[tx_event_order[-1]]['t_center_s']
if config.reference_mode == 'frame_center':
t_ref = 0.5 * (first_center + last_center)
elif config.reference_mode == 'first_tx_event':
t_ref = first_center
else:
raise ValueError("reference_mode must be 'frame_center' or 'first_tx_event'")
cos_theta = np.cos(np.radians(config.look_angle_deg))
pair_timestamps: Dict[Tuple[int, int], Dict] = {}
rows: List[Dict] = []
for order_idx, (tx_phys, rx_phys) in enumerate(config.pair_order_phys):
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]
event = tx_event_timing[tx_phys]
dt_ref = event['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 = {
'order_idx': order_idx,
'tx_event_idx': event['tx_event_idx'],
'tx_phys': tx_phys,
'rx_phys': rx_phys,
'i_tx': i_tx,
'i_rx': i_rx,
't_start_s': event['t_start_s'],
't_center_s': event['t_center_s'],
't_stop_s': event['t_stop_s'],
'dt_ref_s': dt_ref,
'dz_motion_m': dz_motion,
'dtau_motion_s': dtau_motion,
}
rows.append(row)
pair_timestamps[(i_tx, i_rx)] = row.copy()
return pair_timestamps, rows
def compute_frequency_sample_times(freq_full: np.ndarray,
f_start: float,
f_stop: float,
pair_info: Dict,
sweep_time_s: float) -> Tuple[np.ndarray, np.ndarray]:
"""Возвращает индексы выбранных частот и абсолютное время каждой точки sweep."""
mask = (freq_full >= f_start) & (freq_full <= f_stop)
idx = np.flatnonzero(mask)
if idx.size == 0:
raise ValueError('После частотной обрезки не осталось точек для phase correction')
if len(freq_full) < 2:
t_abs = np.full(idx.size, pair_info['t_center_s'], dtype=float)
else:
dt_freq = sweep_time_s / (len(freq_full) - 1)
t_abs = pair_info['t_start_s'] + idx * dt_freq
return idx, t_abs
def apply_intra_sweep_phase_correction(s21: np.ndarray,
freq_full: np.ndarray,
pair_info: Dict,
config: MotionConfig,
wave_speed: float,
f_start: float,
f_stop: float):
"""
Частотная motion correction до IFFT.
Частоты внутри одного Tx-sweep измеряются последовательно снизу вверх.
Каждая частотная точка имеет своё положение радара, поэтому приводим её
фазу к центру этого Tx-события:
phi(f) = 2*pi*f * 2*dz_intra / wave_speed
dz_intra = v_motion*cos(theta)*(t_freq - t_center_tx_event)
После этого A-скан строится уже из phase-corrected S21.
"""
s21_corr = np.array(s21, dtype=np.complex128, copy=True)
idx, t_abs = compute_frequency_sample_times(
freq_full=freq_full,
f_start=f_start,
f_stop=f_stop,
pair_info=pair_info,
sweep_time_s=config.tx_sweep_time_s,
)
dt_intra = t_abs - pair_info['t_center_s']
theta = np.radians(config.look_angle_deg)
delta_range = config.direction_sign * config.speed_m_s * np.cos(theta) * dt_intra
delta_path = 2.0 * delta_range
phi = 2.0 * np.pi * freq_full[idx] * delta_path / wave_speed
if config.apply_freq_phase_correction:
s21_corr[idx] *= np.exp(1j * phi)
meta = {
'enabled': bool(config.apply_freq_phase_correction),
'freq_idx': idx,
't_abs_s': t_abs,
'dt_intra_s': dt_intra,
'delta_range_m': delta_range,
'delta_path_m': delta_path,
'phi_rad': phi,
}
return s21_corr, meta
pair_timestamps, pair_timing_rows = compute_pair_timestamps(MOTION_CONFIG)
print("Вычисление A-сканов из реальных данных...", end=" ", flush=True)
A_RAW = {} # A_RAW[(i,j)] — A-скан без внутрисвиповой phase correction
A = {} # A[(i,j)] — phase-corrected A-скан, используемый дальше
T_h = {} # T_h[(i,j)] — временна́я ось для этой пары [с]
Z_h = {} # Z_h[(i,j)] — ось глубины [м]
phase_meta = {}
for (i, j), s21 in s21_data.items():
s21_proc = np.array(s21, dtype=np.complex128, copy=True)
# Вычитание фона в частотной области
if BG_SUBTRACT and background is not None and (i, j) in background:
s21_proc = s21_proc - background[(i, j)]
# Примечание: вычитаем до обрезки по частоте и до окна —
# фон вычисляется из полных (необрезанных) данных,
# поэтому вычитание корректно в полном частотном диапазоне.
if (i, j) not in pair_timestamps:
raise KeyError(f'Нет временной информации для пары {(i, j)}')
t_pair_raw, a_pair_raw = compute_ascan(s21_proc, freq_data[(i, j)],
f_start=F_START, f_stop=F_STOP)
s21_corr, meta = apply_intra_sweep_phase_correction(
s21=s21_proc,
freq_full=freq_data[(i, j)],
pair_info=pair_timestamps[(i, j)],
config=MOTION_CONFIG,
wave_speed=v,
f_start=F_START,
f_stop=F_STOP,
)
t_pair, a_pair = compute_ascan(s21_corr, 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_RAW[(i, j)] = a_pair_raw
A[(i, j)] = a_pair
phase_meta[(i, j)] = meta
bg_label = "с вычитанием фона" if BG_SUBTRACT else "без вычитания фона"
phase_label = "с внутрисвиповой phase correction" if MOTION_CONFIG.apply_freq_phase_correction else "без внутрисвиповой phase correction"
print(f"готово ({bg_label}, {phase_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, Rrx = bistatic_ranges(i_tx, i_rx, xc, z_app)
geo = 1.0 / (Rtx * Rrx + 1e-12)
cos_tx = antenna_boresight_cos_z(Rtx, z_tx[i_tx], z_app)
cos_rx = antenna_boresight_cos_z(Rrx, z_rx[i_rx], z_app)
pat = cos_tx**2 * cos_rx**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. Tx2 излучает, Rx0..Rx3 принимают одновременно.
2. После переключения Tx3 излучает, Rx0..Rx3 снова принимают одновременно.
3. Поэтому timestamp задаётся не для 8 отдельных пар, а для 2 Tx-событий.
4. Для уже найденных пиков формируем motion-corrected версию:
tau_corr, z_corr
Это first-order модель: считаем, что весь sweep одного Tx-события имеет один
центр времени. Если смещение внутри sweep станет заметным, следующим шагом
нужна per-frequency коррекция до IFFT.
"""
# compute_pair_timestamps(...) уже определён выше, потому что phase correction нужна до IFFT.
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_phase, z_phase — пик после внутрисвиповой phase correction;
tau_corr, z_corr — тот же пик после грубого сдвига Tx-события;
dz_motion, dtau_motion.
Для совместимости также оставляем aliases tau_raw/z_app_raw, но здесь
raw означает 'до грубого pair shift', а не до phase correction.
"""
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_phase = float(pk['tau'])
z_phase = float(pk['z_app'])
tau_corr = tau_phase + dtau_motion
z_corr = 0.5 * v * tau_corr
pk_corr = dict(pk)
pk_corr.update({
'tau_phase': tau_phase,
'z_phase': z_phase,
'tau_raw': tau_phase, # alias для старых диагностических блоков
'z_app_raw': z_phase, # alias: пик до грубого pair shift
'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, Rr = bistatic_ranges(i, j, x_est, z_est)
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(sum(bistatic_ranges(i, j, x_est, z_est)) -
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, z_tx*100, 'r^', ms=10, label='Tx (XZ projection)', zorder=5)
ax.plot(x_rx*100, z_rx*100, 'bv', ms=10, label='Rx (XZ projection)', 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()
plt.show()