diff --git a/music_estimator.py b/music_estimator.py deleted file mode 100644 index 0fba5e9..0000000 --- a/music_estimator.py +++ /dev/null @@ -1,284 +0,0 @@ -""" -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()