231 lines
9.9 KiB
Python
231 lines
9.9 KiB
Python
"""Причинное сопровождение несущей BPSK после известной преамбулы.
|
|
|
|
Модуль не разбирает заголовок, полезную нагрузку, FEC или CRC. Начальное
|
|
состояние P3 оценивается только по переданным в функцию символам преамбулы.
|
|
После неё T2 получает текущий комплексный символ, собственное решение и своё
|
|
предыдущее состояние.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
from dataclasses import dataclass
|
|
import math
|
|
|
|
import numpy as np
|
|
|
|
|
|
PREAMBLE_SYMBOLS = 1280
|
|
P3_BLOCK_SYMBOLS = 20
|
|
NORMALIZED_LOOP_BANDWIDTH = 0.0005
|
|
DAMPING_FACTOR = 1.0 / math.sqrt(2.0)
|
|
DETECTOR_CLIP_RAD = math.pi / 3.0
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class P3CarrierEstimate:
|
|
"""Начальное состояние в физических единицах при времени символа 0."""
|
|
|
|
frequency_hz: float
|
|
gain: complex
|
|
phase_fit_rmse_rad: float
|
|
squared_coherence: float
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class T2Parameters:
|
|
"""Коэффициенты петли на один символ."""
|
|
|
|
normalized_bandwidth: float
|
|
damping_factor: float
|
|
phase_coefficient: float
|
|
frequency_coefficient: float
|
|
detector_clip_rad: float
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class T2TrackingResult:
|
|
"""Применённое состояние и решения для одного последовательного прохода."""
|
|
|
|
corrected: np.ndarray
|
|
hard_decisions: np.ndarray
|
|
phase_rad: np.ndarray
|
|
frequency_hz: np.ndarray
|
|
detector_error_rad: np.ndarray
|
|
initial_estimate: P3CarrierEstimate
|
|
parameters: T2Parameters
|
|
|
|
|
|
def t2_parameters(
|
|
normalized_bandwidth: float = NORMALIZED_LOOP_BANDWIDTH,
|
|
damping_factor: float = DAMPING_FACTOR,
|
|
) -> T2Parameters:
|
|
"""Перевести нормированную полосу в коэффициенты Lab060 на символ."""
|
|
|
|
if not np.isfinite(normalized_bandwidth) or normalized_bandwidth <= 0.0:
|
|
raise ValueError("Нормированная полоса должна быть положительной")
|
|
if not np.isfinite(damping_factor) or damping_factor <= 0.0:
|
|
raise ValueError("Коэффициент затухания должен быть положительным")
|
|
theta = normalized_bandwidth / (
|
|
damping_factor + 0.25 / damping_factor
|
|
)
|
|
denominator = 1.0 + 2.0 * damping_factor * theta + theta * theta
|
|
return T2Parameters(
|
|
normalized_bandwidth=float(normalized_bandwidth),
|
|
damping_factor=float(damping_factor),
|
|
phase_coefficient=float(4.0 * damping_factor * theta / denominator),
|
|
frequency_coefficient=float(4.0 * theta * theta / denominator),
|
|
detector_clip_rad=DETECTOR_CLIP_RAD,
|
|
)
|
|
|
|
|
|
def _huber_phase_line(
|
|
block_values: np.ndarray,
|
|
block_times_seconds: np.ndarray,
|
|
) -> tuple[float, float, float, float]:
|
|
"""Повторить зафиксированную в P3 устойчивую прямую фазы."""
|
|
|
|
values = np.asarray(block_values, dtype=np.complex128)
|
|
times = np.asarray(block_times_seconds, dtype=np.float64)
|
|
if values.ndim != 1 or times.ndim != 1 or len(values) != len(times) or len(values) < 3:
|
|
raise ValueError("Недостаточно фазовых блоков")
|
|
phases = np.unwrap(np.angle(values))
|
|
base_weights = np.maximum(np.abs(values), 1e-12)
|
|
center_time = float(np.sum(base_weights * times) / np.sum(base_weights))
|
|
centered = times - center_time
|
|
design = np.column_stack((np.ones(len(times)), centered))
|
|
weights = base_weights.copy()
|
|
coefficients = np.zeros(2, dtype=np.float64)
|
|
for _iteration in range(12):
|
|
root = np.sqrt(weights / max(float(np.max(weights)), 1e-30))
|
|
updated, *_unused = np.linalg.lstsq(
|
|
design * root[:, None], phases * root, rcond=None
|
|
)
|
|
residual = phases - design @ updated
|
|
median = float(np.median(residual))
|
|
scale = 1.4826 * float(np.median(np.abs(residual - median))) + 1e-9
|
|
limit = 1.345 * scale
|
|
robust = np.ones_like(residual)
|
|
outside = np.abs(residual) > limit
|
|
robust[outside] = limit / np.abs(residual[outside])
|
|
weights = base_weights * robust
|
|
if np.allclose(updated, coefficients, rtol=0.0, atol=1e-12):
|
|
coefficients = updated
|
|
break
|
|
coefficients = updated
|
|
residual = phases - design @ coefficients
|
|
rmse = float(np.sqrt(np.sum(base_weights * residual**2) / np.sum(base_weights)))
|
|
phase_at_zero = float(coefficients[0] - coefficients[1] * center_time)
|
|
coherence = float(abs(np.sum(values)) / max(float(np.sum(abs(values))), 1e-30))
|
|
return phase_at_zero, float(coefficients[1]), rmse, coherence
|
|
|
|
|
|
def estimate_p3(
|
|
preamble_symbols: np.ndarray,
|
|
known_preamble_symbols: np.ndarray,
|
|
symbol_rate: float,
|
|
) -> P3CarrierEstimate:
|
|
"""Оценить P3 только по полной 1280-символьной преамбуле."""
|
|
|
|
received = np.asarray(preamble_symbols, dtype=np.complex128)
|
|
known = np.asarray(known_preamble_symbols, dtype=np.complex128)
|
|
if received.shape != (PREAMBLE_SYMBOLS,) or known.shape != received.shape:
|
|
raise ValueError("P3 требуется ровно 1280 принятых и известных символов")
|
|
if not np.isfinite(symbol_rate) or symbol_rate <= 0.0:
|
|
raise ValueError("Скорость символов должна быть положительной")
|
|
if not np.all(np.isfinite(received)) or not np.all(np.isfinite(known)):
|
|
raise ValueError("P3 не принимает NaN или Inf")
|
|
|
|
indexes = np.arange(PREAMBLE_SYMBOLS, dtype=np.int64)
|
|
block_count = PREAMBLE_SYMBOLS // P3_BLOCK_SYMBOLS
|
|
block_values = np.mean(
|
|
(received**2).reshape(block_count, P3_BLOCK_SYMBOLS), axis=1
|
|
)
|
|
block_times = np.mean(
|
|
((indexes + 0.5) / symbol_rate).reshape(block_count, P3_BLOCK_SYMBOLS),
|
|
axis=1,
|
|
)
|
|
_phase0, squared_slope, rmse, coherence = _huber_phase_line(
|
|
block_values, block_times
|
|
)
|
|
frequency_hz = float(squared_slope / (4.0 * math.pi))
|
|
times = (indexes + 0.5) / symbol_rate
|
|
gain = complex(np.mean(
|
|
received
|
|
* np.conj(known)
|
|
* np.exp(-2j * math.pi * frequency_hz * times)
|
|
))
|
|
if not np.isfinite(gain.real) or not np.isfinite(gain.imag) or abs(gain) == 0.0:
|
|
raise ValueError("P3 получил нулевой или неконечный коэффициент")
|
|
return P3CarrierEstimate(frequency_hz, gain, rmse, coherence)
|
|
|
|
|
|
def normalized_phase_detector(
|
|
corrected_symbol: complex,
|
|
detector_clip_rad: float = DETECTOR_CLIP_RAD,
|
|
) -> tuple[int, float]:
|
|
"""Вернуть собственное решение BPSK и ограниченную фазовую ошибку."""
|
|
|
|
value = complex(corrected_symbol)
|
|
if not np.isfinite(value.real) or not np.isfinite(value.imag):
|
|
raise ValueError("Фазовый детектор не принимает NaN или Inf")
|
|
hard = 1 if value.real >= 0.0 else -1
|
|
error = math.atan2(value.imag * hard, value.real * hard)
|
|
return hard, float(np.clip(error, -detector_clip_rad, detector_clip_rad))
|
|
|
|
|
|
def track_t2(
|
|
symbols: np.ndarray,
|
|
symbol_rate: float,
|
|
initial_estimate: P3CarrierEstimate,
|
|
*,
|
|
start_index: int = PREAMBLE_SYMBOLS,
|
|
parameters: T2Parameters | None = None,
|
|
) -> T2TrackingResult:
|
|
"""Последовательно применить T2; будущие символы не используются."""
|
|
|
|
values = np.asarray(symbols, dtype=np.complex128)
|
|
if values.ndim != 1 or not len(values) or not np.all(np.isfinite(values)):
|
|
raise ValueError("T2 требуется конечная одномерная последовательность")
|
|
if not np.isfinite(symbol_rate) or symbol_rate <= 0.0:
|
|
raise ValueError("Скорость символов должна быть положительной")
|
|
if not 0 <= start_index <= len(values):
|
|
raise ValueError("Некорректная граница начала сопровождения")
|
|
if abs(initial_estimate.gain) == 0.0:
|
|
raise ValueError("Начальный коэффициент P3 не может быть нулевым")
|
|
selected = t2_parameters() if parameters is None else parameters
|
|
|
|
corrected = np.empty(len(values), dtype=np.complex128)
|
|
decisions = np.empty(len(values), dtype=np.int8)
|
|
phases = np.empty(len(values), dtype=np.float64)
|
|
frequencies = np.empty(len(values), dtype=np.float64)
|
|
detector = np.empty(len(values), dtype=np.float64)
|
|
omega = 2.0 * math.pi * initial_estimate.frequency_hz / symbol_rate
|
|
phase0 = float(np.angle(initial_estimate.gain))
|
|
phase = phase0 + omega * (start_index + 0.5)
|
|
amplitude = abs(initial_estimate.gain)
|
|
|
|
for index, symbol in enumerate(values):
|
|
current_phase = phase0 + omega * (index + 0.5) if index < start_index else phase
|
|
value = symbol * np.exp(-1j * current_phase) / amplitude
|
|
hard, error = normalized_phase_detector(value, selected.detector_clip_rad)
|
|
corrected[index] = value
|
|
decisions[index] = hard
|
|
phases[index] = current_phase
|
|
frequencies[index] = omega * symbol_rate / (2.0 * math.pi)
|
|
detector[index] = error
|
|
if index >= start_index:
|
|
phase = phase + omega + selected.phase_coefficient * error
|
|
omega = omega + selected.frequency_coefficient * error
|
|
|
|
for array in (corrected, decisions, phases, frequencies, detector):
|
|
array.flags.writeable = False
|
|
return T2TrackingResult(
|
|
corrected=corrected,
|
|
hard_decisions=decisions,
|
|
phase_rad=phases,
|
|
frequency_hz=frequencies,
|
|
detector_error_rad=detector,
|
|
initial_estimate=initial_estimate,
|
|
parameters=selected,
|
|
)
|