285 lines
11 KiB
Python
285 lines
11 KiB
Python
"""
|
|
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()
|