"""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()