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