From d7eb87d16fdfdc7a722032a7f8033e87382a8b4b Mon Sep 17 00:00:00 2001 From: SandraGuefack Date: Thu, 22 Oct 2026 15:58:45 +0200 Subject: [PATCH] AoA BG22: Hybrid-Algorithmus, Beamformer, Live-Messung --- aoa_beamformer_komplett.py | 670 +++++++++++++++++++++++ aoa_music_komplett.py | 743 +++++++++++++++++++++++++ bg22_AoA_v2.py | 1063 ++++++++++++++++++++++++++++++++++++ bg22_live_hybrid.py | 181 ++++++ 4 files changed, 2657 insertions(+) create mode 100644 aoa_beamformer_komplett.py create mode 100644 aoa_music_komplett.py create mode 100644 bg22_AoA_v2.py create mode 100644 bg22_live_hybrid.py diff --git a/aoa_beamformer_komplett.py b/aoa_beamformer_komplett.py new file mode 100644 index 0000000..1fa8c00 --- /dev/null +++ b/aoa_beamformer_komplett.py @@ -0,0 +1,670 @@ +"""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 .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() diff --git a/aoa_music_komplett.py b/aoa_music_komplett.py new file mode 100644 index 0000000..867f36f --- /dev/null +++ b/aoa_music_komplett.py @@ -0,0 +1,743 @@ +"""AoA-Auswertung mit 2D-MUSIC 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. Qualitaet je Paket aus dem Beamformer-Spektrum (Kohaerenz, Nebenmaximum, Keule, SNR, Jitter) + 4. Richtung je Paket MUSIC mit Spatial Smoothing + Forward-Backward, + Suche um die Beamformer-Richtung, Gegenprobe gegen den Beamformer + 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_music_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 .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 + n_quellen: int = 1 # MUSIC: Anzahl Signale (direkter Pfad) + teilarray: int = 3 # MUSIC: Teilarrays 3 x 3 (4 Stueck im 4 x 4 Raster) + suchradius_grad: float = 12.0 # MUSIC sucht in diesem Umkreis der Beamformer-Richtung + max_abweichung_grad: float = 10.0 # MUSIC weiter entfernt -> Beamformer-Richtung verwenden + + +@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) + + +# ============================================================================= +# MUSIC: Kovarianz mit Spatial Smoothing und Spektrum +# ============================================================================= + +def teilarrays(array: Array, pol: int, groesse: int): + """Antennenindizes aller Teilarrays groesse x groesse einer Polarisation.""" + schritte = 4 - groesse + 1 + return [[array.index(pol, z0 + a, s0 + b) for a in range(groesse) for b in range(groesse)] + for z0 in range(schritte) for s0 in range(schritte)] + + +def geglaettete_kovarianz(x, gruppen): + """Mittel der Teilarray-Kovarianzen, danach Forward-Backward-Mittelung. + Trennt den direkten Pfad von Reflexionen (kohaerente Signale).""" + n = len(gruppen[0]) + R = np.zeros((n, n), complex) + for g in gruppen: + v = x[g] + R += np.outer(v, v.conj()) + R /= len(gruppen) + J = np.eye(n)[::-1] + return (R + J @ R.conj() @ J) / 2 + + +def music_spektrum(R, A, n_quellen): + """P = 1 / |A^H E_n|^2 (E_n = Rauschunterraum), normiert auf Maximum 1.""" + _, V = np.linalg.eigh(R) + En = V[:, : R.shape[0] - n_quellen] + p = 1.0 / np.maximum(np.sum(np.abs(np.conj(A) @ En) ** 2, axis=1), 1e-12) + return p / p.max() + + +# ============================================================================= +# 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) + + +# ============================================================================= +# MUSIC-Schaetzer +# ============================================================================= + +class MusicSchaetzer: + """Ablauf je Paket: Vorverarbeitung -> Kalibrierung -> Qualitaet (Beamformer) -> MUSIC. + Ablauf je Fenster: Median je Kanal -> Hampel -> Kalman.""" + + NAME = "MUSIC" + + 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 + self.gruppen = {pol: teilarrays(array, pol, self.einst.teilarray) for pol in self.einst.polarisationen} + + # ---- 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()])) + + # ---- MUSIC-Spektrum je Polarisation, nach Energie gewichtet ---- + + def music(self, U, x, gueltig, lam, A=None): + A = self.array.steuervektoren(U, lam) if A is None else A + P, E_ges = np.zeros(len(U)), 0.0 + for gruppen in self.gruppen.values(): + alle = sorted({i for g in gruppen for i in g}) + if not np.all(gueltig[alle]): # MUSIC braucht alle Antennen der Polarisation + continue + R = geglaettete_kovarianz(x, gruppen) + E = float(np.sum(np.abs(x[alle]) ** 2)) + P += E * music_spektrum(R, A[:, gruppen[0]], self.einst.n_quellen) + E_ges += E + return P / max(E_ges, 1e-12) + + # ---- Richtung: MUSIC um die Beamformer-Richtung, mit Gegenprobe ---- + + def richtung_bestimmen(self, x_kal, gueltig, lam, u_bf): + e = self.einst + x = np.where(gueltig, x_kal, 0) + nah = self.U @ u_bf > np.cos(e.suchradius_grad * GRAD) + P = self.music(self.U[nah], x, gueltig, lam, self.A_raster(lam)[nah]) + if not np.any(P > 0): + return u_bf, ["MUSIC nicht moeglich, Beamformer verwendet"] + u, _ = self.fein_suchen(lambda Uf: self.music(Uf, x, gueltig, lam), self.U[nah][int(np.argmax(P))]) + if winkelabstand(u, u_bf) > e.max_abweichung_grad * GRAD: + return u_bf, ["MUSIC weicht ab, Beamformer verwendet"] + return u, [] + + # ---- 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") + + # Richtung mit MUSIC + u, g2 = self.richtung_bestimmen(x, gueltig, lam, u_bf) + return PaketMessung(p.t, p.kanal, u, float(q), kohaerenz, neben, breite, snr, gruende + g2) + + # ---- 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-MUSIC und allen Filtern") + ap.add_argument("iq", help="IQ-Datei (.jsonl)") + ap.add_argument("--kalibrierung", help="Kalibrieraufnahme am selben Standort (ohne: keine Kalibrierung)") + 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) + if a.kalibrierung: + kal = Kalibrierung.aus_aufnahme(array, lese_iq_datei(a.kalibrierung, array.n), a.kal_az, a.kal_el) + else: + kal = Kalibrierung(array.n) # ohne Kalibrierung: alle Faktoren = 1 + print("Hinweis: OHNE Kalibrierung") + sch = MusicSchaetzer(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"[MUSIC] 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() diff --git a/bg22_AoA_v2.py b/bg22_AoA_v2.py new file mode 100644 index 0000000..c5de5be --- /dev/null +++ b/bg22_AoA_v2.py @@ -0,0 +1,1063 @@ +#!/usr/bin/env python3 +"""BG22 IQ-Logger + Winkel-Schätzung, NEU AUFGESETZT nur nach den Prinzipien aus +zwei öffentlichen GitHub-Referenzprojekten (keine eigene Switching-Pattern-Debatte +mehr, kein separates Kalibrierfile -- siehe Begründung unten): + + 1) Navigine/Direction-of-Arrival-DoA-Estimation-Algorithm + (github.com/Navigine/Direction-of-Arrival-DoA-Estimation-Algorithm) + -> Kernidee übernommen: das 2D-Problem (Azimut, Elevation) wird NICHT als ein + einziges 2D-Gitter gelöst, sondern in zwei UNABHÄNGIGE 1D-MUSIC-Scans + aufgeteilt, je über ein virtuelles 2-Antennen-Array aus benachbarten + Antennenpaaren. Phasendrift wird live aus einer Referenzperiode pro Paket + geschätzt und abgezogen (dort: phase_ref aus 8 Referenz-Samples) -- + das machen wir hier identisch über compensate_references(). + + 2) MSBeni/AoA_IQsamples (github.com/MSBeni/AoA_IQsamples, Ordner AoA/DoA_Final) + -> Kernidee übernommen: mehrere klassische DOA-Schätzer (Bartlett, MUSIC) + nebeneinander auf DERSELBEN Kovarianzmatrix laufen lassen, als + gegenseitige Plausibilitätsprüfung (Bartlett ist unempfindlicher + gegenüber Rangproblemen der Kovarianzmatrix, aber unschärfer als MUSIC). + +Warum neu aufgesetzt: das bisherige Skript (bg22_AoA.py) hatte zu viele bewegliche, +gegenseitig abhängige Teile (3 Switching-Pattern-Hypothesen, Pro-Kanal-Phasen- +kalibrierungsdatei, 3 Winkel-Algorithmen) -- jede Kombination aus falschem Pattern ++ falscher/fehlender Kalibrierungsdatei hat zu stundenlangem Debugging geführt. +Dieses Skript hier: + - nutzt NUR das offizielle Silicon-Labs-Doku-Beispiel-Switching-Pattern + (0,1,2,...,15 -- keine Alternative mehr, keine Flags dafür), + - braucht KEINE separate Kalibrierungsdatei (Phasendrift wird pro Paket live + aus der Referenzperiode geschätzt, wie bei Navigine), + - schätzt die Winkel NUR über die paarweise 1D-Methode (kein 2D-Gitter, kein + ESPRIT, kein Forward-Backward-Averaging -- bewusst reduziert auf das, was in + den beiden Referenzprojekten tatsächlich gezeigt und getestet wurde). +""" +from __future__ import annotations + +import argparse +import json +import queue +import struct +import sys +import time +from collections import deque +from dataclasses import dataclass +from datetime import datetime, timezone +from typing import Any + +import numpy as np +import bgapi + +# SWITCHING-PATTERN, korrigiert 08.10.26 nach der Silicon-Labs-Doku zur BRD4191A +# ("Silicon Labs Dual Polarized Antenna Array Board", RTL-Lib App Development, +# Appendix): 16 Patches mit je einem VERTIKALEN und einem HORIZONTALEN Element, +# also 32 Antennen. Ein Pattern-Wert waehlt ein ELEMENT, nicht einen Patch. +# Offizielle Reihenfolge V1,H1,V2,H2,...,V16,H16: +# +# Patch 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 +# V,H 2,3 0,1 14,15 12,13 4,5 8,24 20,21 10,11 6,7 22,23 16,17 19,18 9,25 26,27 28,29 31,30 +# +# Frueher stand hier range(0, 16) mit der Annahme "Wert k = ANT(k+1)". Tatsaechlich +# wurden damit nur 9 der 16 Patches abgetastet, 7 davon in BEIDEN Polarisationen, und +# auf Rasterplaetze gelegt, an denen sie nicht liegen. Das erklaert alle Befunde seit +# dem 02.10.26: Amplitudenfaktor 7, Paar-Kohaerenz 0.24, Phasenschritt 133.7 Grad +# ueber dem physikalischen Maximum von 117, Boresight richtig, abseits falsch. +# +# Jetzt: EIN Element pro Patch, eine Polarisation, in Patch-Reihenfolge -- damit gilt +# wieder Spalte k == Patch ANT(k+1), und antenna_position() bleibt unveraendert. +# NACH JEDEM WECHSEL NEU KALIBRIEREN: die Offsets haengen am gewaehlten Element. +_PATTERN_V = (2, 0, 14, 12, 4, 8, 20, 10, 6, 22, 16, 19, 9, 26, 28, 31) +_PATTERN_H = (3, 1, 15, 13, 5, 24, 21, 11, 7, 23, 17, 18, 25, 27, 29, 30) +POLARISATION = "V" # "V" oder "H": die mit der hoeheren, gleichmaessigeren Amplitude +RF_ORDER = _PATTERN_V if POLARISATION == "V" else _PATTERN_H +CP_RF_VALUE = 40 # Patch 6 zirkular polarisiert (CP_MODE | 8) -- laut Doku richtig + + +@dataclass +class Config: + references: int = 8 # Anzahl Referenz-Samples am Paketanfang (fuer Phasendrift-Schaetzung) + expected_samples: int = 45 + slot_us: float = 2.0 + reference_interval_us: float = 1.0 + phase_multiplier: float = 2.0 + pitch_x_m: float = 0.040 # Spaltenabstand (Azimut-Achse) + pitch_y_m: float = 0.040 # Zeilenabstand (Elevation-Achse) + flip_azimuth: bool = True # UMGESTELLT 08.10.26 (vorher False): kehrt das Vorzeichen des + # Azimuts um. Technisch wird u gespiegelt (links/rechts); da + # az = atan2(u, -v) gilt, ergibt -u exakt -az, ueber den ganzen + # Kreis und auch bei +-180. Mit --no-flip-azimuth abschaltbar. + flip_elevation: bool = True # Spiegelt v (oben/unten). Achtung: in der Kegel-Konvention dreht das + # den AZIMUT (0 <-> 180), nicht die Elevation -- die ist jetzt immer >= 0. + # BESTAETIGT per Realtest (29.09-01.10.26) gegen den Silicon Labs + # AoA Analyzer: v-Vorzeichen war invertiert, per Default korrigiert. + swap_axes: bool = False # Zeilen/Spalten-Rollen tauschen (Test: Array evtl. um 90° gedreht/row-col vertauscht) + azimuth_mirror: bool = False # OHNE WIRKUNG seit der Kegel-Konvention (07.10.26). Die Spiegelung + # war die Heuristik, mit der der halbe Azimutbereich [-90,+90] auf die + # andere Hemisphaere abgebildet wurde. Der Azimut laeuft jetzt direkt + # ueber den vollen Kreis, damit ist sie gegenstandslos. Das Feld bleibt + # nur stehen, damit aeltere Aufrufe mit --azimuth-mirror nicht brechen. + azimuth_offset_deg: float = 0.0 # Konstanter Azimut-Offset, ADDIERT ganz am Ende. Dreht die + # Nullrichtung. Referenz ist der Silicon Labs AoA Analyzer: gleiches + # Array, gleiche Firmware, gleiche Pakete, nur anderer Algorithmus. + # Am 07.10.26 gegen den Analyzer geprueft (Pfeil im Oberen-Rechten, + # Anzeige 132,97 Grad, Aufdruck dort 135 Grad): die Nullrichtung stimmt + # bereits. Dieser Wert gehoert also auf 0; ein grosser Wert waere ein + # Fehler in der Rechenkette, kein Konventionsunterschied. + elevation_offset_deg: float = 0.0 # Analog fuer Elevation. + + +def ble_frequency_hz(channel: int) -> float: + """BLE-Kanal (0..39) auf Mittenfrequenz in Hz abbilden.""" + if 0 <= channel <= 10: + mhz = 2404 + 2 * channel + elif 11 <= channel <= 36: + mhz = 2428 + 2 * (channel - 11) + elif channel == 37: + mhz = 2402 + elif channel == 38: + mhz = 2426 + elif channel == 39: + mhz = 2480 + else: + raise ValueError(f"BLE Kanal ausserhalb 0..39: {channel}") + return mhz * 1e6 + + +def iq_complex(raw: Any) -> np.ndarray: + """I/Q-Paare zu komplexem Signal, Konvention I - jQ (Silicon Labs).""" + a = np.asarray(raw) + if a.ndim != 2 or a.shape[1] != 2: + raise ValueError("iq muss eine Liste von [I, Q] sein") + return a[:, 0].astype(float) - 1j * a[:, 1].astype(float) + + +DRIFT_GLAETTUNG = 0.05 # Gewicht des neuen Werts im gleitenden Mittel der Phasendrift. + # 1.0 = kein Mittel, nur der Wert des aktuellen Pakets (frueheres + # Verhalten). 0.05 entspricht einem Gedaechtnis von rund 20 Paketen, + # also etwa einer Sekunde bei 50 ms Sendeintervall. +_drift_zeiger: complex = 0j + + +def drift_schaetzen(samples: np.ndarray, cfg: Config) -> float: + """Phasendrift je Referenz-Sample aus der Referenzperiode schätzen. + + Das ist das Prinzip von Navigine's phase_ref: der Mittelwert der Phasenschritte + zwischen aufeinanderfolgenden Referenz-Samples, hier über alle Referenz-Samples + gemittelt statt nur über die ersten beiden. In der Referenzperiode sendet immer + dieselbe Antenne, jede Drehung dort kommt also allein vom Frequenzunterschied + zwischen Tag und Locator. + """ + nref = cfg.references + if len(samples) < nref: + raise ValueError(f"benötige {nref} Referenz-Samples, habe {len(samples)}") + refs = samples[:nref] + if np.any(np.abs(refs) < 1e-12): + raise ValueError("Referenz-Sample mit Betrag 0") + adjacent_products = refs[1:] * np.conj(refs[:-1]) + return float(np.angle(np.mean(np.exp(1j * np.angle(adjacent_products))))) + + +def geglaettete_drift(phase_step: float) -> float: + """Gleitendes Mittel der Phasendrift über die Pakete hinweg. + + Warum: der Frequenzversatz zweier Quarze ändert sich über Sekunden kaum, die + Schätzung aus nur 8 Referenz-Samples streut aber. Gemessen am 03.10.26 über drei + Aufnahmen: Streuung der Drift je Paket rund 0.4 Grad, zwischen den Kanälen dagegen + nur 0.16 Grad -- die Streuung ist also Messrauschen und keine echte Änderung, und + ein kanalübergreifendes Mittel ist zulässig. + + Dieses Rauschen wird in compensate_references mit phase_multiplier * slot_us + multipliziert und schlägt dann auf alle 16 Antennenphasen durch. Mit dem gleitenden + Mittel stieg die Stabilität der Antennenphasen über die Pakete von 0.942 auf 0.967 + (aufnahme_a), von 0.923 auf 0.982 (aufnahme_b) und von 0.904 auf 0.983 (aufnahme_c). + + Gemittelt werden Einheitszeiger, damit der Sprung bei plus oder minus 180 Grad nicht + stört -- die gemessene Drift liegt mit rund 174 Grad dicht daran. + """ + global _drift_zeiger + neu = np.exp(1j * phase_step) + if _drift_zeiger == 0j: + _drift_zeiger = neu + else: + _drift_zeiger = (1.0 - DRIFT_GLAETTUNG) * _drift_zeiger + DRIFT_GLAETTUNG * neu + return float(np.angle(_drift_zeiger)) + + +def compensate_references(samples: np.ndarray, cfg: Config, + phase_step: float | None = None) -> np.ndarray: + """Phasendrift aus dem Paket herausrechnen. + + phase_step: Drift je Referenz-Sample. Ohne Angabe schätzt die Funktion sie allein + aus diesem Paket (Verhalten bis 03.10.26); process_packet übergibt den geglätteten + Wert. + """ + nref = cfg.references + if phase_step is None: + phase_step = drift_schaetzen(samples, cfg) + slope = phase_step / cfg.reference_interval_us + corrected = samples.copy() + corrected[nref:] *= np.exp(-1j * slope * cfg.phase_multiplier * + (np.arange(len(samples) - nref) + 1) * cfg.slot_us) + return corrected + + +def snapshots_from_samples(data: np.ndarray, cfg: Config) -> np.ndarray: + """Daten in der RF-Schaltreihenfolge gruppieren (16 Antennen pro Snapshot-Spalte). + + WICHTIG -- Zykluslaenge ist 17, nicht 16: + Das an den Stack uebergebene Switching-Pattern ist (CP_RF_VALUE,) + RF_ORDER + und damit 17 Eintraege lang. Die Hardware durchlaeuft ALLE 17, auch waehrend + der Switching-Phase; die Referenzantenne laeuft also im Zyklus mit. + + Gemessen am 02.10.26 (struktur_check.py, 97 Pakete auf einem Kanal): + Deckung aufeinanderfolgender Zyklen 0.999 bei Zykluslaenge 17, dagegen 0.519 + bei der naechstbesten Laenge. Mit dem frueheren 16er-Schnitt verrutschte die + Antennenzuordnung je Zyklus um eine Stelle. Folge auf denselben Paketen: + Eigenwert-Verhaeltnis der Kovarianz 3.6 statt 2313, Kohaerenz je Antennenpaar + 0.796 statt 1.000, Pseudospektrum flach, Ausgabe nur noch conf = 1.0 mit + Werten in den Ecken des Suchrasters. + + Bei cfg.references = 8 beginnt der hier uebergebene Ausschnitt mit + RF_ORDER[0]; CP_RF_VALUE liegt dann an der letzten Stelle des Zyklus und + wird verworfen. Die Spalten 0..15 entsprechen damit weiterhin genau den + Antennenindizes aus antenna_position(). + """ + n_ant = len(RF_ORDER) # 16 Rasterantennen + n_cycle = n_ant + 1 # 17: CP_RF_VALUE laeuft im Zyklus mit + n_full = len(data) // n_cycle + if n_full < 1: + raise ValueError(f"nur {len(data)} Antennen-Samples; brauche {n_cycle}") + used = data[:n_full * n_cycle] + return used.reshape(n_full, n_cycle)[:, :n_ant].T + + +def antenna_position(ant_index: int) -> tuple[float, float, float]: + """Physische Position (x, y, z) einer Antenne in Metern, 0-basiert (ant_index=0 -> ANT1). + Board in der Y-Z-Ebene, Blickrichtung entlang X. Layout bestätigt über den + Silicon Labs AoA Analyzer (3D-View, "Dual Polarized Board" / "Silicon Labs + Ref. 4x4 Uniform Dual Polarized Rectangular Array"): + + ANT13 ANT9 ANT5 ANT1 + ANT14 ANT10 ANT6 ANT2 + ANT15 ANT11 ANT7 ANT3 + ANT16 ANT12 ANT8 ANT4 + + ANT1..16 spaltenweise gruppiert, Spalten von RECHTS nach LINKS; innerhalb + einer Gruppe von oben nach unten. Da wir hier NUR das Simple-Pattern (0..15 + identisch) nutzen, ist logische Position == physischer Index, d.h. diese + Funktion bestimmt die Geometrie direkt (keine rf_order-Indirektion mehr nötig). + """ + group = ant_index // 4 + row_in_col = ant_index % 4 + col = 3 - group + return (0.0, col, row_in_col) # in Pitch-Einheiten, s. coordinates() + + +def coordinates(cfg: Config) -> np.ndarray: + """Antennenpositionen in Metern, in Snapshot-Spaltenreihenfolge (0..15). + cfg.swap_axes tauscht testweise die Rollen von Spalte (Azimut) und Zeile + (Elevation) -- falls das physische Array um 90° gedreht montiert ist oder + die Zeilen/Spalten-Zuordnung aus dem AoA-Analyzer-Screenshot nicht zur + tatsächlichen Verbauung passt.""" + xyz_units = np.array([antenna_position(i) for i in range(16)], dtype=float) + if cfg.swap_axes: + xyz_units = xyz_units[:, [0, 2, 1]] # (x, col, row) -> (x, row, col) + return xyz_units * np.array([1.0, cfg.pitch_x_m, cfg.pitch_y_m]) + + +def find_pairs(xyz: np.ndarray, cfg: Config) -> tuple[list[tuple[int, int]], list[tuple[int, int]]]: + """Vertikale Nachbarn (gleiche Spalte, fuer Elevation) und horizontale Nachbarn + (gleiche Zeile, fuer Azimut) finden -- das sind die "Antennenpaare", auf denen + Navigine's Methode aufbaut.""" + n_ant = xyz.shape[0] + tol = 1e-6 + vertical_pairs, horizontal_pairs = [], [] + for i in range(n_ant): + for j in range(n_ant): + dx, dy, dz = xyz[j] - xyz[i] + if abs(dx) < tol and abs(dy) < tol and abs(dz - cfg.pitch_y_m) < tol: + vertical_pairs.append((i, j)) + if abs(dx) < tol and abs(dy - cfg.pitch_x_m) < tol and abs(dz) < tol: + horizontal_pairs.append((i, j)) + return vertical_pairs, horizontal_pairs + + +def scan_1d(X: np.ndarray, pairs: list[tuple[int, int]], wave: float, spacing: float, + coupling: float = 1.0, step_deg: float = 1.0) -> dict: + """Virtuelles 2-Antennen-Array aus ALLEN (Paar x Snapshot)-Kombinationen bauen + und sowohl MUSIC als auch Bartlett über -90..+90 Grad laufen lassen (Navigine + fuer MUSIC, MSBeni fuer Bartlett als Gegenprobe). Gibt beide Schätzungen + zurueck, damit man sie im Log vergleichen kann.""" + ref = np.array([X[i, :] for i, j in pairs]) + nxt = np.array([X[j, :] for i, j in pairs]) + Xv = np.stack([ref.reshape(-1), nxt.reshape(-1)]) # 2 x (n_pairs*n_snap) + n = Xv.shape[1] + R = (Xv @ Xv.conj().T) / n + R = R + (1e-3 * np.trace(R).real / 2) * np.eye(2) + + vals, vecs = np.linalg.eigh(R) + order = np.argsort(vals)[::-1] + vals, vecs = vals[order], vecs[:, order] + En = vecs[:, 1:] # 1 Quelle, 2 Antennen -> 1-dim. Rauschunterraum + + angles = np.arange(-90.0, 90.0 + step_deg / 2, step_deg) + music_power = np.empty(len(angles)) + bartlett_power = np.empty(len(angles)) + for idx, ang in enumerate(np.radians(angles)): + a = np.array([1.0, np.exp(1j * wave * spacing * coupling * np.sin(ang))])[:, None] + denom = float(np.linalg.norm(En.conj().T @ a) ** 2) + music_power[idx] = 1.0 / max(denom, 1e-15) + bartlett_power[idx] = float(np.abs((a.conj().T @ R @ a))[0, 0]) # Bartlett/Fourier-Beamformer + + music_peak = int(np.argmax(music_power)) + bartlett_peak = int(np.argmax(bartlett_power)) + return { + "music_deg": float(angles[music_peak]), + "music_confidence": float(music_power[music_peak] / (np.median(music_power) + 1e-15)), + "bartlett_deg": float(angles[bartlett_peak]), + "bartlett_confidence": float(bartlett_power[bartlett_peak] / (np.median(bartlett_power) + 1e-15)), + } + + +def mirror_azimuth(az_deg: float) -> float: + """Spiegelt einen im sicheren Bereich [-90,+90] gemessenen Azimut an der 90°-Achse + auf die physikalisch korrekte Vorne/Hinten-Hemisphaere (siehe Config.azimuth_mirror): + 25° -> 155°, -25° -> -155°, 90°/-90° bleiben an der Mehrdeutigkeitsgrenze (dort ist + cos(az)~0, Vorne/Hinten ohnehin nicht unterscheidbar -- s. on_boundary-Check).""" + if az_deg >= 0: + return 180.0 - az_deg + return -180.0 - az_deg + + +def doa_pairs(X: np.ndarray, frequency_hz: float, cfg: Config) -> dict: + """Haupt-Winkel-Schaetzung in der KEGEL-KONVENTION (umgestellt 07.10.26). + + Ausgabe: + Azimut 0 .. +360 Grad -- Drehwinkel UM die Boresight-Achse + (Bereich umstellbar, s. AZIMUT_0_360) + Elevation 0 .. +90 Grad -- Winkel ueber der ARRAY-EBENE + (90 = Boresight, 0 = in der Array-Ebene), + gleiche Achse wie der Silicon Labs AoA Analyzer + + Warum diese Konvention die richtige fuer ein planares Array ist: + gemessen werden zwei Richtungskosinus, u entlang der Spaltenachse (+y) und + v entlang der Zeilenachse (+z). Beide zusammen legen den Drehwinkel um die + Boresight-Achse EINDEUTIG fest -- daher der volle Kreis beim Azimut. Was ein + planares Array prinzipiell NICHT unterscheiden kann, ist vorne von hinten + (eine Quelle bei +w und bei -w erzeugt dieselben Phasen). Genau diese + Mehrdeutigkeit ist der Grund, warum die Elevation nur 0 bis 90 Grad + abdeckt: der gesamte Messbereich ist die vordere Halbkugel. + + Die Elevation zaehlt ab der ARRAY-EBENE, nicht ab der Boresight-Achse: 90 Grad + heisst frontal, 0 Grad heisst in der Ebene liegend. Das ist die Achse des + Silicon Labs AoA Analyzer, am 07.10.26 am Aufbau bestaetigt (Tag frontal -> + Analyzer zeigt nahe 90). Damit sind beide Anzeigen direkt vergleichbar. + + Die alte az/el-Konvention (beide Achsen -90..+90) warf dagegen Information + weg: sie presste den vollen Drehwinkel in einen Halbkreis und brauchte dafuer + mirror_azimuth() als Heuristik. Das entfaellt jetzt. + + Nullrichtung des Azimuts: 0 Grad zeigt NACH UNTEN, +90 Grad nach rechts + (Blick auf die Antennenseite), 180 Grad nach oben, -90 Grad nach links -- + also so, wie es auf der Karte aufgedruckt ist. Stimmt das bei dir nicht, + verdreht cfg.azimuth_offset_deg die Nullrichtung. + + Nebeneffekt: die Azimut-Paare laufen jetzt mit coupling = -1 statt + -cos(Elevation). Der cos-Faktor war ein Artefakt der alten Parametrisierung. + Die beiden 1D-Scans messen u und v unabhaengig voneinander, wie es der + Physik eines planaren Arrays entspricht; die Kopplung entsteht erst bei der + Umrechnung in Winkel. + """ + xyz = coordinates(cfg) + n_ant, n_snap = X.shape + if n_ant != 16 or n_snap < 2: + raise ValueError("doa_pairs braucht 16 Antennen und mind. 2 Snapshots") + vertical_pairs, horizontal_pairs = find_pairs(xyz, cfg) + wave = 2 * np.pi * frequency_hz / 299_792_458.0 + + hor = scan_1d(X, horizontal_pairs, wave, cfg.pitch_x_m, coupling=-1.0) + ver = scan_1d(X, vertical_pairs, wave, cfg.pitch_y_m) + + u_sign = -1.0 if cfg.flip_azimuth else 1.0 + v_sign = -1.0 if cfg.flip_elevation else 1.0 + + def in_winkel(h_deg: float, v_deg: float) -> tuple[float, float]: + u = u_sign * float(np.sin(np.radians(h_deg))) + v = v_sign * float(np.sin(np.radians(v_deg))) + return winkel_aus_richtungskosinus(u, v) + + az_deg, el_deg = in_winkel(hor["music_deg"], ver["music_deg"]) + az_bartlett_deg, el_bartlett_deg = in_winkel(hor["bartlett_deg"], ver["bartlett_deg"]) + + # Konstante Offsets zuletzt. Azimut wird auf -180..180 zurueckgefaltet, + # Elevation auf 0..90 begrenzt -- ausserhalb gibt es keine Messung. + def falten(w: float) -> float: + w = (w + cfg.azimuth_offset_deg) % 360.0 + return w if AZIMUT_0_360 else (w + 180.0) % 360.0 - 180.0 + az_deg = falten(az_deg) + az_bartlett_deg = falten(az_bartlett_deg) + el_deg = min(max(el_deg + cfg.elevation_offset_deg, 0.0), 90.0) + el_bartlett_deg = min(max(el_bartlett_deg + cfg.elevation_offset_deg, 0.0), 90.0) + + return { + "azimuth_deg": az_deg, + "elevation_deg": el_deg, + "confidence": float(min(ver["music_confidence"], hor["music_confidence"])), + # Bartlett-Gegenprobe (MSBeni): grosse Abweichung von MUSIC = Schaetzung unsicher + "azimuth_bartlett_deg": az_bartlett_deg, + "elevation_bartlett_deg": el_bartlett_deg, + # Seit der Umstellung identisch mit azimuth_deg: es gibt keinen getrennten + # "sicheren" Bereich mehr, der Azimut ist ueber den vollen Kreis eindeutig. + "azimuth_internal_deg": az_deg, + } + + +KALIBRIER_DATEI = "antennen_kalibrierung.json" +_kalibrierung: dict | None = None + + +def lade_kalibrierung() -> dict | None: + """Antennen-Phasenoffsets aus antennen_kalibrierung.json laden, falls vorhanden. + + Fehlt die Datei, laeuft alles wie frueher weiter -- dann allerdings mit dem + bekannten Problem, dass die Mittelung ueber die Antennenpaare in scan_1d an + den unterschiedlichen Geraeteoffsets zerbricht (gemessen 02.10.26: Kohaerenz + je Paar 1.000, nach Mittelung 0.474, Ergebnis conf = 1.0). + + Erzeugt wird die Datei von kalibrierung.py aus einer Boresight-Aufnahme. + """ + try: + with open(KALIBRIER_DATEI, "r", encoding="utf-8") as f: + daten = json.load(f) + except FileNotFoundError: + return None + except (json.JSONDecodeError, OSError) as fehler: + print(f"{KALIBRIER_DATEI} nicht lesbar ({fehler}) -- laufe ohne Kalibrierung.", + file=sys.stderr) + return None + + if list(daten.get("rf_order", [])) != list(RF_ORDER): + print(f"{KALIBRIER_DATEI} wurde mit einem ANDEREN Switching-Pattern aufgenommen " + f"({daten.get('rf_order', 'alt, 0..15')}) -- wird NICHT verwendet. " + f"Neu kalibrieren: Boresight-Aufnahme, dann kalibrierung.py.", file=sys.stderr) + return None + global_rad = np.asarray(daten.get("global_rad", []), dtype=float) + if global_rad.shape != (len(RF_ORDER),): + print(f"{KALIBRIER_DATEI} passt nicht zu {len(RF_ORDER)} Antennen " + f"-- laufe ohne Kalibrierung.", file=sys.stderr) + return None + + kanal = {} + for schluessel, werte in (daten.get("kanal_rad") or {}).items(): + reihe = np.asarray(werte, dtype=float) + if reihe.shape == global_rad.shape: + kanal[int(schluessel)] = reihe + + return {"global": global_rad, "kanal": kanal, + "quelle": daten.get("quelle", "?"), + "stabilitaet": daten.get("stabilitaet"), + "pakete": daten.get("pakete")} + + +def kalibrierung_anwenden(X: np.ndarray, channel: int) -> np.ndarray: + """Feste Phasenoffsets der Antennen herausrechnen. + + Die Offsets sind leicht frequenzabhaengig (Leiterbahnlaufzeit), deshalb wird + der zum Kanal passende Satz bevorzugt; fuer Kanaele ohne eigene Messung + dienen die kanaluebergreifenden Werte als Rueckfallebene. + """ + if _kalibrierung is None: + return X + offsets = _kalibrierung["kanal"].get(channel, _kalibrierung["global"]) + return X * np.exp(-1j * offsets)[:, None] + + +# ------------------------------------------------------------------ Selbstkalibrierung +# Gemessen am 08.10.26 auf 1008 Paketen (iq_samples.jsonl, 37 Kanaele): +# +# Stabilitaet des Phasenvektors INNERHALB eines Kanals 0.944 (19.4 Grad) +# Stabilitaet ueber alle 37 Kanaele gemischt 0.830 (35.0 Grad) +# Streuung der Antennenoffsets ZWISCHEN den Kanaelen 32.4 Grad RMS +# +# Die Offsets sind also kanalabhaengig, und antennen_kalibrierung.json stammt aus +# einem ANDEREN Programmlauf, in dem sie andere Werte hatten (gemessen 37.3 Grad +# RMS Unterschied zwischen Laeufen). Ergebnis: die Datei korrigiert zwar etwas, +# aber nicht die Kanalabhaengigkeit DIESES Laufs. +# +# Abhilfe hier: die Kanaltabelle wird AUS DEM LAUFENDEN LAUF selbst geschaetzt. +# Pro Kanal wird der mittlere (normierte) Phasenvektor gesammelt; die Korrektur +# ist die Abweichung dieses Kanals vom Mittel ueber alle Kanaele. Dafuer muss die +# Richtung NICHT bekannt sein -- es genuegt, dass der Tag waehrend der Lernphase +# still steht. Nach SELBSTKAL_LERNPAKETE wird die Tabelle eingefroren, damit ein +# spaeter bewegter Tag sie nicht verdirbt. +# +# WICHTIG, was das NICHT tut: die festen Offsets je Antenne (Leiterbahnlaufzeit) +# bleiben unangetastet, denn die stecken im Mittel ueber alle Kanaele und werden +# hier gerade herausgerechnet. Gegen den raeumlichen Fehler (Kohaerenz je Paar +# 0.38, Ebene-Welle-Fit 0.57) hilft das also nicht -- gemessen: der Fit aendert +# sich dadurch um 0.001. Diese Funktion verbessert die STREUUNG, nicht die +# absolute Richtung. +# Dimensionierung aus einem Out-of-Sample-Test am 08.10.26 (Tabelle auf den ersten +# N Paketen gelernt, Fehler auf ALLEN UEBRIGEN gemessen, 1008 Pakete / 37 Kanaele): +# +# N Kanaele ohne Korrektur mit Korrektur nach Block von 30 +# 200 22 33.4 Grad 11.5 Grad 6.1 -> 2.1 Grad +# 400 36 32.6 Grad 9.0 Grad 6.0 -> 1.7 Grad +# 600 37 32.9 Grad 7.9 Grad 6.0 -> 1.4 Grad +# Decke (Streuung innerhalb eines Kanals) 7.4 Grad -> 1.4 Grad +# +# Ab 600 Paketen ist die Decke praktisch erreicht, deshalb dieser Wert: rund 12 s +# bei 20 ms Advertising-Intervall. Die Korrektur wird ERST NACH der Lernphase +# angewendet, nie mit einer noch wandernden Tabelle -- eine sich aendernde +# Korrektur verschmiert sonst genau das, was sie reparieren soll (gemessen: +# Streuung innerhalb eines Kanals fiel dabei von 0.992 auf 0.881). +SELBSTKAL_AKTIV = True +SELBSTKAL_MIN_PAKETE = 8 # Pakete auf einem Kanal, damit er in die Tabelle kommt +SELBSTKAL_LERNPAKETE = 600 # danach ist die Tabelle fest und wird angewendet + +_selbstkal_summe: dict[int, np.ndarray] = {} +_selbstkal_anzahl: dict[int, int] = {} +_selbstkal_tabelle: dict[int, np.ndarray] = {} +_selbstkal_gesehen = 0 +_selbstkal_fertig = False + + +def _selbstkal_tabelle_rechnen() -> None: + """Kanaltabelle aus den bisher gesammelten Mittelwerten neu aufbauen.""" + reif = {c: summe / _selbstkal_anzahl[c] + for c, summe in _selbstkal_summe.items() + if _selbstkal_anzahl[c] >= SELBSTKAL_MIN_PAKETE} + if len(reif) < 2: + return + stapel = [] + for vektor in reif.values(): + betrag = np.abs(vektor) + if np.any(betrag < 1e-12): + return + stapel.append(vektor / betrag) + mittel = np.array(stapel).mean(axis=0) + betrag = np.abs(mittel) + if np.any(betrag < 1e-12): + return + mittel = mittel / betrag + _selbstkal_tabelle.clear() + for kanal, vektor in reif.items(): + korrektur = (vektor / np.abs(vektor)) * np.conj(mittel) + _selbstkal_tabelle[kanal] = korrektur / np.abs(korrektur) + + +def selbstkalibrierung_lernen(X: np.ndarray, channel: int) -> None: + """Phasenvektor dieses Pakets in die Kanalstatistik einrechnen.""" + global _selbstkal_gesehen, _selbstkal_fertig + if not SELBSTKAL_AKTIV or _selbstkal_fertig: + return + vektor = X.mean(axis=1) + betrag = np.abs(vektor) + if np.any(betrag < 1e-12): + return + vektor = vektor / betrag + # VERANKERN: jedes CTE-Paket startet mit beliebiger Gesamtphase. Ohne diesen + # Schritt mittelt sich ueber die Pakete genau diese Zufallsphase mit ein und + # die Kanaltabelle wird unbrauchbar (gemessen 08.10.26: 16.0 statt 7.9 Grad + # Restfehler). Fuer die Richtungsschaetzung sind nur Phasen-DIFFERENZEN + # zwischen Antennen relevant, eine gemeinsame Drehung ist also unschaedlich. + anker = vektor.mean() + if abs(anker) < 1e-3: + anker = vektor[0] # Rueckfall, falls der Mittelwert ausloescht + vektor = vektor * np.conj(anker / abs(anker)) + if channel in _selbstkal_summe: + _selbstkal_summe[channel] = _selbstkal_summe[channel] + vektor + _selbstkal_anzahl[channel] += 1 + else: + _selbstkal_summe[channel] = vektor + _selbstkal_anzahl[channel] = 1 + _selbstkal_gesehen += 1 + if _selbstkal_gesehen >= SELBSTKAL_LERNPAKETE: + _selbstkal_tabelle_rechnen() + _selbstkal_fertig = True + fehlt = sorted(k for k, n in _selbstkal_anzahl.items() if n < SELBSTKAL_MIN_PAKETE) + print(f"Selbstkalibrierung: aktiv nach {_selbstkal_gesehen} Paketen, " + f"{len(_selbstkal_tabelle)} Kanaele in der Tabelle" + + (f", ohne Korrektur bleiben {fehlt}" if fehlt else "") + ".", flush=True) + + +def selbstkalibrierung_anwenden(X: np.ndarray, channel: int) -> np.ndarray: + """Kanalabhaengigen Anteil der Antennenoffsets herausrechnen.""" + if not SELBSTKAL_AKTIV or not _selbstkal_fertig: + return X # waehrend der Lernphase unveraendert, s. Kommentar oben + korrektur = _selbstkal_tabelle.get(channel) + if korrektur is None: + return X + return X * np.conj(korrektur)[:, None] + + +def process_packet(packet: dict, cfg: Config) -> dict | None: + """Ein IQ-Paket verarbeiten und Snapshots fuer die Winkel-Schätzung liefern (oder None).""" + ch = int(packet["channel"]) + f = ble_frequency_hz(ch) + samples = iq_complex(packet["iq"]) + if len(samples) != cfg.expected_samples: + return None + fixed = compensate_references(samples, cfg, geglaettete_drift(drift_schaetzen(samples, cfg))) + X = snapshots_from_samples(fixed[cfg.references:], cfg) + X = kalibrierung_anwenden(X, ch) + selbstkalibrierung_lernen(X, ch) # lernt auf dem noch unkorrigierten Vektor + X = selbstkalibrierung_anwenden(X, ch) + return {"X": X, "frequency_hz": f} + + +COM_PORT = "COM13" +BAUD_RATE = 115200 +XAPI_PATH = r"C:\Users\sandr\.silabs\slt\installs\conan\p\simpleca33d691c539\p\bluetooth_le_host\api\sl_bt.xapi" +IQ_FILE = "iq_samples.jsonl" +ANGLE_TABLE_FILE = "winkel_tabelle.json" +SLOT_DURATION_US = 2 +CTE_COUNT = 0 +ACCUMULATE_PACKETS = 1 # MUSIC nach JEDEM Paket (03.10.26). Ein Paket liefert 2 Snapshots + # (2 volle 17er-Zyklen aus 45 Samples), also 2 x 12 Paare = 24 Spalten + # im virtuellen Array von scan_1d -- genug fuer die 2x2-Kovarianz, aber + # deutlich verrauschter als vorher mit 8 Paketen. Die Streuung faengt + # dafuer die Glaettung ueber FILTER_WINDOW Werte auf. +FILTER_WINDOW = 30 # Blockgroesse (07.10.26, vorher 100): je 50 angenommene Schaetzungen ergeben EINEN Mittelwert. + # Bei ACCUMULATE_PACKETS = 1 und 40 ms Sendeintervall sind das rund + # 4 Sekunden -- vorausgesetzt, alle Pakete kommen durch den Filter. +FILTER_MAX_ALTER_S = 8.0 # Zusaetzliche Altersgrenze (05.10.26). FILTER_WINDOW allein zaehlt + # ANGENOMMENE Schaetzungen, nicht Pakete: faellt die Annahmequote auf + # 20 %, umfasst das Fenster 20 s statt 4 s und der Ausgabewert haengt + # minutenlang an einer alten Kartenstellung fest. Werte, die aelter + # als FILTER_MAX_ALTER_S sind, verfallen deshalb unabhaengig davon. +AUSREISSER_SIGMA = 2.5 # Verwirft Werte, die weiter als 2.5 Sigma vom gewichteten Schwerpunkt + # liegen (05.10.26). CONFIDENCE_MIN allein genuegt nicht: ein Multipath- + # Peak ist scharf, also hoch-konfident UND falsch. +PRINT_EVERY_N_PACKETS = 10 +CONFIDENCE_MIN = 1.0 # Praktisch AUS seit 07.10.26 (vorher 5.0). Der AoA Analyzer kennt keine + # solche Schwelle und liefert auch bei streifendem Einfall einen Wert; dieses + # Skript soll sich genauso verhalten. Gemessen an der Aufnahme von 23:14: + # mit 5.0 wurden 966 von 969 Paketen verworfen, die Winkel-Tabelle blieb leer. + # Die Konfidenz steht weiter in jeder Logzeile -- sie bewertet die Schaetzung, + # verwirft sie aber nicht mehr. + +lib = None +iq_file = None +already_synced = False +pending_actions = queue.Queue() +snapshot_buffer: dict[int, list[np.ndarray]] = {} +angle_table: list[dict] = [] +last_azimut = None +last_elevation = None +# Ein gemeinsamer Puffer statt zwei getrennter (05.10.26): Azimut und Elevation gehoeren +# zusammen, weil der Azimut-Scan mit coupling = -cos(el) der GLEICHEN Schaetzung rechnet. +# Eintrag: (Zeitstempel, u, v, Gewicht) -- u/v sind Richtungskosinus, s. richtungskosinus(). +_recent: deque = deque() # kein maxlen: der Block wird nach FILTER_WINDOW Werten geleert +_letzter_block: tuple | None = None # (az, el, streuung, n) des zuletzt fertigen Blocks +ACTIVE_CONFIG = Config() +SHUTTING_DOWN = False # verhindert Schreibzugriff auf iq_file nach dessen Schliessung beim Beenden + + +def rssi_weight(rssi_dbm: int) -> float: + """Gewichtungsfaktor aus RSSI: stärkeres Signal zählt mehr im gewichteten Median.""" + return max(rssi_dbm + 100.0, 1.0) + + + +AZIMUT_0_360 = False # Ausgabebereich des Azimuts (08.10.26, umgestellt): -180 bis +180, + # wie der AoA Analyzer. Keine Werte ueber 180 Grad mehr. True: 0 bis 360, keine + # negativen Werte. False: -180 bis +180. Nur die DARSTELLUNG aendert sich, + # gerechnet wird weiter in Richtungskosinus -- 350 und -10 sind dieselbe + # Richtung. Die Mittelung bleibt davon unberuehrt, weil sie u und v mittelt + # und nicht Gradzahlen; ein Mittelwert um 0 herum kippt also nicht. + + +def richtungskosinus(az_deg: float, el_deg: float) -> tuple[float, float]: + """(Azimut, Elevation) -> Richtungskosinus (u, v) in der Array-Ebene. + + u = Anteil entlang +y (Spaltenachse, nach rechts) + v = Anteil entlang +z (Zeilenachse, nach oben) + + Kegel-Konvention (07.10.26): el ist der Kegelwinkel zur Boresight-Achse + (0 = frontal, 90 = in der Array-Ebene), az der Drehwinkel um diese Achse mit + 0 Grad nach unten und +90 Grad nach rechts. + + u und v sind das, was das Array physikalisch misst: die Phasenrampe ueber die + Antennen ist LINEAR in u bzw. v, nicht in Grad. Deshalb wird in diesem Raum + gemittelt und erst am Ende zurueckgerechnet. + """ + el = np.radians(min(max(el_deg, 0.0), 90.0)) + az = np.radians(az_deg) + r = np.cos(el) # 90 Grad = Boresight -> r = 0; 0 Grad = Array-Ebene -> r = 1 + return float(r * np.sin(az)), float(-r * np.cos(az)) + + +def winkel_aus_richtungskosinus(u: float, v: float) -> tuple[float, float]: + """Rueckweg von (u, v) nach (Azimut -180..180, Elevation 0..90). + + u^2 + v^2 > 1 kann durch Rauschen entstehen und hat keine Richtung mehr: + der Punkt wird dann auf den Einheitskreis gezogen, also auf Elevation 90. + """ + r = float(np.hypot(u, v)) + if r > 1.0: + u, v, r = u / r, v / r, 1.0 + el_deg = float(np.degrees(np.arccos(r))) + if r < 1e-9: + return 0.0, el_deg + az_deg = float(np.degrees(np.arctan2(u, -v))) + if AZIMUT_0_360: + az_deg = az_deg % 360.0 + return az_deg, el_deg + + +def schaetz_gewicht(confidence: float) -> float: + """Gewicht einer Einzelschaetzung fuer die Glaettung. + + Bis 04.10.26 wurde mit rssi_weight() gewichtet -- das war der Fehler in der + Glaettung: RSSI sagt, wie STARK das Signal war, nicht wie GUT die Schaetzung + ist. Ueber den praktischen Bereich (-45 bis -65 dBm) spreizt rssi_weight nur + 55:35, also faktisch gar nicht, waehrend die Konfidenz zwischen 5 und 105 + schwankt. Eine Schaetzung, die den Filter gerade so passiert hat (conf 5.1), + zaehlte damit genauso viel wie eine saubere (conf 105). + """ + return (max(confidence - CONFIDENCE_MIN, 0.0) + 1.0) ** 2 + + +def glaettung_zuruecksetzen() -> None: + """Angefangenen Block und letztes Ergebnis verwerfen -- nach dem Umstellen der + Karte aufrufen, dann vermischt der naechste Block keine zwei Stellungen.""" + global _letzter_block + _recent.clear() + _letzter_block = None + + +def smoothed_angle(new_az: float, new_el: float, + weight: float) -> tuple[float | None, float | None, float | None, int, bool]: + """BLOCKMITTELWERT (05.10.26): sammelt FILTER_WINDOW Schaetzungen und gibt dann + EINEN Mittelwert aus, danach beginnt ein neuer, leerer Block. + + Rueckgabe: (Azimut, Elevation, Streuung in Grad, Anzahl, neu). + "neu" ist nur in dem Paket True, in dem ein Block fertig geworden ist -- nur + dann kommt ein Wert in die Winkel-Tabelle. Solange der erste Block laeuft, + sind Azimut/Elevation/Streuung None; danach wird zwischen zwei fertigen + Bloecken der zuletzt fertige Wert weitergereicht, damit die Anzeige nicht leer + faellt. + + Unterschied zum gleitenden Mittel (Stand bis 05.10.26): dort lief ein Fenster + von 100 Werten mit, und jedes Paket lieferte einen neuen, mit dem vorherigen + stark korrelierten Ausgabewert. Hier ist jeder Ausgabewert aus 100 FRISCHEN, + nicht wiederverwendeten Schaetzungen gerechnet. Die Bloecke sind damit + voneinander unabhaengig, was sie fuer eine Messreihe erst auswertbar macht: + zwei aufeinanderfolgende Bloecke sind zwei echte Wiederholungsmessungen. + Preis dafuer: ein Ausgabewert nur noch alle FILTER_WINDOW Pakete, bei rund + 19 Paketen/s also etwa alle 2,6 Sekunden + (und entsprechend laenger, wenn der Konfidenzfilter Pakete verwirft). + + Gerechnet wird weiter in Richtungskosinus (s. richtungskosinus()), damit + Azimut und Elevation ihre Kopplung ueber cos(el) behalten. + """ + global _letzter_block + jetzt = time.monotonic() + + # Lange Pause: angefangenen Block verwerfen, sonst mischt er zwei Stellungen. + if _recent and (jetzt - _recent[-1][0]) > FILTER_MAX_ALTER_S: + _recent.clear() + + u, v = richtungskosinus(new_az, new_el) + _recent.append((jetzt, u, v, max(weight, 1e-6))) + + if len(_recent) < FILTER_WINDOW: + if _letzter_block is None: + return None, None, None, len(_recent), False + az, el, s, n = _letzter_block + return az, el, s, n, False + + daten = np.asarray(_recent, dtype=float) + _recent.clear() # Block abgeschlossen, naechster faengt bei 0 an + uu, vv, ww = daten[:, 1], daten[:, 2], daten[:, 3] + u_m = float(np.average(uu, weights=ww)) + v_m = float(np.average(vv, weights=ww)) + + # Eine Ausreisser-Runde: Mittelwert bleibt Mittelwert, aber ein einzelner + # Multipath-Treffer kippt den Block nicht. + abstand = np.hypot(uu - u_m, vv - v_m) + sigma = float(np.sqrt(np.average(abstand ** 2, weights=ww))) + if sigma > 1e-9: + gut = abstand <= AUSREISSER_SIGMA * sigma + if int(gut.sum()) >= 4: + uu, vv, ww = uu[gut], vv[gut], ww[gut] + u_m = float(np.average(uu, weights=ww)) + v_m = float(np.average(vv, weights=ww)) + + streuung = float(np.degrees(np.sqrt(np.average((uu - u_m) ** 2 + (vv - v_m) ** 2, weights=ww)))) + az_deg, el_deg = winkel_aus_richtungskosinus(u_m, v_m) + _letzter_block = (az_deg, el_deg, streuung, int(len(ww))) + return az_deg, el_deg, streuung, int(len(ww)), True + + +def signed_iq(raw_bytes: bytes) -> tuple[list[int], list[int]]: + """Rohbytes in vorzeichenbehaftete I- und Q-Listen entpacken.""" + values = struct.unpack(f"{len(raw_bytes)}b", raw_bytes) + return list(values[0::2]), list(values[1::2]) + + +# ---------------------------------------------------------------- Ausgabe-Konvention +# Intern rechnet alles in der KEGEL-Konvention: Elevation 90 = Boresight, 0 = in der +# Array-Ebene. In dieser Form wird auch gemittelt (ueber u und v). Sie bleibt +# unangetastet: richtungskosinus() und winkel_aus_richtungskosinus() sind zueinander +# invers und duerfen nicht auseinanderlaufen, sonst bricht die Glaettung. +# +# Der AoA Analyzer misst die Elevation VOM Boresight aus: dort 0 statt 90. Beide +# beschreiben dieselbe Richtung, sie sind komplementaer. Abgeleitet aus zwei Messungen +# am selben Aufbau (08.10.26): +# +# Boresight Analyzer Az 1.54 El 2.06 +# Stellung "133" Analyzer Az 133.67 El 10.83 +# +# Ein Azimut von 133 Grad schliesst aus, dass der Azimut ein horizontaler Winkel ist +# -- ein solcher waere auf +-90 begrenzt. Der Analyzer benutzt also dieselbe polare +# Form wie dieses Skript (Azimut = Drehung um die Boresight-Achse, -180..180), nur mit +# dem anderen Nullpunkt der Elevation. +# +# NOCH ZU BESTAETIGEN: die Elevation stimmt mit beiden Messpunkten ueberein, das +# Vorzeichen des Azimuts abseits der Achse nicht -- am Boresight ist der Azimut +# bedeutungslos und taugt nicht als Beleg. Dafuer eine Messung in Stellung "133" mit +# der frischen Kalibrierung gegen Az 133.67 / El 10.83 halten. +AUSGABE_KONVENTION = "analyzer" # = interne Kegel-Form: Elevation 90 = Boresight, wie der Analyzer + + +def ausgabe_winkel(az_deg: float, el_deg: float) -> tuple[float, float]: + """Ausgabe in der Konvention des AoA Analyzers -- sie ist IDENTISCH mit der + internen Kegel-Konvention, es wird nichts umgerechnet. + + Belegt am 08.10.26 durch zwei Analyzer-Ansichten am selben Aufbau: + - Tag frontal vor der Karte (Pfeil von vorn als Kreisscheibe zu sehen): + Az 4.39, El 82.29 -> Elevation 90 = Boresight + - Azimut-Marken 0/45/90/135/180/-135/-90/-45 rings um die Karte, 0 unten, + 90 rechts -> Azimut = Drehung um die Boresight-Achse + Genau so rechnet dieses Skript intern. Zwei zwischenzeitliche Umrechnungen + (90 - el, dann sphaerisch) beruhten auf der Fehldeutung einer Analyzer-Anzeige + El 2.06 als "frontal" -- der Tag lag dort tatsaechlich fast in der Array-Ebene. + """ + return az_deg, el_deg + + +def add_to_angle_table(zeit: str, counter: int, azimut: float, elevation: float) -> None: + """Neuen Winkel nur speichern, wenn er sich vom letzten unterscheidet.""" + global last_azimut, last_elevation + if azimut == last_azimut and elevation == last_elevation: + return + last_azimut, last_elevation = azimut, elevation + angle_table.append({"zeit": zeit, "counter": counter, "azimut": azimut, "elevation": elevation}) + with open(ANGLE_TABLE_FILE, "w", encoding="utf-8") as f: + json.dump(angle_table, f, indent=2, ensure_ascii=False) + + +def save_iq_report(event: Any) -> None: + """IQ-Paket speichern, Pakete akkumulieren und Winkel schätzen.""" + global snapshot_buffer + i_values, q_values = signed_iq(event.samples) + channel = int(event.channel) + packet = {"channel": channel, "rssi": int(event.rssi), "iq": list(zip(i_values, q_values))} + zeit = datetime.now(timezone.utc).isoformat() + record = {"zeit": zeit, "counter": int(event.event_counter), "channel": channel, + "rssi": int(event.rssi), "i": i_values, "q": q_values, + "azimut": None, "elevation": None} + + result = process_packet(packet, ACTIVE_CONFIG) + if result is not None: + channel_buffer = snapshot_buffer.setdefault(channel, []) + channel_buffer.append(result["X"]) + if len(channel_buffer) >= ACCUMULATE_PACKETS: + combined = np.concatenate(channel_buffer, axis=1) + snapshot_buffer[channel] = [] + try: + angles = doa_pairs(combined, result["frequency_hz"], ACTIVE_CONFIG) + az_raw, el_raw, conf = angles["azimuth_deg"], angles["elevation_deg"], angles["confidence"] + # Randwert-Pruefung auf dem UNGESPIEGELTEN Azimut: nahe +-90° ist cos(az)~0, dort ist + # die Vorne/Hinten-Spiegelung selbst instabil (kleine Fehler koennen die Hemisphaere + # kippen) -- unabhaengig davon, wie weit der gespiegelte, berichtete Wert az_raw liegt. + # Randwert-Filter ENTFERNT am 07.10.26 auf Wunsch: streifende Einfaelle + # (Elevation nahe 0, Quelle in der Array-Ebene) werden nicht mehr verworfen. + # Der AoA Analyzer liefert dort ebenfalls einen Wert, also soll dieses Skript + # es auch. Der Azimut ist dort allerdings empfindlicher, weil u^2+v^2 gegen 1 + # geht -- sichtbar an der Konfidenz, nicht mehr an einem harten Filter. + on_boundary = False + low_confidence = conf < CONFIDENCE_MIN + bartlett_diff = (abs(angles["azimuth_bartlett_deg"] - az_raw) + + abs(angles["elevation_bartlett_deg"] - el_raw)) + if on_boundary or low_confidence: + reason = "Randwert" if on_boundary else "geringe Konfidenz" + print(f">>> AoA roh (verworfen, {reason}, conf={conf:.1f}): " + f"Azimut={az_raw:+.1f}° Elevation={el_raw:+.1f}°", + file=sys.stderr, flush=True) + else: + az_smooth, el_smooth, streuung, n_used, block_fertig = smoothed_angle( + az_raw, el_raw, schaetz_gewicht(conf)) + record["azimut"] = az_smooth + record["elevation"] = el_smooth + record["streuung"] = streuung + record["n_block"] = n_used + record["block_fertig"] = block_fertig + # Bartlett-Werte werden seit 05.10.26 nicht mehr ausgegeben (zu unruhig + # auf der Konsole). Die Gegenprobe LAEUFT weiter, nur die Zahlen bleiben + # unsichtbar; weicht sie stark ab, erscheint weiterhin ein kurzer Hinweis. + bart = " [ACHTUNG: Gegenprobe weicht ab]" if bartlett_diff > 15.0 else "" + az_raw_out, el_raw_out = ausgabe_winkel(az_raw, el_raw) + roh = (f">>> AoA roh: Azimut={az_raw_out:+.1f}° Elevation={el_raw_out:+.1f}° " + f"(conf={conf:.1f})") + if az_smooth is None: + # Erster Block laeuft noch, es gibt schlicht noch keinen Mittelwert. + print(f"{roh} | sammelt {n_used}/{FILTER_WINDOW}{bart}", + file=sys.stderr, flush=True) + else: + marke = " <<<<< BLOCK FERTIG" if block_fertig else "" + stand = (f"{FILTER_WINDOW}/{FILTER_WINDOW}" if block_fertig + else f"sammelt {len(_recent)}/{FILTER_WINDOW}") + az_s_out, el_s_out = ausgabe_winkel(az_smooth, el_smooth) + print(f"{roh} | Block-Mittel: Azimut={az_s_out:+.1f}° " + f"Elevation={el_s_out:+.1f}° (n={n_used}, " + f"Streuung={streuung:.1f}°, {stand}){bart}{marke}", + file=sys.stderr, flush=True) + # Nur fertige Bloecke in die Tabelle: eine Zeile = eine + # unabhaengige Messung aus FILTER_WINDOW frischen Schaetzungen. + if block_fertig: + add_to_angle_table(zeit, int(event.event_counter), az_s_out, el_s_out) + except ValueError: + pass + + if SHUTTING_DOWN: + return + try: + iq_file.write(json.dumps(record, separators=(",", ":")) + "\n") + iq_file.flush() + except ValueError: + # Datei wurde waehrend des Beendens (Strg+C) bereits geschlossen; der BGAPI- + # Event-Thread kann noch kurz nachlaufen, das ist harmlos. + return + if int(event.event_counter) % PRINT_EVERY_N_PACKETS == 0: + print(f"IQ-Paket #{event.event_counter} Kanal={channel} RSSI={event.rssi} dBm " + f"Samples={len(i_values)}", file=sys.stderr, flush=True) + + +def on_event(event: Any) -> None: + """BGAPI-Ereignisse behandeln: Scan, Sync, CTE-Empfang.""" + global already_synced + name = str(event).split("(")[0] + if name in ("bt_evt_scanner_scan_report", "bt_evt_scanner_extended_advertisement_report"): + interval = getattr(event, "periodic_interval", None) + if interval and not already_synced: + already_synced = True + print(f"Tag gefunden RSSI={event.rssi} dBm Intervall={interval} ms", file=sys.stderr) + pending_actions.put(("sync", event.address, event.address_type, getattr(event, "adv_sid", 0))) + elif name in ("bt_evt_sync_opened", "bt_evt_sync_scanner_opened", "bt_evt_periodic_sync_opened"): + print(f"Synchronisation hergestellt (sync={event.sync})", file=sys.stderr) + pending_actions.put(("cte", event.sync)) + elif name in ("bt_evt_cte_receiver_connectionless_iq_report", "bt_evt_periodic_sync_iq_report", + "bt_evt_cte_receiver_iq_report"): + save_iq_report(event) + elif hasattr(event, "result") and event.result: + print(f"Fehler BG22: {event}", file=sys.stderr) + + +def main() -> int: + """Verbindung zum BG22 aufbauen, Tag suchen und CTE-Empfang starten.""" + global lib, iq_file, ACTIVE_CONFIG, ANGLE_TABLE_FILE, CONFIDENCE_MIN, ACCUMULATE_PACKETS + global _kalibrierung + parser = argparse.ArgumentParser( + description="BG22 IQ-Logger + Winkel-Schätzung (Navigine/MSBeni-Prinzip, kein Switching-" + "Pattern-Streit, keine Kalibrierdatei)") + parser.add_argument("--port", default=COM_PORT) + parser.add_argument("--baud", type=int, default=BAUD_RATE) + parser.add_argument("--xapi", default=XAPI_PATH) + parser.add_argument("--iq-out", default=IQ_FILE) + parser.add_argument("--angle-out", default=ANGLE_TABLE_FILE) + parser.add_argument("--pitch-x-m", type=float, default=0.040) + parser.add_argument("--pitch-y-m", type=float, default=0.040) + parser.add_argument("--phase-multiplier", type=float, default=2.0) + parser.add_argument("--accumulate", type=int, default=ACCUMULATE_PACKETS, + help="Anzahl Pakete pro Winkel-Schätzung akkumuliert (mehr = stabiler, langsamer)") + parser.add_argument("--confidence-min", type=float, default=CONFIDENCE_MIN, + help="Minimale Konfidenz (Peak/Median-Verhältnis) zum Akzeptieren einer Schätzung") + parser.add_argument("--flip-azimuth", action="store_true", dest="flip_azimuth", default=True, + help="Azimut-Vorzeichenumkehr EINSCHALTEN (seit 08.10.26 ohnehin der Default)") + parser.add_argument("--no-flip-azimuth", action="store_false", dest="flip_azimuth", + help="Azimut-Vorzeichenumkehr AUSSCHALTEN, liefert wieder das alte Vorzeichen") + parser.add_argument("--no-flip-elevation", action="store_false", dest="flip_elevation", default=True, + help="Spiegelung von v AUSSCHALTEN (per Default AN -- per Realtest am " + "01.10.26 gegen den AoA Analyzer bestätigt)") + parser.add_argument("--swap-axes", action="store_true", + help="Zeilen/Spalten-Rollen tauschen (Test: Array evtl. um 90° gedreht montiert)") + parser.add_argument("--no-azimuth-mirror", action="store_false", dest="azimuth_mirror", default=False, + help="Azimut-Hemisphaeren-Spiegelung AUSSCHALTEN (seit 03.10.26 ohnehin der " + "Default, Flag nur noch zur Abwaertskompatibilitaet)") + parser.add_argument("--azimuth-mirror", action="store_true", dest="azimuth_mirror", default=False, + help="Ohne Wirkung seit der Kegel-Konvention (07.10.26), Flag nur noch " + "zur Abwaertskompatibilitaet, s. Config.azimuth_mirror.") + parser.add_argument("--az-offset", type=float, default=0.0, + help="Konstanter Azimut-Offset in Grad, dreht die Nullrichtung " + "(s. Config.azimuth_offset_deg). Gegen den AoA Analyzer herleiten, " + "nicht raten.") + parser.add_argument("--el-offset", type=float, default=0.0, + help="Analog fuer Elevation (s. Config.elevation_offset_deg).") + a = parser.parse_args() + + ACTIVE_CONFIG = Config(pitch_x_m=a.pitch_x_m, pitch_y_m=a.pitch_y_m, phase_multiplier=a.phase_multiplier, + flip_azimuth=a.flip_azimuth, flip_elevation=a.flip_elevation, swap_axes=a.swap_axes, + azimuth_mirror=a.azimuth_mirror, azimuth_offset_deg=a.az_offset, + elevation_offset_deg=a.el_offset) + CONFIDENCE_MIN = a.confidence_min + ACCUMULATE_PACKETS = a.accumulate + _kalibrierung = lade_kalibrierung() + if _kalibrierung is None: + print(f"Antennenkalibrierung: keine ({KALIBRIER_DATEI} nicht gefunden). " + f"Die Winkel werden dadurch systematisch falsch sein.", file=sys.stderr) + else: + print(f"Antennenkalibrierung: geladen aus {KALIBRIER_DATEI} " + f"(Quelle {_kalibrierung['quelle']}, {_kalibrierung['pakete']} Pakete, " + f"Stabilitaet {_kalibrierung['stabilitaet']:.2f}, " + f"{len(_kalibrierung['kanal'])} Kanaele einzeln)", file=sys.stderr) + switching_pattern = bytes((CP_RF_VALUE,) + RF_ORDER) + ANGLE_TABLE_FILE = a.angle_out + + with open(ANGLE_TABLE_FILE, "w", encoding="utf-8") as f: + json.dump([], f, indent=2, ensure_ascii=False) + iq_file = open(a.iq_out, "a", encoding="utf-8") + print(f"IQ-Datei: {a.iq_out} Winkel-Tabelle: {ANGLE_TABLE_FILE} " + f"RF-Pattern: simple (0..15, fix) Konfidenzschwelle: {CONFIDENCE_MIN} " + f"flip_az={a.flip_azimuth} flip_el={a.flip_elevation} swap_axes={a.swap_axes}", file=sys.stderr) + + try: + connector = bgapi.SerialConnector(a.port, baudrate=a.baud) + lib = bgapi.BGLib(connector, a.xapi, on_event) + lib.open() + print(f"Verbunden mit BG22 auf {a.port}", file=sys.stderr) + lib.bt.system.reboot() + time.sleep(2.0) + lib.bt.system.hello() + lib.bt.scanner.set_parameters(1, 160, 160) + lib.bt.scanner.start(1, 2) + print("Suche nach periodischem Tag...", file=sys.stderr) + while True: + try: + action, *args = pending_actions.get(timeout=0.1) + except queue.Empty: + continue + if action == "sync": + lib.bt.scanner.stop() + lib.bt.sync_scanner.open(*args) + elif action == "cte": + lib.bt.cte_receiver.enable_connectionless_cte(args[0], SLOT_DURATION_US, + CTE_COUNT, switching_pattern) + print(f"CTE-Empfang aktiv. RF-Pattern: {list(switching_pattern)}", file=sys.stderr) + elif action == "stop": + break + except KeyboardInterrupt: + print("Beende auf Benutzerwunsch...", file=sys.stderr) + finally: + global SHUTTING_DOWN + SHUTTING_DOWN = True # ab hier verwirft save_iq_report() neue Events, statt auf iq_file zu schreiben + if lib: + lib.close() # stoppt zuerst den BGAPI-Event-Thread, damit er iq_file nicht mehr anfasst + if iq_file: + iq_file.close() + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/bg22_live_hybrid.py b/bg22_live_hybrid.py new file mode 100644 index 0000000..3191087 --- /dev/null +++ b/bg22_live_hybrid.py @@ -0,0 +1,181 @@ +"""Live-Messung mit dem BG22 und dem hybriden Algorithmus (aoa_music_komplett.py). + +Verbindung, Tag-Suche und CTE-Empfang kommen aus bg22_AoA_v2.py (unveraendert). +Beide Dateien muessen im selben Ordner liegen. + +1. Kalibrierung aufnehmen (Tag vor dem Board, ca. 60 s, Analyzer-Werte notieren): + python bg22_live_hybrid.py --nur-aufnehmen --iq-out kalibrierung_neu.jsonl +2. Messen (ohne --kalibrierung: Test ohne Kalibrierung): + python bg22_live_hybrid.py --kalibrierung kalibrierung_neu.jsonl \ + --kal-az --kal-el --ref-az --ref-el --iq-out test_neu_a.jsonl +""" +import argparse +import importlib.util +import json +import os +import sys +import time as _time +from datetime import datetime, timezone + +import numpy as np + +hier = os.path.dirname(os.path.abspath(__file__)) +sys.path.insert(0, hier) +import aoa_music_komplett as ab # noqa: E402 + +# ============================================================================= +# Kommandozeile +# ============================================================================= + +ap = argparse.ArgumentParser(add_help=False) +ap.add_argument("--kalibrierung") +ap.add_argument("--kal-az", type=float, default=0.0) +ap.add_argument("--kal-el", type=float, default=90.0) +ap.add_argument("--polar", default="VH", choices=["V", "VH"]) +ap.add_argument("--abstand-mm", type=float, default=ab.ABSTAND_M * 1000) +ap.add_argument("--ref-az", type=float) +ap.add_argument("--ref-el", type=float) +ap.add_argument("--nur-aufnehmen", action="store_true") +ap.add_argument("--winkel-out", default="winkel_tabelle.json") +ap.add_argument("--iq-out", default="iq_samples.jsonl") +a, rest = ap.parse_known_args() +sys.argv = [sys.argv[0], "--iq-out", a.iq_out, "--angle-out", "winkel_tabelle_alt.json"] + rest + +# ============================================================================= +# Verbindung aus bg22_AoA_v2.py uebernehmen +# ============================================================================= + +spec = importlib.util.spec_from_file_location("bg22_AoA_v2", os.path.join(hier, "bg22_AoA_v2.py")) +bg22 = importlib.util.module_from_spec(spec) +sys.modules["bg22_AoA_v2"] = bg22 +spec.loader.exec_module(bg22) + +N_ANT = 16 if a.polar == "V" else 32 +if a.polar == "VH": + bg22.RF_ORDER = tuple(ab.MUSTER_V) + tuple(ab.MUSTER_H) +with open(a.iq_out + ".meta.json", "w", encoding="utf-8") as f: + json.dump({"n_antennes": N_ANT, "polarisations": a.polar, "slot_us": bg22.SLOT_DURATION_US, + "motif": [bg22.CP_RF_VALUE] + list(bg22.RF_ORDER)}, f, indent=1) +bg22.SELBSTKAL_AKTIV = False +bg22.KALIBRIER_DATEI = "__nicht_verwendet__.json" # alte Kalibrierung abgeschaltet +bg22.process_packet = lambda paket, cfg: None # alte MUSIC-Berechnung abgeschaltet +open(a.iq_out, "w", encoding="utf-8").close() # IQ-Datei bei jedem Start leeren + +# ============================================================================= +# Schaetzer vorbereiten +# ============================================================================= + +schaetzer = None +if not a.nur_aufnehmen: + array = ab.brd4191a(a.polar, a.abstand_mm / 1000) + if a.kalibrierung: + if not os.path.exists(a.kalibrierung): + sys.exit(f"Kalibrierdatei nicht gefunden: {a.kalibrierung}") + kal = ab.Kalibrierung.aus_aufnahme(array, ab.lese_iq_datei(a.kalibrierung, N_ANT), a.kal_az, a.kal_el) + else: + kal = ab.Kalibrierung(array.n) # ohne Kalibrierung: alle Faktoren = 1 + print("bg22_live: OHNE Kalibrierung (nur zum Test)", file=sys.stderr) + einst = ab.Einstellungen(polarisationen=(0,) if a.polar == "V" else (0, 1)) + schaetzer = ab.MusicSchaetzer(array, kal, einst) + print(f"bg22_live: Hybrid (Beamformer + MUSIC), {N_ANT} Antennen, {a.abstand_mm:.1f} mm, " + f"Kalibrierung {a.kalibrierung or 'keine'} ({len(kal.je_kanal)} Kanaele)", file=sys.stderr) +else: + print(f"bg22_live: nur Aufnahme ({N_ANT} Antennen) nach {a.iq_out}", file=sys.stderr) + +tabelle = [] + +# ============================================================================= +# Verarbeitung je Paket +# ============================================================================= + +def paket_verarbeiten(i_werte, q_werte, kanal, t): + if schaetzer is None: + return + p = ab.paket_aus_iq(i_werte, q_werte, kanal, t, N_ANT, float(bg22.SLOT_DURATION_US)) + s = schaetzer.verarbeiten(p) if p is not None else None + if s is None or not np.isfinite(s.az_grad): + return + vergleich = "" + if a.ref_az is not None and a.ref_el is not None: + d = ab.winkelabstand(ab.richtung(s.az_grad * ab.GRAD, s.el_grad * ab.GRAD), + ab.richtung(a.ref_az * ab.GRAD, a.ref_el * ab.GRAD)) / ab.GRAD + vergleich = f" | Abweichung {d:4.1f}" + print(f">>> Az {s.az_grad:+7.1f} +-{s.unsicherheit_az:4.1f} El {s.el_grad:5.1f} +-{s.unsicherheit_el:4.1f} " + f"Konfidenz {s.konfidenz:.2f}{vergleich} {', '.join(s.gruende)}", file=sys.stderr, flush=True) + tabelle.append({"zeit": datetime.fromtimestamp(t, timezone.utc).isoformat(), + "azimut": round(s.az_grad, 2), "elevation": round(s.el_grad, 2), + "unsicherheit_az": round(s.unsicherheit_az, 2), "unsicherheit_el": round(s.unsicherheit_el, 2), + "konfidenz": round(s.konfidenz, 3), "gruende": s.gruende}) + if len(tabelle) % 5 == 0: + speichern() + + +def speichern(): + with open(a.winkel_out, "w", encoding="utf-8") as f: + json.dump(tabelle, f, indent=1, ensure_ascii=False) + + +_original_speichern = bg22.save_iq_report + + +def save_iq_report(event): + _original_speichern(event) # IQ-Datei wie bisher schreiben + try: + i_werte, q_werte = bg22.signed_iq(event.samples) + paket_verarbeiten(i_werte, q_werte, int(event.channel), _time.time()) + except Exception as fehler: # Rechenfehler darf die Messung nicht abbrechen + print(f"bg22_live: Paket uebersprungen ({fehler})", file=sys.stderr) + + +bg22.save_iq_report = save_iq_report + +# ============================================================================= +# Zusammenfassung beim Beenden (Strg+C) +# ============================================================================= + +def zusammenfassung(): + T = tabelle[len(tabelle) // 10:] # Einschwingphase weglassen + if not T: + return + S = [ab.Schaetzung(0, t["azimut"], t["elevation"], 0, 0, t["konfidenz"], 0, 0, 0, 0) for t in T] + r = ab.ergebnis(S, a.ref_az, a.ref_el) + txt = f"\n===== ERGEBNIS: Azimut {r['az']:+.1f}, Elevation {r['el']:.1f}, Konfidenz {r['konfidenz']:.2f}" + if "abweichung" in r: + txt += (f"\n===== ANALYZER: Azimut {a.ref_az:+.1f}, Elevation {a.ref_el:.1f} -> " + f"ABWEICHUNG {r['abweichung']:.1f} Grad (Az {r['abw_az']:+.1f}, El {r['abw_el']:+.1f})") + print(txt + "\n", file=sys.stderr, flush=True) + +# ============================================================================= +# Start (5 s Bootzeit, bis zu 3 Versuche) +# ============================================================================= + +class _LaengereBootzeit: + def __getattr__(self, name): + return getattr(_time, name) + + def sleep(self, sekunden): + _time.sleep(5.0 if sekunden == 2.0 else sekunden) + + +bg22.time = _LaengereBootzeit() + +if __name__ == "__main__": + try: + from bgapi.bglib import CommandError + except ImportError: + CommandError = Exception + try: + for versuch in range(1, 4): + try: + bg22.main() + break + except CommandError as fehler: + print(f"bg22_live: Versuch {versuch}/3, BG22 antwortet nicht ({fehler}).", file=sys.stderr) + if versuch == 3: + sys.exit("Abbruch: AoA Analyzer trennen, Reset am BG22, neu starten.") + bg22.SHUTTING_DOWN = False + _time.sleep(3.0) + finally: + speichern() + print(f"bg22_live: {len(tabelle)} Schaetzungen in {a.winkel_out}", file=sys.stderr) + zusammenfassung()