diff --git a/requirements.txt b/requirements.txt index 142eea4b..167ad645 100644 --- a/requirements.txt +++ b/requirements.txt @@ -23,3 +23,4 @@ tomli>=2.4.0 typing_extensions>=4.13.2 tzdata>=2025.3 zipp>=3.20.2 +pqdm diff --git a/scripts/experiment_scripts/process_experiment.py b/scripts/experiment_scripts/process_experiment.py new file mode 100644 index 00000000..42c73795 --- /dev/null +++ b/scripts/experiment_scripts/process_experiment.py @@ -0,0 +1,22 @@ +from pathlib import Path +import argparse +from tqdm import tqdm + +from ofdm.processing import pipeline +from ofdm.config import OFDMConfig + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("--experiment_pth", type=str, default="/home/guoyixu/OFDM_Sense/EXPERIMENTS/rj_virtual_multilateration") + parser.add_argument("--ref_pth", type=str, default="./data_files/rand_ofdm_packet_ref.json") + args = parser.parse_args() + + ofdm_conf = OFDMConfig() + experiment_pth = Path(args.experiment_pth) + for archive_pth in experiment_pth.glob("*archive"): + print(f"\n--- Processing {archive_pth} ---") + pipeline.process_archive(archive_dir=archive_pth, ref_path=args.ref_pth, ofdm_conf=ofdm_conf) + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/scripts/localization/multilateration.py b/scripts/localization/multilateration.py new file mode 100644 index 00000000..487b4367 --- /dev/null +++ b/scripts/localization/multilateration.py @@ -0,0 +1,108 @@ +import argparse +import json +import numpy as np +import pandas as pd +import matplotlib.pyplot as plt +from pathlib import Path + +from ofdm.simulation.solver import solve_tdoa +from ofdm.viz.sim_plotter import plot_experiment_results, plot_tdoa_hyperbolas + +def load_rover_position(roaming_positions_path: Path) ->np.ndarray: + with open(roaming_positions_path) as f: + data = json.load(f) + pos = next(iter(data.values())) + return np.array([pos["x"], pos["y"]]) + +def main(): + parser = argparse.ArgumentParser(description="Run TDOA multilateration on a processed experiment.") + parser.add_argument("--experiment", type=str, required=True, help ="Path to experiment directory") + parser.add_argument("--devices", nargs="+", required=True, help="Device archive names e.g. RX3ch1 RX5ch1, RX7ch1") + parser.add_argument("--anchor", type=str, required=True, help="Anchor device key in fixed_positions.json e.g ANCHORch0") + parser.add_argument("--bias-ns", type=float, default=3.661, help="Hardware TDOA bias to subtract (nanoseconds)") + args = parser.parse_args() + + experiment_dir = Path(args.experiment) + + with open(experiment_dir / "fixed_positions.json") as f: + fixed = json.load(f) + + anchor_pos = np.array([fixed[args.anchor]["x"], fixed[args.anchor]["y"]]) + tx_true = np.array([fixed["TX"]["x"], fixed["TX"]["y"]]) + + rover_positions = [] + device_dataframes = [] + + for device in args.devices: + archive_dir = experiment_dir / f"{device}_archive" + rover_positions.append(load_rover_position(archive_dir / "roaming_positions.json")) + + df = pd.read_csv(archive_dir / "delays.csv") + device_dataframes.append(df[["run", "delay0", "delay1"]].rename(columns={"delay0": f"delay0_{device}", "delay1": f"delay1_{device}"})) + + rx_coords = np.array([anchor_pos] + rover_positions) + + print(f"ROVER POSITIONS {rover_positions}") + + merged = device_dataframes[0][["run"]].copy() + for df in device_dataframes: + merged = merged.merge(df, on="run") + + estimated_positions = [] + + MAX_TDOA_NS = 30 + + for _, row in merged.iterrows(): + delay_diffs_s = [] + valid=True + for device in args.devices: + tdoa_ns = row[f"delay0_{device}"] - row[f"delay1_{device}"] - args.bias_ns + if abs(tdoa_ns) > MAX_TDOA_NS: + valid = False + break + delay_diffs_s.append(tdoa_ns * 1e-9) + if not valid: + continue + result = solve_tdoa(rx_coords, np.array(delay_diffs_s)) + if result is not None: + estimated_positions.append(result) + + estimated_positions = np.array(estimated_positions) + n_converged = len(estimated_positions) + n_total = len(merged) + + if n_converged == 0: + print("No successful solves.") + return + + centroid = np.mean(estimated_positions, axis=0) + errors = np.linalg.norm(estimated_positions - tx_true, axis=1) + centroid_error = np.linalg.norm(centroid - tx_true) + + print(f"Converged: {n_converged}/{n_total}") + print(f"Centroid: X={centroid[0]:.4f}m Y={centroid[1]:.4f}m") + print(f"Mean error: {np.mean(errors)*100:.2f} cm") + print(f"RMSE: {np.sqrt(np.mean(errors**2))*100:.2f} cm") + print(f"P95 error: {np.percentile(errors, 95)*100:.2f} cm") + print(f"Centroid error: {centroid_error*100:.2f} cm") + + results = { + "estimates": estimated_positions, + "centroid": centroid, + "rmse": np.sqrt(np.mean(errors**2)), + "mean_error": np.mean(errors), + "p95_error": np.percentile(errors, 95), + "centroid_error": centroid_error, + "n_converged": n_converged, + "n_trials": n_total, + } + + fig, ax = plt.subplots(figsize=(9, 9)) + plot_experiment_results(results, tx_true, rx_coords, ax=ax) + plot_tdoa_hyperbolas(tx_true, rx_coords, results, ax) + ax.set_title(f"OFDM Localization") + plt.tight_layout() + plt.show() + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/scripts/simulation/heapmap.py b/scripts/simulation/heatmap.py similarity index 77% rename from scripts/simulation/heapmap.py rename to scripts/simulation/heatmap.py index 45849b7d..462a2470 100644 --- a/scripts/simulation/heapmap.py +++ b/scripts/simulation/heatmap.py @@ -1,4 +1,5 @@ import numpy as np +from tqdm import tqdm import matplotlib.pyplot as plt from ofdm.simulation.monte_carlo import run_monte_carlo from ofdm.config import loadLayout @@ -11,30 +12,31 @@ def main(): rx_x = rx_coords[:, 0].reshape(-1, 1, 1) rx_y = rx_coords[:, 1].reshape(-1, 1, 1) - x_range = np.linspace(0, 1, num=40) - y_range = np.linspace(0, 1, num=40) + x_range = np.linspace(0, 2, num=50) + y_range = np.linspace(0, 2, num=50) X, Y = np.meshgrid(x_range, y_range) - + bounds = ([0, 2], [0, 2]) error_heatmap = np.zeros(X.shape) - for i in range(len(x_range)): + for i in tqdm(range(len(x_range)), desc="Running Monte Carlo Simulations"): for j in range(len(y_range)): tx_coords = np.array([X[i][j], Y[i][j]]) results = run_monte_carlo( rx_coords=rx_coords, tx_pos=tx_coords, - sigma_ns=0.1, - n_trials=10, + sigma_ns=0.5, + n_trials=30, seed=42, + bounds=bounds ) error_heatmap[i][j] = results['rmse'] plt.figure(figsize=(10, 8)) - v_max = 1 + v_max = 1.5 v_min = 0 cp = plt.pcolormesh(X, Y, error_heatmap,vmin=v_min, vmax=v_max, shading='auto', cmap='viridis') plt.colorbar(cp, label='RMSE (meters)') - plt.scatter(rx_x, rx_y, marker='^', color='red', label='Receivers') + plt.scatter(rx_x, rx_y, marker='^', color='red', label='Receivers', s=150) plt.title('OFDM Localization Error Heatmap') plt.xlabel('X Position (m)') plt.ylabel('Y Position (m)') @@ -43,4 +45,4 @@ def main(): if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/scripts/simulation/monte_carlo.py b/scripts/simulation/monte_carlo.py index 80525f35..4b9e15f5 100644 --- a/scripts/simulation/monte_carlo.py +++ b/scripts/simulation/monte_carlo.py @@ -18,12 +18,15 @@ def main(): if args.tx is not None: tx_true = np.array(args.tx) + bounds = ([0, 2], [0, 2]) + results = run_monte_carlo( tx_pos=tx_true, rx_coords=rx_coords, sigma_ns=args.sigma_ns, n_trials=args.trials, seed=42, + bounds=bounds ) print(f"Converged: {results['n_converged']}/{results['n_trials']}") diff --git a/src/ofdm/modulation/qam.py b/src/ofdm/modulation/qam.py index 8fa1f133..d7a9d4e9 100644 --- a/src/ofdm/modulation/qam.py +++ b/src/ofdm/modulation/qam.py @@ -66,5 +66,16 @@ def get_reference_constalation() -> np.ndarray: points = list(QAM16_MAP.values()) return np.array(points) +def binary_ref_to_iq(binary_string:str)->np.ndarray: + """ + Helper to convert the binary reference data in the ref_data json files into iq data for evaluation + calculations. + """ + full_string = "".join(binary_string) + word_len = 4 + binary_word_list = np.array([full_string[i:word_len + i] for i in range(0 ,len(full_string), word_len)]) + + iq_array = [binary_to_iq(word) for word in binary_word_list] + return np.array(iq_array) * np.sqrt(10) \ No newline at end of file diff --git a/src/ofdm/processing/pipeline.py b/src/ofdm/processing/pipeline.py new file mode 100644 index 00000000..622a8d84 --- /dev/null +++ b/src/ofdm/processing/pipeline.py @@ -0,0 +1,91 @@ +import numpy as np +import json +from pathlib import Path +from collections import defaultdict +from tqdm import tqdm +import pandas as pd + +from ofdm.processing.rx import unpack_rx_file, normalize_rx_signal, extract_packet +from ofdm.channel.delay import calculate_sub_sample_delay_parabolic +from ofdm.modulation import qam +from ofdm.utils.eval import calc_EVM, calc_BER, calc_SER +from ofdm.config import OFDMConfig + + + + +def process_dat_file(dat_path:Path, ref_path:Path, channel:int, ofdm_conf:OFDMConfig) -> dict: + """ + Processes a single .dat file and returns delay and transfer quality metrics. + Only the delay is used for localization, demodulated IQ data is discarded + """ + + # get refined_packet_start for delay calculation + demodulated_data, ref_data, refined_packet_start = unpack_rx_file( + ofdm_conf = ofdm_conf, + rx_path = dat_path, + ref_path = ref_path, + ) + + # re-read raw signal for delay calc — unpack_rx_file applies CFO correction internally + # which we do not want for the matched filter time delay estimate + rx_data = normalize_rx_signal(extract_packet( + rx_data=np.fromfile(dat_path, dtype=np.complex64), + start_idx=refined_packet_start, + total_symbols = (1 + 1 + ref_data["n_data_symb"]), + ofdm_conf=ofdm_conf + )) + tx_pilot = np.array(ref_data['pilot_ref_real']) + 1j * np.array(ref_data['pilot_ref_imag']) + + delay_s = calculate_sub_sample_delay_parabolic( + rx_signal=rx_data, + ref_signal = tx_pilot, + fs = ofdm_conf.FS + ) + delay_ns = delay_s * 1e9 # convert to nano seconds + + ref_binary = ref_data['binary_data'] + ref_iq_data = qam.binary_ref_to_iq(binary_string=ref_binary) + evm = calc_EVM(iq_rx=demodulated_data, iq_ref=ref_iq_data) + ser = calc_SER(iq_rx=demodulated_data, iq_ref=ref_iq_data) + ber = calc_BER(iq_rx=demodulated_data, iq_ref = ref_iq_data) + + return {f"delay{channel}":delay_ns, + f"evm{channel}": evm, + f"ser{channel}": ser, + f"ber{channel}": ber} + + +def process_archive(archive_dir:Path, ref_path:Path, ofdm_conf:OFDMConfig): + """ + Script to automate experiment unpacking. + + reads all of the .dat files in each of the channel dirs in a device archive and saves the unpacked experiment data in a csv + """ + runs = defaultdict(dict) + for channel_dir in archive_dir.glob("channel*/"): + channel = int(channel_dir.name.replace("channel", "")) + for dat_file in channel_dir.glob("*.dat"): + run_number = int(dat_file.name.split("_run_")[1].split("_")[0]) + runs[run_number][channel] = dat_file + + results = [] + for run_number, channel_files in tqdm(sorted(runs.items()), desc="Processing runs"): + row = {"run": run_number} + for channel, dat_file in sorted(channel_files.items()): + try: + unpacked_data = process_dat_file( + dat_path=dat_file, + ref_path=ref_path, + channel= channel, + ofdm_conf= ofdm_conf + ) + row.update(unpacked_data) + except Exception as e: + tqdm.write(f" [WARNING] run {run_number} channel {channel} failed: {e}") + results.append(row) + + output_path = archive_dir / "delays.csv" + pd.DataFrame(results).to_csv(output_path, index=False) + print(f"Saved {len(results)} runs to {output_path}") + diff --git a/src/ofdm/processing/rx.py b/src/ofdm/processing/rx.py new file mode 100644 index 00000000..94ca45e5 --- /dev/null +++ b/src/ofdm/processing/rx.py @@ -0,0 +1,157 @@ +import numpy as np +import json +from ofdm.config import OFDMConfig +from ofdm.core import sync, waveform, preamble, payload +from ofdm.channel import CHEST, cfo + + +def unpack_rx_file(ofdm_conf:OFDMConfig, rx_path:str, ref_path:str, sim:bool = False)->np.ndarray: + """ + Take raw ofdm binary data from the rx and unpack it including syncronization, channel estimation, and cfo calibration. + """ + + + if sim == False: + rx_raw = np.fromfile(rx_path, dtype=np.complex64) + else: + rx_raw = np.fromfile(rx_path, dtype=np.complex64) + with open(ref_path) as f: + ref_data = json.load(f) + + #Unpack Referense Sync Symbol + sync_ref_real = np.array(ref_data['sync_ref_real']).astype(complex) + sync_ref_imag = np.array(ref_data["sync_ref_imag"]).astype(complex) + sync_ref_time = sync_ref_real + 1j * sync_ref_imag + n_payload_syms = ref_data["n_data_symb"] + + # ---------- Syncronization ------------- + M, P = sync.calculate_schmidl_cox_metrics(rx_signal=rx_raw, config=ofdm_conf) + start_idx = sync.find_start_idx( + M_metric=M, + config=ofdm_conf, + rx_signal=rx_raw, + known_sync_time=sync_ref_time, + search_window=500 + ) + + # locate pilot symbol + sync_len = ofdm_conf.CP_LEN + ofdm_conf.N + pilot_len = ofdm_conf.CP_LEN + ofdm_conf.N + coarse_pilot_start = start_idx + sync_len + + #Create a search window + search_margin = ofdm_conf.CP_LEN + pilot_chunk_start = coarse_pilot_start - search_margin + pilot_chunk_end = coarse_pilot_start + pilot_len + search_margin + rx_pilot_search_area = rx_raw[pilot_chunk_start: pilot_chunk_end] + + #Prepare Reference + tx_pilot_ref = np.array(ref_data['pilot_ref_real']).astype(complex) + 1j * np.array(ref_data['pilot_ref_imag']).astype(complex) + tx_pilot_no_cp = waveform.remove_cp(tx_pilot_ref, cp_len=ofdm_conf.CP_LEN) + + #Estimate + best_cfo, best_delay_rel, heatmap = cfo.estimate_cfo( + tx_ref = tx_pilot_ref, + rx_signal = rx_pilot_search_area, + fs = 100e6, + n_bins = 2 ** 14 + ) + best_cfo = best_cfo * -1 + + #Global Correction + actual_pilot_start = pilot_chunk_start + best_delay_rel + refined_packet_start = actual_pilot_start - sync_len + + #Apply CFO to enite signal + time_vec = np.arange(len(rx_raw)) / ofdm_conf.FS + correction_vector = np.exp(-1j * 2 * np.pi * best_cfo * time_vec) + rx_corrected = rx_raw * correction_vector + + #---------- Extract Symbols ---------- + sym_len = ofdm_conf.N + ofdm_conf.CP_LEN + total_symbols = 1 + 1 + n_payload_syms # (1 and 1 for Sync and Pilot) This needs to up updated eventually for dynamic + total_samlpes = sym_len * total_symbols + if refined_packet_start + total_samlpes > len(rx_raw): + print(f"[Error] Packet end index {refined_packet_start + total_samlpes} exceeds file size{len(rx_raw)}") + print(f"Setting start idx to 0") + refined_packet_start = 0 + + packet_time = rx_corrected[refined_packet_start: refined_packet_start + total_samlpes] + all_symbols = np.split(packet_time, total_symbols) + rx_sync_sym = all_symbols[0] + rx_pilot_sym = all_symbols[1] + rx_payload_syms = all_symbols[2:] + + #-------- Pilot CFO Calc -------------- + tx_pilot_ref = np.array(ref_data['pilot_ref_real']).astype(complex) + 1j * np.array(ref_data['pilot_ref_imag']).astype(complex) + tx_pilot_no_cp = waveform.remove_cp(tx_pilot_ref, cp_len=ofdm_conf.CP_LEN) + + + # ------ Channel Estimation Calc -----------t + rx_pilot_sym_no_cp = waveform.remove_cp(rx_pilot_sym, cp_len=ofdm_conf.CP_LEN) + rx_pilot_freq = waveform.time_to_freq(rx_pilot_sym_no_cp) + tx_pilot_freq = waveform.time_to_freq(tx_pilot_no_cp) + Lambda_est = CHEST.channel_estimation_calc(rx_pilot_freq=rx_pilot_freq, tx_pilot_ref=tx_pilot_freq, config=ofdm_conf) + + #----- Payload Extraction --------- + pilots_idx = ofdm_conf._idx(np.array(ofdm_conf.pilot_carriers)) + tx_pilot_vals = preamble.generate_pilot_vals(config=ofdm_conf) + demodulated_data = [] + + for sym_time in rx_payload_syms: + sym_no_cp = waveform.remove_cp(sym_time, cp_len=ofdm_conf.CP_LEN) + sym_freq = waveform.time_to_freq(sym_no_cp) + sym_eq = CHEST.apply_gains(sym_freq, Lambda_est=Lambda_est) + + # cal and apply phase drift + rx_pilots_eq = sym_eq[pilots_idx] + correlation = np.vdot(tx_pilot_vals, rx_pilots_eq) + phase_drift = np.angle(correlation) + phase_correction = np.exp(-1j * phase_drift) + sym_final = sym_eq * phase_correction + + data_only = payload.extract_data(sym_final, config=ofdm_conf) + demodulated_data.extend(data_only) + + demodulated_data = np.array(demodulated_data) + demodulated_data = demodulated_data*np.sqrt(10) + return demodulated_data, ref_data, refined_packet_start + +#TODO: NEEDS TESTS +def extract_packet(rx_data:np.ndarray, start_idx:int, total_symbols:int, ofdm_conf:OFDMConfig)->np.ndarray: + """ + Removes leading and trialing zeros from the signal + + Args: + rx_data: Recieved RX IQ data from .dat file + start_idx: Packet start index, get from unpack_rx_file() + total_symbols: Total number of sync, pilot, and data symbols + ofdm_conf: OFDMConfig data class + + Returns: + Sclies raw rx_data and returns only the actual packet iq data. + """ + sym_len = ofdm_conf.N + ofdm_conf.CP_LEN + total_samples = sym_len * total_symbols + start = int(start_idx) + end = int(start_idx + total_samples) + if (start < 0): start = 0 + if (end > len(rx_data)): end = len(rx_data) + return rx_data[start:end] + +def normalize_rx_signal(rx_data:np.ndarray)->np.ndarray: + """ + Normalizes raw rx IQ data between +-0.9 + + Args: + rx_data: raw recieved IQ data from .dat file. + + Returns: + rx_data normalized between +- 0.9 + + """ + max_val = np.max(np.abs(rx_data)) + if max_val == 0: + raise ValueError("Cannot normalize signal: max amplitude is zero. Check the .dat file for bad capture.") + return rx_data * (0.9) / max_val + \ No newline at end of file diff --git a/src/ofdm/simulation/monte_carlo.py b/src/ofdm/simulation/monte_carlo.py index 0ea0ee13..b3fd30ce 100644 --- a/src/ofdm/simulation/monte_carlo.py +++ b/src/ofdm/simulation/monte_carlo.py @@ -4,7 +4,7 @@ from ofdm.simulation.solver import solve_tdoa -def run_monte_carlo(tx_pos, rx_coords, sigma_ns, n_trials=1000, seed=None): +def run_monte_carlo(tx_pos, rx_coords, sigma_ns, n_trials=1000, seed=None, bounds = None): """ Run monte carlo TDOA localization simluation. Returns dict with estimates, errors, and summary stats @@ -15,7 +15,7 @@ def run_monte_carlo(tx_pos, rx_coords, sigma_ns, n_trials=1000, seed=None): estimates = [] for _ in range(n_trials): noisy_tdoas = add_gausian_nosie(ideal_tdoas, sigma_ns, rng=rng) - est = solve_tdoa(rx_coords, noisy_tdoas) + est = solve_tdoa(rx_coords, noisy_tdoas, bounds=bounds) if est is not None: estimates.append(est) estimates = np.array(estimates) diff --git a/src/ofdm/simulation/solver.py b/src/ofdm/simulation/solver.py index cb85203f..4c7fbf65 100644 --- a/src/ofdm/simulation/solver.py +++ b/src/ofdm/simulation/solver.py @@ -25,7 +25,7 @@ def tdoa_cost_function(guess, rx_coords, delay_diffs_sec): return residuals -def solve_tdoa(rx_coords, delay_diffs_sec, initial_guess=None): +def solve_tdoa(rx_coords, delay_diffs_sec, initial_guess=None, bounds=None): """ Runs LM least-squares TDOA solve for a single set of delay measurements. Returns (x, y) estimate or None if solve failed. @@ -42,6 +42,11 @@ def solve_tdoa(rx_coords, delay_diffs_sec, initial_guess=None): ) if result.success: + x, y = result.x + if bounds is not None: + (x_min, x_max), (y_min, y_max) = bounds + if not (x_min <= x <= x_max and y_min <= y <= y_max): + return None return result.x return None diff --git a/src/ofdm/viz/sim_plotter.py b/src/ofdm/viz/sim_plotter.py index 6825cc7d..0889fb66 100644 --- a/src/ofdm/viz/sim_plotter.py +++ b/src/ofdm/viz/sim_plotter.py @@ -5,6 +5,7 @@ from scipy import constants from ofdm.simulation.geometry import compute_tdoa from ofdm.simulation.monte_carlo import run_monte_carlo +from matplotlib.widgets import Slider C = constants.c @@ -26,7 +27,7 @@ def plot_mc_results( centroid = results["centroid"] ax.scatter(estimates[:, 0], estimates[:, 1], - c='red', marker='o', s=20, alpha=0.3, label='Estimates', zorder=2) + c='blue', marker='o', s=20, alpha=0.3, label='Estimates', zorder=2) # centroid ax.scatter(*centroid, c='purple', marker='X', s=200, edgecolor='black', linewidths=1.5, label='Centroid', zorder=5) @@ -39,7 +40,7 @@ def plot_mc_results( ax.scatter(rx_coords[1:, 0], rx_coords[1:, 1], c='cyan', marker='^', s= 100, label='Roaming RX', zorder=4) stats = ( - f"$\\sigma$ = {sigma_ns} ns\n" + f"$\\sigma$ = {sigma_ns:.3f} ns\n" f"RMSE = {results['rmse']*100:.2f} cm\n" f"Mean err = {results['mean_error']*100:.2f} cm\n" f"P95 err = {results['p95_error']*100:2.2f} cm\n" @@ -65,8 +66,8 @@ def plot_tdoa_hyperbolas(tx_pos, rx_coords, results, ax, sigma_ns=None): ideal_tdoas = compute_tdoa(tx_pos, rx_coords) - x = np.linspace(-0.5, 1.5, 800) - y = np.linspace(-0.5, 1.5, 800) + x = np.linspace(-0.5, 2.0, 800) + y = np.linspace(-0.5, 2.0, 800) X, Y = np.meshgrid(x, y) anchor = rx_coords[0] @@ -93,6 +94,20 @@ def __init__(self, ax, tx_pos, rx_coords, sigma_ns, n_trials=1000): fig.canvas.mpl_connect('motion_notify_event', self._on_motion) fig.canvas.mpl_connect('button_release_event', self._on_release) + ax_sigma = fig.add_axes([0.15, 0.08, 0.7, 0.03]) + ax_trials = fig.add_axes([0.15, 0.03, 0.7, 0.03]) + + self._slider_sigma = Slider(ax_sigma, 'σ (ns)', 0.01, 1.5, valinit=sigma_ns) + self._slider_trials = Slider(ax_trials, 'Trials', 10, 1000, valinit=n_trials, valstep=10) + + self._slider_sigma.on_changed(self._on_slider) + self._slider_trials.on_changed(self._on_slider) + + def _on_slider(self, val): + self.sigma_ns = self._slider_sigma.val + self.n_trials = int(self._slider_trials.val) + self._redraw() + def _hit(self, event, pos): if event.xdata is None: return False @@ -131,3 +146,41 @@ def _redraw(self): self.ax.set_ylim(ylim) self.ax.get_figure().canvas.draw_idle() +def plot_experiment_results( + results: dict, + tx_pos: np.ndarray, + rx_coords: np.ndarray, + ax: Optional[plt.Axes] = None, +) -> plt.Axes: + """ + Plot localization results from a real experiemnt. + """ + if ax is None: + fix, ax = plt.subplots(figsize=(9,9)) + + estimates = results["estimates"] + centroid = results["centroid"] + + ax.scatter(estimates[:, 0], estimates[:, 1], c='red', marker='o', s=20, alpha=0.3, label="Estimates", zorder=2) + ax.scatter(*centroid, c='purple', marker='X', s=200, edgecolor='black', linewidths=1.5, label='Centroid', zorder=5) + ax.scatter(*tx_pos, c='green', marker='s', s=150, label='TX (Ground Truth)', zorder=5) + ax.scatter(*rx_coords[0], c='blue', marker='^', s=150, label='Anchor RX', zorder=4) + ax.scatter(rx_coords[1:, 0], rx_coords[1:, 1], c='cyan', marker='^', s=100, label='Roaming RX', zorder=4) + + stats = ( + f"Converged: {results['n_converged']}/{results['n_trials']}\n" + f"Mean err: {results['mean_error']*100:.2f} cm\n" + f"RMSE: {results['rmse']*100:.2f} cm\n" + f"P95 err: {results['p95_error']*100:.2f} cm\n" + f"Centroid err: {results['centroid_error']*100:.2f} cm" + ) + ax.text(0.02, 0.98, stats, transform=ax.transAxes, fontsize=9, verticalalignment='top', + family='monospace', bbox=dict(boxstyle='round', facecolor='white', alpha=0.8)) + + ax.set_xlabel("X (meters)") + ax.set_ylabel("Y (meters)") + ax.legend(loc='lower right') + ax.grid(True, linestyle='--', alpha=0.6) + ax.set_aspect('equal') + + return ax