Merge pull request #8 from osherlock1/add-simulation

add: Monte Carlo Simulatioins

Author: osherlock1

configs/layout.json.example

@@ -0,0 +1,10 @@
+{
+    "rx_coords" : [
+        [0.0, 0.0],
+        [0.508, 0.137],
+        [0.0, 0.615], 
+	[0.7, 0.1]
+    ],
+    "tx_true" :
+        [0.565, 0.906]
+}

scripts/experiment_scripts/collect_raw_data.py

@@ -11,8 +11,8 @@
 SOURCE_DAT_FILE = "./data_files/rand_ofdm_packet_rx"
 
 # --------- MODIFY --------------
-EXPERIMENT_NAME = "virtual_multilateration_3" # CHOOSE NAME OF EXPERIMENT TO BE RUN
-ROAMING_DEVICES = ["RX2ch1"] # NAME OF DEVICE THAT IS MOVED (WILL ASK FOR POSITIONS EACH RUN)
+EXPERIMENT_NAME = "rj_virtual_multilateration_300" # CHOOSE NAME OF EXPERIMENT TO BE RUN
+ROAMING_DEVICES = ["RX4ch1"] # NAME OF DEVICE THAT IS MOVED (WILL ASK FOR POSITIONS EACH RUN)
 FIXED_DEVICES = ["ANCHORch0", "TX"] # NAME OF DEVICES THAT ARE FIXED (WILL ONLY ASK ONCE PER EXPERIMENT)
 # -------------------------------
 

scripts/simulation/heapmap.py

@@ -0,0 +1,46 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from ofdm.simulation.monte_carlo import run_monte_carlo
+from ofdm.config import loadLayout
+
+
+def main():
+    layout_config_pth = "./configs/layout.json"
+    rx_coords, tx_true = loadLayout(layout_config_pth)
+
+    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, Y = np.meshgrid(x_range, y_range)
+
+    error_heatmap = np.zeros(X.shape)
+
+    for i in range(len(x_range)):
+        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,
+                seed=42,
+            )
+            error_heatmap[i][j] = results['rmse']
+    
+    plt.figure(figsize=(10, 8))
+    v_max = 1  
+    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.title('OFDM Localization Error Heatmap')
+    plt.xlabel('X Position (m)')
+    plt.ylabel('Y Position (m)')
+    plt.legend()
+    plt.show()
+
+
+if __name__ == "__main__":
+    main()

scripts/simulation/monte_carlo.py

@@ -0,0 +1,49 @@
+import argparse
+import numpy as np 
+import matplotlib.pyplot as plt
+from ofdm.viz.sim_plotter import plot_mc_results, plot_tdoa_hyperbolas, DraggableSimulation
+from ofdm.simulation.monte_carlo import run_monte_carlo
+from ofdm.config import loadLayout
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--tx", nargs=2, type=float)
+    parser.add_argument("--sigma-ns", type=float, default=0.05)
+    parser.add_argument("--trials", type=int, default=1000)
+    args = parser.parse_args()
+
+    layout_conf_path = "./configs/layout.json"
+    rx_coords, tx_true = loadLayout(layout_conf_path)
+
+    if args.tx is not None:
+        tx_true = np.array(args.tx)
+
+    results = run_monte_carlo(
+        tx_pos=tx_true,
+        rx_coords=rx_coords,
+        sigma_ns=args.sigma_ns,
+        n_trials=args.trials,
+        seed=42,
+    )
+
+    print(f"Converged:  {results['n_converged']}/{results['n_trials']}")
+    print(f"RMSE:       {results['rmse']*100:.2f} cm")
+    print(f"Mean error: {results['mean_error']*100:.2f} cm")
+    print(f"P95 errors: {results['p95_error']*100:.2f} cm")
+    print(f"Centroid:   X={results['centroid'][0]:.4f} cm, Y={results['centroid'][1]:.4f} cm")
+
+    ax = plot_mc_results(results, tx_true, rx_coords, args.sigma_ns)
+    plot_tdoa_hyperbolas(tx_true, rx_coords, results, ax)
+    plt.tight_layout()
+    sim = DraggableSimulation(
+        ax=ax, 
+        tx_pos=tx_true,
+        rx_coords=rx_coords,
+        sigma_ns=args.sigma_ns,
+        n_trials=args.trials
+    )
+    
+    plt.show()
+
+if __name__ == "__main__":
+    main()

