diff --git a/ch4_rf_point_positioning/README.md b/ch4_rf_point_positioning/README.md index 1ab185c..b0de3fb 100644 --- a/ch4_rf_point_positioning/README.md +++ b/ch4_rf_point_positioning/README.md @@ -4,6 +4,10 @@ This module implements RF (Radio Frequency) positioning algorithms described in **Chapter 4** of *Principles of Indoor Positioning and Indoor Navigation*. It provides simulation-based examples of various RF positioning techniques including TOA, TDOA, AOA, and RSS-based positioning. +**Anchor-setting convention in this chapter:** +- Inline demos use a shared **asymmetric** anchor layout to reduce symmetry bias. +- Square/circular/linear layouts are retained for explicit geometry-comparison datasets only. + ## Quick Start ```bash @@ -330,7 +334,7 @@ This properly accounts for: from core.rf import AOAPositioner, aoa_angle_vector import numpy as np -anchors = np.array([[0, 0], [10, 0], [10, 10], [0, 10]], dtype=float) +anchors = np.array([[0, 0], [12, 1], [10.5, 11.5], [1.5, 9]], dtype=float) aoa = aoa_angle_vector(anchors, np.array([4.0, 6.0])) positioner = AOAPositioner(anchors) @@ -424,8 +428,8 @@ Where σ symbols are **standard deviations** (not variances): ```python from core.rf import compute_geometry_matrix, compute_dop, position_error_from_dop -# Square anchor layout -anchors = np.array([[0, 0], [10, 0], [10, 10], [0, 10]], dtype=float) +# Default inline anchor layout (asymmetric) +anchors = np.array([[0, 0], [12, 1], [10.5, 11.5], [1.5, 9]], dtype=float) position = np.array([5.0, 5.0]) # Compute geometry matrix and DOP @@ -455,8 +459,8 @@ print(f"Expected horizontal error: {sigma_horizontal:.2f} m") # 0.42 m import numpy as np from core.rf import TOAPositioner -# Define anchor layout (square) -anchors = np.array([[0, 0], [10, 0], [10, 10], [0, 10]], dtype=float) +# Define default Chapter 4 inline layout (asymmetric) +anchors = np.array([[0, 0], [12, 1], [10.5, 11.5], [1.5, 9]], dtype=float) # True position and compute ranges true_pos = np.array([5.0, 5.0]) @@ -568,7 +572,7 @@ print(f"Total fading {total_fading:.1f} dB -> {factor:.2f}x distance") ```python from core.rf import TDOAPositioner -anchors = np.array([[0, 0], [10, 0], [10, 10], [0, 10]], dtype=float) +anchors = np.array([[0, 0], [12, 1], [10.5, 11.5], [1.5, 9]], dtype=float) true_pos = np.array([5.0, 5.0]) # Compute TDOA measurements (relative to anchor 0) @@ -595,7 +599,7 @@ Chapter 4: TOA Positioning Example ====================================================================== --- Setting up test scenario --- - Anchors: 4 anchors in square configuration (10m x 10m) + Anchors: 4 anchors in asymmetric configuration True position: [5.0, 5.0] m --- TOA Positioning (Perfect Measurements) --- diff --git a/ch4_rf_point_positioning/example_aoa_positioning.py b/ch4_rf_point_positioning/example_aoa_positioning.py index c4bb399..8c6a21b 100644 --- a/ch4_rf_point_positioning/example_aoa_positioning.py +++ b/ch4_rf_point_positioning/example_aoa_positioning.py @@ -38,6 +38,15 @@ aoa_tan_azimuth, ) +# Chapter 4 default anchor layouts. +DEFAULT_ANCHORS_2D = np.array( + [[0.0, 0.0], [12.0, 1.0], [10.5, 11.5], [1.5, 9.0]], dtype=float +) +DEFAULT_ANCHORS_3D = np.array( + [[0.0, 0.0, 5.0], [12.0, 1.0, 5.0], [10.5, 11.5, 5.0], [1.5, 9.0, 5.0]], + dtype=float, +) + def demo_aoa_basic(): """Demonstrate basic AOA positioning with I-WLS.""" @@ -45,9 +54,9 @@ def demo_aoa_basic(): print("Demo 1: Basic AOA Positioning (I-WLS)") print("=" * 70) - # Setup anchors (4 anchors at corners) in ENU coordinates + # Setup anchors (4 anchors, asymmetric) in ENU coordinates # E=x, N=y in 2D - anchors = np.array([[0, 0], [10, 0], [10, 10], [0, 10]], dtype=float) + anchors = DEFAULT_ANCHORS_2D.copy() # True position true_position = np.array([4.0, 6.0]) @@ -213,9 +222,7 @@ def demo_minimum_anchors(): anchor_configs = { "2 anchors": np.array([[0, 0], [10, 0]], dtype=float), "3 anchors": np.array([[0, 0], [10, 0], [5, 10]], dtype=float), - "4 anchors": np.array( - [[0, 0], [10, 0], [10, 10], [0, 10]], dtype=float - ), + "4 anchors": DEFAULT_ANCHORS_2D.copy(), } print(f"\nTrue position (E, N): {true_position}") @@ -256,7 +263,7 @@ def visualize_aoa_geometry(): print("=" * 70) # Setup in ENU coordinates (E=x-axis, N=y-axis) - anchors = np.array([[0, 0], [12, 0], [12, 12], [0, 12]], dtype=float) + anchors = DEFAULT_ANCHORS_2D.copy() true_position = np.array([5.0, 7.0]) # Generate AOA using new convention (Eq. 4.64: psi from North) @@ -380,7 +387,7 @@ def demo_closed_form_algorithms(): # === 2D Comparison === print("\n--- 2D Comparison (I-WLS vs PLE) ---") - anchors_2d = np.array([[0, 0], [10, 0], [10, 10], [0, 10]], dtype=float) + anchors_2d = DEFAULT_ANCHORS_2D.copy() true_pos_2d = np.array([4.0, 6.0]) # Generate azimuth angles @@ -405,9 +412,7 @@ def demo_closed_form_algorithms(): # === 3D Comparison === print("\n--- 3D Comparison (I-WLS vs OVE vs PLE) ---") - anchors_3d = np.array( - [[0, 0, 5], [10, 0, 5], [10, 10, 5], [0, 10, 5]], dtype=float - ) + anchors_3d = DEFAULT_ANCHORS_3D.copy() true_pos_3d = np.array([4.0, 6.0, 0.0]) # Generate angles @@ -503,7 +508,7 @@ def demo_geometry_sensitivity(): # Define different anchor geometries geometries = { - "Square (good)": np.array([[0, 0], [10, 0], [10, 10], [0, 10]], dtype=float), + "Asymmetric (good)": DEFAULT_ANCHORS_2D.copy(), "Triangle (good)": np.array([[0, 0], [10, 0], [5, 10]], dtype=float), "Linear (poor)": np.array([[0, 0], [5, 0], [10, 0], [15, 0]], dtype=float), "Near-collinear (very poor)": np.array( diff --git a/ch4_rf_point_positioning/example_comparison.py b/ch4_rf_point_positioning/example_comparison.py index f3cfbcc..b27553f 100644 --- a/ch4_rf_point_positioning/example_comparison.py +++ b/ch4_rf_point_positioning/example_comparison.py @@ -261,13 +261,15 @@ def generate_scenario(seed=42): """Generate a test scenario with anchors and true positions (inline mode).""" np.random.seed(seed) - # Square anchor layout (10m x 10m area) - anchors = np.array([[0, 0], [10, 0], [10, 10], [0, 10]], dtype=float) + # Chapter 4 default: mildly asymmetric anchor geometry. + anchors = np.array( + [[0.0, 0.0], [12.0, 1.0], [10.5, 11.5], [1.5, 9.0]], dtype=float + ) # Generate test positions n_points = 50 - x = np.random.uniform(1, 9, n_points) - y = np.random.uniform(1, 9, n_points) + x = np.random.uniform(2.0, 10.0, n_points) + y = np.random.uniform(2.0, 9.5, n_points) true_positions = np.column_stack([x, y]) return anchors, true_positions @@ -295,7 +297,9 @@ def toa_positioning_test( try: positioner = TOAPositioner(anchors, method="iterative_ls") - est_pos, info = positioner.solve(ranges, initial_guess=np.array([5.0, 5.0])) + est_pos, info = positioner.solve( + ranges, initial_guess=np.mean(anchors, axis=0) + ) if info["converged"]: error = np.linalg.norm(est_pos - true_pos) errors.append(error) @@ -322,7 +326,9 @@ def tdoa_positioning_test(anchors, true_positions, noise_std=0.0): try: positioner = TDOAPositioner(anchors, reference_idx=0) - est_pos, info = positioner.solve(tdoa, initial_guess=np.array([5.0, 5.0])) + est_pos, info = positioner.solve( + tdoa, initial_guess=np.mean(anchors, axis=0) + ) if info["converged"]: error = np.linalg.norm(est_pos - true_pos) errors.append(error) @@ -333,18 +339,40 @@ def tdoa_positioning_test(anchors, true_positions, noise_std=0.0): def aoa_positioning_test(anchors, true_positions, noise_std=0.0): - """Test AOA positioning (inline mode).""" + """Test AOA positioning (inline mode) with stability guards. + + Uses sigma-based weighting when angle noise is known and rejects + implausible far-field estimates to avoid numerical outliers from + near-singular AOA geometry. + """ errors = [] + anchors = np.asarray(anchors, dtype=float) + anchor_min = np.min(anchors, axis=0) + anchor_max = np.max(anchors, axis=0) + anchor_span = np.linalg.norm(anchor_max - anchor_min) + # Allow estimates moderately outside the anchor hull. + max_dist_from_centroid = 3.0 * anchor_span + centroid = np.mean(anchors, axis=0) for true_pos in tqdm(true_positions, desc=" AOA", leave=False, unit="pt"): aoa = np.array([aoa_azimuth(anchor, true_pos) for anchor in anchors]) if noise_std > 0: aoa += np.random.randn(len(aoa)) * noise_std + # Normalize wrapped angles to [-pi, pi] for numerical consistency. + aoa = (aoa + np.pi) % (2.0 * np.pi) - np.pi try: positioner = AOAPositioner(anchors) - est_pos, info = positioner.solve(aoa, initial_guess=np.array([5.0, 5.0])) + solve_kwargs = {"initial_guess": centroid} + if noise_std > 0: + solve_kwargs["sigma_psi"] = noise_std + + est_pos, info = positioner.solve(aoa, **solve_kwargs) if info["converged"]: + # Reject physically implausible solutions produced by + # poor angle conditioning (e.g., nearly parallel bearings). + if np.linalg.norm(est_pos - centroid) > max_dist_from_centroid: + continue error = np.linalg.norm(est_pos - true_pos) errors.append(error) except Exception: @@ -407,7 +435,9 @@ def rss_positioning_test( try: positioner = TOAPositioner(anchors, method="iterative_ls") - est_pos, info = positioner.solve(ranges, initial_guess=np.array([5.0, 5.0])) + est_pos, info = positioner.solve( + ranges, initial_guess=np.mean(anchors, axis=0) + ) if info["converged"]: error = np.linalg.norm(est_pos - true_pos) errors.append(error) @@ -435,7 +465,7 @@ def run_inline_comparison(): print(f"Test scenario created:") print(f" Anchors: {len(anchors)}") print(f" Test points: {len(true_positions)}") - print(f" Area: 10m x 10m") + print(" Area: irregular indoor region (asymmetric anchor layout)") # ---- Independent noise schedules per method ---- toa_noise_levels = [0.0, 0.05, 0.1, 0.2, 0.5] # metres diff --git a/ch4_rf_point_positioning/example_tdoa_positioning.py b/ch4_rf_point_positioning/example_tdoa_positioning.py index a295800..23de3ff 100644 --- a/ch4_rf_point_positioning/example_tdoa_positioning.py +++ b/ch4_rf_point_positioning/example_tdoa_positioning.py @@ -25,6 +25,14 @@ toa_fang_solver, ) +# Chapter 4 default anchor layouts. +DEFAULT_ANCHORS_2D = np.array( + [[0.0, 0.0], [12.0, 1.0], [10.5, 11.5], [1.5, 9.0]], dtype=float +) +DEFAULT_ANCHORS_2D_WITH_EXTRA = np.array( + [[0.0, 0.0], [12.0, 1.0], [10.5, 11.5], [1.5, 9.0], [6.0, 3.5]], dtype=float +) + def demo_tdoa_basic(): """Demonstrate basic TDOA positioning with I-WLS.""" @@ -32,10 +40,8 @@ def demo_tdoa_basic(): print("Demo 1: Basic TDOA Positioning (I-WLS)") print("=" * 70) - # Setup anchors (5 anchors in a larger area) - anchors = np.array( - [[0, 0], [15, 0], [15, 15], [0, 15], [7.5, 7.5]], dtype=float - ) + # Setup anchors (5 anchors, asymmetric) + anchors = DEFAULT_ANCHORS_2D_WITH_EXTRA.copy() # True position true_position = np.array([5.0, 8.0]) @@ -76,9 +82,7 @@ def demo_tdoa_with_noise(): print("=" * 70) # Setup - anchors = np.array( - [[0, 0], [20, 0], [20, 20], [0, 20], [10, 10]], dtype=float - ) + anchors = DEFAULT_ANCHORS_2D_WITH_EXTRA.copy() true_position = np.array([7.0, 12.0]) # Generate noiseless TDOA @@ -162,9 +166,7 @@ def demo_correlated_covariance(): print("=" * 70) # Setup: 5 anchors with heterogeneous noise levels - anchors = np.array( - [[0, 0], [20, 0], [20, 20], [0, 20], [10, 10]], dtype=float - ) + anchors = DEFAULT_ANCHORS_2D_WITH_EXTRA.copy() # True position true_position = np.array([7.0, 12.0]) @@ -300,9 +302,7 @@ def demo_covariance_sensitivity(): print("=" * 70) # Setup: 4 anchors - anchors = np.array( - [[0, 0], [20, 0], [20, 20], [0, 20]], dtype=float - ) + anchors = DEFAULT_ANCHORS_2D.copy() true_position = np.array([8.0, 12.0]) print(f"\nTrue position: {true_position}") @@ -476,10 +476,8 @@ def demo_geometry_effect(): true_position = np.array([5.0, 5.0]) - # Good geometry: anchors surrounding the target - good_anchors = np.array( - [[0, 0], [10, 0], [10, 10], [0, 10]], dtype=float - ) + # Good geometry: anchors surrounding the target (asymmetric quadrilateral) + good_anchors = DEFAULT_ANCHORS_2D.copy() # Poor geometry: anchors on one side poor_anchors = np.array( @@ -492,7 +490,7 @@ def demo_geometry_effect(): print(f"Noise std: {noise_std} m") # Test with good geometry - print("\n--- Good Geometry (surrounding) ---") + print("\n--- Good Geometry (surrounding, asymmetric) ---") dist_ref = np.linalg.norm(true_position - good_anchors[0]) tdoa_good = np.array( [ @@ -571,8 +569,8 @@ def demo_fang_toa_solver(): print("Demo 7: Fang's TOA Closed-Form vs I-WLS (Eqs. 4.43-4.49)") print("=" * 70) - # Setup: 4 anchors in a square - anchors = np.array([[0, 0], [20, 0], [20, 20], [0, 20]], dtype=float) + # Setup: 4 anchors with default asymmetric geometry + anchors = DEFAULT_ANCHORS_2D.copy() true_position = np.array([7.0, 12.0]) print(f"\nTrue position: {true_position}") @@ -669,10 +667,8 @@ def demo_chan_tdoa_solver(): print("Demo 8: Chan's TDOA Closed-Form vs I-WLS (Eqs. 4.50-4.62)") print("=" * 70) - # Setup: 5 anchors for good geometry - anchors = np.array( - [[0, 0], [20, 0], [20, 20], [0, 20], [10, 10]], dtype=float - ) + # Setup: 5 anchors with default asymmetric geometry + anchors = DEFAULT_ANCHORS_2D_WITH_EXTRA.copy() true_position = np.array([8.0, 12.0]) ref_idx = 0 @@ -791,9 +787,7 @@ def demo_closed_form_comparison(): print("=" * 70) # Setup - anchors = np.array( - [[0, 0], [20, 0], [20, 20], [0, 20], [10, 10]], dtype=float - ) + anchors = DEFAULT_ANCHORS_2D_WITH_EXTRA.copy() true_position = np.array([7.5, 11.0]) ref_idx = 0 diff --git a/ch4_rf_point_positioning/example_toa_positioning.py b/ch4_rf_point_positioning/example_toa_positioning.py index 5c7584c..89587b1 100644 --- a/ch4_rf_point_positioning/example_toa_positioning.py +++ b/ch4_rf_point_positioning/example_toa_positioning.py @@ -30,6 +30,12 @@ toa_solve_with_clock_bias, ) +# Chapter 4 default anchor layout: +# use a mildly irregular quadrilateral to avoid overly symmetric geometry. +DEFAULT_ANCHORS_2D = np.array( + [[0.0, 0.0], [12.0, 1.0], [10.5, 11.5], [1.5, 9.0]], dtype=float +) + def example_toa_perfect(): """Example 1: TOA positioning with perfect measurements.""" @@ -37,8 +43,8 @@ def example_toa_perfect(): print("Example 1: TOA Positioning with Perfect Measurements") print("=" * 70) - # Square anchor layout - anchors = np.array([[0, 0], [10, 0], [10, 10], [0, 10]], dtype=float) + # Chapter 4 default: asymmetric anchor layout. + anchors = DEFAULT_ANCHORS_2D.copy() true_pos = np.array([5.0, 5.0]) print(f"\nAnchor positions:\n{anchors}") @@ -73,7 +79,7 @@ def example_toa_with_noise(): np.random.seed(42) - anchors = np.array([[0, 0], [10, 0], [10, 10], [0, 10]], dtype=float) + anchors = DEFAULT_ANCHORS_2D.copy() true_pos = np.array([3.0, 7.0]) print(f"\nTrue position: {true_pos}") @@ -118,7 +124,7 @@ def example_toa_with_clock_bias(): print("Example 3: Joint Position and Clock Bias Estimation") print("=" * 70) - anchors = np.array([[0, 0], [10, 0], [10, 10], [0, 10]], dtype=float) + anchors = DEFAULT_ANCHORS_2D.copy() true_pos = np.array([5.0, 5.0]) # Define clock bias in SECONDS (timing domain) @@ -200,7 +206,7 @@ def example_rss_positioning(): # RSS-based positioning example print("\nRSS-Based Positioning:") - anchors = np.array([[0, 0], [10, 0], [10, 10], [0, 10]], dtype=float) + anchors = DEFAULT_ANCHORS_2D.copy() true_pos = np.array([5.0, 5.0]) rss_measurements = [] @@ -457,12 +463,15 @@ def example_wls_vs_ls(): np.random.seed(42) - # Deliberately asymmetric layout: three close anchors on the left, - # one distant anchor on the right. + # Deliberately asymmetric layout: + # - three anchors clustered on the lower-left + # - one anchor far to the upper-right + # - user position shifted toward the far anchor side + # This avoids near-symmetry and emphasizes WLS benefit. anchors = np.array( - [[0, 0], [0, 8], [2, 4], [15, 5]], dtype=float, + [[0, 0], [1, 9], [4, 2], [19, 15]], dtype=float, ) - true_pos = np.array([6.0, 4.0]) + true_pos = np.array([11.5, 6.5]) # Per-anchor noise std (far anchor has much higher noise) sigma_per_anchor = np.array([0.1, 0.1, 0.1, 0.8]) @@ -481,7 +490,7 @@ def example_wls_vs_ls(): ) noisy_ranges = true_ranges + np.random.randn(len(anchors)) * sigma_per_anchor - init = np.array([5.0, 5.0]) + init = np.array([8.0, 8.0]) pos_ls, info_ls = TOAPositioner(anchors, method="iterative_ls").solve( noisy_ranges, initial_guess=init, @@ -540,9 +549,11 @@ def main(): fig = plot_toa_positioning(anchors1, true_pos1, est_pos1, info1["history"]) plt.savefig( - "ch4_rf_point_positioning/toa_positioning_example.png", dpi=150, bbox_inches="tight" + "ch4_rf_point_positioning/figs/toa_positioning_example.png", + dpi=150, + bbox_inches="tight", ) - print("\nFigure saved: toa_positioning_example.png") + print("\nFigure saved: ch4_rf_point_positioning/figs/toa_positioning_example.png") plt.show() diff --git a/ch4_rf_point_positioning/figs/ch4_aoa_geometry.png b/ch4_rf_point_positioning/figs/ch4_aoa_geometry.png index 798152f..04f93b5 100644 Binary files a/ch4_rf_point_positioning/figs/ch4_aoa_geometry.png and b/ch4_rf_point_positioning/figs/ch4_aoa_geometry.png differ diff --git a/ch4_rf_point_positioning/figs/ch4_rf_comparison.png b/ch4_rf_point_positioning/figs/ch4_rf_comparison.png index 42b1512..fc35a78 100644 Binary files a/ch4_rf_point_positioning/figs/ch4_rf_comparison.png and b/ch4_rf_point_positioning/figs/ch4_rf_comparison.png differ diff --git a/ch4_rf_point_positioning/figs/closed_form_comparison.png b/ch4_rf_point_positioning/figs/closed_form_comparison.png index 5e05892..7847e11 100644 Binary files a/ch4_rf_point_positioning/figs/closed_form_comparison.png and b/ch4_rf_point_positioning/figs/closed_form_comparison.png differ diff --git a/ch4_rf_point_positioning/figs/tdoa_covariance_matrix.png b/ch4_rf_point_positioning/figs/tdoa_covariance_matrix.png index 8ab1666..9977fef 100644 Binary files a/ch4_rf_point_positioning/figs/tdoa_covariance_matrix.png and b/ch4_rf_point_positioning/figs/tdoa_covariance_matrix.png differ diff --git a/ch4_rf_point_positioning/figs/toa_positioning_example.png b/ch4_rf_point_positioning/figs/toa_positioning_example.png index 48ab878..d8c1804 100644 Binary files a/ch4_rf_point_positioning/figs/toa_positioning_example.png and b/ch4_rf_point_positioning/figs/toa_positioning_example.png differ