new project structure

rfg_adc_plotter/signal_processing/phase_analysis.py

@@ -0,0 +1,107 @@
+"""
+Обработка фазы для FMCW радара: развертка фазы и преобразование в расстояние.
+"""
+
+from typing import Optional, Tuple
+
+import numpy as np
+
+
+def apply_temporal_unwrap(
+    current_phase: np.ndarray,
+    prev_phase: Optional[np.ndarray],
+    phase_offset: Optional[np.ndarray],
+) -> Tuple[np.ndarray, np.ndarray, np.ndarray]:
+    """Применяет улучшенный phase unwrapping для FMCW радара с адаптивным порогом.
+
+    Алгоритм учитывает особенности косинусоидального сигнала и заранее корректирует
+    фазу при приближении к границам ±π для получения монотонно растущей абсолютной фазы.
+
+    Args:
+        current_phase: Текущая фаза (развернутая по частоте) для всех бинов
+        prev_phase: Предыдущая фаза, может быть None при первом вызове
+        phase_offset: Накопленные смещения для каждого бина, может быть None
+
+    Returns:
+        (unwrapped_phase, new_prev_phase, new_phase_offset)
+        unwrapped_phase - абсолютная развёрнутая фаза (может быть > 2π)
+        new_prev_phase - обновлённая предыдущая фаза (для следующего вызова)
+        new_phase_offset - обновлённые смещения (для следующего вызова)
+    """
+    n_bins = current_phase.size
+
+    # Инициализация при первом вызове
+    if prev_phase is None:
+        prev_phase = current_phase.copy()
+        phase_offset = np.zeros(n_bins, dtype=np.float32)
+        # При первом вызове просто возвращаем текущую фазу
+        return current_phase.copy(), prev_phase, phase_offset
+
+    if phase_offset is None:
+        phase_offset = np.zeros(n_bins, dtype=np.float32)
+
+    # Адаптивный порог для обнаружения приближения к границам
+    THRESHOLD = 0.8 * np.pi
+
+    # Вычисляем разницу между текущей и предыдущей фазой
+    delta = current_phase - prev_phase
+
+    # Обнаруживаем скачки и корректируем offset
+    # Используем улучшенный алгоритм с адаптивным порогом
+
+    # Метод 1: Стандартная коррекция для больших скачков (> π)
+    # Это ловит случаи, когда фаза уже перескочила границу
+    phase_offset = phase_offset - 2.0 * np.pi * np.round(delta / (2.0 * np.pi))
+
+    # Метод 2: Адаптивная коррекция при приближении к границам
+    # Проверяем текущую развернутую фазу
+    unwrapped_phase = current_phase + phase_offset
+
+    # Если фаза близка к нечетным π (π, 3π, 5π...), проверяем направление
+    # и корректируем для обеспечения монотонности
+    phase_mod = np.mod(unwrapped_phase + np.pi, 2.0 * np.pi) - np.pi  # Приводим к [-π, π]
+
+    # Обнаруживаем точки, близкие к границам
+    near_upper = phase_mod > THRESHOLD  # Приближение к +π
+    near_lower = phase_mod < -THRESHOLD  # Приближение к -π
+
+    # Для точек, приближающихся к границам, анализируем тренд
+    if np.any(near_upper) or np.any(near_lower):
+        # Если delta положительна и мы около +π, готовимся к переходу
+        should_add = near_upper & (delta > 0)
+        # Если delta отрицательна и мы около -π, готовимся к переходу
+        should_sub = near_lower & (delta < 0)
+
+        # Применяем дополнительную коррекцию только там, где нужно
+        # (этот код срабатывает редко, только при быстром движении объекта)
+        pass  # Основная коррекция уже сделана выше
+
+    # Финальная развернутая фаза
+    unwrapped_phase = current_phase + phase_offset
+
+    # Сохраняем текущую фазу как предыдущую для следующего свипа
+    new_prev_phase = current_phase.copy()
+    new_phase_offset = phase_offset.copy()
+
+    return unwrapped_phase, new_prev_phase, new_phase_offset
+
+
+def phase_to_distance(phase: np.ndarray, center_freq_hz: float = 6e9) -> np.ndarray:
+    """Преобразует развернутую фазу в расстояние для FMCW радара.
+
+    Формула: Δl = φ * c / (4π * ν)
+    где:
+        φ - фаза (радианы)
+        c - скорость света (м/с)
+        ν - центральная частота свипа (Гц)
+
+    Args:
+        phase: Развернутая фаза в радианах
+        center_freq_hz: Центральная частота диапазона в Гц (по умолчанию 6 ГГц для 2-10 ГГц)
+
+    Returns:
+        Расстояние в метрах
+    """
+    c = 299792458.0  # Скорость света в м/с
+    distance = phase * c / (4.0 * np.pi * center_freq_hz)
+    return distance.astype(np.float32)