Entfernen von music_estimator.py aus main
This commit is contained in:
SandraGuefack committed
1 parent
e63a0d690c
commit
45080d5959
1 file changed
-284
@@ -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 <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