Compare commits
1
Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 SandraGuefack
|
75868d549c |
No files matched your search
@@ -1,7 +0,0 @@
|
||||
__pycache__/
|
||||
*.pyc
|
||||
.venv/
|
||||
venv/
|
||||
*.jsonl
|
||||
*.log
|
||||
.vscode/
|
||||
@@ -1,670 +0,0 @@
|
||||
"""AoA-Auswertung mit 2D-Beamformer fuer den Silicon Labs BG22 (Board BRD4191A, 4 x 4, V + H).
|
||||
|
||||
Eigenstaendiges Skript, keine RTL-Bibliothek. Enthaelt alle Schritte und Filter:
|
||||
1. Vorverarbeitung Frequenzversatz entfernen, Mittelwert je Antenne
|
||||
2. Kalibrierung je Kanal und Polarisation, mit der echten Tag-Richtung
|
||||
3. Richtung je Paket Bartlett-Beamformer ueber alle Antennen (grobe Suche + Verfeinerung)
|
||||
4. Qualitaet je Paket Kohaerenz, Nebenmaximum, Keulenbreite, SNR, Jitter
|
||||
5. Zeitfilter Fenster 0,2 s, Median je Kanal, Hampel-Filter, Kalman-Filter mit Gating
|
||||
|
||||
Winkel wie im AoA Analyzer: Azimut 0 = Unterkante, +90 = rechts,
|
||||
Elevation 90 = direkt vor dem Board, 0 = in der Board-Ebene.
|
||||
|
||||
Aufruf:
|
||||
python aoa_beamformer_komplett.py test_a.jsonl --kalibrierung boresight_vh3.jsonl \
|
||||
--kal-az -134 --kal-el 87.8 --ref-az -89.1 --ref-el 52.0
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import os
|
||||
from collections import deque
|
||||
from dataclasses import dataclass, field
|
||||
from datetime import datetime
|
||||
|
||||
import numpy as np
|
||||
|
||||
# =============================================================================
|
||||
# Konstanten
|
||||
# =============================================================================
|
||||
|
||||
C0 = 299_792_458.0
|
||||
GRAD = np.pi / 180.0
|
||||
|
||||
# Umschaltreihenfolge des BRD4191A (Patch 1..16), laut Silicon-Labs-Dokumentation
|
||||
MUSTER_V = (2, 0, 14, 12, 4, 8, 20, 10, 6, 22, 16, 19, 9, 26, 28, 31)
|
||||
MUSTER_H = (3, 1, 15, 13, 5, 24, 21, 11, 7, 23, 17, 18, 25, 27, 29, 30)
|
||||
CP_ANTENNE = 40 # Referenzantenne (Patch 6, zirkular)
|
||||
ABSTAND_M = 0.036 # Antennenabstand, gemessen ca. 36,7 mm
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# Winkel und Richtungen
|
||||
# =============================================================================
|
||||
|
||||
def kanal_frequenz(kanal: int) -> float:
|
||||
"""Frequenz (Hz) eines BLE-Kanals 0..39."""
|
||||
if kanal == 37:
|
||||
return 2402e6
|
||||
if kanal == 38:
|
||||
return 2426e6
|
||||
if kanal == 39:
|
||||
return 2480e6
|
||||
if 0 <= kanal <= 10:
|
||||
return (2404 + 2 * kanal) * 1e6
|
||||
if 11 <= kanal <= 36:
|
||||
return (2428 + 2 * (kanal - 11)) * 1e6
|
||||
raise ValueError(f"ungueltiger BLE-Kanal: {kanal}")
|
||||
|
||||
|
||||
def richtung(az, el):
|
||||
"""Einheitsvektor aus Azimut und Elevation (rad)."""
|
||||
az, el = np.asarray(az, float), np.asarray(el, float)
|
||||
return np.stack([np.cos(el) * np.cos(az), np.cos(el) * np.sin(az), np.sin(el)], axis=-1)
|
||||
|
||||
|
||||
def az_el(u):
|
||||
"""Azimut und Elevation (rad) aus einem Einheitsvektor."""
|
||||
u = np.asarray(u, float)
|
||||
return np.arctan2(u[..., 1], u[..., 0]), np.arcsin(np.clip(u[..., 2], -1, 1))
|
||||
|
||||
|
||||
def winkelabstand(u, v):
|
||||
"""Tatsaechlicher Winkel (rad) zwischen zwei Richtungen ("Abweichung gesamt")."""
|
||||
return np.arccos(np.clip(np.sum(np.asarray(u) * np.asarray(v), -1), -1, 1))
|
||||
|
||||
|
||||
def sphaerischer_median(U, w=None, iterationen=50):
|
||||
"""Gewichteter geometrischer Median von Richtungen (robust gegen Ausreisser)."""
|
||||
U = np.atleast_2d(U)
|
||||
w = np.ones(len(U)) if w is None else np.asarray(w, float)
|
||||
m = (w[:, None] * U).sum(0)
|
||||
m /= np.linalg.norm(m) + 1e-12
|
||||
for _ in range(iterationen):
|
||||
d = np.maximum(winkelabstand(U, m), 1e-4)
|
||||
neu = ((w / d)[:, None] * U).sum(0)
|
||||
neu /= np.linalg.norm(neu) + 1e-12
|
||||
if winkelabstand(neu, m) < 1e-7:
|
||||
return neu
|
||||
m = neu
|
||||
return m
|
||||
|
||||
|
||||
def gewichteter_median(v, w):
|
||||
v, w = np.asarray(v, float), np.asarray(w, float)
|
||||
i = np.argsort(v)
|
||||
c = np.cumsum(w[i])
|
||||
return v[i][np.searchsorted(c, 0.5 * c[-1])]
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# Antennenboard
|
||||
# =============================================================================
|
||||
|
||||
@dataclass
|
||||
class Array:
|
||||
positionen: np.ndarray # (N, 2) in m: x nach unten, y nach rechts
|
||||
polarisation: np.ndarray # (N,) 0 = vertikal, 1 = horizontal
|
||||
abstand: float
|
||||
|
||||
@property
|
||||
def n(self) -> int:
|
||||
return len(self.positionen)
|
||||
|
||||
def steuervektoren(self, U, lam):
|
||||
"""Steuervektoren a(theta) fuer die Richtungen U, Form (G, N)."""
|
||||
return np.exp(1j * 2 * np.pi / lam * (np.asarray(U)[..., :2] @ self.positionen.T))
|
||||
|
||||
def index(self, pol: int, zeile: int, spalte: int) -> int:
|
||||
"""Antennenindex an Rasterposition (zeile 0 = oben, spalte 0 = links)."""
|
||||
z = np.rint(self.positionen[:, 0] / self.abstand + 1.5)
|
||||
s = np.rint(self.positionen[:, 1] / self.abstand + 1.5)
|
||||
return int(np.flatnonzero((self.polarisation == pol) & (z == zeile) & (s == spalte))[0])
|
||||
|
||||
|
||||
def brd4191a(polar: str = "VH", abstand_m: float = ABSTAND_M) -> Array:
|
||||
"""BRD4191A: Patch 1..16 spaltenweise von rechts nach links, je von oben nach unten."""
|
||||
pos = []
|
||||
for k in range(16):
|
||||
zeile, spalte = k % 4, 3 - k // 4
|
||||
pos.append(((zeile - 1.5) * abstand_m, (spalte - 1.5) * abstand_m))
|
||||
pos = np.array(pos)
|
||||
if polar == "V":
|
||||
return Array(pos, np.zeros(16, int), abstand_m)
|
||||
if polar == "VH":
|
||||
return Array(np.vstack([pos, pos]), np.r_[np.zeros(16, int), np.ones(16, int)], abstand_m)
|
||||
raise ValueError("polar muss 'V' oder 'VH' sein")
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# IQ-Daten einlesen
|
||||
# =============================================================================
|
||||
|
||||
@dataclass
|
||||
class Paket:
|
||||
t: float # Zeit (s)
|
||||
kanal: int
|
||||
ref: np.ndarray # 8 Referenz-Samples
|
||||
t_ref: np.ndarray # Zeitpunkte (us)
|
||||
samples: np.ndarray # Samples der Umschaltphase
|
||||
t_samples: np.ndarray # Zeitpunkte (us)
|
||||
antennen: np.ndarray # Antennenindex je Sample
|
||||
|
||||
|
||||
def paket_aus_iq(i_werte, q_werte, kanal, t, n_ant, slot_us=2.0):
|
||||
"""Ein Paket aus I/Q-Listen: 8 Referenz-Samples, danach Zyklus CP + n_ant Antennen."""
|
||||
z = np.asarray(i_werte, float) - 1j * np.asarray(q_werte, float)
|
||||
if len(z) <= 8 + n_ant:
|
||||
return None
|
||||
k = np.arange(len(z) - 8)
|
||||
idx = k % (n_ant + 1)
|
||||
g = idx < n_ant
|
||||
return Paket(t, int(kanal), z[:8], np.arange(8.0), z[8:][g],
|
||||
(8.0 + slot_us + 2 * slot_us * k)[g], idx[g])
|
||||
|
||||
|
||||
def lese_iq_datei(pfad: str, n_ant: int | None = None, jedes: int = 1) -> list[Paket]:
|
||||
"""Liest eine IQ-Datei (.jsonl). Anzahl Antennen aus <pfad>.meta.json, sonst 16."""
|
||||
meta = {}
|
||||
if os.path.exists(pfad + ".meta.json"):
|
||||
with open(pfad + ".meta.json", encoding="utf-8") as f:
|
||||
meta = json.load(f)
|
||||
n_ant = n_ant or int(meta.get("n_antennes", 16))
|
||||
slot_us = float(meta.get("slot_us", 2.0))
|
||||
pakete = []
|
||||
with open(pfad, encoding="utf-8") as f:
|
||||
for nr, zeile in enumerate(f):
|
||||
if nr % jedes:
|
||||
continue
|
||||
d = json.loads(zeile)
|
||||
t = datetime.fromisoformat(d["zeit"]).timestamp()
|
||||
p = paket_aus_iq(d["i"], d["q"], d["channel"], t, n_ant, slot_us)
|
||||
if p is not None:
|
||||
pakete.append(p)
|
||||
return pakete
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# Vorverarbeitung: Frequenzversatz entfernen, Mittelwert je Antenne
|
||||
# =============================================================================
|
||||
|
||||
def vorverarbeiten(p: Paket, n_ant: int):
|
||||
"""Gibt x (N,) komplex (NaN = Antenne fehlt), SNR und Phasen-Jitter je Antenne zurueck."""
|
||||
ref, tr = np.asarray(p.ref), np.asarray(p.t_ref, float)
|
||||
omega = np.angle(np.sum(ref[1:] * np.conj(ref[:-1]))) / np.median(np.diff(tr))
|
||||
phi0 = np.angle(np.sum(ref * np.exp(-1j * omega * tr)))
|
||||
r = ref * np.exp(-1j * (omega * tr + phi0))
|
||||
m = r.mean()
|
||||
snr = abs(m) ** 2 / max(np.mean(np.abs(r - m) ** 2), 1e-12)
|
||||
|
||||
s, t, ant = np.asarray(p.samples), np.asarray(p.t_samples, float), np.asarray(p.antennen)
|
||||
y = s * np.exp(-1j * (omega * t + phi0))
|
||||
|
||||
# Feinkorrektur der Frequenz ueber wiederholte Antennen
|
||||
num, dts = 0j, []
|
||||
for i in np.unique(ant):
|
||||
k = np.flatnonzero(ant == i)
|
||||
if k.size >= 2:
|
||||
num += np.sum(y[k[1:]] * np.conj(y[k[:-1]]))
|
||||
dts.extend(np.diff(t[k]))
|
||||
if dts:
|
||||
y = y * np.exp(-1j * (np.angle(num) / np.mean(dts)) * t)
|
||||
|
||||
x = np.full(n_ant, np.nan + 0j)
|
||||
jitter = np.zeros(n_ant)
|
||||
for i in range(n_ant):
|
||||
si = y[ant == i]
|
||||
if si.size == 0:
|
||||
continue
|
||||
x[i] = si.mean()
|
||||
if si.size > 1:
|
||||
R = abs(np.mean(np.exp(1j * np.angle(si * np.conj(x[i])))))
|
||||
jitter[i] = np.sqrt(-2 * np.log(max(R, 1e-9)))
|
||||
return x, snr, jitter
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# Kalibrierung
|
||||
# =============================================================================
|
||||
|
||||
class Kalibrierung:
|
||||
"""Komplexer Faktor je Antenne und Kanal, aus einer Aufnahme mit bekannter Tag-Richtung."""
|
||||
|
||||
def __init__(self, n: int):
|
||||
self.global_ = np.ones(n, complex)
|
||||
self.je_kanal: dict[int, np.ndarray] = {}
|
||||
|
||||
def anwenden(self, x, kanal):
|
||||
return x / self.je_kanal.get(kanal, self.global_)
|
||||
|
||||
@classmethod
|
||||
def aus_aufnahme(cls, array: Array, pakete, az_grad: float, el_grad: float, min_pakete: int = 5):
|
||||
u = richtung(az_grad * GRAD, el_grad * GRAD)
|
||||
gruppen: dict[int, list] = {}
|
||||
for p in pakete:
|
||||
x, _, _ = vorverarbeiten(p, array.n)
|
||||
if np.all(np.isfinite(x)):
|
||||
gruppen.setdefault(p.kanal, []).append(x)
|
||||
|
||||
kal, alle = cls(array.n), []
|
||||
for kanal, X in gruppen.items():
|
||||
if len(X) < min_pakete:
|
||||
continue
|
||||
X = np.array(X)
|
||||
a = array.steuervektoren(u[None], C0 / kanal_frequenz(kanal))[0]
|
||||
c = np.empty(array.n, complex)
|
||||
for pol in np.unique(array.polarisation): # jede Polarisation getrennt
|
||||
m = array.polarisation == pol
|
||||
R = X[:, m].T @ X[:, m].conj() / len(X)
|
||||
v = np.linalg.eigh(R)[1][:, -1] # dominanter Pfad
|
||||
cm = v / a[m]
|
||||
c[m] = cm * np.exp(-1j * np.angle(cm[0])) / np.mean(np.abs(cm))
|
||||
kal.je_kanal[kanal] = c
|
||||
alle.append(c)
|
||||
if not alle:
|
||||
raise ValueError("zu wenige Pakete fuer die Kalibrierung")
|
||||
g = np.mean(alle, axis=0)
|
||||
for pol in np.unique(array.polarisation):
|
||||
m = array.polarisation == pol
|
||||
g[m] /= np.mean(np.abs(g[m]))
|
||||
kal.global_ = g
|
||||
return kal
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# Einstellungen und Ergebnisse
|
||||
# =============================================================================
|
||||
|
||||
@dataclass
|
||||
class Einstellungen:
|
||||
raster_grad: float = 2.0 # Suchraster
|
||||
polarisationen: tuple = (0, 1) # (0,) = nur V, (0, 1) = V und H
|
||||
schwache_antenne: float = 0.3 # Amplitude < 30 % des Medians -> ignorieren
|
||||
q_min: float = 0.15 # Pakete mit geringerer Qualitaet verwerfen
|
||||
fenster_s: float = 0.2 # Pakete je Ausgabewert
|
||||
hampel_n: int = 9
|
||||
hampel_k: float = 3.5
|
||||
hampel_boden_grad: float = 2.0
|
||||
sigma_min_grad: float = 1.5 # Messrauschen bei bester Qualitaet
|
||||
k_rauschen: float = 4e-4 # R = sigma_min^2 + k_rauschen / Qualitaet
|
||||
beschleunigung: float = 0.05 # Prozessrauschen Kalman
|
||||
gating: float = 9.21 # Chi^2, 2 Freiheitsgrade, 99 %
|
||||
n_neustart: int = 4 # Verwerfungen in Folge -> neue Position uebernehmen
|
||||
|
||||
|
||||
@dataclass
|
||||
class PaketMessung:
|
||||
t: float
|
||||
kanal: int
|
||||
u: np.ndarray
|
||||
qualitaet: float
|
||||
kohaerenz: float
|
||||
nebenmaximum: float
|
||||
keulenbreite: float
|
||||
snr: float
|
||||
gruende: list = field(default_factory=list)
|
||||
|
||||
|
||||
@dataclass
|
||||
class Schaetzung:
|
||||
t: float
|
||||
az_grad: float
|
||||
el_grad: float
|
||||
unsicherheit_az: float
|
||||
unsicherheit_el: float
|
||||
konfidenz: float
|
||||
roh_az_grad: float
|
||||
roh_el_grad: float
|
||||
pakete: int
|
||||
pakete_gueltig: int
|
||||
gruende: list = field(default_factory=list)
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# Kalman-Filter auf den Richtungskosinus (ux, uy)
|
||||
# =============================================================================
|
||||
|
||||
class KalmanRichtung:
|
||||
"""Zustand [ux, uy, vx, vy], Modell konstanter Geschwindigkeit."""
|
||||
|
||||
def __init__(self, beschleunigung):
|
||||
self.q = beschleunigung
|
||||
self.x = self.P = self.t = None
|
||||
|
||||
def starten(self, z, R, t):
|
||||
self.x = np.r_[z, 0.0, 0.0]
|
||||
self.P = np.diag([R[0, 0], R[1, 1], 0.05, 0.05])
|
||||
self.t = t
|
||||
|
||||
def vorhersagen(self, t):
|
||||
dt = max(t - self.t, 1e-3)
|
||||
F = np.eye(4)
|
||||
F[0, 2] = F[1, 3] = dt
|
||||
Qb = self.q * np.array([[dt**4 / 4, dt**3 / 2], [dt**3 / 2, dt**2]])
|
||||
Q = np.zeros((4, 4))
|
||||
Q[np.ix_([0, 2], [0, 2])] = Qb
|
||||
Q[np.ix_([1, 3], [1, 3])] = Qb
|
||||
self.x = F @ self.x
|
||||
self.P = F @ self.P @ F.T + Q
|
||||
self.t = t
|
||||
self._begrenzen()
|
||||
|
||||
def abstand(self, z, R):
|
||||
nu = z - self.x[:2]
|
||||
return float(nu @ np.linalg.solve(self.P[:2, :2] + R, nu))
|
||||
|
||||
def korrigieren(self, z, R):
|
||||
H = np.zeros((2, 4))
|
||||
H[0, 0] = H[1, 1] = 1
|
||||
K = self.P @ H.T @ np.linalg.inv(H @ self.P @ H.T + R)
|
||||
self.x = self.x + K @ (z - self.x[:2])
|
||||
self.P = (np.eye(4) - K @ H) @ self.P
|
||||
self._begrenzen()
|
||||
|
||||
def _begrenzen(self):
|
||||
r = np.hypot(*self.x[:2])
|
||||
if r > 0.9999:
|
||||
self.x[:2] *= 0.9999 / r
|
||||
|
||||
def ausgabe(self):
|
||||
"""Azimut, Elevation und Unsicherheiten in Grad."""
|
||||
ux, uy = self.x[:2]
|
||||
r = max(np.hypot(ux, uy), 1e-3)
|
||||
uz = np.sqrt(max(1 - r * r, 0.0))
|
||||
az, el = np.arctan2(uy, ux), np.arctan2(uz, r)
|
||||
P = self.P[:2, :2]
|
||||
j_az = np.array([-uy, ux]) / r**2
|
||||
j_r = np.array([ux, uy]) / r
|
||||
s_r = np.sqrt(max(j_r @ P @ j_r, 0))
|
||||
s_az = np.sqrt(max(j_az @ P @ j_az, 0)) / GRAD
|
||||
s_el = 0.5 * abs(np.arccos(np.clip(r - s_r, 0, 1)) - np.arccos(np.clip(r + s_r, 0, 1))) / GRAD
|
||||
return az / GRAD, el / GRAD, min(s_az, 180.0), min(s_el, 90.0)
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# Beamformer-Schaetzer
|
||||
# =============================================================================
|
||||
|
||||
class BeamformerSchaetzer:
|
||||
"""Ablauf je Paket: Vorverarbeitung -> Kalibrierung -> Beamformer -> Qualitaet.
|
||||
Ablauf je Fenster: Median je Kanal -> Hampel -> Kalman."""
|
||||
|
||||
NAME = "Beamformer"
|
||||
|
||||
def __init__(self, array: Array, kal: Kalibrierung | None = None, einst: Einstellungen | None = None):
|
||||
self.array = array
|
||||
self.kal = kal or Kalibrierung(array.n)
|
||||
self.einst = einst or Einstellungen()
|
||||
schritt = self.einst.raster_grad * GRAD
|
||||
AZ, EL = np.meshgrid(np.arange(-np.pi, np.pi, schritt), np.arange(0, np.pi / 2 + 1e-9, schritt))
|
||||
self.form = AZ.shape
|
||||
self.U = richtung(AZ.ravel(), EL.ravel())
|
||||
self.flaeche = np.cos(EL.ravel()) * schritt * schritt
|
||||
self._A_cache: dict[float, np.ndarray] = {}
|
||||
self.kalman = KalmanRichtung(self.einst.beschleunigung)
|
||||
self.verlauf = deque(maxlen=self.einst.hampel_n)
|
||||
self.puffer: list[PaketMessung] = []
|
||||
self.verworfen = 0
|
||||
|
||||
# ---- Steuervektoren auf dem Suchraster (Cache je Wellenlaenge) ----
|
||||
|
||||
def A_raster(self, lam):
|
||||
key = round(lam * 1e6, 3)
|
||||
if key not in self._A_cache:
|
||||
self._A_cache[key] = self.array.steuervektoren(self.U, lam).astype(np.complex64)
|
||||
return self._A_cache[key]
|
||||
|
||||
# ---- Bartlett-Spektrum (Beamformer), Werte 0..1 ----
|
||||
|
||||
def bartlett(self, U, x, maske, lam, A=None):
|
||||
"""V und H getrennt berechnen, nach Energie gewichtet zusammenfassen."""
|
||||
A = self.array.steuervektoren(U, lam) if A is None else A
|
||||
P, E_ges = np.zeros(len(U)), 0.0
|
||||
for pol in self.einst.polarisationen:
|
||||
m = maske & (self.array.polarisation == pol)
|
||||
if m.sum() < 3:
|
||||
continue
|
||||
xs = x[m]
|
||||
E = float(np.sum(np.abs(xs) ** 2))
|
||||
P += E * np.abs(np.conj(A[:, m]) @ xs) ** 2 / (m.sum() * E)
|
||||
E_ges += E
|
||||
return P / E_ges if E_ges > 0 else P
|
||||
|
||||
def fein_suchen(self, spektrum, u0, breite_grad=3.0, schritt_grad=0.25):
|
||||
"""Verfeinerung um u0: gibt beste Richtung und Spektrumswert zurueck."""
|
||||
az0, el0 = az_el(u0)
|
||||
d = np.arange(-breite_grad, breite_grad + 1e-3, schritt_grad) * GRAD
|
||||
AZ, EL = np.meshgrid(az0 + d, np.clip(el0 + d, 0, np.pi / 2))
|
||||
Uf = richtung(AZ.ravel(), EL.ravel())
|
||||
P = spektrum(Uf)
|
||||
k = int(np.argmax(P))
|
||||
return Uf[k], float(P[k])
|
||||
|
||||
def _nebenmaximum(self, P, u, ausschluss_grad=15.0):
|
||||
"""Hoechstes lokales Maximum ausserhalb der Hauptkeule."""
|
||||
G = P.reshape(self.form)
|
||||
nachbarn = [np.roll(G, 1, 1), np.roll(G, -1, 1),
|
||||
np.vstack([G[:1] * 0, G[:-1]]), np.vstack([G[1:], G[-1:] * 0])]
|
||||
lokal = np.all([G >= v for v in nachbarn], axis=0).ravel()
|
||||
lokal &= self.U @ u < np.cos(ausschluss_grad * GRAD)
|
||||
return float(P[lokal].max()) if lokal.any() else 0.0
|
||||
|
||||
def _keulenflaeche(self, P_nah, nah):
|
||||
if not nah.any() or P_nah.max() <= 0:
|
||||
return np.nan
|
||||
return float(np.sum(self.flaeche[nah][P_nah >= 0.5 * P_nah.max()]))
|
||||
|
||||
# ---- Auswertung eines Pakets ----
|
||||
|
||||
def paket_auswerten(self, p: Paket) -> PaketMessung:
|
||||
e = self.einst
|
||||
gruende = []
|
||||
x, snr, jitter = vorverarbeiten(p, self.array.n)
|
||||
x = self.kal.anwenden(x, p.kanal)
|
||||
gueltig = np.isfinite(x)
|
||||
amp = np.abs(x)
|
||||
|
||||
# Schwache Antennen ausblenden (je Polarisation)
|
||||
maske = np.zeros(self.array.n, bool)
|
||||
for pol in np.unique(self.array.polarisation):
|
||||
m = gueltig & (self.array.polarisation == pol)
|
||||
if m.any():
|
||||
maske |= m & (amp >= e.schwache_antenne * np.median(amp[m]))
|
||||
maske &= np.isin(self.array.polarisation, e.polarisationen)
|
||||
erwartet = np.isin(self.array.polarisation, e.polarisationen).sum()
|
||||
if maske.sum() < erwartet:
|
||||
gruende.append("schwache oder fehlende Antenne")
|
||||
lam = C0 / kanal_frequenz(p.kanal)
|
||||
xm = np.where(maske, x, 0)
|
||||
|
||||
# Beamformer: grobe Suche, Verfeinerung
|
||||
A = self.A_raster(lam)
|
||||
P = self.bartlett(self.U, xm, maske, lam, A)
|
||||
k = int(np.argmax(P))
|
||||
u_bf, kohaerenz = self.fein_suchen(lambda Uf: self.bartlett(Uf, xm, maske, lam), self.U[k])
|
||||
|
||||
# Qualitaetsmerkmale
|
||||
neben = self._nebenmaximum(P, u_bf) / P[k] if P[k] > 0 else 0.0
|
||||
nah = self.U @ u_bf > np.cos(30 * GRAD)
|
||||
a_ideal = self.array.steuervektoren(u_bf[None], lam)[0] * maske
|
||||
P_ideal = self.bartlett(self.U[nah], a_ideal, maske, lam, A[nah])
|
||||
breite = self._keulenflaeche(P[nah], nah) / max(self._keulenflaeche(P_ideal, nah), 1e-9)
|
||||
|
||||
jit = float(np.median(jitter[maske])) if maske.any() else np.pi
|
||||
q = (np.clip((kohaerenz - 0.3) / 0.6, 0, 1)
|
||||
* np.clip((0.85 - neben) / 0.5, 0, 1)
|
||||
* (np.clip(1.0 / max(breite, 1e-6), 0, 1) if np.isfinite(breite) else 0.5)
|
||||
* snr / (snr + 10.0)
|
||||
* maske.sum() / max(erwartet, 1)
|
||||
* np.exp(-0.5 * (jit / (20 * GRAD)) ** 2))
|
||||
|
||||
if kohaerenz < 0.6:
|
||||
gruende.append("geringe Kohaerenz (mehrere Pfade)")
|
||||
if neben > 0.6:
|
||||
gruende.append("mehrere aehnliche Maxima")
|
||||
if breite > 1.6:
|
||||
gruende.append("verbreiterte Keule")
|
||||
if snr < 5:
|
||||
gruende.append("geringes SNR")
|
||||
|
||||
return PaketMessung(p.t, p.kanal, u_bf, float(q), kohaerenz, neben, breite, snr, gruende)
|
||||
|
||||
# ---- Laufende Verarbeitung ----
|
||||
|
||||
def verarbeiten(self, p: Paket) -> Schaetzung | None:
|
||||
"""Fuer jedes Paket aufrufen. Liefert am Ende jedes Fensters eine Schaetzung."""
|
||||
m = self.paket_auswerten(p)
|
||||
if self.puffer and m.t - self.puffer[0].t >= self.einst.fenster_s:
|
||||
s = self._fenster_abschliessen(self.puffer)
|
||||
self.puffer = [m]
|
||||
return s
|
||||
self.puffer.append(m)
|
||||
return None
|
||||
|
||||
def _fenster_abschliessen(self, messungen: list[PaketMessung]) -> Schaetzung:
|
||||
e = self.einst
|
||||
t = messungen[-1].t
|
||||
gueltig = [m for m in messungen if m.qualitaet >= e.q_min]
|
||||
gruende = sorted({g for m in messungen for g in m.gruende
|
||||
if sum(g in mm.gruende for mm in messungen) > len(messungen) / 2})
|
||||
|
||||
# Keine gueltige Messung: nur Vorhersage
|
||||
if not gueltig:
|
||||
if self.kalman.x is not None:
|
||||
self.kalman.vorhersagen(t)
|
||||
az, el, saz, sel = self.kalman.ausgabe()
|
||||
else:
|
||||
az = el = saz = sel = np.nan
|
||||
return Schaetzung(t, az, el, saz, sel, 0.0, np.nan, np.nan,
|
||||
len(messungen), 0, gruende + ["keine gueltige Messung"])
|
||||
|
||||
# Median je Kanal, dann ueber die Kanaele
|
||||
je_kanal: dict[int, list] = {}
|
||||
for m in gueltig:
|
||||
je_kanal.setdefault(m.kanal, []).append(m)
|
||||
Uk = np.array([sphaerischer_median(np.array([m.u for m in ms]), [m.qualitaet for m in ms])
|
||||
for ms in je_kanal.values()])
|
||||
Wk = np.array([np.median([m.qualitaet for m in ms]) for ms in je_kanal.values()])
|
||||
u_f = sphaerischer_median(Uk, Wk)
|
||||
streuung = gewichteter_median(winkelabstand(Uk, u_f), Wk) / GRAD if len(Uk) > 1 else 0.0
|
||||
U_alle = np.array([m.u for m in gueltig])
|
||||
q_alle = np.array([m.qualitaet for m in gueltig])
|
||||
anteil_weit = float(np.sum(q_alle[winkelabstand(U_alle, u_f) > 15 * GRAD]) / q_alle.sum())
|
||||
q_f = float(np.median(q_alle)) * np.exp(-0.5 * (streuung / 10.0) ** 2)
|
||||
if streuung > 10:
|
||||
gruende.append("Kanaele uneinig")
|
||||
if anteil_weit > 0.3:
|
||||
gruende.append("Pakete gestreut")
|
||||
|
||||
# Hampel: Sprung gegenueber dem juengsten Verlauf?
|
||||
sprung = False
|
||||
if len(self.verlauf) >= 5:
|
||||
H = np.array(self.verlauf)
|
||||
c = sphaerischer_median(H)
|
||||
skala = 1.4826 * np.median(winkelabstand(H, c)) + e.hampel_boden_grad * GRAD
|
||||
sprung = winkelabstand(u_f, c) > e.hampel_k * skala
|
||||
self.verlauf.append(u_f)
|
||||
|
||||
# Kalman mit qualitaetsabhaengigem Messrauschen und Gating
|
||||
z = u_f[:2]
|
||||
R = np.eye(2) * ((e.sigma_min_grad * GRAD) ** 2 + e.k_rauschen / max(q_f, 1e-3))
|
||||
angenommen = True
|
||||
if self.kalman.x is None:
|
||||
self.kalman.starten(z, R, t)
|
||||
else:
|
||||
self.kalman.vorhersagen(t)
|
||||
if sprung or self.kalman.abstand(z, R) > e.gating:
|
||||
angenommen = False
|
||||
self.verworfen += 1
|
||||
gruende.append("Sprung erkannt (Hampel)" if sprung else "vom Kalman verworfen")
|
||||
if self.verworfen >= e.n_neustart: # Tag hat sich wirklich bewegt
|
||||
neu = np.array(list(self.verlauf)[-e.n_neustart:])
|
||||
self.kalman.starten(sphaerischer_median(neu)[:2], R, t)
|
||||
self.verlauf.clear()
|
||||
self.verlauf.extend(neu)
|
||||
self.verworfen = 0
|
||||
gruende.append("Neustart (neue Position)")
|
||||
else:
|
||||
self.kalman.korrigieren(z, R)
|
||||
self.verworfen = 0
|
||||
|
||||
az, el, saz, sel = self.kalman.ausgabe()
|
||||
if el < 15:
|
||||
gruende.append("nahe der Board-Ebene: Elevation unsicher")
|
||||
konfidenz = q_f * (1.0 if angenommen else 0.3)
|
||||
raz, rel = az_el(u_f)
|
||||
return Schaetzung(t, az, el, saz, sel, float(np.clip(konfidenz, 0, 1)),
|
||||
raz / GRAD, rel / GRAD, len(messungen), len(gueltig), gruende)
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# Ergebnis und Vergleich mit dem AoA Analyzer
|
||||
# =============================================================================
|
||||
def ergebnis(schaetzungen: list[Schaetzung], ref_az=None, ref_el=None) -> dict:
|
||||
"""Median aller Schaetzungen, optional Abweichung zum Analyzer."""
|
||||
S = [s for s in schaetzungen if np.isfinite(s.az_grad)]
|
||||
if not S:
|
||||
return {}
|
||||
u = sphaerischer_median(np.array([richtung(s.az_grad * GRAD, s.el_grad * GRAD) for s in S]))
|
||||
az, el = az_el(u)
|
||||
res = {"az": az / GRAD, "el": el / GRAD, "konfidenz": float(np.median([s.konfidenz for s in S])),
|
||||
"anzahl": len(S)}
|
||||
if ref_az is not None and ref_el is not None:
|
||||
res["abweichung"] = float(winkelabstand(u, richtung(ref_az * GRAD, ref_el * GRAD)) / GRAD)
|
||||
res["abw_az"] = ((az / GRAD - ref_az + 180) % 360) - 180
|
||||
res["abw_el"] = el / GRAD - ref_el
|
||||
return res
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# Kommandozeile
|
||||
# =============================================================================
|
||||
|
||||
def main():
|
||||
ap = argparse.ArgumentParser(description="AoA-Auswertung mit 2D-Beamformer und allen Filtern")
|
||||
ap.add_argument("iq", help="IQ-Datei (.jsonl)")
|
||||
ap.add_argument("--kalibrierung", required=True, help="Kalibrieraufnahme am selben Standort")
|
||||
ap.add_argument("--kal-az", type=float, default=0.0, help="Analyzer-Azimut waehrend der Kalibrierung")
|
||||
ap.add_argument("--kal-el", type=float, default=90.0, help="Analyzer-Elevation waehrend der Kalibrierung")
|
||||
ap.add_argument("--polar", default="VH", choices=["V", "VH"])
|
||||
ap.add_argument("--abstand-mm", type=float, default=ABSTAND_M * 1000)
|
||||
ap.add_argument("--ref-az", type=float, help="Analyzer-Azimut an dieser Position (Vergleich)")
|
||||
ap.add_argument("--ref-el", type=float, help="Analyzer-Elevation an dieser Position (Vergleich)")
|
||||
ap.add_argument("--jedes", type=int, default=1, help="nur jedes n-te Paket auswerten")
|
||||
ap.add_argument("--ausgabe", help="Schaetzungen als JSON speichern")
|
||||
ap.add_argument("--verlauf", action="store_true", help="jede Schaetzung ausgeben")
|
||||
a = ap.parse_args()
|
||||
|
||||
# Board, Kalibrierung, Schaetzer
|
||||
array = brd4191a(a.polar, a.abstand_mm / 1000)
|
||||
kal = Kalibrierung.aus_aufnahme(array, lese_iq_datei(a.kalibrierung, array.n), a.kal_az, a.kal_el)
|
||||
sch = BeamformerSchaetzer(array, kal, Einstellungen(polarisationen=(0,) if a.polar == "V" else (0, 1)))
|
||||
|
||||
# Pakete auswerten
|
||||
schaetzungen = []
|
||||
for p in lese_iq_datei(a.iq, array.n, a.jedes):
|
||||
s = sch.verarbeiten(p)
|
||||
if s is None:
|
||||
continue
|
||||
schaetzungen.append(s)
|
||||
if a.verlauf and np.isfinite(s.az_grad):
|
||||
print(f"{s.az_grad:+7.1f} / {s.el_grad:5.1f} Konfidenz {s.konfidenz:.2f} {', '.join(s.gruende)}")
|
||||
if a.ausgabe:
|
||||
with open(a.ausgabe, "w", encoding="utf-8") as f:
|
||||
json.dump([s.__dict__ for s in schaetzungen], f, indent=1, ensure_ascii=False, default=float)
|
||||
|
||||
# Ergebnis ausgeben
|
||||
r = ergebnis(schaetzungen, a.ref_az, a.ref_el)
|
||||
if not r:
|
||||
print("Keine gueltige Schaetzung.")
|
||||
return
|
||||
zeile = f"[Beamformer] Ergebnis: Azimut {r['az']:+.1f}, Elevation {r['el']:.1f}, Konfidenz {r['konfidenz']:.2f}"
|
||||
if "abweichung" in r:
|
||||
zeile += (f" | Abweichung zum Analyzer {r['abweichung']:.1f} Grad "
|
||||
f"(Az {r['abw_az']:+.1f}, El {r['abw_el']:+.1f})")
|
||||
print(zeile)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -1,743 +0,0 @@
|
||||
"""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 <pfad>.meta.json, sonst 16."""
|
||||
meta = {}
|
||||
if os.path.exists(pfad + ".meta.json"):
|
||||
with open(pfad + ".meta.json", encoding="utf-8") as f:
|
||||
meta = json.load(f)
|
||||
n_ant = n_ant or int(meta.get("n_antennes", 16))
|
||||
slot_us = float(meta.get("slot_us", 2.0))
|
||||
pakete = []
|
||||
with open(pfad, encoding="utf-8") as f:
|
||||
for nr, zeile in enumerate(f):
|
||||
if nr % jedes:
|
||||
continue
|
||||
d = json.loads(zeile)
|
||||
t = datetime.fromisoformat(d["zeit"]).timestamp()
|
||||
p = paket_aus_iq(d["i"], d["q"], d["channel"], t, n_ant, slot_us)
|
||||
if p is not None:
|
||||
pakete.append(p)
|
||||
return pakete
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# Vorverarbeitung: Frequenzversatz entfernen, Mittelwert je Antenne
|
||||
# =============================================================================
|
||||
|
||||
def vorverarbeiten(p: Paket, n_ant: int):
|
||||
"""Gibt x (N,) komplex (NaN = Antenne fehlt), SNR und Phasen-Jitter je Antenne zurueck."""
|
||||
ref, tr = np.asarray(p.ref), np.asarray(p.t_ref, float)
|
||||
omega = np.angle(np.sum(ref[1:] * np.conj(ref[:-1]))) / np.median(np.diff(tr))
|
||||
phi0 = np.angle(np.sum(ref * np.exp(-1j * omega * tr)))
|
||||
r = ref * np.exp(-1j * (omega * tr + phi0))
|
||||
m = r.mean()
|
||||
snr = abs(m) ** 2 / max(np.mean(np.abs(r - m) ** 2), 1e-12)
|
||||
|
||||
s, t, ant = np.asarray(p.samples), np.asarray(p.t_samples, float), np.asarray(p.antennen)
|
||||
y = s * np.exp(-1j * (omega * t + phi0))
|
||||
|
||||
# Feinkorrektur der Frequenz ueber wiederholte Antennen
|
||||
num, dts = 0j, []
|
||||
for i in np.unique(ant):
|
||||
k = np.flatnonzero(ant == i)
|
||||
if k.size >= 2:
|
||||
num += np.sum(y[k[1:]] * np.conj(y[k[:-1]]))
|
||||
dts.extend(np.diff(t[k]))
|
||||
if dts:
|
||||
y = y * np.exp(-1j * (np.angle(num) / np.mean(dts)) * t)
|
||||
|
||||
x = np.full(n_ant, np.nan + 0j)
|
||||
jitter = np.zeros(n_ant)
|
||||
for i in range(n_ant):
|
||||
si = y[ant == i]
|
||||
if si.size == 0:
|
||||
continue
|
||||
x[i] = si.mean()
|
||||
if si.size > 1:
|
||||
R = abs(np.mean(np.exp(1j * np.angle(si * np.conj(x[i])))))
|
||||
jitter[i] = np.sqrt(-2 * np.log(max(R, 1e-9)))
|
||||
return x, snr, jitter
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# Kalibrierung
|
||||
# =============================================================================
|
||||
|
||||
class Kalibrierung:
|
||||
"""Komplexer Faktor je Antenne und Kanal, aus einer Aufnahme mit bekannter Tag-Richtung."""
|
||||
|
||||
def __init__(self, n: int):
|
||||
self.global_ = np.ones(n, complex)
|
||||
self.je_kanal: dict[int, np.ndarray] = {}
|
||||
|
||||
def anwenden(self, x, kanal):
|
||||
return x / self.je_kanal.get(kanal, self.global_)
|
||||
|
||||
@classmethod
|
||||
def aus_aufnahme(cls, array: Array, pakete, az_grad: float, el_grad: float, min_pakete: int = 5):
|
||||
u = richtung(az_grad * GRAD, el_grad * GRAD)
|
||||
gruppen: dict[int, list] = {}
|
||||
for p in pakete:
|
||||
x, _, _ = vorverarbeiten(p, array.n)
|
||||
if np.all(np.isfinite(x)):
|
||||
gruppen.setdefault(p.kanal, []).append(x)
|
||||
|
||||
kal, alle = cls(array.n), []
|
||||
for kanal, X in gruppen.items():
|
||||
if len(X) < min_pakete:
|
||||
continue
|
||||
X = np.array(X)
|
||||
a = array.steuervektoren(u[None], C0 / kanal_frequenz(kanal))[0]
|
||||
c = np.empty(array.n, complex)
|
||||
for pol in np.unique(array.polarisation): # jede Polarisation getrennt
|
||||
m = array.polarisation == pol
|
||||
R = X[:, m].T @ X[:, m].conj() / len(X)
|
||||
v = np.linalg.eigh(R)[1][:, -1] # dominanter Pfad
|
||||
cm = v / a[m]
|
||||
c[m] = cm * np.exp(-1j * np.angle(cm[0])) / np.mean(np.abs(cm))
|
||||
kal.je_kanal[kanal] = c
|
||||
alle.append(c)
|
||||
if not alle:
|
||||
raise ValueError("zu wenige Pakete fuer die Kalibrierung")
|
||||
g = np.mean(alle, axis=0)
|
||||
for pol in np.unique(array.polarisation):
|
||||
m = array.polarisation == pol
|
||||
g[m] /= np.mean(np.abs(g[m]))
|
||||
kal.global_ = g
|
||||
return kal
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# Einstellungen und Ergebnisse
|
||||
# =============================================================================
|
||||
|
||||
@dataclass
|
||||
class Einstellungen:
|
||||
raster_grad: float = 2.0 # Suchraster
|
||||
polarisationen: tuple = (0, 1) # (0,) = nur V, (0, 1) = V und H
|
||||
schwache_antenne: float = 0.3 # Amplitude < 30 % des Medians -> ignorieren
|
||||
q_min: float = 0.15 # Pakete mit geringerer Qualitaet verwerfen
|
||||
fenster_s: float = 0.2 # Pakete je Ausgabewert
|
||||
hampel_n: int = 9
|
||||
hampel_k: float = 3.5
|
||||
hampel_boden_grad: float = 2.0
|
||||
sigma_min_grad: float = 1.5 # Messrauschen bei bester Qualitaet
|
||||
k_rauschen: float = 4e-4 # R = sigma_min^2 + k_rauschen / Qualitaet
|
||||
beschleunigung: float = 0.05 # Prozessrauschen Kalman
|
||||
gating: float = 9.21 # Chi^2, 2 Freiheitsgrade, 99 %
|
||||
n_neustart: int = 4 # Verwerfungen in Folge -> neue Position uebernehmen
|
||||
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()
|
||||
-1063
File diff suppressed because it is too large.
Load diff
@@ -6,6 +6,7 @@ import queue
|
||||
from datetime import datetime, timezone
|
||||
|
||||
import bgapi
|
||||
import music_estimator
|
||||
|
||||
# --- Einstellungen, bei Bedarf anpassen ---
|
||||
|
||||
@@ -19,6 +20,8 @@ SLOT_DURATIONS = 2 # 1 oder 2 (Mikrosekunden pro Slot)
|
||||
SWITCHING_PATTERN = bytes(range(0, 16)) # welche Antennen der Reihe nach genutzt werden
|
||||
CTE_COUNT = 0 # 0 = unbegrenzt weiter empfangen
|
||||
|
||||
CALIBRATION_PACKET_COUNT = 20 # wie viele Pakete für die Auto-Kalibrierung gesammelt werden
|
||||
|
||||
# --- Ab hier läuft das Programm, nichts mehr anpassen ---
|
||||
|
||||
lib = None
|
||||
@@ -27,6 +30,9 @@ sync_handle = None
|
||||
already_synced = False
|
||||
pending_actions = queue.Queue()
|
||||
|
||||
calibrating = "--calibrate" in sys.argv
|
||||
calibration_samples = []
|
||||
|
||||
|
||||
def on_event(event):
|
||||
"""Diese Funktion wird jedes Mal aufgerufen, wenn der BG22 uns etwas meldet."""
|
||||
@@ -89,16 +95,36 @@ def enable_cte(sync):
|
||||
|
||||
|
||||
def save_iq_report(event):
|
||||
"""Ein IQ-Paket ist angekommen: dekodieren, anzeigen und speichern."""
|
||||
"""Ein IQ-Paket ist angekommen: dekodieren, Winkel schätzen, anzeigen und speichern."""
|
||||
global calibrating
|
||||
|
||||
raw_bytes = event.samples
|
||||
values = list(struct.unpack(f"{len(raw_bytes)}b", raw_bytes))
|
||||
i_values = values[0::2]
|
||||
q_values = values[1::2]
|
||||
|
||||
if calibrating:
|
||||
calibration_samples.append((i_values, q_values))
|
||||
print(f"Kalibrierung: Paket {len(calibration_samples)}/{CALIBRATION_PACKET_COUNT} "
|
||||
f"(Tag muss bei 0°/0° stehen)")
|
||||
if len(calibration_samples) >= CALIBRATION_PACKET_COUNT:
|
||||
print("\nGenug Pakete gesammelt, berechne Kalibrierung ...")
|
||||
music_estimator.compute_calibration_from_samples(calibration_samples, 0, 0)
|
||||
music_estimator.reset_calibration_cache()
|
||||
calibrating = False
|
||||
print("Kalibrierung aktiv. Normaler Betrieb läuft weiter.\n")
|
||||
return
|
||||
|
||||
azimuth, elevation = music_estimator.estimate_angle(i_values, q_values)
|
||||
|
||||
print(f"IQ-Paket #{event.event_counter} Kanal={event.channel} "
|
||||
f"RSSI={event.rssi} dBm {len(i_values)} Samples")
|
||||
print(f" Rohbytes: {raw_bytes[:8]} ...")
|
||||
print(f" I/Q-Werte: {list(zip(i_values[:4], q_values[:4]))} ...")
|
||||
if azimuth is not None:
|
||||
print(f" Winkel: Azimut={azimuth:.0f}° Elevation={elevation:.0f}°")
|
||||
else:
|
||||
print(" Winkel: zu wenige Samples für eine Schätzung")
|
||||
|
||||
entry = {
|
||||
"zeit": datetime.now(timezone.utc).isoformat(),
|
||||
@@ -109,6 +135,8 @@ def save_iq_report(event):
|
||||
"antennen_pattern": list(SWITCHING_PATTERN),
|
||||
"i": i_values,
|
||||
"q": q_values,
|
||||
"azimut": azimuth,
|
||||
"elevation": elevation,
|
||||
}
|
||||
output_file.write(json.dumps(entry) + "\n")
|
||||
output_file.flush()
|
||||
@@ -120,6 +148,11 @@ def main():
|
||||
output_file = open(OUTPUT_FILE, "a", encoding="utf-8")
|
||||
print(f"IQ-Samples werden gespeichert in: {OUTPUT_FILE}")
|
||||
|
||||
if calibrating:
|
||||
print(f"\n*** KALIBRIERUNGSMODUS ***")
|
||||
print(f"Tag jetzt bei 0° Azimut / 0° Elevation vor dem Array platzieren!")
|
||||
print(f"Sammle automatisch {CALIBRATION_PACKET_COUNT} Pakete ...\n")
|
||||
|
||||
connector = bgapi.SerialConnector(COM_PORT, baudrate=BAUD_RATE)
|
||||
lib = bgapi.BGLib(connector, XAPI_PATH, on_event)
|
||||
lib.open()
|
||||
@@ -1,374 +0,0 @@
|
||||
import time
|
||||
import sys
|
||||
import json
|
||||
import struct
|
||||
import queue
|
||||
from datetime import datetime, timezone
|
||||
|
||||
import numpy as np
|
||||
import bgapi
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# EINSTELLUNGEN: BG22 / CTE
|
||||
# =============================================================================
|
||||
|
||||
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"
|
||||
|
||||
OUTPUT_FILE = "iq_samples.jsonl"
|
||||
ANGLE_TABLE_FILE = "winkel_tabelle.json" # nur Azimut/Elevation, ein Eintrag pro neuem Wert
|
||||
|
||||
SLOT_DURATIONS = 2 # 1 oder 2 Mikrosekunden pro Slot
|
||||
SWITCHING_PATTERN = bytes(range(0, 16)) # Reihenfolge der Antennen
|
||||
CTE_COUNT = 0 # 0 = unbegrenzt weiter empfangen
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# EINSTELLUNGEN: EIGENER MUSIC-ALGORITHMUS
|
||||
# =============================================================================
|
||||
|
||||
CALIBRATION_FILE = "calibration.json"
|
||||
|
||||
FREQUENCY_HZ = 2.44e9
|
||||
SPEED_OF_LIGHT = 3e8
|
||||
WAVELENGTH = SPEED_OF_LIGHT / FREQUENCY_HZ
|
||||
|
||||
ANTENNA_SPACING = 0.0400 # 40 mm, laut AN1195 (nicht lambda/2)
|
||||
|
||||
ARRAY_ROWS = 4
|
||||
ARRAY_COLS = 4
|
||||
NUM_ANTENNAS = ARRAY_ROWS * ARRAY_COLS
|
||||
|
||||
AZIMUTH_SIGN = 1
|
||||
ELEVATION_SIGN = 1
|
||||
|
||||
REFERENCE_SAMPLES = 8 # feste Referenzperiode am Anfang jedes CTE
|
||||
NUM_SOURCES = 1 # nur ein Tag sendet gleichzeitig
|
||||
|
||||
AZIMUTH_RANGE = np.arange(-180, 181, 1)
|
||||
ELEVATION_RANGE = np.arange(-90, 91, 1)
|
||||
|
||||
ACCUMULATE_PACKETS = 20 # Pakete pro Winkel-Update
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# GLOBALER ZUSTAND
|
||||
# =============================================================================
|
||||
|
||||
lib = None
|
||||
output_file = None
|
||||
sync_handle = None
|
||||
already_synced = False
|
||||
pending_actions = queue.Queue()
|
||||
|
||||
_cached_positions = None
|
||||
_cached_offsets = None
|
||||
_calibration_loaded = False
|
||||
|
||||
_snapshot_buffer = []
|
||||
_latest_stable_angle = (None, None)
|
||||
_angle_table = []
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# BLUETOOTH-EVENTS
|
||||
# =============================================================================
|
||||
|
||||
def on_event(event):
|
||||
"""Wird bei jedem eingehenden Ereignis vom BG22 aufgerufen."""
|
||||
global sync_handle
|
||||
|
||||
name = str(event).split("(")[0]
|
||||
|
||||
if name in ("bt_evt_scanner_scan_report",
|
||||
"bt_evt_scanner_extended_advertisement_report"):
|
||||
look_for_periodic_advertising(event)
|
||||
|
||||
elif name in ("bt_evt_sync_opened",
|
||||
"bt_evt_sync_scanner_opened",
|
||||
"bt_evt_periodic_sync_opened"):
|
||||
sync_handle = event.sync
|
||||
print(f"Verbindung zum Tag hergestellt (sync={sync_handle}).")
|
||||
pending_actions.put(("enable_cte", sync_handle))
|
||||
|
||||
elif name in ("bt_evt_sync_closed",
|
||||
"bt_evt_sync_scanner_closed",
|
||||
"bt_evt_periodic_sync_closed"):
|
||||
print("Verbindung zum Tag wurde beendet.")
|
||||
|
||||
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 != 0:
|
||||
print(f"Fehler vom BG22: {event}")
|
||||
|
||||
|
||||
def look_for_periodic_advertising(event):
|
||||
"""Prüft ein gefundenes Gerät: sendet es CTE über periodisches Advertising?"""
|
||||
global already_synced
|
||||
|
||||
if already_synced:
|
||||
return
|
||||
|
||||
periodic_interval = getattr(event, "periodic_interval", None)
|
||||
adv_sid = getattr(event, "adv_sid", None)
|
||||
|
||||
if not periodic_interval:
|
||||
return
|
||||
|
||||
print(f"Tag gefunden! RSSI={event.rssi} dBm, Interval={periodic_interval} ms")
|
||||
|
||||
already_synced = True
|
||||
pending_actions.put(("connect_to_tag", event.address, event.address_type, adv_sid))
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# BLUETOOTH-BEFEHLE
|
||||
# =============================================================================
|
||||
|
||||
def start_scan():
|
||||
"""Startet die Suche nach Bluetooth-Geräten in der Umgebung."""
|
||||
print("Suche nach einem Tag ...")
|
||||
lib.bt.scanner.set_parameters(1, 160, 160)
|
||||
lib.bt.scanner.start(1, 2)
|
||||
|
||||
|
||||
def connect_to_tag(address, address_type, adv_sid):
|
||||
"""Beendet die Suche und verbindet sich mit dem gefundenen Tag."""
|
||||
lib.bt.scanner.stop()
|
||||
lib.bt.sync_scanner.open(address, address_type, adv_sid)
|
||||
|
||||
|
||||
def enable_cte(sync):
|
||||
"""Aktiviert den CTE-Empfang auf der bestehenden Synchronisation."""
|
||||
lib.bt.cte_receiver.enable_connectionless_cte(
|
||||
sync, SLOT_DURATIONS, CTE_COUNT, SWITCHING_PATTERN
|
||||
)
|
||||
print("CTE-Empfang läuft. Warte auf IQ-Daten ...\n")
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# EIGENER MUSIC-ALGORITHMUS (2D, Azimut + Elevation, 4x4 URA)
|
||||
# =============================================================================
|
||||
|
||||
def antenna_positions():
|
||||
"""(x, y)-Position jeder Antenne im 4x4-Raster."""
|
||||
positions = []
|
||||
for row in range(ARRAY_ROWS):
|
||||
for col in range(ARRAY_COLS):
|
||||
positions.append((row * ANTENNA_SPACING, col * ANTENNA_SPACING))
|
||||
return np.array(positions)
|
||||
|
||||
|
||||
def steering_vector(azimuth_deg, elevation_deg, positions):
|
||||
"""Theoretischer Steering-Vector für einen Winkel."""
|
||||
az = np.radians(azimuth_deg) * AZIMUTH_SIGN
|
||||
el = np.radians(elevation_deg) * ELEVATION_SIGN
|
||||
u = np.cos(el) * np.sin(az)
|
||||
v = np.sin(el)
|
||||
phase = (2 * np.pi / WAVELENGTH) * (positions[:, 0] * u + positions[:, 1] * v)
|
||||
return np.exp(-1j * phase)
|
||||
|
||||
|
||||
def extract_snapshots(i_values, q_values):
|
||||
"""Wandelt IQ-Rohwerte in Antennen-Snapshots um."""
|
||||
complex_samples = np.array(i_values) + 1j * np.array(q_values)
|
||||
switching_samples = complex_samples[REFERENCE_SAMPLES:]
|
||||
|
||||
num_full_cycles = len(switching_samples) // NUM_ANTENNAS
|
||||
if num_full_cycles == 0:
|
||||
return None
|
||||
|
||||
usable = switching_samples[: num_full_cycles * NUM_ANTENNAS]
|
||||
return usable.reshape(num_full_cycles, NUM_ANTENNAS).T
|
||||
|
||||
|
||||
def circular_mean(angles):
|
||||
"""Mittelwert von Winkeln, robust gegen den Sprung bei ±π."""
|
||||
return np.angle(np.mean(np.exp(1j * angles)))
|
||||
|
||||
|
||||
def music_spectrum(snapshots, positions):
|
||||
"""MUSIC-Pseudospektrum, gibt den stärksten Peak zurück."""
|
||||
covariance = snapshots @ snapshots.conj().T / snapshots.shape[1]
|
||||
eigenvalues, eigenvectors = np.linalg.eigh(covariance)
|
||||
noise_subspace = eigenvectors[:, : NUM_ANTENNAS - NUM_SOURCES]
|
||||
|
||||
best_azimuth, best_elevation, best_power = 0.0, 0.0, -np.inf
|
||||
for az in AZIMUTH_RANGE:
|
||||
for el in ELEVATION_RANGE:
|
||||
a = steering_vector(az, el, positions)
|
||||
denom = a.conj().T @ noise_subspace @ noise_subspace.conj().T @ a
|
||||
power = 1.0 / np.abs(denom)
|
||||
if power > best_power:
|
||||
best_power = power
|
||||
best_azimuth, best_elevation = az, el
|
||||
|
||||
return int(best_azimuth), int(best_elevation)
|
||||
|
||||
|
||||
def load_calibration():
|
||||
"""Lädt gespeicherte Phasenoffsets, falls vorhanden."""
|
||||
try:
|
||||
with open(CALIBRATION_FILE, "r", encoding="utf-8") as f:
|
||||
data = json.load(f)
|
||||
return np.array(data["phasenoffsets_rad"])
|
||||
except FileNotFoundError:
|
||||
return None
|
||||
|
||||
|
||||
def apply_calibration(snapshots, offsets):
|
||||
"""Entfernt bekannte Phasenoffsets aus den Rohdaten."""
|
||||
correction = np.exp(-1j * offsets).reshape(-1, 1)
|
||||
return snapshots * correction
|
||||
|
||||
|
||||
def add_packet_and_maybe_estimate(i_values, q_values):
|
||||
"""Sammelt Snapshots; berechnet alle ACCUMULATE_PACKETS Pakete einen Winkel."""
|
||||
global _cached_positions, _cached_offsets, _calibration_loaded
|
||||
global _snapshot_buffer, _latest_stable_angle
|
||||
|
||||
if _cached_positions is None:
|
||||
_cached_positions = antenna_positions()
|
||||
|
||||
if not _calibration_loaded:
|
||||
_cached_offsets = load_calibration()
|
||||
_calibration_loaded = True
|
||||
|
||||
snapshots = extract_snapshots(i_values, q_values)
|
||||
if snapshots is not None:
|
||||
_snapshot_buffer.append(snapshots)
|
||||
|
||||
if len(_snapshot_buffer) < ACCUMULATE_PACKETS:
|
||||
return _latest_stable_angle
|
||||
|
||||
combined = np.concatenate(_snapshot_buffer, axis=1)
|
||||
_snapshot_buffer = []
|
||||
|
||||
if _cached_offsets is not None:
|
||||
combined = apply_calibration(combined, _cached_offsets)
|
||||
|
||||
_latest_stable_angle = music_spectrum(combined, _cached_positions)
|
||||
return _latest_stable_angle
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# IQ-DATEN VERARBEITEN
|
||||
# =============================================================================
|
||||
|
||||
def decode_samples(raw_bytes):
|
||||
"""Wandelt die rohen Bytes in I- und Q-Wertelisten um."""
|
||||
values = list(struct.unpack(f"{len(raw_bytes)}b", raw_bytes))
|
||||
return values[0::2], values[1::2]
|
||||
|
||||
|
||||
def add_to_angle_table(zeit, counter, azimuth, elevation):
|
||||
"""Fügt einen neuen Winkel-Eintrag hinzu und schreibt die Tabelle neu."""
|
||||
_angle_table.append({
|
||||
"zeit": zeit, "counter": counter,
|
||||
"azimut": azimuth, "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):
|
||||
"""Dekodiert, berechnet den Winkel, zeigt an, speichert."""
|
||||
i_values, q_values = decode_samples(event.samples)
|
||||
|
||||
azimuth, elevation = add_packet_and_maybe_estimate(i_values, q_values)
|
||||
|
||||
print(f"IQ-Paket #{event.event_counter} Kanal={event.channel} "
|
||||
f"RSSI={event.rssi} dBm {len(i_values)} Samples "
|
||||
f"({len(_snapshot_buffer)}/{ACCUMULATE_PACKETS} gesammelt)")
|
||||
print(f" Rohbytes: {event.samples[:8]} ...")
|
||||
print(f" I/Q-Werte: {list(zip(i_values[:4], q_values[:4]))} ...")
|
||||
if azimuth is not None:
|
||||
print(f" Azimut={azimuth}° Elevation={elevation}°")
|
||||
if not _angle_table or (azimuth, elevation) != (
|
||||
_angle_table[-1]["azimut"], _angle_table[-1]["elevation"]
|
||||
):
|
||||
add_to_angle_table(
|
||||
datetime.now(timezone.utc).isoformat(),
|
||||
event.event_counter, azimuth, elevation,
|
||||
)
|
||||
else:
|
||||
print(" sammle noch Pakete ...")
|
||||
|
||||
entry = {
|
||||
"zeit": datetime.now(timezone.utc).isoformat(),
|
||||
"counter": event.event_counter,
|
||||
"kanal": event.channel,
|
||||
"rssi": event.rssi,
|
||||
"slot_dauer_us": SLOT_DURATIONS,
|
||||
"antennen_pattern": list(SWITCHING_PATTERN),
|
||||
"i": i_values,
|
||||
"q": q_values,
|
||||
"azimut": azimuth,
|
||||
"elevation": elevation,
|
||||
}
|
||||
output_file.write(json.dumps(entry) + "\n")
|
||||
output_file.flush()
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# HAUPTPROGRAMM
|
||||
# =============================================================================
|
||||
|
||||
def connect_to_bg22():
|
||||
"""Öffnet die serielle Verbindung, stellt sauberen Zustand her."""
|
||||
connector = bgapi.SerialConnector(COM_PORT, baudrate=BAUD_RATE)
|
||||
module = bgapi.BGLib(connector, XAPI_PATH, on_event)
|
||||
module.open()
|
||||
print(f"Verbunden mit {COM_PORT}.")
|
||||
|
||||
module.bt.system.reboot()
|
||||
time.sleep(2.0)
|
||||
|
||||
module.bt.system.hello()
|
||||
print("BG22 antwortet, alles bereit.")
|
||||
return module
|
||||
|
||||
|
||||
def process_pending_actions():
|
||||
"""Führt Aktionen aus, die nicht direkt im Event-Handler laufen dürfen."""
|
||||
while True:
|
||||
try:
|
||||
action, *details = pending_actions.get(timeout=0.1)
|
||||
except queue.Empty:
|
||||
continue
|
||||
|
||||
if action == "connect_to_tag":
|
||||
connect_to_tag(*details)
|
||||
elif action == "enable_cte":
|
||||
enable_cte(*details)
|
||||
|
||||
|
||||
def main():
|
||||
global lib, output_file
|
||||
|
||||
output_file = open(OUTPUT_FILE, "a", encoding="utf-8")
|
||||
print(f"IQ-Samples werden gespeichert in: {OUTPUT_FILE}")
|
||||
|
||||
lib = connect_to_bg22()
|
||||
start_scan()
|
||||
process_pending_actions()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
try:
|
||||
main()
|
||||
except KeyboardInterrupt:
|
||||
print("\nBeendet.")
|
||||
except Exception as error:
|
||||
print(f"Es ist ein Fehler aufgetreten: {error}")
|
||||
sys.exit(1)
|
||||
finally:
|
||||
if lib is not None:
|
||||
lib.close()
|
||||
if output_file is not None:
|
||||
output_file.close()
|
||||
@@ -1,181 +0,0 @@
|
||||
"""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 <Az> --kal-el <El> --ref-az <Az Analyzer> --ref-el <El Analyzer> --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()
|
||||
@@ -0,0 +1,284 @@
|
||||
"""
|
||||
music_estimator.py
|
||||
|
||||
Eigene 2D-MUSIC-Implementierung (Azimut + Elevation) fuer ein 4x4 URA
|
||||
(Uniform Rectangular Array), passend zu den Daten aus bg22_iq_logger.py
|
||||
(Datei iq_samples.jsonl).
|
||||
|
||||
Diese Implementierung basiert auf der allgemeinen, oeffentlich bekannten
|
||||
MUSIC-Theorie (Schmidt, 1986) und wurde von Grund auf fuer dieses Projekt
|
||||
geschrieben - keine Uebernahme aus einem bestehenden Repository, da die
|
||||
gefundenen Beispiele im Netz auf andere Array-Geometrien/Datenformate
|
||||
(Radar, 1D-Array) zugeschnitten sind.
|
||||
|
||||
Voraussetzungen:
|
||||
pip install numpy --break-system-packages
|
||||
|
||||
Kalibrierung (Phasenoffsets pro Antenne):
|
||||
1. Tag bei bekanntem Winkel (Standard: 0°/0°, direkt vor dem Array)
|
||||
platzieren und mit bg22_iq_logger.py eine eigene Datei aufnehmen,
|
||||
z. B. "iq_samples_0deg.jsonl".
|
||||
2. Kalibrierung berechnen und speichern:
|
||||
python music_estimator.py calibrate iq_samples_0deg.jsonl
|
||||
Das schreibt "calibration.json" mit den Phasenoffsets pro Antenne.
|
||||
3. Normale Auswertung (nutzt calibration.json automatisch, falls
|
||||
vorhanden):
|
||||
python music_estimator.py
|
||||
"""
|
||||
|
||||
import sys
|
||||
import json
|
||||
import numpy as np
|
||||
|
||||
|
||||
# --- Einstellungen ---
|
||||
|
||||
INPUT_FILE = "iq_samples.jsonl"
|
||||
CALIBRATION_FILE = "calibration.json"
|
||||
|
||||
FREQUENCY_HZ = 2.44e9 # Bluetooth-Mittenfrequenz (Kanal ~19, ca. 2.44 GHz)
|
||||
SPEED_OF_LIGHT = 3e8
|
||||
WAVELENGTH = SPEED_OF_LIGHT / FREQUENCY_HZ
|
||||
|
||||
# WICHTIG: Der reale Antennenabstand des BRD4191A ist NICHT lambda/2!
|
||||
# Laut AN1195 (Antenna Array Design Guidelines): Patch-Antennen 27,9 x 27,9 mm,
|
||||
# mit 12,1 mm Abstand zueinander platziert -> Mitte-zu-Mitte-Abstand = 40,0 mm.
|
||||
# (lambda/2 waere hier ca. 61,5 mm gewesen - deutlich zu groß!)
|
||||
ANTENNA_SPACING = 0.0400 # Meter, aus AN1195 (27.9mm Patch + 12.1mm Abstand)
|
||||
|
||||
ARRAY_ROWS = 4
|
||||
ARRAY_COLS = 4
|
||||
NUM_ANTENNAS = ARRAY_ROWS * ARRAY_COLS # 16, entspricht SWITCHING_PATTERN
|
||||
|
||||
# Vorzeichen-Konvention testen/anpassen, falls sich beim Vergleich mit der
|
||||
# RTL-Bibliothek ein systematisches Vorzeichen-Muster zeigt (siehe Chat-
|
||||
# Diskussion: (-37,-65) eigener Algo. vs (50,58) RTL-Referenz -> Verdacht
|
||||
# auf umgekehrte Vorzeichen-Konvention).
|
||||
AZIMUTH_SIGN = -1 # auf -1 setzen, um das Vorzeichen zu testen/korrigieren
|
||||
ELEVATION_SIGN = -1 # auf -1 setzen, um das Vorzeichen zu testen/korrigieren
|
||||
|
||||
REFERENCE_SAMPLES = 8 # feste Referenzperiode, siehe Chat-Diskussion
|
||||
NUM_SOURCES = 1 # nur ein Tag sendet gleichzeitig
|
||||
|
||||
# Suchraster fuer die Winkelsuche (Grad)
|
||||
AZIMUTH_RANGE = np.arange(-90, 91, 1)
|
||||
ELEVATION_RANGE = np.arange(-90, 91, 1)
|
||||
|
||||
|
||||
def antenna_positions():
|
||||
"""Liefert die (x, y)-Position jeder Antenne im 4x4-Raster,
|
||||
in der Reihenfolge des SWITCHING_PATTERN (0..15)."""
|
||||
positions = []
|
||||
for row in range(ARRAY_ROWS):
|
||||
for col in range(ARRAY_COLS):
|
||||
x = col * ANTENNA_SPACING
|
||||
y = row * ANTENNA_SPACING
|
||||
positions.append((x, y))
|
||||
return np.array(positions) # Form: (16, 2)
|
||||
|
||||
|
||||
def steering_vector(azimuth_deg, elevation_deg, positions):
|
||||
"""Berechnet den theoretischen Steering-Vector fuer einen Winkel.
|
||||
Noch OHNE Phasenkalibrierung der einzelnen Antennen."""
|
||||
az = np.radians(azimuth_deg) * AZIMUTH_SIGN
|
||||
el = np.radians(elevation_deg) * ELEVATION_SIGN
|
||||
|
||||
# Richtungsvektor der ankommenden Welle
|
||||
u = np.cos(el) * np.sin(az)
|
||||
v = np.sin(el)
|
||||
|
||||
phase = (2 * np.pi / WAVELENGTH) * (positions[:, 0] * u + positions[:, 1] * v)
|
||||
return np.exp(1j * phase) # Form: (16,)
|
||||
|
||||
|
||||
def extract_snapshots(i_values, q_values):
|
||||
"""Wandelt die IQ-Rohwerte eines Pakets in mehrere Antennen-Snapshots um.
|
||||
Die Referenzperiode wird uebersprungen; danach wird zyklisch durch die
|
||||
16 Antennen des Switching-Patterns gegangen. Jeder volle Durchlauf durch
|
||||
alle 16 Antennen ergibt einen Snapshot (eine Spalte der Datenmatrix)."""
|
||||
complex_samples = np.array(i_values) + 1j * np.array(q_values)
|
||||
switching_samples = complex_samples[REFERENCE_SAMPLES:]
|
||||
|
||||
num_full_cycles = len(switching_samples) // NUM_ANTENNAS
|
||||
if num_full_cycles == 0:
|
||||
return None # zu wenige Samples fuer einen vollen Durchlauf
|
||||
|
||||
usable = switching_samples[: num_full_cycles * NUM_ANTENNAS]
|
||||
snapshots = usable.reshape(num_full_cycles, NUM_ANTENNAS).T
|
||||
return snapshots # Form: (16, num_full_cycles)
|
||||
|
||||
|
||||
def circular_mean(angles):
|
||||
"""Mittelwert von Winkeln (Radiant), robust gegen den Sprung bei ±π."""
|
||||
return np.angle(np.mean(np.exp(1j * angles)))
|
||||
|
||||
|
||||
def compute_calibration_from_samples(samples, known_azimuth, known_elevation, positions=None):
|
||||
"""Wie compute_calibration(), aber nimmt eine Liste von (i_values, q_values)
|
||||
Tupeln direkt entgegen, statt eine Datei zu lesen. Nützlich für eine
|
||||
LIVE-Kalibrierung waehrend das Skript laeuft (siehe bg22_iq_logger.py)."""
|
||||
if positions is None:
|
||||
positions = antenna_positions()
|
||||
|
||||
theoretical = steering_vector(known_azimuth, known_elevation, positions)
|
||||
theoretical_phase = np.angle(theoretical / theoretical[0])
|
||||
|
||||
offsets_per_antenna = [[] for _ in range(NUM_ANTENNAS)]
|
||||
|
||||
for i_values, q_values in samples:
|
||||
snapshots = extract_snapshots(i_values, q_values)
|
||||
if snapshots is None:
|
||||
continue
|
||||
for snapshot in snapshots.T:
|
||||
observed_phase = np.angle(snapshot / snapshot[0])
|
||||
error = observed_phase - theoretical_phase
|
||||
for antenna in range(NUM_ANTENNAS):
|
||||
offsets_per_antenna[antenna].append(error[antenna])
|
||||
|
||||
offsets = [circular_mean(np.array(values)) if values else 0.0
|
||||
for values in offsets_per_antenna]
|
||||
|
||||
with open(CALIBRATION_FILE, "w", encoding="utf-8") as f:
|
||||
json.dump({
|
||||
"referenz_azimut": known_azimuth,
|
||||
"referenz_elevation": known_elevation,
|
||||
"phasenoffsets_rad": offsets,
|
||||
}, f, indent=2)
|
||||
|
||||
print(f"Kalibrierung gespeichert in {CALIBRATION_FILE}")
|
||||
print(f"Phasenoffsets (Grad): {np.degrees(offsets).round(1).tolist()}")
|
||||
return offsets
|
||||
|
||||
|
||||
def compute_calibration(input_file, known_azimuth, known_elevation, positions):
|
||||
"""Berechnet den Phasenoffset jeder Antenne relativ zu Antenne 0, aus
|
||||
einer Aufnahme bei bekanntem Winkel (Standard: 0°/0°), gelesen aus
|
||||
einer JSON-Lines-Datei."""
|
||||
samples = []
|
||||
with open(input_file, "r", encoding="utf-8") as f:
|
||||
for line in f:
|
||||
record = json.loads(line)
|
||||
samples.append((record["i"], record["q"]))
|
||||
return compute_calibration_from_samples(samples, known_azimuth, known_elevation, positions)
|
||||
|
||||
|
||||
def reset_calibration_cache():
|
||||
"""Erzwingt, dass estimate_angle() die Kalibrierungsdatei beim naechsten
|
||||
Aufruf neu einliest (z. B. nachdem gerade eine neue Kalibrierung
|
||||
gespeichert wurde)."""
|
||||
global _calibration_loaded
|
||||
_calibration_loaded = False
|
||||
|
||||
|
||||
def load_calibration():
|
||||
"""Laedt gespeicherte Phasenoffsets, falls vorhanden. Sonst None."""
|
||||
try:
|
||||
with open(CALIBRATION_FILE, "r", encoding="utf-8") as f:
|
||||
data = json.load(f)
|
||||
return np.array(data["phasenoffsets_rad"])
|
||||
except FileNotFoundError:
|
||||
return None
|
||||
|
||||
|
||||
def apply_calibration(snapshots, offsets):
|
||||
"""Entfernt die bekannten Phasenoffsets aus den Rohdaten, bevor MUSIC
|
||||
darauf angewendet wird."""
|
||||
correction = np.exp(-1j * offsets).reshape(-1, 1)
|
||||
return snapshots * correction
|
||||
|
||||
|
||||
def music_spectrum(snapshots, positions):
|
||||
"""Berechnet das MUSIC-Pseudospektrum ueber das Winkelraster und gibt
|
||||
den Winkel mit dem staerksten Peak zurueck."""
|
||||
# Kovarianzmatrix des Signals (16x16)
|
||||
covariance = snapshots @ snapshots.conj().T / snapshots.shape[1]
|
||||
|
||||
eigenvalues, eigenvectors = np.linalg.eigh(covariance)
|
||||
# Sortierung: groesster Eigenwert zuletzt -> Rauschraum sind die ersten
|
||||
noise_subspace = eigenvectors[:, : NUM_ANTENNAS - NUM_SOURCES]
|
||||
|
||||
best_azimuth, best_elevation, best_power = 0.0, 0.0, -np.inf
|
||||
|
||||
for az in AZIMUTH_RANGE:
|
||||
for el in ELEVATION_RANGE:
|
||||
a = steering_vector(az, el, positions)
|
||||
denom = a.conj().T @ noise_subspace @ noise_subspace.conj().T @ a
|
||||
power = 1.0 / np.abs(denom)
|
||||
if power > best_power:
|
||||
best_power = power
|
||||
best_azimuth, best_elevation = az, el
|
||||
|
||||
return int(best_azimuth), int(best_elevation)
|
||||
|
||||
|
||||
# --- Öffentliche Funktion zum direkten Aufruf aus anderem Code ---
|
||||
|
||||
_cached_positions = None
|
||||
_cached_offsets = None
|
||||
_calibration_loaded = False
|
||||
|
||||
|
||||
def estimate_angle(i_values, q_values):
|
||||
"""Nimmt die dekodierten I- und Q-Werte EINES IQ-Reports entgegen
|
||||
(genau das, was decode_samples() in bg22_iq_logger.py liefert) und
|
||||
gibt (azimuth, elevation) in Grad zurück, oder (None, None), falls
|
||||
zu wenige Samples für einen vollen Antennen-Durchlauf vorliegen.
|
||||
|
||||
Kalibrierung wird beim ersten Aufruf automatisch geladen (falls
|
||||
calibration.json existiert) und danach zwischengespeichert.
|
||||
"""
|
||||
global _cached_positions, _cached_offsets, _calibration_loaded
|
||||
|
||||
if _cached_positions is None:
|
||||
_cached_positions = antenna_positions()
|
||||
|
||||
if not _calibration_loaded:
|
||||
_cached_offsets = load_calibration()
|
||||
_calibration_loaded = True
|
||||
|
||||
snapshots = extract_snapshots(i_values, q_values)
|
||||
if snapshots is None or snapshots.shape[1] < 2:
|
||||
return None, None
|
||||
|
||||
if _cached_offsets is not None:
|
||||
snapshots = apply_calibration(snapshots, _cached_offsets)
|
||||
|
||||
return music_spectrum(snapshots, _cached_positions)
|
||||
|
||||
|
||||
def main():
|
||||
positions = antenna_positions()
|
||||
|
||||
if len(sys.argv) >= 2 and sys.argv[1] == "calibrate":
|
||||
calibration_file = sys.argv[2] if len(sys.argv) >= 3 else INPUT_FILE
|
||||
compute_calibration(calibration_file, known_azimuth=0, known_elevation=0,
|
||||
positions=positions)
|
||||
return
|
||||
|
||||
offsets = load_calibration()
|
||||
if offsets is None:
|
||||
print("Keine Kalibrierung gefunden (calibration.json fehlt). "
|
||||
"Ergebnisse sind unkalibriert und möglicherweise verzerrt.\n"
|
||||
"Kalibrierung erstellen mit: python music_estimator.py calibrate <datei>\n")
|
||||
else:
|
||||
print("Kalibrierung geladen, wird angewendet.\n")
|
||||
|
||||
with open(INPUT_FILE, "r", encoding="utf-8") as f:
|
||||
for line_number, line in enumerate(f, start=1):
|
||||
record = json.loads(line)
|
||||
snapshots = extract_snapshots(record["i"], record["q"])
|
||||
|
||||
if snapshots is None or snapshots.shape[1] < 2:
|
||||
print(f"Zeile {line_number}: zu wenige Snapshots, übersprungen.")
|
||||
continue
|
||||
|
||||
if offsets is not None:
|
||||
snapshots = apply_calibration(snapshots, offsets)
|
||||
|
||||
azimuth, elevation = music_spectrum(snapshots, positions)
|
||||
print(f"Zeile {line_number} (counter={record['counter']}): "
|
||||
f"Azimut={azimuth:.0f}° Elevation={elevation:.0f}°")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Reference in new issue
Block a user