mirror of
https://github.com/NawfalMotii79/PLFM_RADAR.git
synced 2026-06-08 22:47:16 +00:00
Jason
5a7e8b8689
Add host-side 3-PRI Chinese-Remainder velocity unfolding and a cluster extractor that reads the 48-bin Doppler frame, splits it into the 3 sub-frames (SHORT/MEDIUM/LONG), and resolves Doppler aliases across coprime PRIs. Resolves the algorithm half of audit C-5; the data is now in extract_targets_from_frame_crt's hands but workers still call the legacy single-PRI extractor (PR-Q.6 wires it). v7/processing.py: - unfold_velocity_crt(v_meas, v_unamb, v_res, max_alias_k=6, tol_factor=0.5) -> (v_est, confidence, alias_set). Brute-force candidate search over PRI-0 fold depth, per-PRI half-bin tolerance. Confidence: CONFIRMED (3-PRI unique), LIKELY (3-PRI with 2 cands, or 2-PRI with unique cand), AMBIGUOUS (1-PRI, 3+ cands, 2-PRI multi-cand, or no fold within tol). - extract_targets_from_frame_crt(frame, waveform, gps, max_alias_k): groups detections by range bin, picks strongest bin per (rbin, sf), decodes signed Doppler via sub_frame = dbin // 16 / bin_in_sf = dbin % 16, calls unfold_velocity_crt, attaches velocity_confidence and alias_set to RadarTarget. Falls back to legacy extract_targets_from_frame for non-48-bin frames. v7/models.py: - RadarTarget gains velocity_confidence (str default "UNKNOWN") and alias_set (list[float] | None). v7/__init__.py: - Re-exports unfold_velocity_crt + extract_targets_from_frame_crt. test_v7.py (16 new tests, 0 failures): - TestUnfoldVelocityCRT (8): zero-velocity CONFIRMED, below per-PRI v_unamb CONFIRMED, above per-PRI (100 m/s) CONFIRMED, near CRT ceiling (~261 m/s) CONFIRMED, negative velocity, 1-PRI AMBIGUOUS, 2-PRI LIKELY, inconsistent measurements AMBIGUOUS+fallback. - TestExtractTargetsFromFrameCrt (8): 3-PRI CONFIRMED target, LONG-only AMBIGUOUS (the 20-km blindspot regime), 2-PRI LIKELY, strongest-bin picking, two targets at distinct ranges, legacy 32-bin frame fallback, no-detections empty, GPS georef. Local: test_v7 100/0/0 (9 graceful skips), test_GUI_V65_Tk 117/0/2.
773 lines
27 KiB
Python
773 lines
27 KiB
Python
"""
|
||
v7.processing — Radar signal processing and GPS parsing.
|
||
|
||
Classes:
|
||
- RadarProcessor — dual-CPI fusion, multi-PRF unwrap, DBSCAN clustering,
|
||
association, Kalman tracking
|
||
- USBPacketParser — parse GPS text/binary frames from STM32 CDC
|
||
|
||
Note: RadarPacketParser (old A5/C3 sync + CRC16 format) was removed.
|
||
All packet parsing now uses production RadarProtocol (0xAA/0xBB format)
|
||
from radar_protocol.py.
|
||
"""
|
||
|
||
import struct
|
||
import time
|
||
import logging
|
||
import math
|
||
|
||
import numpy as np
|
||
|
||
from .models import (
|
||
RadarTarget, GPSData, ProcessingConfig,
|
||
SCIPY_AVAILABLE, SKLEARN_AVAILABLE, FILTERPY_AVAILABLE,
|
||
)
|
||
|
||
if SKLEARN_AVAILABLE:
|
||
from sklearn.cluster import DBSCAN
|
||
|
||
if FILTERPY_AVAILABLE:
|
||
from filterpy.kalman import KalmanFilter
|
||
|
||
if SCIPY_AVAILABLE:
|
||
from scipy.signal import windows as scipy_windows
|
||
|
||
logger = logging.getLogger(__name__)
|
||
|
||
|
||
# =============================================================================
|
||
# Utility: pitch correction (Bug #4 fix — was never defined in V6)
|
||
# =============================================================================
|
||
|
||
def apply_pitch_correction(raw_elevation: float, pitch: float) -> float:
|
||
"""
|
||
Apply platform pitch correction to a raw elevation angle.
|
||
|
||
Returns the corrected elevation = raw_elevation - pitch.
|
||
"""
|
||
return raw_elevation - pitch
|
||
|
||
|
||
# =============================================================================
|
||
# Radar Processor — signal-level processing & tracking pipeline
|
||
# =============================================================================
|
||
|
||
class RadarProcessor:
|
||
"""Full radar processing pipeline: fusion, clustering, association, tracking."""
|
||
|
||
def __init__(self):
|
||
self.range_doppler_map = np.zeros((1024, 32))
|
||
self.detected_targets: list[RadarTarget] = []
|
||
self.track_id_counter: int = 0
|
||
self.tracks: dict[int, dict] = {}
|
||
self.frame_count: int = 0
|
||
self.config = ProcessingConfig()
|
||
|
||
# MTI state: store previous frames for cancellation
|
||
self._mti_history: list[np.ndarray] = []
|
||
|
||
# ---- Configuration -----------------------------------------------------
|
||
|
||
def set_config(self, config: ProcessingConfig):
|
||
"""Update the processing configuration and reset MTI history if needed."""
|
||
old_order = self.config.mti_order
|
||
self.config = config
|
||
if config.mti_order != old_order:
|
||
self._mti_history.clear()
|
||
|
||
# ---- Windowing ----------------------------------------------------------
|
||
|
||
@staticmethod
|
||
def apply_window(data: np.ndarray, window_type: str) -> np.ndarray:
|
||
"""Apply a window function along each column (slow-time dimension).
|
||
|
||
*data* shape: (range_bins, doppler_bins). Window is applied along
|
||
axis-1 (Doppler / slow-time).
|
||
"""
|
||
if window_type == "None" or not window_type:
|
||
return data
|
||
|
||
n = data.shape[1]
|
||
if n < 2:
|
||
return data
|
||
|
||
if SCIPY_AVAILABLE:
|
||
wtype = window_type.lower()
|
||
if wtype == "hann":
|
||
w = scipy_windows.hann(n, sym=False)
|
||
elif wtype == "hamming":
|
||
w = scipy_windows.hamming(n, sym=False)
|
||
elif wtype == "blackman":
|
||
w = scipy_windows.blackman(n)
|
||
elif wtype == "kaiser":
|
||
w = scipy_windows.kaiser(n, beta=14)
|
||
elif wtype == "chebyshev":
|
||
w = scipy_windows.chebwin(n, at=80)
|
||
else:
|
||
w = np.ones(n)
|
||
else:
|
||
# Fallback: numpy Hann
|
||
wtype = window_type.lower()
|
||
if wtype == "hann":
|
||
w = np.hanning(n)
|
||
elif wtype == "hamming":
|
||
w = np.hamming(n)
|
||
elif wtype == "blackman":
|
||
w = np.blackman(n)
|
||
else:
|
||
w = np.ones(n)
|
||
|
||
return data * w[np.newaxis, :]
|
||
|
||
# ---- DC Notch (zero-Doppler removal) ------------------------------------
|
||
|
||
@staticmethod
|
||
def dc_notch(data: np.ndarray) -> np.ndarray:
|
||
"""Remove the DC (zero-Doppler) component by subtracting the
|
||
mean along the slow-time axis for each range bin."""
|
||
return data - np.mean(data, axis=1, keepdims=True)
|
||
|
||
# ---- MTI (Moving Target Indication) -------------------------------------
|
||
|
||
def mti_filter(self, frame: np.ndarray) -> np.ndarray:
|
||
"""Apply MTI cancellation of order 1, 2, or 3.
|
||
|
||
Order-1: y[n] = x[n] - x[n-1]
|
||
Order-2: y[n] = x[n] - 2*x[n-1] + x[n-2]
|
||
Order-3: y[n] = x[n] - 3*x[n-1] + 3*x[n-2] - x[n-3]
|
||
|
||
The internal history buffer stores up to 3 previous frames.
|
||
"""
|
||
order = self.config.mti_order
|
||
self._mti_history.append(frame.copy())
|
||
|
||
# Trim history to order + 1 frames
|
||
max_len = order + 1
|
||
if len(self._mti_history) > max_len:
|
||
self._mti_history = self._mti_history[-max_len:]
|
||
|
||
if len(self._mti_history) < order + 1:
|
||
# Not enough history yet — return zeros (suppress output)
|
||
return np.zeros_like(frame)
|
||
|
||
h = self._mti_history
|
||
if order == 1:
|
||
return h[-1] - h[-2]
|
||
if order == 2:
|
||
return h[-1] - 2.0 * h[-2] + h[-3]
|
||
if order == 3:
|
||
return h[-1] - 3.0 * h[-2] + 3.0 * h[-3] - h[-4]
|
||
return h[-1] - h[-2]
|
||
|
||
# ---- CFAR (Constant False Alarm Rate) -----------------------------------
|
||
|
||
@staticmethod
|
||
def cfar_1d(signal_vec: np.ndarray, guard: int, train: int,
|
||
threshold_factor: float, cfar_type: str = "CA-CFAR") -> np.ndarray:
|
||
"""1-D CFAR detector.
|
||
|
||
Parameters
|
||
----------
|
||
signal_vec : 1-D array (power in linear scale)
|
||
guard : number of guard cells on each side
|
||
train : number of training cells on each side
|
||
threshold_factor : multiplier on estimated noise level
|
||
cfar_type : CA-CFAR, OS-CFAR, GO-CFAR, or SO-CFAR
|
||
|
||
Returns
|
||
-------
|
||
detections : boolean array, True where target detected
|
||
"""
|
||
n = len(signal_vec)
|
||
detections = np.zeros(n, dtype=bool)
|
||
half = guard + train
|
||
|
||
for i in range(half, n - half):
|
||
# Leading training cells
|
||
lead = signal_vec[i - half: i - guard]
|
||
# Lagging training cells
|
||
lag = signal_vec[i + guard + 1: i + half + 1]
|
||
|
||
if cfar_type == "CA-CFAR":
|
||
noise = (np.sum(lead) + np.sum(lag)) / (2 * train)
|
||
elif cfar_type == "GO-CFAR":
|
||
noise = max(np.mean(lead), np.mean(lag))
|
||
elif cfar_type == "SO-CFAR":
|
||
noise = min(np.mean(lead), np.mean(lag))
|
||
elif cfar_type == "OS-CFAR":
|
||
all_train = np.concatenate([lead, lag])
|
||
all_train.sort()
|
||
k = int(0.75 * len(all_train)) # 75th percentile
|
||
noise = all_train[min(k, len(all_train) - 1)]
|
||
else:
|
||
noise = (np.sum(lead) + np.sum(lag)) / (2 * train)
|
||
|
||
threshold = noise * threshold_factor
|
||
if signal_vec[i] > threshold:
|
||
detections[i] = True
|
||
|
||
return detections
|
||
|
||
def cfar_2d(self, rdm: np.ndarray) -> np.ndarray:
|
||
"""Apply 1-D CFAR along each range bin (across Doppler dimension).
|
||
|
||
Returns a boolean mask of the same shape as *rdm*.
|
||
"""
|
||
cfg = self.config
|
||
mask = np.zeros_like(rdm, dtype=bool)
|
||
for r in range(rdm.shape[0]):
|
||
row = rdm[r, :]
|
||
if row.max() > 0:
|
||
mask[r, :] = self.cfar_1d(
|
||
row, cfg.cfar_guard_cells, cfg.cfar_training_cells,
|
||
cfg.cfar_threshold_factor, cfg.cfar_type,
|
||
)
|
||
return mask
|
||
|
||
# ---- Full processing pipeline -------------------------------------------
|
||
|
||
def process_frame(self, raw_frame: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
|
||
"""Run the full signal processing chain on a Range x Doppler frame.
|
||
|
||
Parameters
|
||
----------
|
||
raw_frame : 2-D array (range_bins x doppler_bins), complex or real
|
||
|
||
Returns
|
||
-------
|
||
(processed_rdm, detection_mask)
|
||
processed_rdm — processed Range-Doppler map (power, linear)
|
||
detection_mask — boolean mask of CFAR / threshold detections
|
||
"""
|
||
cfg = self.config
|
||
data = raw_frame.astype(np.float64)
|
||
|
||
# 1. DC Notch
|
||
if cfg.dc_notch_enabled:
|
||
data = self.dc_notch(data)
|
||
|
||
# 2. Windowing (before FFT — applied along slow-time axis)
|
||
if cfg.window_type and cfg.window_type != "None":
|
||
data = self.apply_window(data, cfg.window_type)
|
||
|
||
# 3. MTI
|
||
if cfg.mti_enabled:
|
||
data = self.mti_filter(data)
|
||
|
||
# 4. Power (magnitude squared)
|
||
power = np.abs(data) ** 2
|
||
power = np.maximum(power, 1e-20) # avoid log(0)
|
||
|
||
# 5. CFAR detection or simple threshold
|
||
if cfg.cfar_enabled:
|
||
detection_mask = self.cfar_2d(power)
|
||
else:
|
||
# Simple threshold: convert dB threshold to linear
|
||
power_db = 10.0 * np.log10(power)
|
||
noise_floor = np.median(power_db)
|
||
detection_mask = power_db > (noise_floor + cfg.detection_threshold_db)
|
||
|
||
# Update stored RDM
|
||
self.range_doppler_map = power
|
||
self.frame_count += 1
|
||
|
||
return power, detection_mask
|
||
|
||
# ---- Dual-CPI fusion ---------------------------------------------------
|
||
|
||
@staticmethod
|
||
def dual_cpi_fusion(range_profiles_1: np.ndarray,
|
||
range_profiles_2: np.ndarray) -> np.ndarray:
|
||
"""Dual-CPI fusion for better detection."""
|
||
return np.mean(range_profiles_1, axis=0) + np.mean(range_profiles_2, axis=0)
|
||
|
||
# ---- DBSCAN clustering -------------------------------------------------
|
||
|
||
@staticmethod
|
||
def clustering(detections: list[RadarTarget],
|
||
eps: float = 100, min_samples: int = 2) -> list:
|
||
"""DBSCAN clustering of detections (requires sklearn)."""
|
||
if not SKLEARN_AVAILABLE or len(detections) == 0:
|
||
return []
|
||
|
||
points = np.array([[d.range, d.velocity] for d in detections])
|
||
labels = DBSCAN(eps=eps, min_samples=min_samples).fit(points).labels_
|
||
|
||
clusters = []
|
||
for label in set(labels):
|
||
if label == -1:
|
||
continue
|
||
cluster_points = points[labels == label]
|
||
clusters.append({
|
||
"center": np.mean(cluster_points, axis=0),
|
||
"points": cluster_points,
|
||
"size": len(cluster_points),
|
||
})
|
||
return clusters
|
||
|
||
# ---- Association -------------------------------------------------------
|
||
|
||
def association(self, detections: list[RadarTarget],
|
||
_clusters: list) -> list[RadarTarget]:
|
||
"""Associate detections to existing tracks (nearest-neighbour)."""
|
||
associated = []
|
||
for det in detections:
|
||
best_track = None
|
||
min_dist = float("inf")
|
||
for tid, track in self.tracks.items():
|
||
dist = math.sqrt(
|
||
(det.range - track["state"][0]) ** 2
|
||
+ (det.velocity - track["state"][2]) ** 2
|
||
)
|
||
if dist < min_dist and dist < 500:
|
||
min_dist = dist
|
||
best_track = tid
|
||
|
||
if best_track is not None:
|
||
det.track_id = best_track
|
||
else:
|
||
det.track_id = self.track_id_counter
|
||
self.track_id_counter += 1
|
||
|
||
associated.append(det)
|
||
return associated
|
||
|
||
# ---- Kalman tracking ---------------------------------------------------
|
||
|
||
def tracking(self, associated_detections: list[RadarTarget]):
|
||
"""Kalman filter tracking (requires filterpy)."""
|
||
if not FILTERPY_AVAILABLE:
|
||
return
|
||
|
||
now = time.time()
|
||
|
||
for det in associated_detections:
|
||
if det.track_id not in self.tracks:
|
||
kf = KalmanFilter(dim_x=4, dim_z=2)
|
||
kf.x = np.array([det.range, 0, det.velocity, 0])
|
||
kf.F = np.array([
|
||
[1, 1, 0, 0],
|
||
[0, 1, 0, 0],
|
||
[0, 0, 1, 1],
|
||
[0, 0, 0, 1],
|
||
])
|
||
kf.H = np.array([
|
||
[1, 0, 0, 0],
|
||
[0, 0, 1, 0],
|
||
])
|
||
kf.P *= 1000
|
||
kf.R = np.diag([10, 1])
|
||
kf.Q = np.eye(4) * 0.1
|
||
|
||
self.tracks[det.track_id] = {
|
||
"filter": kf,
|
||
"state": kf.x,
|
||
"last_update": now,
|
||
"hits": 1,
|
||
}
|
||
else:
|
||
track = self.tracks[det.track_id]
|
||
track["filter"].predict()
|
||
track["filter"].update([det.range, det.velocity])
|
||
track["state"] = track["filter"].x
|
||
track["last_update"] = now
|
||
track["hits"] += 1
|
||
|
||
# Prune stale tracks (> 5 s without update)
|
||
stale = [tid for tid, t in self.tracks.items()
|
||
if now - t["last_update"] > 5.0]
|
||
for tid in stale:
|
||
del self.tracks[tid]
|
||
|
||
|
||
# =============================================================================
|
||
# USB / GPS Packet Parser
|
||
# =============================================================================
|
||
|
||
class USBPacketParser:
|
||
"""
|
||
Parse GPS (and general) data arriving from the STM32 via USB CDC.
|
||
|
||
Supports:
|
||
- Text format: ``GPS:lat,lon,alt,pitch\\r\\n``
|
||
- Binary format: ``GPSB`` header, 30 bytes total
|
||
"""
|
||
|
||
def __init__(self):
|
||
pass
|
||
|
||
def parse_gps_data(self, data: bytes) -> GPSData | None:
|
||
"""Attempt to parse GPS data from a raw USB CDC frame."""
|
||
if not data:
|
||
return None
|
||
|
||
try:
|
||
# Text format: "GPS:lat,lon,alt,pitch\r\n"
|
||
text = data.decode("utf-8", errors="ignore").strip()
|
||
if text.startswith("GPS:"):
|
||
parts = text.split(":")[1].split(",")
|
||
if len(parts) >= 4:
|
||
return GPSData(
|
||
latitude=float(parts[0]),
|
||
longitude=float(parts[1]),
|
||
altitude=float(parts[2]),
|
||
pitch=float(parts[3]),
|
||
timestamp=time.time(),
|
||
)
|
||
|
||
# Binary format: [GPSB 4][lat 8][lon 8][alt 4][pitch 4][CRC 2] = 30 bytes
|
||
if len(data) >= 30 and data[0:4] == b"GPSB":
|
||
return self._parse_binary_gps(data)
|
||
except (ValueError, struct.error) as e:
|
||
logger.error(f"Error parsing GPS data: {e}")
|
||
return None
|
||
|
||
@staticmethod
|
||
def _parse_binary_gps(data: bytes) -> GPSData | None:
|
||
"""Parse 30-byte binary GPS frame."""
|
||
try:
|
||
if len(data) < 30:
|
||
return None
|
||
|
||
# Simple checksum CRC
|
||
crc_rcv = (data[28] << 8) | data[29]
|
||
crc_calc = sum(data[0:28]) & 0xFFFF
|
||
if crc_rcv != crc_calc:
|
||
logger.warning("GPS binary CRC mismatch")
|
||
return None
|
||
|
||
lat = struct.unpack(">d", data[4:12])[0]
|
||
lon = struct.unpack(">d", data[12:20])[0]
|
||
alt = struct.unpack(">f", data[20:24])[0]
|
||
pitch = struct.unpack(">f", data[24:28])[0]
|
||
|
||
return GPSData(
|
||
latitude=lat,
|
||
longitude=lon,
|
||
altitude=alt,
|
||
pitch=pitch,
|
||
timestamp=time.time(),
|
||
)
|
||
except (ValueError, struct.error) as e:
|
||
logger.error(f"Error parsing binary GPS: {e}")
|
||
return None
|
||
|
||
|
||
# ============================================================================
|
||
# Utility: polar → geographic coordinate conversion
|
||
# ============================================================================
|
||
|
||
def polar_to_geographic(
|
||
radar_lat: float,
|
||
radar_lon: float,
|
||
range_m: float,
|
||
azimuth_deg: float,
|
||
) -> tuple:
|
||
"""Convert polar (range, azimuth) relative to radar → (lat, lon).
|
||
|
||
azimuth_deg: 0 = North, clockwise.
|
||
"""
|
||
r_earth = 6_371_000.0 # Earth radius in metres
|
||
|
||
lat1 = math.radians(radar_lat)
|
||
lon1 = math.radians(radar_lon)
|
||
bearing = math.radians(azimuth_deg)
|
||
|
||
lat2 = math.asin(
|
||
math.sin(lat1) * math.cos(range_m / r_earth)
|
||
+ math.cos(lat1) * math.sin(range_m / r_earth) * math.cos(bearing)
|
||
)
|
||
lon2 = lon1 + math.atan2(
|
||
math.sin(bearing) * math.sin(range_m / r_earth) * math.cos(lat1),
|
||
math.cos(range_m / r_earth) - math.sin(lat1) * math.sin(lat2),
|
||
)
|
||
return (math.degrees(lat2), math.degrees(lon2))
|
||
|
||
|
||
# ============================================================================
|
||
# Shared target extraction (used by both RadarDataWorker and ReplayWorker)
|
||
# ============================================================================
|
||
|
||
def extract_targets_from_frame(
|
||
frame,
|
||
range_resolution: float = 1.0,
|
||
velocity_resolution: float = 1.0,
|
||
gps: GPSData | None = None,
|
||
) -> list[RadarTarget]:
|
||
"""Extract RadarTarget list from a RadarFrame's detection mask.
|
||
|
||
This is the bin-to-physical conversion + geo-mapping shared between
|
||
the live and replay data paths.
|
||
|
||
Parameters
|
||
----------
|
||
frame : RadarFrame
|
||
Frame with populated ``detections``, ``magnitude``, ``range_doppler_i/q``.
|
||
range_resolution : float
|
||
Meters per range bin.
|
||
velocity_resolution : float
|
||
m/s per Doppler bin.
|
||
gps : GPSData | None
|
||
GPS position for geo-mapping (latitude/longitude).
|
||
|
||
Returns
|
||
-------
|
||
list[RadarTarget]
|
||
One target per detection cell.
|
||
"""
|
||
det_indices = np.argwhere(frame.detections > 0)
|
||
n_doppler = frame.detections.shape[1] if frame.detections.ndim == 2 else 48
|
||
doppler_center = n_doppler // 2
|
||
|
||
targets: list[RadarTarget] = []
|
||
for idx in det_indices:
|
||
rbin, dbin = int(idx[0]), int(idx[1])
|
||
mag = float(frame.magnitude[rbin, dbin])
|
||
snr = 10.0 * math.log10(max(mag, 1.0)) if mag > 0 else 0.0
|
||
|
||
range_m = float(rbin) * range_resolution
|
||
velocity_ms = float(dbin - doppler_center) * velocity_resolution
|
||
|
||
lat, lon, azimuth, elevation = 0.0, 0.0, 0.0, 0.0
|
||
if gps is not None:
|
||
azimuth = gps.heading
|
||
# Spread detections across ±15° sector for single-beam radar
|
||
if len(det_indices) > 1:
|
||
spread = (dbin - doppler_center) / max(doppler_center, 1) * 15.0
|
||
azimuth = gps.heading + spread
|
||
lat, lon = polar_to_geographic(
|
||
gps.latitude, gps.longitude, range_m, azimuth,
|
||
)
|
||
|
||
targets.append(RadarTarget(
|
||
id=len(targets),
|
||
range=range_m,
|
||
velocity=velocity_ms,
|
||
azimuth=azimuth,
|
||
elevation=elevation,
|
||
latitude=lat,
|
||
longitude=lon,
|
||
snr=snr,
|
||
timestamp=frame.timestamp,
|
||
))
|
||
return targets
|
||
|
||
|
||
# ============================================================================
|
||
# PR-Q.5 — 3-PRI Chinese-Remainder Doppler unfolding (audit C-5)
|
||
# ============================================================================
|
||
|
||
def unfold_velocity_crt(
|
||
v_meas_per_sf: list[float],
|
||
v_unamb_per_sf: list[float],
|
||
v_res_per_sf: list[float] | None = None,
|
||
max_alias_k: int = 6,
|
||
tol_factor: float = 0.5,
|
||
) -> tuple[float, str, list[float]]:
|
||
"""3-PRI Chinese-Remainder Doppler velocity unfolding.
|
||
|
||
Each per-subframe FFT measures v_true folded into a signed
|
||
[-v_unamb_i, +v_unamb_i] interval (the standard fftshift
|
||
convention). With 3 coprime PRIs (PR-Q ladder: 175/161/167 us,
|
||
giving v_unamb ≈ 40.79/44.34/42.79 m/s), brute-force search over
|
||
alias depth k_0 ∈ [-K, K] generates candidates
|
||
``v_true = v_meas_0 + k_0 · 2 · v_unamb_0``. A candidate is
|
||
*valid* when it folds back into all other active PRIs to within
|
||
``tol_factor × max(v_res)``.
|
||
|
||
Args:
|
||
v_meas_per_sf: signed velocity measurement per active sub-frame
|
||
(m/s), already folded by the FFT. Length 1, 2, or 3.
|
||
v_unamb_per_sf: per-sub-frame v_unamb (m/s), same length.
|
||
v_res_per_sf: per-sub-frame v_res (m/s). If None, assumes
|
||
``v_res = v_unamb / 8`` (matches chirps_per_subframe = 16).
|
||
max_alias_k: alias search depth in PRI-0 fold steps. K=6 covers
|
||
±6 · 2 · v_unamb_0 ≈ ±490 m/s, well above
|
||
``WaveformConfig.extended_max_velocity_mps_crt(K=6) ≈ ±266``.
|
||
tol_factor: per-PRI agreement tolerance, in units of max(v_res).
|
||
1.0 = within one bin width.
|
||
|
||
Returns:
|
||
(v_est, confidence, alias_set):
|
||
|
||
- v_est (m/s): best-fit unfolded velocity. Falls back to PRI-0's
|
||
measurement if no candidate satisfies all PRIs within tolerance.
|
||
- confidence: ``"CONFIRMED"`` / ``"LIKELY"`` / ``"AMBIGUOUS"``.
|
||
* CONFIRMED — 3-PRI input, exactly one fold within tolerance.
|
||
* LIKELY — 3-PRI input with 2 candidates, or 2-PRI input
|
||
with a unique solution.
|
||
* AMBIGUOUS — 1-PRI input (no CRT possible), 3+ candidates,
|
||
2-PRI input with 2 candidates, or no candidate
|
||
within tolerance.
|
||
- alias_set (m/s): all candidate v_true within tolerance, sorted
|
||
by goodness-of-fit (best first).
|
||
"""
|
||
n_sf = len(v_meas_per_sf)
|
||
if n_sf != len(v_unamb_per_sf):
|
||
raise ValueError("v_meas_per_sf and v_unamb_per_sf must have same length")
|
||
if n_sf == 0:
|
||
return (0.0, "AMBIGUOUS", [])
|
||
|
||
# 1-PRI input — no CRT possible (LONG-only-at-20-km regime).
|
||
if n_sf == 1:
|
||
return (v_meas_per_sf[0], "AMBIGUOUS", [v_meas_per_sf[0]])
|
||
|
||
if v_res_per_sf is None:
|
||
v_res_per_sf = [vu / 8.0 for vu in v_unamb_per_sf]
|
||
elif len(v_res_per_sf) != n_sf:
|
||
raise ValueError("v_res_per_sf, when provided, must match v_meas_per_sf length")
|
||
|
||
pri0_meas = v_meas_per_sf[0]
|
||
pri0_span = 2.0 * v_unamb_per_sf[0]
|
||
|
||
candidates: list[tuple[float, float]] = [] # (v_candidate, max_err)
|
||
for k in range(-max_alias_k, max_alias_k + 1):
|
||
v_cand = pri0_meas + k * pri0_span
|
||
max_err = 0.0
|
||
rejected = False
|
||
for i in range(1, n_sf):
|
||
vu_i = v_unamb_per_sf[i]
|
||
span_i = 2.0 * vu_i
|
||
v_pred_i = ((v_cand + vu_i) % span_i) - vu_i
|
||
err = abs(v_pred_i - v_meas_per_sf[i])
|
||
tol_i = tol_factor * v_res_per_sf[i]
|
||
if err > tol_i:
|
||
rejected = True
|
||
break
|
||
if err > max_err:
|
||
max_err = err
|
||
if not rejected:
|
||
candidates.append((v_cand, max_err))
|
||
|
||
if not candidates:
|
||
# No fold satisfies all PRIs — fall back to PRI-0, mark AMBIGUOUS.
|
||
return (pri0_meas, "AMBIGUOUS", [pri0_meas])
|
||
|
||
candidates.sort(key=lambda c: c[1])
|
||
v_best = candidates[0][0]
|
||
alias_set = [v for (v, _) in candidates]
|
||
n_cands = len(alias_set)
|
||
|
||
if n_cands >= 3:
|
||
confidence = "AMBIGUOUS"
|
||
elif n_sf == 3 and n_cands == 1:
|
||
confidence = "CONFIRMED"
|
||
elif n_sf == 3 and n_cands == 2:
|
||
confidence = "LIKELY"
|
||
elif n_sf == 2 and n_cands == 1:
|
||
confidence = "LIKELY"
|
||
else: # n_sf == 2 and n_cands == 2
|
||
confidence = "AMBIGUOUS"
|
||
|
||
return (v_best, confidence, alias_set)
|
||
|
||
|
||
def extract_targets_from_frame_crt(
|
||
frame,
|
||
waveform,
|
||
gps: GPSData | None = None,
|
||
max_alias_k: int = 6,
|
||
) -> list[RadarTarget]:
|
||
"""Extract RadarTargets from a 48-bin frame using 3-PRI CRT unfolding.
|
||
|
||
The 48 Doppler bins are organized as 3 sub-frames of 16:
|
||
|
||
bins 0..15: SHORT PRI (``waveform.pri_short_s``)
|
||
bins 16..31: MEDIUM PRI (``waveform.pri_medium_s``)
|
||
bins 32..47: LONG PRI (``waveform.pri_long_s``)
|
||
|
||
Within each sub-frame, the 16-pt FFT uses the standard signed-bin
|
||
convention: bin 0 = DC, bins 1..7 = positive v, bin 8 = Nyquist
|
||
(treated as +v_unamb), bins 9..15 = negative v.
|
||
|
||
Detections at the same range bin across different sub-frames are
|
||
grouped, and the strongest bin per (rbin, sub-frame) is taken as
|
||
that PRI's primary Doppler measurement. ``unfold_velocity_crt``
|
||
resolves aliases when ≥2 sub-frames see the target.
|
||
|
||
Falls back to the legacy single-PRI ``extract_targets_from_frame``
|
||
when the frame is not 48-bin (e.g. 32-bin legacy recordings).
|
||
"""
|
||
if frame.detections.ndim != 2 or frame.detections.shape[1] != 48:
|
||
return extract_targets_from_frame(
|
||
frame,
|
||
range_resolution=waveform.range_resolution_m,
|
||
velocity_resolution=waveform.velocity_resolution_long_mps,
|
||
gps=gps,
|
||
)
|
||
|
||
chirps_per_sf = waveform.chirps_per_subframe # 16
|
||
v_res_per_sf_all = [
|
||
waveform.velocity_resolution_short_mps,
|
||
waveform.velocity_resolution_medium_mps,
|
||
waveform.velocity_resolution_long_mps,
|
||
]
|
||
v_unamb_per_sf_all = [
|
||
waveform.max_velocity_short_mps,
|
||
waveform.max_velocity_medium_mps,
|
||
waveform.max_velocity_long_mps,
|
||
]
|
||
|
||
# Group detections: rbin -> {sf_id: (peak_bin_in_sf, peak_mag)}
|
||
clusters: dict[int, dict[int, tuple[int, float]]] = {}
|
||
det_indices = np.argwhere(frame.detections > 0)
|
||
for idx in det_indices:
|
||
rbin, dbin = int(idx[0]), int(idx[1])
|
||
sf_id = dbin // chirps_per_sf
|
||
bin_in_sf = dbin % chirps_per_sf
|
||
mag = float(frame.magnitude[rbin, dbin])
|
||
existing = clusters.setdefault(rbin, {}).get(sf_id)
|
||
if existing is None or mag > existing[1]:
|
||
clusters[rbin][sf_id] = (bin_in_sf, mag)
|
||
|
||
targets: list[RadarTarget] = []
|
||
range_resolution = waveform.range_resolution_m
|
||
|
||
for rbin in sorted(clusters.keys()):
|
||
sf_map = clusters[rbin]
|
||
active_sfs = sorted(sf_map.keys())
|
||
v_meas_list: list[float] = []
|
||
v_unamb_list: list[float] = []
|
||
v_res_list: list[float] = []
|
||
peak_mag = 0.0
|
||
for sf_id in active_sfs:
|
||
bin_in_sf, mag = sf_map[sf_id]
|
||
# Signed bin: 0..7 positive, 8 = Nyquist (treat as +8),
|
||
# 9..15 negative. Yields v in [-8·v_res, +8·v_res].
|
||
signed_bin = bin_in_sf if bin_in_sf <= 8 else bin_in_sf - chirps_per_sf
|
||
v_meas_list.append(float(signed_bin) * v_res_per_sf_all[sf_id])
|
||
v_unamb_list.append(v_unamb_per_sf_all[sf_id])
|
||
v_res_list.append(v_res_per_sf_all[sf_id])
|
||
if mag > peak_mag:
|
||
peak_mag = mag
|
||
|
||
v_est, confidence, alias_set = unfold_velocity_crt(
|
||
v_meas_list, v_unamb_list, v_res_list, max_alias_k=max_alias_k,
|
||
)
|
||
|
||
range_m = float(rbin) * range_resolution
|
||
snr = 10.0 * math.log10(max(peak_mag, 1.0)) if peak_mag > 0 else 0.0
|
||
|
||
lat, lon, azimuth, elevation = 0.0, 0.0, 0.0, 0.0
|
||
if gps is not None:
|
||
azimuth = gps.heading
|
||
lat, lon = polar_to_geographic(
|
||
gps.latitude, gps.longitude, range_m, azimuth,
|
||
)
|
||
|
||
targets.append(RadarTarget(
|
||
id=len(targets),
|
||
range=range_m,
|
||
velocity=v_est,
|
||
azimuth=azimuth,
|
||
elevation=elevation,
|
||
latitude=lat,
|
||
longitude=lon,
|
||
snr=snr,
|
||
timestamp=frame.timestamp,
|
||
velocity_confidence=confidence,
|
||
alias_set=alias_set if alias_set else None,
|
||
))
|
||
|
||
return targets
|