671 lines
27 KiB
Python
671 lines
27 KiB
Python
"""AoA-Auswertung mit 2D-Beamformer fuer den Silicon Labs BG22 (Board BRD4191A, 4 x 4, V + H).
|
|
|
|
Eigenstaendiges Skript, keine RTL-Bibliothek. Enthaelt alle Schritte und Filter:
|
|
1. Vorverarbeitung Frequenzversatz entfernen, Mittelwert je Antenne
|
|
2. Kalibrierung je Kanal und Polarisation, mit der echten Tag-Richtung
|
|
3. Richtung je Paket Bartlett-Beamformer ueber alle Antennen (grobe Suche + Verfeinerung)
|
|
4. Qualitaet je Paket Kohaerenz, Nebenmaximum, Keulenbreite, SNR, Jitter
|
|
5. Zeitfilter Fenster 0,2 s, Median je Kanal, Hampel-Filter, Kalman-Filter mit Gating
|
|
|
|
Winkel wie im AoA Analyzer: Azimut 0 = Unterkante, +90 = rechts,
|
|
Elevation 90 = direkt vor dem Board, 0 = in der Board-Ebene.
|
|
|
|
Aufruf:
|
|
python aoa_beamformer_komplett.py test_a.jsonl --kalibrierung boresight_vh3.jsonl \
|
|
--kal-az -134 --kal-el 87.8 --ref-az -89.1 --ref-el 52.0
|
|
"""
|
|
from __future__ import annotations
|
|
|
|
import argparse
|
|
import json
|
|
import os
|
|
from collections import deque
|
|
from dataclasses import dataclass, field
|
|
from datetime import datetime
|
|
|
|
import numpy as np
|
|
|
|
# =============================================================================
|
|
# Konstanten
|
|
# =============================================================================
|
|
|
|
C0 = 299_792_458.0
|
|
GRAD = np.pi / 180.0
|
|
|
|
# Umschaltreihenfolge des BRD4191A (Patch 1..16), laut Silicon-Labs-Dokumentation
|
|
MUSTER_V = (2, 0, 14, 12, 4, 8, 20, 10, 6, 22, 16, 19, 9, 26, 28, 31)
|
|
MUSTER_H = (3, 1, 15, 13, 5, 24, 21, 11, 7, 23, 17, 18, 25, 27, 29, 30)
|
|
CP_ANTENNE = 40 # Referenzantenne (Patch 6, zirkular)
|
|
ABSTAND_M = 0.036 # Antennenabstand, gemessen ca. 36,7 mm
|
|
|
|
|
|
# =============================================================================
|
|
# Winkel und Richtungen
|
|
# =============================================================================
|
|
|
|
def kanal_frequenz(kanal: int) -> float:
|
|
"""Frequenz (Hz) eines BLE-Kanals 0..39."""
|
|
if kanal == 37:
|
|
return 2402e6
|
|
if kanal == 38:
|
|
return 2426e6
|
|
if kanal == 39:
|
|
return 2480e6
|
|
if 0 <= kanal <= 10:
|
|
return (2404 + 2 * kanal) * 1e6
|
|
if 11 <= kanal <= 36:
|
|
return (2428 + 2 * (kanal - 11)) * 1e6
|
|
raise ValueError(f"ungueltiger BLE-Kanal: {kanal}")
|
|
|
|
|
|
def richtung(az, el):
|
|
"""Einheitsvektor aus Azimut und Elevation (rad)."""
|
|
az, el = np.asarray(az, float), np.asarray(el, float)
|
|
return np.stack([np.cos(el) * np.cos(az), np.cos(el) * np.sin(az), np.sin(el)], axis=-1)
|
|
|
|
|
|
def az_el(u):
|
|
"""Azimut und Elevation (rad) aus einem Einheitsvektor."""
|
|
u = np.asarray(u, float)
|
|
return np.arctan2(u[..., 1], u[..., 0]), np.arcsin(np.clip(u[..., 2], -1, 1))
|
|
|
|
|
|
def winkelabstand(u, v):
|
|
"""Tatsaechlicher Winkel (rad) zwischen zwei Richtungen ("Abweichung gesamt")."""
|
|
return np.arccos(np.clip(np.sum(np.asarray(u) * np.asarray(v), -1), -1, 1))
|
|
|
|
|
|
def sphaerischer_median(U, w=None, iterationen=50):
|
|
"""Gewichteter geometrischer Median von Richtungen (robust gegen Ausreisser)."""
|
|
U = np.atleast_2d(U)
|
|
w = np.ones(len(U)) if w is None else np.asarray(w, float)
|
|
m = (w[:, None] * U).sum(0)
|
|
m /= np.linalg.norm(m) + 1e-12
|
|
for _ in range(iterationen):
|
|
d = np.maximum(winkelabstand(U, m), 1e-4)
|
|
neu = ((w / d)[:, None] * U).sum(0)
|
|
neu /= np.linalg.norm(neu) + 1e-12
|
|
if winkelabstand(neu, m) < 1e-7:
|
|
return neu
|
|
m = neu
|
|
return m
|
|
|
|
|
|
def gewichteter_median(v, w):
|
|
v, w = np.asarray(v, float), np.asarray(w, float)
|
|
i = np.argsort(v)
|
|
c = np.cumsum(w[i])
|
|
return v[i][np.searchsorted(c, 0.5 * c[-1])]
|
|
|
|
|
|
# =============================================================================
|
|
# Antennenboard
|
|
# =============================================================================
|
|
|
|
@dataclass
|
|
class Array:
|
|
positionen: np.ndarray # (N, 2) in m: x nach unten, y nach rechts
|
|
polarisation: np.ndarray # (N,) 0 = vertikal, 1 = horizontal
|
|
abstand: float
|
|
|
|
@property
|
|
def n(self) -> int:
|
|
return len(self.positionen)
|
|
|
|
def steuervektoren(self, U, lam):
|
|
"""Steuervektoren a(theta) fuer die Richtungen U, Form (G, N)."""
|
|
return np.exp(1j * 2 * np.pi / lam * (np.asarray(U)[..., :2] @ self.positionen.T))
|
|
|
|
def index(self, pol: int, zeile: int, spalte: int) -> int:
|
|
"""Antennenindex an Rasterposition (zeile 0 = oben, spalte 0 = links)."""
|
|
z = np.rint(self.positionen[:, 0] / self.abstand + 1.5)
|
|
s = np.rint(self.positionen[:, 1] / self.abstand + 1.5)
|
|
return int(np.flatnonzero((self.polarisation == pol) & (z == zeile) & (s == spalte))[0])
|
|
|
|
|
|
def brd4191a(polar: str = "VH", abstand_m: float = ABSTAND_M) -> Array:
|
|
"""BRD4191A: Patch 1..16 spaltenweise von rechts nach links, je von oben nach unten."""
|
|
pos = []
|
|
for k in range(16):
|
|
zeile, spalte = k % 4, 3 - k // 4
|
|
pos.append(((zeile - 1.5) * abstand_m, (spalte - 1.5) * abstand_m))
|
|
pos = np.array(pos)
|
|
if polar == "V":
|
|
return Array(pos, np.zeros(16, int), abstand_m)
|
|
if polar == "VH":
|
|
return Array(np.vstack([pos, pos]), np.r_[np.zeros(16, int), np.ones(16, int)], abstand_m)
|
|
raise ValueError("polar muss 'V' oder 'VH' sein")
|
|
|
|
|
|
# =============================================================================
|
|
# IQ-Daten einlesen
|
|
# =============================================================================
|
|
|
|
@dataclass
|
|
class Paket:
|
|
t: float # Zeit (s)
|
|
kanal: int
|
|
ref: np.ndarray # 8 Referenz-Samples
|
|
t_ref: np.ndarray # Zeitpunkte (us)
|
|
samples: np.ndarray # Samples der Umschaltphase
|
|
t_samples: np.ndarray # Zeitpunkte (us)
|
|
antennen: np.ndarray # Antennenindex je Sample
|
|
|
|
|
|
def paket_aus_iq(i_werte, q_werte, kanal, t, n_ant, slot_us=2.0):
|
|
"""Ein Paket aus I/Q-Listen: 8 Referenz-Samples, danach Zyklus CP + n_ant Antennen."""
|
|
z = np.asarray(i_werte, float) - 1j * np.asarray(q_werte, float)
|
|
if len(z) <= 8 + n_ant:
|
|
return None
|
|
k = np.arange(len(z) - 8)
|
|
idx = k % (n_ant + 1)
|
|
g = idx < n_ant
|
|
return Paket(t, int(kanal), z[:8], np.arange(8.0), z[8:][g],
|
|
(8.0 + slot_us + 2 * slot_us * k)[g], idx[g])
|
|
|
|
|
|
def lese_iq_datei(pfad: str, n_ant: int | None = None, jedes: int = 1) -> list[Paket]:
|
|
"""Liest eine IQ-Datei (.jsonl). Anzahl Antennen aus <pfad>.meta.json, sonst 16."""
|
|
meta = {}
|
|
if os.path.exists(pfad + ".meta.json"):
|
|
with open(pfad + ".meta.json", encoding="utf-8") as f:
|
|
meta = json.load(f)
|
|
n_ant = n_ant or int(meta.get("n_antennes", 16))
|
|
slot_us = float(meta.get("slot_us", 2.0))
|
|
pakete = []
|
|
with open(pfad, encoding="utf-8") as f:
|
|
for nr, zeile in enumerate(f):
|
|
if nr % jedes:
|
|
continue
|
|
d = json.loads(zeile)
|
|
t = datetime.fromisoformat(d["zeit"]).timestamp()
|
|
p = paket_aus_iq(d["i"], d["q"], d["channel"], t, n_ant, slot_us)
|
|
if p is not None:
|
|
pakete.append(p)
|
|
return pakete
|
|
|
|
|
|
# =============================================================================
|
|
# Vorverarbeitung: Frequenzversatz entfernen, Mittelwert je Antenne
|
|
# =============================================================================
|
|
|
|
def vorverarbeiten(p: Paket, n_ant: int):
|
|
"""Gibt x (N,) komplex (NaN = Antenne fehlt), SNR und Phasen-Jitter je Antenne zurueck."""
|
|
ref, tr = np.asarray(p.ref), np.asarray(p.t_ref, float)
|
|
omega = np.angle(np.sum(ref[1:] * np.conj(ref[:-1]))) / np.median(np.diff(tr))
|
|
phi0 = np.angle(np.sum(ref * np.exp(-1j * omega * tr)))
|
|
r = ref * np.exp(-1j * (omega * tr + phi0))
|
|
m = r.mean()
|
|
snr = abs(m) ** 2 / max(np.mean(np.abs(r - m) ** 2), 1e-12)
|
|
|
|
s, t, ant = np.asarray(p.samples), np.asarray(p.t_samples, float), np.asarray(p.antennen)
|
|
y = s * np.exp(-1j * (omega * t + phi0))
|
|
|
|
# Feinkorrektur der Frequenz ueber wiederholte Antennen
|
|
num, dts = 0j, []
|
|
for i in np.unique(ant):
|
|
k = np.flatnonzero(ant == i)
|
|
if k.size >= 2:
|
|
num += np.sum(y[k[1:]] * np.conj(y[k[:-1]]))
|
|
dts.extend(np.diff(t[k]))
|
|
if dts:
|
|
y = y * np.exp(-1j * (np.angle(num) / np.mean(dts)) * t)
|
|
|
|
x = np.full(n_ant, np.nan + 0j)
|
|
jitter = np.zeros(n_ant)
|
|
for i in range(n_ant):
|
|
si = y[ant == i]
|
|
if si.size == 0:
|
|
continue
|
|
x[i] = si.mean()
|
|
if si.size > 1:
|
|
R = abs(np.mean(np.exp(1j * np.angle(si * np.conj(x[i])))))
|
|
jitter[i] = np.sqrt(-2 * np.log(max(R, 1e-9)))
|
|
return x, snr, jitter
|
|
|
|
|
|
# =============================================================================
|
|
# Kalibrierung
|
|
# =============================================================================
|
|
|
|
class Kalibrierung:
|
|
"""Komplexer Faktor je Antenne und Kanal, aus einer Aufnahme mit bekannter Tag-Richtung."""
|
|
|
|
def __init__(self, n: int):
|
|
self.global_ = np.ones(n, complex)
|
|
self.je_kanal: dict[int, np.ndarray] = {}
|
|
|
|
def anwenden(self, x, kanal):
|
|
return x / self.je_kanal.get(kanal, self.global_)
|
|
|
|
@classmethod
|
|
def aus_aufnahme(cls, array: Array, pakete, az_grad: float, el_grad: float, min_pakete: int = 5):
|
|
u = richtung(az_grad * GRAD, el_grad * GRAD)
|
|
gruppen: dict[int, list] = {}
|
|
for p in pakete:
|
|
x, _, _ = vorverarbeiten(p, array.n)
|
|
if np.all(np.isfinite(x)):
|
|
gruppen.setdefault(p.kanal, []).append(x)
|
|
|
|
kal, alle = cls(array.n), []
|
|
for kanal, X in gruppen.items():
|
|
if len(X) < min_pakete:
|
|
continue
|
|
X = np.array(X)
|
|
a = array.steuervektoren(u[None], C0 / kanal_frequenz(kanal))[0]
|
|
c = np.empty(array.n, complex)
|
|
for pol in np.unique(array.polarisation): # jede Polarisation getrennt
|
|
m = array.polarisation == pol
|
|
R = X[:, m].T @ X[:, m].conj() / len(X)
|
|
v = np.linalg.eigh(R)[1][:, -1] # dominanter Pfad
|
|
cm = v / a[m]
|
|
c[m] = cm * np.exp(-1j * np.angle(cm[0])) / np.mean(np.abs(cm))
|
|
kal.je_kanal[kanal] = c
|
|
alle.append(c)
|
|
if not alle:
|
|
raise ValueError("zu wenige Pakete fuer die Kalibrierung")
|
|
g = np.mean(alle, axis=0)
|
|
for pol in np.unique(array.polarisation):
|
|
m = array.polarisation == pol
|
|
g[m] /= np.mean(np.abs(g[m]))
|
|
kal.global_ = g
|
|
return kal
|
|
|
|
|
|
# =============================================================================
|
|
# Einstellungen und Ergebnisse
|
|
# =============================================================================
|
|
|
|
@dataclass
|
|
class Einstellungen:
|
|
raster_grad: float = 2.0 # Suchraster
|
|
polarisationen: tuple = (0, 1) # (0,) = nur V, (0, 1) = V und H
|
|
schwache_antenne: float = 0.3 # Amplitude < 30 % des Medians -> ignorieren
|
|
q_min: float = 0.15 # Pakete mit geringerer Qualitaet verwerfen
|
|
fenster_s: float = 0.2 # Pakete je Ausgabewert
|
|
hampel_n: int = 9
|
|
hampel_k: float = 3.5
|
|
hampel_boden_grad: float = 2.0
|
|
sigma_min_grad: float = 1.5 # Messrauschen bei bester Qualitaet
|
|
k_rauschen: float = 4e-4 # R = sigma_min^2 + k_rauschen / Qualitaet
|
|
beschleunigung: float = 0.05 # Prozessrauschen Kalman
|
|
gating: float = 9.21 # Chi^2, 2 Freiheitsgrade, 99 %
|
|
n_neustart: int = 4 # Verwerfungen in Folge -> neue Position uebernehmen
|
|
|
|
|
|
@dataclass
|
|
class PaketMessung:
|
|
t: float
|
|
kanal: int
|
|
u: np.ndarray
|
|
qualitaet: float
|
|
kohaerenz: float
|
|
nebenmaximum: float
|
|
keulenbreite: float
|
|
snr: float
|
|
gruende: list = field(default_factory=list)
|
|
|
|
|
|
@dataclass
|
|
class Schaetzung:
|
|
t: float
|
|
az_grad: float
|
|
el_grad: float
|
|
unsicherheit_az: float
|
|
unsicherheit_el: float
|
|
konfidenz: float
|
|
roh_az_grad: float
|
|
roh_el_grad: float
|
|
pakete: int
|
|
pakete_gueltig: int
|
|
gruende: list = field(default_factory=list)
|
|
|
|
|
|
# =============================================================================
|
|
# Kalman-Filter auf den Richtungskosinus (ux, uy)
|
|
# =============================================================================
|
|
|
|
class KalmanRichtung:
|
|
"""Zustand [ux, uy, vx, vy], Modell konstanter Geschwindigkeit."""
|
|
|
|
def __init__(self, beschleunigung):
|
|
self.q = beschleunigung
|
|
self.x = self.P = self.t = None
|
|
|
|
def starten(self, z, R, t):
|
|
self.x = np.r_[z, 0.0, 0.0]
|
|
self.P = np.diag([R[0, 0], R[1, 1], 0.05, 0.05])
|
|
self.t = t
|
|
|
|
def vorhersagen(self, t):
|
|
dt = max(t - self.t, 1e-3)
|
|
F = np.eye(4)
|
|
F[0, 2] = F[1, 3] = dt
|
|
Qb = self.q * np.array([[dt**4 / 4, dt**3 / 2], [dt**3 / 2, dt**2]])
|
|
Q = np.zeros((4, 4))
|
|
Q[np.ix_([0, 2], [0, 2])] = Qb
|
|
Q[np.ix_([1, 3], [1, 3])] = Qb
|
|
self.x = F @ self.x
|
|
self.P = F @ self.P @ F.T + Q
|
|
self.t = t
|
|
self._begrenzen()
|
|
|
|
def abstand(self, z, R):
|
|
nu = z - self.x[:2]
|
|
return float(nu @ np.linalg.solve(self.P[:2, :2] + R, nu))
|
|
|
|
def korrigieren(self, z, R):
|
|
H = np.zeros((2, 4))
|
|
H[0, 0] = H[1, 1] = 1
|
|
K = self.P @ H.T @ np.linalg.inv(H @ self.P @ H.T + R)
|
|
self.x = self.x + K @ (z - self.x[:2])
|
|
self.P = (np.eye(4) - K @ H) @ self.P
|
|
self._begrenzen()
|
|
|
|
def _begrenzen(self):
|
|
r = np.hypot(*self.x[:2])
|
|
if r > 0.9999:
|
|
self.x[:2] *= 0.9999 / r
|
|
|
|
def ausgabe(self):
|
|
"""Azimut, Elevation und Unsicherheiten in Grad."""
|
|
ux, uy = self.x[:2]
|
|
r = max(np.hypot(ux, uy), 1e-3)
|
|
uz = np.sqrt(max(1 - r * r, 0.0))
|
|
az, el = np.arctan2(uy, ux), np.arctan2(uz, r)
|
|
P = self.P[:2, :2]
|
|
j_az = np.array([-uy, ux]) / r**2
|
|
j_r = np.array([ux, uy]) / r
|
|
s_r = np.sqrt(max(j_r @ P @ j_r, 0))
|
|
s_az = np.sqrt(max(j_az @ P @ j_az, 0)) / GRAD
|
|
s_el = 0.5 * abs(np.arccos(np.clip(r - s_r, 0, 1)) - np.arccos(np.clip(r + s_r, 0, 1))) / GRAD
|
|
return az / GRAD, el / GRAD, min(s_az, 180.0), min(s_el, 90.0)
|
|
|
|
|
|
# =============================================================================
|
|
# Beamformer-Schaetzer
|
|
# =============================================================================
|
|
|
|
class BeamformerSchaetzer:
|
|
"""Ablauf je Paket: Vorverarbeitung -> Kalibrierung -> Beamformer -> Qualitaet.
|
|
Ablauf je Fenster: Median je Kanal -> Hampel -> Kalman."""
|
|
|
|
NAME = "Beamformer"
|
|
|
|
def __init__(self, array: Array, kal: Kalibrierung | None = None, einst: Einstellungen | None = None):
|
|
self.array = array
|
|
self.kal = kal or Kalibrierung(array.n)
|
|
self.einst = einst or Einstellungen()
|
|
schritt = self.einst.raster_grad * GRAD
|
|
AZ, EL = np.meshgrid(np.arange(-np.pi, np.pi, schritt), np.arange(0, np.pi / 2 + 1e-9, schritt))
|
|
self.form = AZ.shape
|
|
self.U = richtung(AZ.ravel(), EL.ravel())
|
|
self.flaeche = np.cos(EL.ravel()) * schritt * schritt
|
|
self._A_cache: dict[float, np.ndarray] = {}
|
|
self.kalman = KalmanRichtung(self.einst.beschleunigung)
|
|
self.verlauf = deque(maxlen=self.einst.hampel_n)
|
|
self.puffer: list[PaketMessung] = []
|
|
self.verworfen = 0
|
|
|
|
# ---- Steuervektoren auf dem Suchraster (Cache je Wellenlaenge) ----
|
|
|
|
def A_raster(self, lam):
|
|
key = round(lam * 1e6, 3)
|
|
if key not in self._A_cache:
|
|
self._A_cache[key] = self.array.steuervektoren(self.U, lam).astype(np.complex64)
|
|
return self._A_cache[key]
|
|
|
|
# ---- Bartlett-Spektrum (Beamformer), Werte 0..1 ----
|
|
|
|
def bartlett(self, U, x, maske, lam, A=None):
|
|
"""V und H getrennt berechnen, nach Energie gewichtet zusammenfassen."""
|
|
A = self.array.steuervektoren(U, lam) if A is None else A
|
|
P, E_ges = np.zeros(len(U)), 0.0
|
|
for pol in self.einst.polarisationen:
|
|
m = maske & (self.array.polarisation == pol)
|
|
if m.sum() < 3:
|
|
continue
|
|
xs = x[m]
|
|
E = float(np.sum(np.abs(xs) ** 2))
|
|
P += E * np.abs(np.conj(A[:, m]) @ xs) ** 2 / (m.sum() * E)
|
|
E_ges += E
|
|
return P / E_ges if E_ges > 0 else P
|
|
|
|
def fein_suchen(self, spektrum, u0, breite_grad=3.0, schritt_grad=0.25):
|
|
"""Verfeinerung um u0: gibt beste Richtung und Spektrumswert zurueck."""
|
|
az0, el0 = az_el(u0)
|
|
d = np.arange(-breite_grad, breite_grad + 1e-3, schritt_grad) * GRAD
|
|
AZ, EL = np.meshgrid(az0 + d, np.clip(el0 + d, 0, np.pi / 2))
|
|
Uf = richtung(AZ.ravel(), EL.ravel())
|
|
P = spektrum(Uf)
|
|
k = int(np.argmax(P))
|
|
return Uf[k], float(P[k])
|
|
|
|
def _nebenmaximum(self, P, u, ausschluss_grad=15.0):
|
|
"""Hoechstes lokales Maximum ausserhalb der Hauptkeule."""
|
|
G = P.reshape(self.form)
|
|
nachbarn = [np.roll(G, 1, 1), np.roll(G, -1, 1),
|
|
np.vstack([G[:1] * 0, G[:-1]]), np.vstack([G[1:], G[-1:] * 0])]
|
|
lokal = np.all([G >= v for v in nachbarn], axis=0).ravel()
|
|
lokal &= self.U @ u < np.cos(ausschluss_grad * GRAD)
|
|
return float(P[lokal].max()) if lokal.any() else 0.0
|
|
|
|
def _keulenflaeche(self, P_nah, nah):
|
|
if not nah.any() or P_nah.max() <= 0:
|
|
return np.nan
|
|
return float(np.sum(self.flaeche[nah][P_nah >= 0.5 * P_nah.max()]))
|
|
|
|
# ---- Auswertung eines Pakets ----
|
|
|
|
def paket_auswerten(self, p: Paket) -> PaketMessung:
|
|
e = self.einst
|
|
gruende = []
|
|
x, snr, jitter = vorverarbeiten(p, self.array.n)
|
|
x = self.kal.anwenden(x, p.kanal)
|
|
gueltig = np.isfinite(x)
|
|
amp = np.abs(x)
|
|
|
|
# Schwache Antennen ausblenden (je Polarisation)
|
|
maske = np.zeros(self.array.n, bool)
|
|
for pol in np.unique(self.array.polarisation):
|
|
m = gueltig & (self.array.polarisation == pol)
|
|
if m.any():
|
|
maske |= m & (amp >= e.schwache_antenne * np.median(amp[m]))
|
|
maske &= np.isin(self.array.polarisation, e.polarisationen)
|
|
erwartet = np.isin(self.array.polarisation, e.polarisationen).sum()
|
|
if maske.sum() < erwartet:
|
|
gruende.append("schwache oder fehlende Antenne")
|
|
lam = C0 / kanal_frequenz(p.kanal)
|
|
xm = np.where(maske, x, 0)
|
|
|
|
# Beamformer: grobe Suche, Verfeinerung
|
|
A = self.A_raster(lam)
|
|
P = self.bartlett(self.U, xm, maske, lam, A)
|
|
k = int(np.argmax(P))
|
|
u_bf, kohaerenz = self.fein_suchen(lambda Uf: self.bartlett(Uf, xm, maske, lam), self.U[k])
|
|
|
|
# Qualitaetsmerkmale
|
|
neben = self._nebenmaximum(P, u_bf) / P[k] if P[k] > 0 else 0.0
|
|
nah = self.U @ u_bf > np.cos(30 * GRAD)
|
|
a_ideal = self.array.steuervektoren(u_bf[None], lam)[0] * maske
|
|
P_ideal = self.bartlett(self.U[nah], a_ideal, maske, lam, A[nah])
|
|
breite = self._keulenflaeche(P[nah], nah) / max(self._keulenflaeche(P_ideal, nah), 1e-9)
|
|
|
|
jit = float(np.median(jitter[maske])) if maske.any() else np.pi
|
|
q = (np.clip((kohaerenz - 0.3) / 0.6, 0, 1)
|
|
* np.clip((0.85 - neben) / 0.5, 0, 1)
|
|
* (np.clip(1.0 / max(breite, 1e-6), 0, 1) if np.isfinite(breite) else 0.5)
|
|
* snr / (snr + 10.0)
|
|
* maske.sum() / max(erwartet, 1)
|
|
* np.exp(-0.5 * (jit / (20 * GRAD)) ** 2))
|
|
|
|
if kohaerenz < 0.6:
|
|
gruende.append("geringe Kohaerenz (mehrere Pfade)")
|
|
if neben > 0.6:
|
|
gruende.append("mehrere aehnliche Maxima")
|
|
if breite > 1.6:
|
|
gruende.append("verbreiterte Keule")
|
|
if snr < 5:
|
|
gruende.append("geringes SNR")
|
|
|
|
return PaketMessung(p.t, p.kanal, u_bf, float(q), kohaerenz, neben, breite, snr, gruende)
|
|
|
|
# ---- Laufende Verarbeitung ----
|
|
|
|
def verarbeiten(self, p: Paket) -> Schaetzung | None:
|
|
"""Fuer jedes Paket aufrufen. Liefert am Ende jedes Fensters eine Schaetzung."""
|
|
m = self.paket_auswerten(p)
|
|
if self.puffer and m.t - self.puffer[0].t >= self.einst.fenster_s:
|
|
s = self._fenster_abschliessen(self.puffer)
|
|
self.puffer = [m]
|
|
return s
|
|
self.puffer.append(m)
|
|
return None
|
|
|
|
def _fenster_abschliessen(self, messungen: list[PaketMessung]) -> Schaetzung:
|
|
e = self.einst
|
|
t = messungen[-1].t
|
|
gueltig = [m for m in messungen if m.qualitaet >= e.q_min]
|
|
gruende = sorted({g for m in messungen for g in m.gruende
|
|
if sum(g in mm.gruende for mm in messungen) > len(messungen) / 2})
|
|
|
|
# Keine gueltige Messung: nur Vorhersage
|
|
if not gueltig:
|
|
if self.kalman.x is not None:
|
|
self.kalman.vorhersagen(t)
|
|
az, el, saz, sel = self.kalman.ausgabe()
|
|
else:
|
|
az = el = saz = sel = np.nan
|
|
return Schaetzung(t, az, el, saz, sel, 0.0, np.nan, np.nan,
|
|
len(messungen), 0, gruende + ["keine gueltige Messung"])
|
|
|
|
# Median je Kanal, dann ueber die Kanaele
|
|
je_kanal: dict[int, list] = {}
|
|
for m in gueltig:
|
|
je_kanal.setdefault(m.kanal, []).append(m)
|
|
Uk = np.array([sphaerischer_median(np.array([m.u for m in ms]), [m.qualitaet for m in ms])
|
|
for ms in je_kanal.values()])
|
|
Wk = np.array([np.median([m.qualitaet for m in ms]) for ms in je_kanal.values()])
|
|
u_f = sphaerischer_median(Uk, Wk)
|
|
streuung = gewichteter_median(winkelabstand(Uk, u_f), Wk) / GRAD if len(Uk) > 1 else 0.0
|
|
U_alle = np.array([m.u for m in gueltig])
|
|
q_alle = np.array([m.qualitaet for m in gueltig])
|
|
anteil_weit = float(np.sum(q_alle[winkelabstand(U_alle, u_f) > 15 * GRAD]) / q_alle.sum())
|
|
q_f = float(np.median(q_alle)) * np.exp(-0.5 * (streuung / 10.0) ** 2)
|
|
if streuung > 10:
|
|
gruende.append("Kanaele uneinig")
|
|
if anteil_weit > 0.3:
|
|
gruende.append("Pakete gestreut")
|
|
|
|
# Hampel: Sprung gegenueber dem juengsten Verlauf?
|
|
sprung = False
|
|
if len(self.verlauf) >= 5:
|
|
H = np.array(self.verlauf)
|
|
c = sphaerischer_median(H)
|
|
skala = 1.4826 * np.median(winkelabstand(H, c)) + e.hampel_boden_grad * GRAD
|
|
sprung = winkelabstand(u_f, c) > e.hampel_k * skala
|
|
self.verlauf.append(u_f)
|
|
|
|
# Kalman mit qualitaetsabhaengigem Messrauschen und Gating
|
|
z = u_f[:2]
|
|
R = np.eye(2) * ((e.sigma_min_grad * GRAD) ** 2 + e.k_rauschen / max(q_f, 1e-3))
|
|
angenommen = True
|
|
if self.kalman.x is None:
|
|
self.kalman.starten(z, R, t)
|
|
else:
|
|
self.kalman.vorhersagen(t)
|
|
if sprung or self.kalman.abstand(z, R) > e.gating:
|
|
angenommen = False
|
|
self.verworfen += 1
|
|
gruende.append("Sprung erkannt (Hampel)" if sprung else "vom Kalman verworfen")
|
|
if self.verworfen >= e.n_neustart: # Tag hat sich wirklich bewegt
|
|
neu = np.array(list(self.verlauf)[-e.n_neustart:])
|
|
self.kalman.starten(sphaerischer_median(neu)[:2], R, t)
|
|
self.verlauf.clear()
|
|
self.verlauf.extend(neu)
|
|
self.verworfen = 0
|
|
gruende.append("Neustart (neue Position)")
|
|
else:
|
|
self.kalman.korrigieren(z, R)
|
|
self.verworfen = 0
|
|
|
|
az, el, saz, sel = self.kalman.ausgabe()
|
|
if el < 15:
|
|
gruende.append("nahe der Board-Ebene: Elevation unsicher")
|
|
konfidenz = q_f * (1.0 if angenommen else 0.3)
|
|
raz, rel = az_el(u_f)
|
|
return Schaetzung(t, az, el, saz, sel, float(np.clip(konfidenz, 0, 1)),
|
|
raz / GRAD, rel / GRAD, len(messungen), len(gueltig), gruende)
|
|
|
|
|
|
# =============================================================================
|
|
# Ergebnis und Vergleich mit dem AoA Analyzer
|
|
# =============================================================================
|
|
def ergebnis(schaetzungen: list[Schaetzung], ref_az=None, ref_el=None) -> dict:
|
|
"""Median aller Schaetzungen, optional Abweichung zum Analyzer."""
|
|
S = [s for s in schaetzungen if np.isfinite(s.az_grad)]
|
|
if not S:
|
|
return {}
|
|
u = sphaerischer_median(np.array([richtung(s.az_grad * GRAD, s.el_grad * GRAD) for s in S]))
|
|
az, el = az_el(u)
|
|
res = {"az": az / GRAD, "el": el / GRAD, "konfidenz": float(np.median([s.konfidenz for s in S])),
|
|
"anzahl": len(S)}
|
|
if ref_az is not None and ref_el is not None:
|
|
res["abweichung"] = float(winkelabstand(u, richtung(ref_az * GRAD, ref_el * GRAD)) / GRAD)
|
|
res["abw_az"] = ((az / GRAD - ref_az + 180) % 360) - 180
|
|
res["abw_el"] = el / GRAD - ref_el
|
|
return res
|
|
|
|
|
|
# =============================================================================
|
|
# Kommandozeile
|
|
# =============================================================================
|
|
|
|
def main():
|
|
ap = argparse.ArgumentParser(description="AoA-Auswertung mit 2D-Beamformer und allen Filtern")
|
|
ap.add_argument("iq", help="IQ-Datei (.jsonl)")
|
|
ap.add_argument("--kalibrierung", required=True, help="Kalibrieraufnahme am selben Standort")
|
|
ap.add_argument("--kal-az", type=float, default=0.0, help="Analyzer-Azimut waehrend der Kalibrierung")
|
|
ap.add_argument("--kal-el", type=float, default=90.0, help="Analyzer-Elevation waehrend der Kalibrierung")
|
|
ap.add_argument("--polar", default="VH", choices=["V", "VH"])
|
|
ap.add_argument("--abstand-mm", type=float, default=ABSTAND_M * 1000)
|
|
ap.add_argument("--ref-az", type=float, help="Analyzer-Azimut an dieser Position (Vergleich)")
|
|
ap.add_argument("--ref-el", type=float, help="Analyzer-Elevation an dieser Position (Vergleich)")
|
|
ap.add_argument("--jedes", type=int, default=1, help="nur jedes n-te Paket auswerten")
|
|
ap.add_argument("--ausgabe", help="Schaetzungen als JSON speichern")
|
|
ap.add_argument("--verlauf", action="store_true", help="jede Schaetzung ausgeben")
|
|
a = ap.parse_args()
|
|
|
|
# Board, Kalibrierung, Schaetzer
|
|
array = brd4191a(a.polar, a.abstand_mm / 1000)
|
|
kal = Kalibrierung.aus_aufnahme(array, lese_iq_datei(a.kalibrierung, array.n), a.kal_az, a.kal_el)
|
|
sch = BeamformerSchaetzer(array, kal, Einstellungen(polarisationen=(0,) if a.polar == "V" else (0, 1)))
|
|
|
|
# Pakete auswerten
|
|
schaetzungen = []
|
|
for p in lese_iq_datei(a.iq, array.n, a.jedes):
|
|
s = sch.verarbeiten(p)
|
|
if s is None:
|
|
continue
|
|
schaetzungen.append(s)
|
|
if a.verlauf and np.isfinite(s.az_grad):
|
|
print(f"{s.az_grad:+7.1f} / {s.el_grad:5.1f} Konfidenz {s.konfidenz:.2f} {', '.join(s.gruende)}")
|
|
if a.ausgabe:
|
|
with open(a.ausgabe, "w", encoding="utf-8") as f:
|
|
json.dump([s.__dict__ for s in schaetzungen], f, indent=1, ensure_ascii=False, default=float)
|
|
|
|
# Ergebnis ausgeben
|
|
r = ergebnis(schaetzungen, a.ref_az, a.ref_el)
|
|
if not r:
|
|
print("Keine gueltige Schaetzung.")
|
|
return
|
|
zeile = f"[Beamformer] Ergebnis: Azimut {r['az']:+.1f}, Elevation {r['el']:.1f}, Konfidenz {r['konfidenz']:.2f}"
|
|
if "abweichung" in r:
|
|
zeile += (f" | Abweichung zum Analyzer {r['abweichung']:.1f} Grad "
|
|
f"(Az {r['abw_az']:+.1f}, El {r['abw_el']:+.1f})")
|
|
print(zeile)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|