scripts/trilateration.py

@@ -121,7 +121,6 @@ def toa_cost_function(guess, rx_coords, measured_distance):
     Calculates difference between theoretical distances based on guessed (x,y) and the actual measured distnace.
 
     """
-
     x, y = guess
     residuals = np.zeros(len(rx_coords))
 

src/ofdm/config.py

@@ -1,5 +1,7 @@
 from dataclasses import dataclass
 import random
+import json
+import numpy as np
 
 
 @dataclass
@@ -49,8 +51,6 @@ def _load_random_map(self):
         self.pilot_carriers.sort()
         self.data_carriers.sort()
 
-
-
     def _load_default_map(self):
         """
         Define Default OFDM mapping if no Config is provided
@@ -71,3 +71,12 @@ def _idx(self, k: int) -> int:
         Helper to convert from python indexing to freq bin indexing
         """
         return (k + self.N) % self.N
+    
+
+def loadLayout(layout_config_path:str)->np.ndarray:
+    """
+    Loads configs/layout.json.  Returns rx_coords, tx_true as np.ndarrays
+    """ 
+    with open(layout_config_path, "r") as f:
+        coords = json.load(f)
+    return np.array(coords['rx_coords']), np.array(coords['tx_true'])

src/ofdm/simulation/__init__.py

@@ -0,0 +1,15 @@
+import numpy as np
+from scipy import constants
+
+C = constants.c
+
+def compute_toa(tx_pos, rx_coords):
+    distances = np.linalg.norm(rx_coords - tx_pos, axis = 1)
+    return distances / C
+
+def compute_tdoa(tx_pos, rx_coords):
+    toa = compute_toa(tx_pos, rx_coords)
+    return toa[1:] - toa[0] # roaming rx - anchor
+
+def compute_distances(tx_pos, rx_coords):
+    return np.linalg.norm(rx_coords - tx_pos, axis = 1)

src/ofdm/simulation/geometry.py

@@ -0,0 +1,33 @@
+import numpy as np
+from ofdm.simulation.geometry import compute_tdoa
+from ofdm.simulation.noise_model import add_gausian_nosie
+from ofdm.simulation.solver import solve_tdoa
+
+
+def run_monte_carlo(tx_pos, rx_coords, sigma_ns, n_trials=1000, seed=None):
+    """
+    Run monte carlo TDOA localization simluation.
+    Returns dict with estimates, errors, and summary stats
+    """
+    rng = np.random.default_rng(seed)
+    ideal_tdoas = compute_tdoa(tx_pos, rx_coords)
+
+    estimates = []
+    for _ in range(n_trials):
+        noisy_tdoas = add_gausian_nosie(ideal_tdoas, sigma_ns, rng=rng)
+        est = solve_tdoa(rx_coords, noisy_tdoas)
+        if est is not None:
+            estimates.append(est)
+    estimates = np.array(estimates)
+    errors = np.linalg.norm(estimates - tx_pos, axis=1)
+    return {
+        "estimates": estimates,
+        "errors": errors,
+        "rmse": np.sqrt(np.mean(errors**2)),
+        "mean_error": np.mean(errors),
+        "p95_error": np.percentile(errors, 95),
+        "centroid": np.mean(estimates, axis=0),
+        "n_converged": len(estimates),
+        "n_trials": n_trials,
+    }
+

src/ofdm/simulation/monte_carlo.py

@@ -0,0 +1,16 @@
+import numpy as np
+
+def add_gausian_nosie(tdoa_values, sigma_ns, rng=None):
+    """
+    Add gausian noise to time difference of arrival delay values.
+    signa_ns: noise standard deviation in ns
+    rng: optional rng for repoducibility
+    """
+    if rng is None:
+        rng = np.random.default_rng()
+    sigma_sec = sigma_ns * 1e-9
+    noise = rng.normal(0, sigma_sec, size=tdoa_values.shape)
+    return tdoa_values + noise
+
+    
+