Skip to content

Unify Chapter 4 anchor geometry and stabilize AOA comparison outputs. #3

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
Togure wants to merge 1 commit into
base: main
Choose a base branch
from
Open

Unify Chapter 4 anchor geometry and stabilize AOA comparison outputs. #3

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
18 changes: 11 additions & 7 deletions ch4_rf_point_positioning/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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])
Expand Down Expand Up @@ -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)
Expand All @@ -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) ---
Expand Down
27 changes: 16 additions & 11 deletions ch4_rf_point_positioning/example_aoa_positioning.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,16 +38,25 @@
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."""
print("\n" + "=" * 70)
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])
Expand Down Expand Up @@ -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}")
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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(
Expand Down
50 changes: 40 additions & 10 deletions ch4_rf_point_positioning/example_comparison.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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)
Expand All @@ -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)
Expand All @@ -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:
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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
Expand Down
48 changes: 21 additions & 27 deletions ch4_rf_point_positioning/example_tdoa_positioning.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -25,17 +25,23 @@
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."""
print("\n" + "=" * 70)
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])
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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])
Expand Down Expand Up @@ -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}")
Expand Down Expand Up @@ -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(
Expand All @@ -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(
[
Expand Down Expand Up @@ -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}")
Expand Down Expand Up @@ -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

Expand Down Expand Up @@ -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

Expand Down
Loading