├── docs ├── ch8_comparison_tool_summary.md ├── CH2_QUICK_REFERENCE.md ├── guides │ └── README.md ├── engineering │ ├── ch3_bugfix_summary.md │ └── README.md └── README.md ├── tests ├── __init__.py ├── core │ ├── __init__.py │ ├── coords │ │ └── __init__.py │ ├── rf │ │ └── __init__.py │ ├── estimators │ │ └── __init__.py │ ├── fusion │ │ └── __init__.py │ ├── fingerprinting │ │ └── __init__.py │ ├── sensors │ │ └── __init__.py │ └── slam │ │ └── __init__.py └── docs │ ├── __init__.py │ └── test_ch6_examples.py ├── tools └── __init__.py ├── notebooks ├── .gitkeep └── README.md ├── data └── sim │ ├── ch7_slam_2d_square │ ├── loop_closures.txt │ ├── scans.npz │ ├── config.json │ ├── landmarks.txt │ ├── ground_truth_poses.txt │ └── odometry_poses.txt │ ├── ch7_slam_2d_high_drift │ ├── loop_closures.txt │ ├── scans.npz │ ├── config.json │ ├── landmarks.txt │ ├── ground_truth_poses.txt │ └── odometry_poses.txt │ ├── ch6_foot_zupt_walk │ ├── imu.npz │ ├── truth.npz │ └── config.json │ ├── ch6_strapdown_basic │ ├── imu.npz │ ├── truth.npz │ └── config.json │ ├── ch8_fusion_2d_imu_uwb │ ├── imu.npz │ ├── truth.npz │ ├── uwb_anchors.npy │ ├── uwb_ranges.npz │ └── config.json │ ├── ch4_rf_2d_nlos │ ├── beacons.txt │ ├── config.json │ ├── gdop_tdoa.txt │ ├── gdop_toa.txt │ ├── gdop_aoa.txt │ ├── ground_truth_positions.txt │ ├── tdoa_diffs.txt │ ├── aoa_angles.txt │ └── toa_ranges.txt │ ├── ch4_rf_2d_square │ ├── beacons.txt │ ├── config.json │ ├── gdop_tdoa.txt │ ├── gdop_toa.txt │ ├── gdop_aoa.txt │ ├── ground_truth_positions.txt │ ├── tdoa_diffs.txt │ ├── aoa_angles.txt │ └── toa_ranges.txt │ ├── ch8_fusion_2d_imu_uwb_nlos │ ├── imu.npz │ ├── truth.npz │ ├── uwb_anchors.npy │ ├── uwb_ranges.npz │ └── config.json │ ├── ch4_rf_2d_linear │ ├── beacons.txt │ ├── config.json │ ├── gdop_toa.txt │ ├── gdop_tdoa.txt │ ├── gdop_aoa.txt │ ├── ground_truth_positions.txt │ ├── tdoa_diffs.txt │ ├── toa_ranges.txt │ └── aoa_angles.txt │ ├── ch4_rf_2d_optimal │ ├── beacons.txt │ ├── config.json │ ├── gdop_toa.txt │ ├── gdop_tdoa.txt │ ├── gdop_aoa.txt │ ├── ground_truth_positions.txt │ ├── tdoa_diffs.txt │ ├── aoa_angles.txt │ └── toa_ranges.txt │ ├── ch2_coords_san_francisco │ ├── reference_llh.txt │ ├── enu_coordinates.txt │ ├── euler_angles.txt │ ├── config.json │ ├── llh_coordinates.txt │ ├── quaternions.txt │ ├── ecef_coordinates.txt │ └── rotation_matrices.txt │ ├── ch5_wifi_fingerprint_dense │ ├── features.npy │ ├── floor_ids.npy │ ├── locations.npy │ └── metadata.json │ ├── ch5_wifi_fingerprint_grid │ ├── features.npy │ ├── floor_ids.npy │ ├── locations.npy │ └── metadata.json │ ├── ch5_wifi_fingerprint_sparse │ ├── features.npy │ ├── floor_ids.npy │ ├── locations.npy │ └── metadata.json │ ├── ch8_fusion_2d_imu_uwb_timeoffset │ ├── imu.npz │ ├── truth.npz │ ├── uwb_ranges.npz │ ├── uwb_anchors.npy │ └── config.json │ ├── ch3_estimator_nonlinear │ ├── beacons.txt │ ├── config.json │ └── time.txt │ ├── ch3_estimator_high_nonlinear │ ├── beacons.txt │ ├── config.json │ └── time.txt │ ├── ch6_wheel_odom_square │ └── config.json │ ├── ch6_env_sensors_heading_altitude │ └── config.json │ └── ch6_pdr_corridor_walk │ ├── config.json │ └── step_times.txt ├── ch6_dead_reckoning ├── figs │ ├── pdr_error.pdf │ ├── pdr_heading.pdf │ ├── pdr_trajectory.pdf │ ├── wheel_odom_error.pdf │ ├── zupt_error_time.pdf │ ├── zupt_trajectory.pdf │ ├── comparison_error_cdf.pdf │ ├── comparison_error_time.pdf │ ├── wheel_odom_trajectory.pdf │ ├── comparison_trajectories.pdf │ ├── environment_mag_heading.pdf │ ├── imu_strapdown_attitude.pdf │ ├── zupt_detector_timeline.pdf │ ├── allan_gyroscope_consumer.pdf │ ├── environment_baro_altitude.pdf │ ├── imu_strapdown_error_time.pdf │ ├── imu_strapdown_trajectory.pdf │ └── allan_accelerometer_consumer.pdf └── __init__.py ├── ch7_slam ├── figs │ ├── pose_graph_slam_results.png │ └── bundle_adjustment_results.png └── __init__.py ├── ch3_estimators ├── figs │ ├── ch3_ekf_range_bearing.png │ ├── ch3_kalman_1d_tracking.png │ ├── ch3_estimator_comparison.png │ └── ch3_least_squares_examples.png └── __init__.py ├── ch4_rf_point_positioning ├── figs │ ├── ch4_rf_comparison.png │ └── toa_positioning_example.png └── __init__.py ├── ch5_fingerprinting ├── figs │ ├── comparison_all_methods.png │ └── deterministic_positioning.png └── __init__.py ├── ch2_coords └── __init__.py ├── .flake8 ├── core ├── __init__.py ├── utils │ ├── __init__.py │ └── angles.py ├── eval │ ├── __init__.py │ └── metrics.py ├── models │ └── __init__.py ├── estimators │ ├── __init__.py │ └── base.py ├── coords │ ├── __init__.py │ └── frames.py ├── fusion │ └── __init__.py ├── rf │ └── __init__.py ├── fingerprinting │ └── __init__.py └── slam │ └── __init__.py ├── ch8_sensor_fusion ├── __init__.py └── figs │ └── lc_tc_comparison.json ├── .editorconfig ├── .pylintrc ├── .gitignore └── pyproject.toml /docs/ch8_comparison_tool_summary.md: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /tests/__init__.py: -------------------------------------------------------------------------------- 1 | """Unit tests for IPIN book examples.""" 2 | 3 | -------------------------------------------------------------------------------- /tests/core/__init__.py: -------------------------------------------------------------------------------- 1 | """Unit tests for core modules.""" 2 | 3 | -------------------------------------------------------------------------------- /tools/__init__.py: -------------------------------------------------------------------------------- 1 | # Tools for repository maintenance and CI/CD 2 | 3 | 4 | -------------------------------------------------------------------------------- /tests/core/coords/__init__.py: -------------------------------------------------------------------------------- 1 | """Unit tests for coordinate transformations.""" 2 | -------------------------------------------------------------------------------- /tests/core/rf/__init__.py: -------------------------------------------------------------------------------- 1 | """Unit tests for core.rf module.""" 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /tests/core/estimators/__init__.py: -------------------------------------------------------------------------------- 1 | """Unit tests for state estimators.""" 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /notebooks/.gitkeep: -------------------------------------------------------------------------------- 1 | # This file ensures the notebooks directory is tracked by git 2 | 3 | 4 | -------------------------------------------------------------------------------- /tests/core/fusion/__init__.py: -------------------------------------------------------------------------------- 1 | # Tests for core.fusion module (gating, tuning, types) 2 | 3 | 4 | -------------------------------------------------------------------------------- /data/sim/ch7_slam_2d_square/loop_closures.txt: -------------------------------------------------------------------------------- 1 | # pose_i, pose_j (index pairs for loop closures) 2 | 0 40 3 | -------------------------------------------------------------------------------- /data/sim/ch7_slam_2d_high_drift/loop_closures.txt: -------------------------------------------------------------------------------- 1 | # pose_i, pose_j (index pairs for loop closures) 2 | 0 40 3 | -------------------------------------------------------------------------------- /tests/core/fingerprinting/__init__.py: -------------------------------------------------------------------------------- 1 | """Unit tests for core.fingerprinting module (Chapter 5).""" 2 | 3 | 4 | -------------------------------------------------------------------------------- /tests/core/sensors/__init__.py: -------------------------------------------------------------------------------- 1 | # Tests for core.sensors module (IMU, PDR, wheel odometry, calibration, etc.) 2 | 3 | 4 | -------------------------------------------------------------------------------- /data/sim/ch6_foot_zupt_walk/imu.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/data/sim/ch6_foot_zupt_walk/imu.npz -------------------------------------------------------------------------------- /data/sim/ch6_strapdown_basic/imu.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/data/sim/ch6_strapdown_basic/imu.npz -------------------------------------------------------------------------------- /ch6_dead_reckoning/figs/pdr_error.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/ch6_dead_reckoning/figs/pdr_error.pdf -------------------------------------------------------------------------------- /ch6_dead_reckoning/figs/pdr_heading.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/ch6_dead_reckoning/figs/pdr_heading.pdf -------------------------------------------------------------------------------- /data/sim/ch6_foot_zupt_walk/truth.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/data/sim/ch6_foot_zupt_walk/truth.npz -------------------------------------------------------------------------------- /data/sim/ch6_strapdown_basic/truth.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/data/sim/ch6_strapdown_basic/truth.npz -------------------------------------------------------------------------------- /data/sim/ch7_slam_2d_square/scans.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/data/sim/ch7_slam_2d_square/scans.npz -------------------------------------------------------------------------------- /data/sim/ch8_fusion_2d_imu_uwb/imu.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/data/sim/ch8_fusion_2d_imu_uwb/imu.npz -------------------------------------------------------------------------------- /ch7_slam/figs/pose_graph_slam_results.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/ch7_slam/figs/pose_graph_slam_results.png -------------------------------------------------------------------------------- /data/sim/ch7_slam_2d_high_drift/scans.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/data/sim/ch7_slam_2d_high_drift/scans.npz -------------------------------------------------------------------------------- /data/sim/ch8_fusion_2d_imu_uwb/truth.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/data/sim/ch8_fusion_2d_imu_uwb/truth.npz -------------------------------------------------------------------------------- /ch6_dead_reckoning/figs/pdr_trajectory.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/ch6_dead_reckoning/figs/pdr_trajectory.pdf -------------------------------------------------------------------------------- /ch6_dead_reckoning/figs/wheel_odom_error.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/ch6_dead_reckoning/figs/wheel_odom_error.pdf -------------------------------------------------------------------------------- /ch6_dead_reckoning/figs/zupt_error_time.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/ch6_dead_reckoning/figs/zupt_error_time.pdf -------------------------------------------------------------------------------- /ch6_dead_reckoning/figs/zupt_trajectory.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/ch6_dead_reckoning/figs/zupt_trajectory.pdf -------------------------------------------------------------------------------- /ch7_slam/figs/bundle_adjustment_results.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/ch7_slam/figs/bundle_adjustment_results.png -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_nlos/beacons.txt: -------------------------------------------------------------------------------- 1 | # x (m), y (m) 2 | 0.000000 0.000000 3 | 20.000000 0.000000 4 | 20.000000 20.000000 5 | 0.000000 20.000000 6 | -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_square/beacons.txt: -------------------------------------------------------------------------------- 1 | # x (m), y (m) 2 | 0.000000 0.000000 3 | 20.000000 0.000000 4 | 20.000000 20.000000 5 | 0.000000 20.000000 6 | -------------------------------------------------------------------------------- /data/sim/ch8_fusion_2d_imu_uwb_nlos/imu.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/data/sim/ch8_fusion_2d_imu_uwb_nlos/imu.npz -------------------------------------------------------------------------------- /ch3_estimators/figs/ch3_ekf_range_bearing.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/ch3_estimators/figs/ch3_ekf_range_bearing.png -------------------------------------------------------------------------------- /ch3_estimators/figs/ch3_kalman_1d_tracking.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/ch3_estimators/figs/ch3_kalman_1d_tracking.png -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_linear/beacons.txt: -------------------------------------------------------------------------------- 1 | # x (m), y (m) 2 | 4.000000 10.000000 3 | 8.000000 10.000000 4 | 12.000000 10.000000 5 | 16.000000 10.000000 6 | -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_optimal/beacons.txt: -------------------------------------------------------------------------------- 1 | # x (m), y (m) 2 | 20.000000 10.000000 3 | 10.000000 20.000000 4 | 0.000000 10.000000 5 | 10.000000 0.000000 6 | -------------------------------------------------------------------------------- /data/sim/ch8_fusion_2d_imu_uwb/uwb_anchors.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/data/sim/ch8_fusion_2d_imu_uwb/uwb_anchors.npy -------------------------------------------------------------------------------- /data/sim/ch8_fusion_2d_imu_uwb/uwb_ranges.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/data/sim/ch8_fusion_2d_imu_uwb/uwb_ranges.npz -------------------------------------------------------------------------------- /data/sim/ch8_fusion_2d_imu_uwb_nlos/truth.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/data/sim/ch8_fusion_2d_imu_uwb_nlos/truth.npz -------------------------------------------------------------------------------- /ch3_estimators/figs/ch3_estimator_comparison.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/ch3_estimators/figs/ch3_estimator_comparison.png -------------------------------------------------------------------------------- /ch6_dead_reckoning/figs/comparison_error_cdf.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/ch6_dead_reckoning/figs/comparison_error_cdf.pdf -------------------------------------------------------------------------------- /ch6_dead_reckoning/figs/comparison_error_time.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/ch6_dead_reckoning/figs/comparison_error_time.pdf -------------------------------------------------------------------------------- /ch6_dead_reckoning/figs/wheel_odom_trajectory.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/ch6_dead_reckoning/figs/wheel_odom_trajectory.pdf -------------------------------------------------------------------------------- /data/sim/ch2_coords_san_francisco/reference_llh.txt: -------------------------------------------------------------------------------- 1 | # Reference point: latitude (rad), longitude (rad), height (m) 2 | 0.6592963796 -2.1366215983 15.000 3 | -------------------------------------------------------------------------------- /data/sim/ch5_wifi_fingerprint_dense/features.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/data/sim/ch5_wifi_fingerprint_dense/features.npy -------------------------------------------------------------------------------- /data/sim/ch5_wifi_fingerprint_dense/floor_ids.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/data/sim/ch5_wifi_fingerprint_dense/floor_ids.npy -------------------------------------------------------------------------------- /data/sim/ch5_wifi_fingerprint_dense/locations.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/data/sim/ch5_wifi_fingerprint_dense/locations.npy -------------------------------------------------------------------------------- /data/sim/ch5_wifi_fingerprint_grid/features.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/data/sim/ch5_wifi_fingerprint_grid/features.npy -------------------------------------------------------------------------------- /data/sim/ch5_wifi_fingerprint_grid/floor_ids.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/data/sim/ch5_wifi_fingerprint_grid/floor_ids.npy -------------------------------------------------------------------------------- /data/sim/ch5_wifi_fingerprint_grid/locations.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/data/sim/ch5_wifi_fingerprint_grid/locations.npy -------------------------------------------------------------------------------- /data/sim/ch5_wifi_fingerprint_sparse/features.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/data/sim/ch5_wifi_fingerprint_sparse/features.npy -------------------------------------------------------------------------------- /data/sim/ch8_fusion_2d_imu_uwb_timeoffset/imu.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/data/sim/ch8_fusion_2d_imu_uwb_timeoffset/imu.npz -------------------------------------------------------------------------------- /ch3_estimators/figs/ch3_least_squares_examples.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/ch3_estimators/figs/ch3_least_squares_examples.png -------------------------------------------------------------------------------- /ch4_rf_point_positioning/figs/ch4_rf_comparison.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/ch4_rf_point_positioning/figs/ch4_rf_comparison.png -------------------------------------------------------------------------------- /ch5_fingerprinting/figs/comparison_all_methods.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/ch5_fingerprinting/figs/comparison_all_methods.png -------------------------------------------------------------------------------- /ch6_dead_reckoning/figs/comparison_trajectories.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/ch6_dead_reckoning/figs/comparison_trajectories.pdf -------------------------------------------------------------------------------- /ch6_dead_reckoning/figs/environment_mag_heading.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/ch6_dead_reckoning/figs/environment_mag_heading.pdf -------------------------------------------------------------------------------- /ch6_dead_reckoning/figs/imu_strapdown_attitude.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/ch6_dead_reckoning/figs/imu_strapdown_attitude.pdf -------------------------------------------------------------------------------- /ch6_dead_reckoning/figs/zupt_detector_timeline.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/ch6_dead_reckoning/figs/zupt_detector_timeline.pdf -------------------------------------------------------------------------------- /data/sim/ch3_estimator_nonlinear/beacons.txt: -------------------------------------------------------------------------------- 1 | # x (m), y (m) 2 | -15.000000 -15.000000 3 | 15.000000 -15.000000 4 | 15.000000 15.000000 5 | -15.000000 15.000000 6 | -------------------------------------------------------------------------------- /data/sim/ch5_wifi_fingerprint_sparse/floor_ids.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/data/sim/ch5_wifi_fingerprint_sparse/floor_ids.npy -------------------------------------------------------------------------------- /data/sim/ch5_wifi_fingerprint_sparse/locations.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/data/sim/ch5_wifi_fingerprint_sparse/locations.npy -------------------------------------------------------------------------------- /data/sim/ch8_fusion_2d_imu_uwb_nlos/uwb_anchors.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/data/sim/ch8_fusion_2d_imu_uwb_nlos/uwb_anchors.npy -------------------------------------------------------------------------------- /data/sim/ch8_fusion_2d_imu_uwb_nlos/uwb_ranges.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/data/sim/ch8_fusion_2d_imu_uwb_nlos/uwb_ranges.npz -------------------------------------------------------------------------------- /data/sim/ch8_fusion_2d_imu_uwb_timeoffset/truth.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/data/sim/ch8_fusion_2d_imu_uwb_timeoffset/truth.npz -------------------------------------------------------------------------------- /ch5_fingerprinting/figs/deterministic_positioning.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/ch5_fingerprinting/figs/deterministic_positioning.png -------------------------------------------------------------------------------- /ch6_dead_reckoning/figs/allan_gyroscope_consumer.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/ch6_dead_reckoning/figs/allan_gyroscope_consumer.pdf -------------------------------------------------------------------------------- /ch6_dead_reckoning/figs/environment_baro_altitude.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/ch6_dead_reckoning/figs/environment_baro_altitude.pdf -------------------------------------------------------------------------------- /ch6_dead_reckoning/figs/imu_strapdown_error_time.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/ch6_dead_reckoning/figs/imu_strapdown_error_time.pdf -------------------------------------------------------------------------------- /ch6_dead_reckoning/figs/imu_strapdown_trajectory.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/ch6_dead_reckoning/figs/imu_strapdown_trajectory.pdf -------------------------------------------------------------------------------- /data/sim/ch3_estimator_high_nonlinear/beacons.txt: -------------------------------------------------------------------------------- 1 | # x (m), y (m) 2 | -15.000000 -15.000000 3 | 15.000000 -15.000000 4 | 15.000000 15.000000 5 | -15.000000 15.000000 6 | -------------------------------------------------------------------------------- /ch6_dead_reckoning/figs/allan_accelerometer_consumer.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/ch6_dead_reckoning/figs/allan_accelerometer_consumer.pdf -------------------------------------------------------------------------------- /data/sim/ch8_fusion_2d_imu_uwb_timeoffset/uwb_ranges.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/data/sim/ch8_fusion_2d_imu_uwb_timeoffset/uwb_ranges.npz -------------------------------------------------------------------------------- /ch4_rf_point_positioning/figs/toa_positioning_example.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/ch4_rf_point_positioning/figs/toa_positioning_example.png -------------------------------------------------------------------------------- /data/sim/ch8_fusion_2d_imu_uwb_timeoffset/uwb_anchors.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/IPNL-POLYU/IPIN-Examples/HEAD/data/sim/ch8_fusion_2d_imu_uwb_timeoffset/uwb_anchors.npy -------------------------------------------------------------------------------- /tests/core/slam/__init__.py: -------------------------------------------------------------------------------- 1 | """Unit tests for core.slam module (Chapter 7). 2 | 3 | Tests for SLAM geometry, SE(2) operations, and related algorithms. 4 | 5 | Author: Navigation Engineer 6 | Date: 2024 7 | """ 8 | 9 | 10 | -------------------------------------------------------------------------------- /ch3_estimators/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | Chapter 3: State Estimation Examples 3 | 4 | This package contains example scripts demonstrating various state estimation 5 | algorithms for indoor positioning as described in Chapter 3 of the IPIN book. 6 | """ 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /ch2_coords/__init__.py: -------------------------------------------------------------------------------- 1 | """Chapter 2: Coordinate Systems and Transformations. 2 | 3 | This module contains examples demonstrating coordinate transformations 4 | and rotation representations for indoor positioning applications. 5 | 6 | Reference: Chapter 2 of the IPIN book 7 | """ 8 | 9 | -------------------------------------------------------------------------------- /.flake8: -------------------------------------------------------------------------------- 1 | [flake8] 2 | max-line-length = 88 3 | extend-ignore = E203, E266, E501, W503 4 | exclude = 5 | .git, 6 | __pycache__, 7 | .venv, 8 | venv, 9 | build, 10 | dist, 11 | *.egg-info, 12 | .mypy_cache, 13 | .pytest_cache 14 | max-complexity = 10 15 | per-file-ignores = 16 | __init__.py:F401 17 | 18 | -------------------------------------------------------------------------------- /core/__init__.py: -------------------------------------------------------------------------------- 1 | """Core modules for indoor positioning and navigation. 2 | 3 | This package contains reusable core components for IPIN applications: 4 | - coords: Coordinate systems and transformations 5 | - estimators: State estimation algorithms (LS, KF, EKF, etc.) 6 | - rf: Radio frequency positioning models 7 | - sensors: Sensor models (IMU, odometry, etc.) 8 | """ 9 | 10 | __version__ = "0.1.0" 11 | -------------------------------------------------------------------------------- /tests/docs/__init__.py: -------------------------------------------------------------------------------- 1 | """Documentation validation tests. 2 | 3 | This package contains tests that validate code examples in dataset README files 4 | and other documentation. These are integration/smoke tests that ensure 5 | documentation examples are accurate and runnable. 6 | 7 | Structure: 8 | test_ch6_examples.py - Tests Ch6 dataset README code blocks 9 | (future) test_ch8_examples.py - Tests Ch8 dataset README code blocks 10 | """ 11 | 12 | 13 | -------------------------------------------------------------------------------- /ch4_rf_point_positioning/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | Chapter 4: RF Point Positioning Examples. 3 | 4 | This module provides example scripts and demonstrations for Chapter 4 5 | of "Principles of Indoor Positioning and Indoor Navigation". 6 | 7 | Examples: 8 | - TOA/RSS positioning with I-WLS 9 | - TDOA positioning with closed-form algorithms (Fang, Chan) 10 | - AOA positioning 11 | - RF challenges and limitations 12 | """ 13 | 14 | __version__ = "1.0.0" 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /ch8_sensor_fusion/__init__.py: -------------------------------------------------------------------------------- 1 | """Chapter 8: Sensor Fusion Examples. 2 | 3 | This package demonstrates practical sensor fusion concepts from Chapter 8: 4 | - Tightly coupled (TC) vs loosely coupled (LC) fusion 5 | - Innovation monitoring and chi-square gating 6 | - Robust measurement down-weighting 7 | - Temporal calibration and synchronization 8 | - Observability analysis 9 | 10 | Author: Navigation Engineer 11 | References: Chapter 8 - Sensor Fusion 12 | """ 13 | 14 | __all__ = [] 15 | 16 | 17 | -------------------------------------------------------------------------------- /ch5_fingerprinting/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | Chapter 5: Fingerprinting-based Indoor Positioning 3 | 4 | This module implements fingerprinting algorithms from Chapter 5 of 5 | *Principles of Indoor Positioning and Indoor Navigation*. 6 | 7 | Provides examples comparing: 8 | - Deterministic methods (NN, k-NN) 9 | - Probabilistic methods (Bayesian, MAP, Posterior Mean) 10 | - Pattern recognition (Linear Regression) 11 | 12 | Author: Navigation Engineer 13 | Date: December 2024 14 | """ 15 | 16 | __version__ = "0.1.0" 17 | __all__ = [] 18 | 19 | 20 | -------------------------------------------------------------------------------- /.editorconfig: -------------------------------------------------------------------------------- 1 | # EditorConfig helps maintain consistent coding styles 2 | # https://editorconfig.org 3 | 4 | root = true 5 | 6 | [*] 7 | charset = utf-8 8 | end_of_line = lf 9 | insert_final_newline = true 10 | trim_trailing_whitespace = true 11 | 12 | [*.py] 13 | indent_style = space 14 | indent_size = 4 15 | max_line_length = 88 16 | 17 | [*.{yml,yaml}] 18 | indent_style = space 19 | indent_size = 2 20 | 21 | [*.md] 22 | trim_trailing_whitespace = false 23 | max_line_length = off 24 | 25 | [*.{json,toml}] 26 | indent_style = space 27 | indent_size = 2 28 | 29 | -------------------------------------------------------------------------------- /data/sim/ch2_coords_san_francisco/enu_coordinates.txt: -------------------------------------------------------------------------------- 1 | # East (m), North (m), Up (m) relative to reference 2 | 742.359 784.807 14.908 3 | -418.487 -175.078 5.984 4 | 1353.865 1027.347 -0.226 5 | 1130.796 565.457 11.875 6 | 800.926 -1162.476 8.844 7 | -878.274 1362.513 11.794 8 | -95.723 748.059 -0.045 9 | -1312.187 819.562 2.812 10 | -994.619 -1065.242 11.833 11 | 526.578 -142.107 11.977 12 | 704.127 -370.081 5.950 13 | 1344.665 1222.619 11.741 14 | -501.016 412.131 14.967 15 | -372.601 924.589 5.922 16 | -87.579 -162.095 14.997 17 | -893.367 -781.300 2.890 18 | -1064.567 156.432 2.909 19 | -69.899 -1249.485 11.877 20 | -785.498 938.568 8.882 21 | 488.473 377.179 -0.030 22 | -------------------------------------------------------------------------------- /data/sim/ch2_coords_san_francisco/euler_angles.txt: -------------------------------------------------------------------------------- 1 | # roll (rad), pitch (rad), yaw (rad) 2 | 0.286886 0.270269 2.746707 3 | -0.064006 -0.152340 5.231871 4 | 0.375523 0.492914 4.399895 5 | 0.206683 0.411675 1.962657 6 | -0.424976 0.291523 5.229243 7 | 0.498071 -0.319774 5.056484 8 | 0.273465 -0.034850 2.434598 9 | 0.299566 -0.477728 1.811619 10 | -0.389438 -0.362027 4.288246 11 | -0.051956 0.191688 0.878091 12 | -0.135300 0.256314 1.256060 13 | 0.446907 0.489575 0.046259 14 | 0.150655 -0.182395 4.944392 15 | 0.337995 -0.135654 4.177381 16 | -0.059257 -0.031881 4.430685 17 | -0.285635 -0.325185 4.905465 18 | 0.057161 -0.387545 2.883453 19 | -0.456770 -0.025442 3.573506 20 | 0.343095 -0.285980 0.878370 21 | 0.137879 0.177829 0.719614 22 | -------------------------------------------------------------------------------- /data/sim/ch6_strapdown_basic/config.json: -------------------------------------------------------------------------------- 1 | { 2 | "dataset_info": { 3 | "description": "IMU strapdown integration dataset for Ch6", 4 | "seed": 42, 5 | "duration_sec": 60.0, 6 | "num_samples": 6000 7 | }, 8 | "trajectory": { 9 | "type": "circular", 10 | "radius_m": 10.0, 11 | "speed_m_s": 1.0 12 | }, 13 | "imu": { 14 | "rate_hz": 100.0, 15 | "dt_sec": 0.01, 16 | "accel_noise_std_m_s2": 0.1, 17 | "gyro_noise_std_rad_s": 0.01, 18 | "accel_bias_m_s2": [ 19 | 0.0, 20 | 0.0 21 | ], 22 | "gyro_bias_rad_s": 0.0 23 | }, 24 | "coordinate_frame": { 25 | "description": "ENU (East-North-Up)", 26 | "origin": "Circle center at (0, 0)", 27 | "units": "meters" 28 | } 29 | } -------------------------------------------------------------------------------- /data/sim/ch7_slam_2d_square/config.json: -------------------------------------------------------------------------------- 1 | { 2 | "dataset": "ch7_slam_2d", 3 | "preset": "baseline", 4 | "trajectory": { 5 | "type": "square", 6 | "size_m": 20.0, 7 | "n_poses_per_side": 10, 8 | "total_poses": 41 9 | }, 10 | "landmarks": { 11 | "count": 50 12 | }, 13 | "sensor": { 14 | "max_range_m": 15.0, 15 | "scan_noise_std_m": 0.05 16 | }, 17 | "odometry": { 18 | "translation_noise_std_m": 0.1, 19 | "rotation_noise_std_rad": 0.02, 20 | "final_drift_m": 0.5456714857632495 21 | }, 22 | "loop_closures": { 23 | "count": 1, 24 | "min_index_diff": 15, 25 | "max_distance_m": 2.0 26 | }, 27 | "equations": [ 28 | "7.10-7.11 (ICP)", 29 | "Section 7.3 (Pose graph)" 30 | ], 31 | "seed": 42 32 | } -------------------------------------------------------------------------------- /data/sim/ch7_slam_2d_high_drift/config.json: -------------------------------------------------------------------------------- 1 | { 2 | "dataset": "ch7_slam_2d", 3 | "preset": "high_drift", 4 | "trajectory": { 5 | "type": "square", 6 | "size_m": 20.0, 7 | "n_poses_per_side": 10, 8 | "total_poses": 41 9 | }, 10 | "landmarks": { 11 | "count": 50 12 | }, 13 | "sensor": { 14 | "max_range_m": 15.0, 15 | "scan_noise_std_m": 0.05 16 | }, 17 | "odometry": { 18 | "translation_noise_std_m": 0.3, 19 | "rotation_noise_std_rad": 0.05, 20 | "final_drift_m": 1.1239129439247115 21 | }, 22 | "loop_closures": { 23 | "count": 1, 24 | "min_index_diff": 15, 25 | "max_distance_m": 2.0 26 | }, 27 | "equations": [ 28 | "7.10-7.11 (ICP)", 29 | "Section 7.3 (Pose graph)" 30 | ], 31 | "seed": 42 32 | } -------------------------------------------------------------------------------- /data/sim/ch2_coords_san_francisco/config.json: -------------------------------------------------------------------------------- 1 | { 2 | "dataset": "ch2_coordinate_transforms", 3 | "preset": "san_francisco", 4 | "reference_point": { 5 | "latitude_deg": 37.7749, 6 | "longitude_deg": -122.4194, 7 | "height_m": 0.0, 8 | "location": "san_francisco" 9 | }, 10 | "building": { 11 | "size_m": 50.0, 12 | "num_points": 20 13 | }, 14 | "accuracy": { 15 | "llh_roundtrip_lat_arcsec": 4.579998741307464e-11, 16 | "llh_roundtrip_lon_arcsec": 0.0, 17 | "llh_roundtrip_height_m": 9.313225746154785e-10, 18 | "rotation_roundtrip_deg": 360.00000000000006 19 | }, 20 | "equations": [ 21 | "2.1 (LLH->ECEF)", 22 | "2.2 (ECEF->LLH)", 23 | "2.3 (ECEF->ENU)", 24 | "2.5-2.10 (Rotations)" 25 | ], 26 | "seed": 42 27 | } -------------------------------------------------------------------------------- /data/sim/ch2_coords_san_francisco/llh_coordinates.txt: -------------------------------------------------------------------------------- 1 | # latitude (rad), longitude (rad), height (m) 2 | 0.6594197832 -2.1364745180 15.000 3 | 0.6592688474 -2.1367045018 6.000 4 | 0.6594579102 -2.1363533546 0.000 5 | 0.6593852841 -2.1363975645 12.000 6 | 0.6591135766 -2.1364629517 9.000 7 | 0.6595106239 -2.1367956191 12.000 8 | 0.6594140101 -2.1366405635 0.000 9 | 0.6594252374 -2.1368815777 3.000 10 | 0.6591288632 -2.1368186135 12.000 11 | 0.6592740309 -2.1365172815 12.000 12 | 0.6592381805 -2.1364821120 6.000 13 | 0.6594886161 -2.1363551715 12.000 14 | 0.6593611837 -2.1367208578 15.000 15 | 0.6594417677 -2.1366954214 6.000 16 | 0.6592708905 -2.1366389480 15.000 17 | 0.6591735142 -2.1367985639 3.000 18 | 0.6593209674 -2.1368325005 3.000 19 | 0.6590999009 -2.1366354437 12.000 20 | 0.6594439612 -2.1367772286 9.000 21 | 0.6593556879 -2.1365248239 0.000 22 | -------------------------------------------------------------------------------- /data/sim/ch2_coords_san_francisco/quaternions.txt: -------------------------------------------------------------------------------- 1 | # qw, qx, qy, qz (unit quaternion) 2 | 0.211263 -0.102963 0.165053 0.957875 3 | -0.860823 0.065762 0.049780 0.502176 4 | -0.523828 -0.300312 0.005317 0.797113 5 | 0.558806 -0.112851 0.196955 0.797627 6 | -0.851340 0.108945 -0.227654 0.459918 7 | -0.804976 -0.110159 0.266231 0.518651 8 | 0.340670 0.063374 0.121884 0.930093 9 | 0.565033 0.273565 -0.030265 0.777807 10 | -0.494220 0.251623 -0.064090 0.829653 11 | 0.899643 -0.064068 0.075602 0.425232 12 | 0.795639 -0.129172 0.063791 0.588392 13 | 0.947057 0.209470 0.241234 -0.031810 14 | -0.782976 -0.002420 0.117719 0.610807 15 | -0.496781 -0.025025 0.178876 0.848873 16 | -0.600118 0.030533 -0.014105 0.799204 17 | -0.739388 0.210278 0.034411 0.638674 18 | 0.120794 0.194492 0.003034 0.973433 19 | -0.205888 0.060613 -0.218473 0.951948 20 | 0.872368 0.212633 -0.055242 0.436703 21 | 0.932207 0.033028 0.107072 0.344144 22 | -------------------------------------------------------------------------------- /data/sim/ch2_coords_san_francisco/ecef_coordinates.txt: -------------------------------------------------------------------------------- 1 | # X (m), Y (m), Z (m) in ECEF frame 2 | -2705296.772 -4261061.608 3886354.952 3 | -2706588.143 -4260929.657 3885590.770 4 | -2704694.508 -4261253.930 3886537.391 5 | -2705039.624 -4261381.254 3886179.715 6 | -2705884.254 -4262095.883 3884812.059 7 | -2706473.785 -4259891.961 3886809.677 8 | -2706009.968 -4260621.327 3886316.746 9 | -2707014.571 -4259934.098 3886375.014 10 | -2707369.292 -4261084.985 3884890.746 11 | -2705782.081 -4261423.268 3885620.502 12 | -2705704.517 -4261632.317 3885436.615 13 | -2704643.218 -4261156.010 3886699.069 14 | -2706468.775 -4260587.766 3886060.416 15 | -2706188.249 -4260385.588 3886459.934 16 | -2706308.364 -4261106.362 3885606.553 17 | -2707186.784 -4260986.475 3885109.702 18 | -2707023.357 -4260409.813 3885850.920 19 | -2706649.217 -4261676.038 3884745.143 20 | -2706533.459 -4260158.976 3886472.797 21 | -2705638.625 -4261126.310 3886023.603 22 | -------------------------------------------------------------------------------- /.pylintrc: -------------------------------------------------------------------------------- 1 | [MASTER] 2 | ignore-patterns=test_.*\.py 3 | jobs=0 4 | suggestion-mode=yes 5 | 6 | [MESSAGES CONTROL] 7 | disable= 8 | missing-module-docstring, 9 | missing-class-docstring, 10 | missing-function-docstring, 11 | too-few-public-methods, 12 | too-many-arguments, 13 | too-many-instance-attributes, 14 | too-many-locals, 15 | too-many-branches, 16 | too-many-statements 17 | 18 | [FORMAT] 19 | max-line-length=88 20 | indent-string=' ' 21 | indent-after-paren=4 22 | expected-line-ending-format=LF 23 | 24 | [BASIC] 25 | good-names=i,j,k,ex,Run,_,x,y,z,dx,dy,dz 26 | bad-names=foo,bar,baz,toto,tutu,tata 27 | 28 | [DESIGN] 29 | max-args=7 30 | max-attributes=12 31 | max-locals=20 32 | max-branches=15 33 | max-statements=60 34 | max-parents=7 35 | max-public-methods=25 36 | max-returns=8 37 | max-bool-expr=5 38 | 39 | [TYPECHECK] 40 | ignored-modules= 41 | numpy, 42 | scipy, 43 | matplotlib 44 | 45 | -------------------------------------------------------------------------------- /core/utils/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | Utility functions for navigation algorithms. 3 | 4 | This module provides common utility functions used across the codebase, 5 | including angle operations, singularity handling, and observability checks. 6 | """ 7 | 8 | from .angles import wrap_angle, wrap_angle_array, angle_diff 9 | from .geometry import normalize_jacobian_singularities, check_anchor_geometry, compute_gdop_2d 10 | from .observability import ( 11 | check_observability, 12 | compute_observability_matrix, 13 | check_range_only_observability_2d, 14 | estimate_observability_time_constant 15 | ) 16 | 17 | __all__ = [ 18 | 'wrap_angle', 19 | 'wrap_angle_array', 20 | 'angle_diff', 21 | 'normalize_jacobian_singularities', 22 | 'check_anchor_geometry', 23 | 'compute_gdop_2d', 24 | 'check_observability', 25 | 'compute_observability_matrix', 26 | 'check_range_only_observability_2d', 27 | 'estimate_observability_time_constant', 28 | ] 29 | 30 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Python 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | *.so 6 | .Python 7 | build/ 8 | develop-eggs/ 9 | dist/ 10 | downloads/ 11 | eggs/ 12 | .eggs/ 13 | lib/ 14 | lib64/ 15 | parts/ 16 | sdist/ 17 | var/ 18 | wheels/ 19 | pip-wheel-metadata/ 20 | share/python-wheels/ 21 | *.egg-info/ 22 | .installed.cfg 23 | *.egg 24 | MANIFEST 25 | 26 | # Virtual environments 27 | .venv/ 28 | venv/ 29 | ENV/ 30 | env/ 31 | 32 | # IDEs 33 | .vscode/ 34 | .idea/ 35 | *.swp 36 | *.swo 37 | *~ 38 | .DS_Store 39 | 40 | # Testing 41 | .pytest_cache/ 42 | .coverage 43 | htmlcov/ 44 | .tox/ 45 | .hypothesis/ 46 | 47 | # Type checking 48 | .mypy_cache/ 49 | .dmypy.json 50 | dmypy.json 51 | 52 | # Jupyter Notebook 53 | .ipynb_checkpoints 54 | *.ipynb_checkpoints 55 | 56 | # Documentation 57 | docs/_build/ 58 | docs/.doctrees/ 59 | 60 | # Data files (optional - uncomment if you want to ignore data) 61 | # data/ 62 | # *.csv 63 | # *.h5 64 | # *.hdf5 65 | 66 | # OS 67 | .DS_Store 68 | Thumbs.db 69 | 70 | -------------------------------------------------------------------------------- /data/sim/ch3_estimator_nonlinear/config.json: -------------------------------------------------------------------------------- 1 | { 2 | "dataset": "ch3_estimator_comparison", 3 | "preset": "nonlinear", 4 | "trajectory": { 5 | "type": "circular", 6 | "duration_s": 30.0, 7 | "dt_s": 0.1, 8 | "num_samples": 300 9 | }, 10 | "beacons": { 11 | "count": 4, 12 | "positions": [ 13 | [ 14 | -15.0, 15 | -15.0 16 | ], 17 | [ 18 | 15.0, 19 | -15.0 20 | ], 21 | [ 22 | 15.0, 23 | 15.0 24 | ], 25 | [ 26 | -15.0, 27 | 15.0 28 | ] 29 | ] 30 | }, 31 | "measurements": { 32 | "range_noise_std_m": 0.5, 33 | "bearing_noise_std_deg": 5.0, 34 | "outlier_rate": 0.0 35 | }, 36 | "statistics": { 37 | "range_rmse_m": 0.5057720858534224, 38 | "range_bias_m": -0.011525898403638839 39 | }, 40 | "equations": [ 41 | "3.11-3.19 (KF)", 42 | "3.21 (EKF)", 43 | "3.24-3.30 (UKF)", 44 | "3.32-3.34 (PF)" 45 | ], 46 | "seed": 42 47 | } -------------------------------------------------------------------------------- /data/sim/ch3_estimator_high_nonlinear/config.json: -------------------------------------------------------------------------------- 1 | { 2 | "dataset": "ch3_estimator_comparison", 3 | "preset": "high_nonlinearity", 4 | "trajectory": { 5 | "type": "figure8", 6 | "duration_s": 30.0, 7 | "dt_s": 0.1, 8 | "num_samples": 300 9 | }, 10 | "beacons": { 11 | "count": 4, 12 | "positions": [ 13 | [ 14 | -15.0, 15 | -15.0 16 | ], 17 | [ 18 | 15.0, 19 | -15.0 20 | ], 21 | [ 22 | 15.0, 23 | 15.0 24 | ], 25 | [ 26 | -15.0, 27 | 15.0 28 | ] 29 | ] 30 | }, 31 | "measurements": { 32 | "range_noise_std_m": 0.5, 33 | "bearing_noise_std_deg": 5.0, 34 | "outlier_rate": 0.0 35 | }, 36 | "statistics": { 37 | "range_rmse_m": 0.5057720858534224, 38 | "range_bias_m": -0.011525898403638856 39 | }, 40 | "equations": [ 41 | "3.11-3.19 (KF)", 42 | "3.21 (EKF)", 43 | "3.24-3.30 (UKF)", 44 | "3.32-3.34 (PF)" 45 | ], 46 | "seed": 42 47 | } -------------------------------------------------------------------------------- /data/sim/ch6_wheel_odom_square/config.json: -------------------------------------------------------------------------------- 1 | { 2 | "dataset": "ch6_wheel_odometry", 3 | "preset": "baseline", 4 | "trajectory": { 5 | "shape": "square", 6 | "side_length_m": 20.0, 7 | "speed_m_s": 5.0, 8 | "num_laps": 2, 9 | "duration_s": 73.88, 10 | "total_distance_m": 327.51000000000124 11 | }, 12 | "dt_s": 0.01, 13 | "sample_rate_hz": 100.0, 14 | "num_samples": 7389, 15 | "encoder": { 16 | "noise_std_m_s": 0.03, 17 | "bias_m_s": 0.005 18 | }, 19 | "gyro": { 20 | "noise_std_rad_s": 0.0005, 21 | "bias_rad_s": 0.0002 22 | }, 23 | "lever_arm_m": [ 24 | 1.5, 25 | 0.0, 26 | -0.3 27 | ], 28 | "slip": { 29 | "enabled": false, 30 | "magnitude": 0.3, 31 | "num_events": 0 32 | }, 33 | "performance": { 34 | "final_error_m": 0.8047426444277428, 35 | "mean_error_m": 1.7497487032347776, 36 | "max_error_m": 2.82576108706386, 37 | "drift_rate_percent": 0.24571544210184107 38 | }, 39 | "equations": [ 40 | "6.11", 41 | "6.12", 42 | "6.14", 43 | "6.15" 44 | ], 45 | "seed": 42 46 | } -------------------------------------------------------------------------------- /ch7_slam/__init__.py: -------------------------------------------------------------------------------- 1 | """Chapter 7: SLAM (Simultaneous Localization and Mapping) Examples. 2 | 3 | This package provides reference implementations and examples for SLAM algorithms 4 | from Chapter 7 of Principles of Indoor Positioning and Indoor Navigation. 5 | 6 | Examples: 7 | - example_pose_graph_slam.py: Complete 2D pose graph SLAM with ICP/NDT 8 | - (Future: example_loop_closure.py, example_landmark_slam.py) 9 | 10 | Key Concepts Demonstrated: 11 | - Scan matching: ICP and NDT alignment 12 | - Odometry integration: Building trajectory from dead-reckoning 13 | - Loop closure detection: Recognizing previously visited places 14 | - Pose graph optimization: Correcting drift with factor graphs 15 | - Visualization: Trajectory plots and error analysis 16 | 17 | Dependencies: 18 | - core.slam: SE(2) operations, ICP, NDT, factors 19 | - core.estimators: Factor graph optimization 20 | - matplotlib: Visualization 21 | - numpy: Numerical operations 22 | 23 | Author: Navigation Engineer 24 | Date: 2024 25 | """ 26 | 27 | __version__ = "0.1.0" 28 | 29 | __all__ = [] 30 | 31 | 32 | -------------------------------------------------------------------------------- /data/sim/ch6_foot_zupt_walk/config.json: -------------------------------------------------------------------------------- 1 | { 2 | "dataset_info": { 3 | "description": "Foot-mounted IMU with ZUPT for Ch6", 4 | "seed": 42, 5 | "duration_sec": 11.99, 6 | "num_samples": 1200, 7 | "total_distance_m": 14.0 8 | }, 9 | "trajectory": { 10 | "type": "walking_linear", 11 | "num_steps": 20, 12 | "step_length_m": 0.7, 13 | "step_duration_sec": 0.6, 14 | "stance_duration_sec": 0.2, 15 | "swing_duration_sec": 0.39999999999999997, 16 | "stance_ratio": 0.335 17 | }, 18 | "imu": { 19 | "rate_hz": 100.0, 20 | "dt_sec": 0.01, 21 | "accel_noise_std_m_s2": 0.1, 22 | "gyro_noise_std_rad_s": 0.01, 23 | "accel_bias_m_s2": [ 24 | 0.0, 25 | 0.0 26 | ], 27 | "gyro_bias_rad_s": 0.0 28 | }, 29 | "zupt": { 30 | "stance_threshold_description": "Use is_stance from truth.npz for ideal ZUPT", 31 | "detection_note": "In practice, detect stance from IMU statistics" 32 | }, 33 | "coordinate_frame": { 34 | "description": "ENU (East-North-Up)", 35 | "origin": "Starting position at (0, 0)", 36 | "units": "meters" 37 | } 38 | } -------------------------------------------------------------------------------- /core/eval/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | Evaluation and Visualization Module. 3 | 4 | This module provides evaluation metrics and visualization utilities 5 | for indoor positioning algorithms. 6 | 7 | Modules: 8 | metrics: Error metrics (RMSE, CDF, NEES, NIS) 9 | plots: Visualization functions for trajectories, errors, and DOP 10 | """ 11 | 12 | from .metrics import ( 13 | compute_error_stats, 14 | compute_nees, 15 | compute_nis, 16 | compute_position_errors, 17 | compute_rmse, 18 | ) 19 | from .plots import ( 20 | plot_dop_map, 21 | plot_error_cdf, 22 | plot_error_hist, 23 | plot_position_error_time, 24 | plot_rf_geometry, 25 | plot_trajectory_2d, 26 | save_figure, 27 | ) 28 | 29 | __all__ = [ 30 | # Metrics 31 | "compute_position_errors", 32 | "compute_rmse", 33 | "compute_error_stats", 34 | "compute_nees", 35 | "compute_nis", 36 | # Plots 37 | "plot_trajectory_2d", 38 | "plot_position_error_time", 39 | "plot_error_hist", 40 | "plot_error_cdf", 41 | "plot_rf_geometry", 42 | "plot_dop_map", 43 | "save_figure", 44 | ] 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /data/sim/ch6_env_sensors_heading_altitude/config.json: -------------------------------------------------------------------------------- 1 | { 2 | "dataset": "ch6_env_sensors", 3 | "preset": "baseline", 4 | "trajectory": { 5 | "type": "building_walk", 6 | "duration_s": 180.0, 7 | "num_floors": 3, 8 | "floor_height_m": 3.5, 9 | "max_altitude_m": 7.0 10 | }, 11 | "dt_s": 0.1, 12 | "sample_rate_hz": 10.0, 13 | "num_samples": 1800, 14 | "sensors": { 15 | "magnetometer": { 16 | "noise_std_uT": 1.5, 17 | "disturbances_enabled": false, 18 | "num_disturbance_events": 0 19 | }, 20 | "barometer": { 21 | "noise_std_Pa": 8.0, 22 | "weather_drift_Pa": 30.0 23 | } 24 | }, 25 | "performance": { 26 | "magnetometer_heading": { 27 | "mean_error_deg": 65.69172546776898, 28 | "max_error_deg": 179.29925429570406 29 | }, 30 | "barometric_altitude": { 31 | "mean_error_m": 1.566487262667991, 32 | "max_error_m": 2.8453562195842728, 33 | "floor_detection_accuracy_percent": 50.0 34 | } 35 | }, 36 | "equations": [ 37 | "6.51", 38 | "6.52", 39 | "6.53", 40 | "6.54", 41 | "6.55" 42 | ], 43 | "seed": 42 44 | } -------------------------------------------------------------------------------- /core/models/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | Common motion and measurement models for state estimation. 3 | 4 | This module provides reusable motion models (process models) and measurement models 5 | used across multiple estimators and examples. 6 | 7 | Benefits: 8 | - Code reuse and consistency 9 | - Tested implementations 10 | - Clear documentation 11 | - Easy to extend 12 | """ 13 | 14 | from .motion_models import ( 15 | ConstantVelocity2D, 16 | ConstantVelocity1D, 17 | ConstantAcceleration2D, 18 | create_process_noise_continuous_white_acceleration 19 | ) 20 | 21 | from .measurement_models import ( 22 | RangeMeasurement2D, 23 | RangeBearingMeasurement2D, 24 | PositionMeasurement2D, 25 | validate_measurement_inputs 26 | ) 27 | 28 | __all__ = [ 29 | # Motion models 30 | 'ConstantVelocity2D', 31 | 'ConstantVelocity1D', 32 | 'ConstantAcceleration2D', 33 | 'create_process_noise_continuous_white_acceleration', 34 | 35 | # Measurement models 36 | 'RangeMeasurement2D', 37 | 'RangeBearingMeasurement2D', 38 | 'PositionMeasurement2D', 39 | 'validate_measurement_inputs', 40 | ] 41 | 42 | 43 | -------------------------------------------------------------------------------- /data/sim/ch8_fusion_2d_imu_uwb/config.json: -------------------------------------------------------------------------------- 1 | { 2 | "dataset_info": { 3 | "description": "2D IMU + UWB fusion dataset for Chapter 8", 4 | "seed": 42, 5 | "duration_sec": 60.0, 6 | "imu_samples": 6000, 7 | "uwb_samples": 600 8 | }, 9 | "trajectory": { 10 | "type": "rectangular_walk", 11 | "width_m": 20.0, 12 | "height_m": 15.0, 13 | "speed_m_s": 1.0 14 | }, 15 | "imu": { 16 | "rate_hz": 100.0, 17 | "dt_sec": 0.01, 18 | "accel_noise_std_m_s2": 0.1, 19 | "gyro_noise_std_rad_s": 0.01, 20 | "accel_bias_m_s2": [ 21 | 0.0, 22 | 0.0 23 | ], 24 | "gyro_bias_rad_s": 0.0 25 | }, 26 | "uwb": { 27 | "rate_hz": 10.0, 28 | "n_anchors": 4, 29 | "range_noise_std_m": 0.05, 30 | "nlos_anchors": [], 31 | "nlos_bias_m": 0.5, 32 | "dropout_rate": 0.05 33 | }, 34 | "temporal_calibration": { 35 | "time_offset_sec": 0.0, 36 | "clock_drift": 0.0, 37 | "note": "Use TimeSyncModel to apply these during fusion" 38 | }, 39 | "coordinate_frame": { 40 | "description": "ENU (East-North-Up)", 41 | "origin": "Bottom-left corner (0, 0)", 42 | "units": "meters" 43 | } 44 | } -------------------------------------------------------------------------------- /data/sim/ch8_fusion_2d_imu_uwb_timeoffset/config.json: -------------------------------------------------------------------------------- 1 | { 2 | "dataset_info": { 3 | "description": "2D IMU + UWB fusion dataset for Chapter 8", 4 | "seed": 42, 5 | "duration_sec": 60.0, 6 | "imu_samples": 6000, 7 | "uwb_samples": 600 8 | }, 9 | "trajectory": { 10 | "type": "rectangular_walk", 11 | "width_m": 20.0, 12 | "height_m": 15.0, 13 | "speed_m_s": 1.0 14 | }, 15 | "imu": { 16 | "rate_hz": 100.0, 17 | "dt_sec": 0.01, 18 | "accel_noise_std_m_s2": 0.1, 19 | "gyro_noise_std_rad_s": 0.01, 20 | "accel_bias_m_s2": [ 21 | 0.0, 22 | 0.0 23 | ], 24 | "gyro_bias_rad_s": 0.0 25 | }, 26 | "uwb": { 27 | "rate_hz": 10.0, 28 | "n_anchors": 4, 29 | "range_noise_std_m": 0.05, 30 | "nlos_anchors": [], 31 | "nlos_bias_m": 0.5, 32 | "dropout_rate": 0.05 33 | }, 34 | "temporal_calibration": { 35 | "time_offset_sec": -0.05, 36 | "clock_drift": 0.0001, 37 | "note": "Use TimeSyncModel to apply these during fusion" 38 | }, 39 | "coordinate_frame": { 40 | "description": "ENU (East-North-Up)", 41 | "origin": "Bottom-left corner (0, 0)", 42 | "units": "meters" 43 | } 44 | } -------------------------------------------------------------------------------- /data/sim/ch8_fusion_2d_imu_uwb_nlos/config.json: -------------------------------------------------------------------------------- 1 | { 2 | "dataset_info": { 3 | "description": "2D IMU + UWB fusion dataset for Chapter 8", 4 | "seed": 42, 5 | "duration_sec": 60.0, 6 | "imu_samples": 6000, 7 | "uwb_samples": 600 8 | }, 9 | "trajectory": { 10 | "type": "rectangular_walk", 11 | "width_m": 20.0, 12 | "height_m": 15.0, 13 | "speed_m_s": 1.0 14 | }, 15 | "imu": { 16 | "rate_hz": 100.0, 17 | "dt_sec": 0.01, 18 | "accel_noise_std_m_s2": 0.1, 19 | "gyro_noise_std_rad_s": 0.01, 20 | "accel_bias_m_s2": [ 21 | 0.0, 22 | 0.0 23 | ], 24 | "gyro_bias_rad_s": 0.0 25 | }, 26 | "uwb": { 27 | "rate_hz": 10.0, 28 | "n_anchors": 4, 29 | "range_noise_std_m": 0.05, 30 | "nlos_anchors": [ 31 | 1, 32 | 2 33 | ], 34 | "nlos_bias_m": 0.8, 35 | "dropout_rate": 0.05 36 | }, 37 | "temporal_calibration": { 38 | "time_offset_sec": 0.0, 39 | "clock_drift": 0.0, 40 | "note": "Use TimeSyncModel to apply these during fusion" 41 | }, 42 | "coordinate_frame": { 43 | "description": "ENU (East-North-Up)", 44 | "origin": "Bottom-left corner (0, 0)", 45 | "units": "meters" 46 | } 47 | } -------------------------------------------------------------------------------- /ch6_dead_reckoning/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | Chapter 6: Dead Reckoning and Sensor Fusion for Indoor Navigation 3 | 4 | This module implements dead reckoning and sensor fusion algorithms from 5 | Chapter 6 of *Principles of Indoor Positioning and Indoor Navigation*. 6 | 7 | Provides examples demonstrating: 8 | - IMU strapdown integration (quaternion, velocity, position) 9 | - Wheel odometry (vehicle dead reckoning with lever arm) 10 | - Drift correction constraints (ZUPT, ZARU, NHC) 11 | - Pedestrian dead reckoning (step-and-heading) 12 | - Environmental sensors (magnetometer, barometer) 13 | - Allan variance IMU calibration 14 | 15 | Examples: 16 | - example_imu_strapdown.py: Pure IMU drift demonstration 17 | - example_zupt.py: Zero-velocity update drift correction 18 | - example_pdr.py: Pedestrian dead reckoning 19 | - example_wheel_odometry.py: Vehicle wheel odometry 20 | - example_environment.py: Magnetometer and barometer 21 | - example_allan_variance.py: IMU noise characterization 22 | - example_comparison.py: Compare all methods 23 | 24 | Author: Navigation Engineer 25 | Date: December 2024 26 | """ 27 | 28 | __version__ = "1.0.0" 29 | __all__ = [] 30 | 31 | 32 | -------------------------------------------------------------------------------- /data/sim/ch6_pdr_corridor_walk/config.json: -------------------------------------------------------------------------------- 1 | { 2 | "dataset": "ch6_pdr", 3 | "preset": "baseline", 4 | "trajectory": { 5 | "type": "corridor_walk", 6 | "num_legs": 4, 7 | "leg_length_m": 30.0, 8 | "total_distance_m": 123.87948075307025, 9 | "duration_s": 47.99 10 | }, 11 | "pedestrian": { 12 | "height_m": 1.75, 13 | "step_freq_hz": 2.0, 14 | "num_steps": 90 15 | }, 16 | "dt_s": 0.01, 17 | "sample_rate_hz": 100.0, 18 | "num_samples": 4800, 19 | "sensors": { 20 | "accel_noise_std_m_s2": 0.15, 21 | "gyro_noise_std_rad_s": 0.005, 22 | "gyro_bias_rad_s": 0.002, 23 | "mag_noise_std": 0.05 24 | }, 25 | "performance": { 26 | "gyro_heading": { 27 | "steps_detected": 90, 28 | "final_error_m": 1.9144521596750737, 29 | "mean_error_m": 1.0556632517327817 30 | }, 31 | "mag_heading": { 32 | "steps_detected": 90, 33 | "final_error_m": 0.9773079814169204, 34 | "mean_error_m": 32.71967700256364 35 | }, 36 | "improvement_factor": 1.958903637417821 37 | }, 38 | "equations": [ 39 | "6.46", 40 | "6.47", 41 | "6.48", 42 | "6.49", 43 | "6.50", 44 | "6.51", 45 | "6.52", 46 | "6.53" 47 | ], 48 | "seed": 42 49 | } -------------------------------------------------------------------------------- /data/sim/ch7_slam_2d_square/landmarks.txt: -------------------------------------------------------------------------------- 1 | # x (m), y (m) 2 | 18.218681 8.166353 3 | 20.757938 15.921041 4 | -2.174680 24.268671 5 | 17.834191 18.581929 6 | -1.156591 8.511578 7 | 6.123941 22.802950 8 | 14.315954 19.682848 9 | 8.302426 1.817162 10 | 11.637544 -3.085482 11 | 19.828935 13.949932 12 | 17.742632 5.635779 13 | 24.120941 21.793634 14 | 18.351505 0.839161 15 | 9.001630 -3.685887 16 | -0.371315 15.491469 17 | 17.342865 24.025292 18 | 4.774761 6.113791 19 | 9.086674 0.684141 20 | -1.102355 9.271148 21 | 1.807280 15.094420 22 | 8.114558 19.980346 23 | 16.007953 4.370999 24 | 19.967794 19.142931 25 | 6.624351 3.649843 26 | 15.474865 -0.807425 27 | 0.997246 -4.779132 28 | 18.607731 14.945526 29 | 16.154961 18.421871 30 | 8.767473 12.062236 31 | -0.806090 -1.564098 32 | 15.052089 9.132886 33 | 11.957083 17.949966 34 | 14.041550 11.607382 35 | 11.776215 4.118503 36 | -4.075465 8.101522 37 | 1.437540 7.255859 38 | 20.602092 2.018185 39 | -3.250918 3.441517 40 | 3.807813 14.857495 41 | 11.710965 18.516946 42 | 14.929406 7.191606 43 | 19.420612 0.009188 44 | -4.318638 -2.298564 45 | 16.670781 8.856317 46 | -0.161847 10.031343 47 | -0.430637 15.889611 48 | 8.384688 6.430637 49 | 4.045363 13.908478 50 | 5.854378 -2.370502 51 | -1.459823 23.856930 52 | -------------------------------------------------------------------------------- /data/sim/ch7_slam_2d_high_drift/landmarks.txt: -------------------------------------------------------------------------------- 1 | # x (m), y (m) 2 | 18.218681 8.166353 3 | 20.757938 15.921041 4 | -2.174680 24.268671 5 | 17.834191 18.581929 6 | -1.156591 8.511578 7 | 6.123941 22.802950 8 | 14.315954 19.682848 9 | 8.302426 1.817162 10 | 11.637544 -3.085482 11 | 19.828935 13.949932 12 | 17.742632 5.635779 13 | 24.120941 21.793634 14 | 18.351505 0.839161 15 | 9.001630 -3.685887 16 | -0.371315 15.491469 17 | 17.342865 24.025292 18 | 4.774761 6.113791 19 | 9.086674 0.684141 20 | -1.102355 9.271148 21 | 1.807280 15.094420 22 | 8.114558 19.980346 23 | 16.007953 4.370999 24 | 19.967794 19.142931 25 | 6.624351 3.649843 26 | 15.474865 -0.807425 27 | 0.997246 -4.779132 28 | 18.607731 14.945526 29 | 16.154961 18.421871 30 | 8.767473 12.062236 31 | -0.806090 -1.564098 32 | 15.052089 9.132886 33 | 11.957083 17.949966 34 | 14.041550 11.607382 35 | 11.776215 4.118503 36 | -4.075465 8.101522 37 | 1.437540 7.255859 38 | 20.602092 2.018185 39 | -3.250918 3.441517 40 | 3.807813 14.857495 41 | 11.710965 18.516946 42 | 14.929406 7.191606 43 | 19.420612 0.009188 44 | -4.318638 -2.298564 45 | 16.670781 8.856317 46 | -0.161847 10.031343 47 | -0.430637 15.889611 48 | 8.384688 6.430637 49 | 4.045363 13.908478 50 | 5.854378 -2.370502 51 | -1.459823 23.856930 52 | -------------------------------------------------------------------------------- /data/sim/ch5_wifi_fingerprint_dense/metadata.json: -------------------------------------------------------------------------------- 1 | { 2 | "ap_ids": [ 3 | "AP1", 4 | "AP2", 5 | "AP3", 6 | "AP4", 7 | "AP5", 8 | "AP6", 9 | "AP7", 10 | "AP8" 11 | ], 12 | "ap_positions": [ 13 | [ 14 | 0.0, 15 | 0.0, 16 | 2.5 17 | ], 18 | [ 19 | 50.0, 20 | 0.0, 21 | 2.5 22 | ], 23 | [ 24 | 50.0, 25 | 50.0, 26 | 2.5 27 | ], 28 | [ 29 | 0.0, 30 | 50.0, 31 | 2.5 32 | ], 33 | [ 34 | 25.0, 35 | 0.0, 36 | 2.5 37 | ], 38 | [ 39 | 25.0, 40 | 50.0, 41 | 2.5 42 | ], 43 | [ 44 | 0.0, 45 | 25.0, 46 | 2.5 47 | ], 48 | [ 49 | 50.0, 50 | 25.0, 51 | 2.5 52 | ] 53 | ], 54 | "area_size": [ 55 | 50.0, 56 | 50.0 57 | ], 58 | "grid_spacing": 2.0, 59 | "floor_height": 3.0, 60 | "n_floors": 3, 61 | "path_loss_model": { 62 | "type": "log_distance", 63 | "P0_dBm": -30.0, 64 | "path_loss_exponent": 2.5, 65 | "shadow_fading_std_dBm": 4.0, 66 | "floor_attenuation_dB": 15.0 67 | }, 68 | "description": "Synthetic Wi-Fi RSS fingerprint database for indoor positioning", 69 | "generation_date": "2024-12-13" 70 | } -------------------------------------------------------------------------------- /data/sim/ch5_wifi_fingerprint_grid/metadata.json: -------------------------------------------------------------------------------- 1 | { 2 | "ap_ids": [ 3 | "AP1", 4 | "AP2", 5 | "AP3", 6 | "AP4", 7 | "AP5", 8 | "AP6", 9 | "AP7", 10 | "AP8" 11 | ], 12 | "ap_positions": [ 13 | [ 14 | 0.0, 15 | 0.0, 16 | 2.5 17 | ], 18 | [ 19 | 50.0, 20 | 0.0, 21 | 2.5 22 | ], 23 | [ 24 | 50.0, 25 | 50.0, 26 | 2.5 27 | ], 28 | [ 29 | 0.0, 30 | 50.0, 31 | 2.5 32 | ], 33 | [ 34 | 25.0, 35 | 0.0, 36 | 2.5 37 | ], 38 | [ 39 | 25.0, 40 | 50.0, 41 | 2.5 42 | ], 43 | [ 44 | 0.0, 45 | 25.0, 46 | 2.5 47 | ], 48 | [ 49 | 50.0, 50 | 25.0, 51 | 2.5 52 | ] 53 | ], 54 | "area_size": [ 55 | 50.0, 56 | 50.0 57 | ], 58 | "grid_spacing": 5.0, 59 | "floor_height": 3.0, 60 | "n_floors": 3, 61 | "path_loss_model": { 62 | "type": "log_distance", 63 | "P0_dBm": -30.0, 64 | "path_loss_exponent": 2.5, 65 | "shadow_fading_std_dBm": 4.0, 66 | "floor_attenuation_dB": 15.0 67 | }, 68 | "description": "Synthetic Wi-Fi RSS fingerprint database for indoor positioning", 69 | "generation_date": "2024-12-13" 70 | } -------------------------------------------------------------------------------- /data/sim/ch5_wifi_fingerprint_sparse/metadata.json: -------------------------------------------------------------------------------- 1 | { 2 | "ap_ids": [ 3 | "AP1", 4 | "AP2", 5 | "AP3", 6 | "AP4", 7 | "AP5", 8 | "AP6", 9 | "AP7", 10 | "AP8" 11 | ], 12 | "ap_positions": [ 13 | [ 14 | 0.0, 15 | 0.0, 16 | 2.5 17 | ], 18 | [ 19 | 50.0, 20 | 0.0, 21 | 2.5 22 | ], 23 | [ 24 | 50.0, 25 | 50.0, 26 | 2.5 27 | ], 28 | [ 29 | 0.0, 30 | 50.0, 31 | 2.5 32 | ], 33 | [ 34 | 25.0, 35 | 0.0, 36 | 2.5 37 | ], 38 | [ 39 | 25.0, 40 | 50.0, 41 | 2.5 42 | ], 43 | [ 44 | 0.0, 45 | 25.0, 46 | 2.5 47 | ], 48 | [ 49 | 50.0, 50 | 25.0, 51 | 2.5 52 | ] 53 | ], 54 | "area_size": [ 55 | 50.0, 56 | 50.0 57 | ], 58 | "grid_spacing": 10.0, 59 | "floor_height": 3.0, 60 | "n_floors": 3, 61 | "path_loss_model": { 62 | "type": "log_distance", 63 | "P0_dBm": -30.0, 64 | "path_loss_exponent": 2.5, 65 | "shadow_fading_std_dBm": 4.0, 66 | "floor_attenuation_dB": 15.0 67 | }, 68 | "description": "Synthetic Wi-Fi RSS fingerprint database for indoor positioning", 69 | "generation_date": "2024-12-13" 70 | } -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_square/config.json: -------------------------------------------------------------------------------- 1 | { 2 | "dataset": "ch4_rf_2d_positioning", 3 | "preset": "baseline", 4 | "geometry": { 5 | "type": "square", 6 | "num_beacons": 4, 7 | "area_size_m": 20.0 8 | }, 9 | "trajectory": { 10 | "type": "grid", 11 | "num_points": 100 12 | }, 13 | "measurements": { 14 | "toa_noise_std_m": 0.1, 15 | "tdoa_noise_std_m": 0.1, 16 | "aoa_noise_std_deg": 2.0 17 | }, 18 | "nlos": { 19 | "enabled": false, 20 | "beacon_indices": [], 21 | "bias_m": 0.0 22 | }, 23 | "dop": { 24 | "toa": { 25 | "mean": 1.0219465647849286, 26 | "min": 1.0000307286851629, 27 | "max": 1.086116532793336 28 | }, 29 | "tdoa": { 30 | "mean": 0.8730091275640508, 31 | "min": 0.8123666717511334, 32 | "max": 1.0264266551690753 33 | }, 34 | "aoa": { 35 | "mean": 15.041446611915411, 36 | "min": 13.836623684400164, 37 | "max": 16.73654084730976 38 | } 39 | }, 40 | "performance": { 41 | "toa_error_mean_m": 0.10055482565933645, 42 | "tdoa_error_mean_m": 14.604601663591403, 43 | "aoa_error_mean_m": 0.45871857688221496 44 | }, 45 | "equations": [ 46 | "4.1-4.3", 47 | "4.27-4.33", 48 | "4.63-4.66", 49 | "4.5 (DOP)" 50 | ], 51 | "seed": 42 52 | } -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_linear/config.json: -------------------------------------------------------------------------------- 1 | { 2 | "dataset": "ch4_rf_2d_positioning", 3 | "preset": "poor_geometry", 4 | "geometry": { 5 | "type": "linear", 6 | "num_beacons": 4, 7 | "area_size_m": 20.0 8 | }, 9 | "trajectory": { 10 | "type": "grid", 11 | "num_points": 100 12 | }, 13 | "measurements": { 14 | "toa_noise_std_m": 0.1, 15 | "tdoa_noise_std_m": 0.1, 16 | "aoa_noise_std_deg": 2.0 17 | }, 18 | "nlos": { 19 | "enabled": false, 20 | "beacon_indices": [], 21 | "bias_m": 0.0 22 | }, 23 | "dop": { 24 | "toa": { 25 | "mean": 1.425818824502396, 26 | "min": 1.0157306468940555, 27 | "max": 3.6025411120931365 28 | }, 29 | "tdoa": { 30 | "mean": 10.355143542144095, 31 | "min": 1.613248582115639, 32 | "max": 111.01821514685874 33 | }, 34 | "aoa": { 35 | "mean": 9.252573662678621, 36 | "min": 3.417930411577225, 37 | "max": 19.098663167411868 38 | } 39 | }, 40 | "performance": { 41 | "toa_error_mean_m": 0.27274437733247614, 42 | "tdoa_error_mean_m": 6.926330597395595e+16, 43 | "aoa_error_mean_m": 5.85304016883121e+18 44 | }, 45 | "equations": [ 46 | "4.1-4.3", 47 | "4.27-4.33", 48 | "4.63-4.66", 49 | "4.5 (DOP)" 50 | ], 51 | "seed": 42 52 | } -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_optimal/config.json: -------------------------------------------------------------------------------- 1 | { 2 | "dataset": "ch4_rf_2d_positioning", 3 | "preset": "optimal", 4 | "geometry": { 5 | "type": "optimal", 6 | "num_beacons": 4, 7 | "area_size_m": 20.0 8 | }, 9 | "trajectory": { 10 | "type": "grid", 11 | "num_points": 100 12 | }, 13 | "measurements": { 14 | "toa_noise_std_m": 0.1, 15 | "tdoa_noise_std_m": 0.1, 16 | "aoa_noise_std_deg": 2.0 17 | }, 18 | "nlos": { 19 | "enabled": false, 20 | "beacon_indices": [], 21 | "bias_m": 0.0 22 | }, 23 | "dop": { 24 | "toa": { 25 | "mean": 1.0193490627190913, 26 | "min": 1.0000980986200851, 27 | "max": 1.0688183597827718 28 | }, 29 | "tdoa": { 30 | "mean": 1.0891930437738122, 31 | "min": 0.8129881360873321, 32 | "max": 2.1149176841596615 33 | }, 34 | "aoa": { 35 | "mean": 11.535332314551304, 36 | "min": 9.817492271027291, 37 | "max": 13.411265984749894 38 | } 39 | }, 40 | "performance": { 41 | "toa_error_mean_m": 0.10489347297050827, 42 | "tdoa_error_mean_m": 8.574911745437058e+16, 43 | "aoa_error_mean_m": 0.33635545501930514 44 | }, 45 | "equations": [ 46 | "4.1-4.3", 47 | "4.27-4.33", 48 | "4.63-4.66", 49 | "4.5 (DOP)" 50 | ], 51 | "seed": 42 52 | } -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_nlos/config.json: -------------------------------------------------------------------------------- 1 | { 2 | "dataset": "ch4_rf_2d_positioning", 3 | "preset": "nlos", 4 | "geometry": { 5 | "type": "square", 6 | "num_beacons": 4, 7 | "area_size_m": 20.0 8 | }, 9 | "trajectory": { 10 | "type": "grid", 11 | "num_points": 100 12 | }, 13 | "measurements": { 14 | "toa_noise_std_m": 0.1, 15 | "tdoa_noise_std_m": 0.1, 16 | "aoa_noise_std_deg": 2.0 17 | }, 18 | "nlos": { 19 | "enabled": true, 20 | "beacon_indices": [ 21 | 1, 22 | 2 23 | ], 24 | "bias_m": 0.8 25 | }, 26 | "dop": { 27 | "toa": { 28 | "mean": 1.0219465647849286, 29 | "min": 1.0000307286851629, 30 | "max": 1.086116532793336 31 | }, 32 | "tdoa": { 33 | "mean": 0.8730091275640508, 34 | "min": 0.8123666717511334, 35 | "max": 1.0264266551690753 36 | }, 37 | "aoa": { 38 | "mean": 15.041446611915411, 39 | "min": 13.836623684400164, 40 | "max": 16.73654084730976 41 | } 42 | }, 43 | "performance": { 44 | "toa_error_mean_m": 0.6247543704919135, 45 | "tdoa_error_mean_m": 14.604601663591403, 46 | "aoa_error_mean_m": 0.45871857688221496 47 | }, 48 | "equations": [ 49 | "4.1-4.3", 50 | "4.27-4.33", 51 | "4.63-4.66", 52 | "4.5 (DOP)" 53 | ], 54 | "seed": 42 55 | } -------------------------------------------------------------------------------- /data/sim/ch6_pdr_corridor_walk/step_times.txt: -------------------------------------------------------------------------------- 1 | # step occurrence times (s) 2 | 0.490000 3 | 0.990000 4 | 1.490000 5 | 1.990000 6 | 2.490000 7 | 2.990000 8 | 3.490000 9 | 3.990000 10 | 4.490000 11 | 4.990000 12 | 5.490000 13 | 5.990000 14 | 6.490000 15 | 6.990000 16 | 7.490000 17 | 7.990000 18 | 8.490000 19 | 8.990000 20 | 9.490000 21 | 9.990000 22 | 10.740000 23 | 11.490000 24 | 12.010000 25 | 12.510000 26 | 13.010000 27 | 13.510000 28 | 14.010000 29 | 14.510000 30 | 15.010000 31 | 15.510000 32 | 16.010000 33 | 16.510000 34 | 17.010000 35 | 17.510000 36 | 18.010000 37 | 18.510000 38 | 19.010000 39 | 19.510000 40 | 20.010000 41 | 20.510000 42 | 21.010000 43 | 21.510000 44 | 22.260000 45 | 23.010000 46 | 23.760000 47 | 24.270000 48 | 24.770000 49 | 25.270000 50 | 25.770000 51 | 26.270000 52 | 26.770000 53 | 27.270000 54 | 27.770000 55 | 28.270000 56 | 28.770000 57 | 29.270000 58 | 29.770000 59 | 30.270000 60 | 30.770000 61 | 31.270000 62 | 31.770000 63 | 32.270000 64 | 32.770000 65 | 33.270000 66 | 33.770000 67 | 34.520000 68 | 35.270000 69 | 36.010000 70 | 36.510000 71 | 37.010000 72 | 37.510000 73 | 38.010000 74 | 38.510000 75 | 39.010000 76 | 39.510000 77 | 40.010000 78 | 40.510000 79 | 41.010000 80 | 41.510000 81 | 42.010000 82 | 42.510000 83 | 43.010000 84 | 43.510000 85 | 44.010000 86 | 44.510000 87 | 45.010000 88 | 45.510000 89 | 46.260000 90 | 47.010000 91 | 47.760000 92 | -------------------------------------------------------------------------------- /core/estimators/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | State estimation algorithms for indoor positioning. 3 | 4 | This module provides implementations of various state estimation techniques 5 | described in Chapter 3 of Principles of Indoor Positioning and Indoor Navigation. 6 | 7 | Available estimators: 8 | - Least Squares (LS, WLS, Robust LS) 9 | - Kalman Filter (KF) 10 | - Extended Kalman Filter (EKF) 11 | - Unscented Kalman Filter (UKF) 12 | - Particle Filter (PF) 13 | - Factor Graph Optimization (FGO) 14 | """ 15 | 16 | from core.estimators.least_squares import ( 17 | linear_least_squares, 18 | weighted_least_squares, 19 | iterative_least_squares, 20 | robust_least_squares, 21 | ) 22 | from core.estimators.kalman_filter import KalmanFilter 23 | from core.estimators.extended_kalman_filter import ExtendedKalmanFilter 24 | from core.estimators.unscented_kalman_filter import UnscentedKalmanFilter 25 | from core.estimators.particle_filter import ParticleFilter 26 | from core.estimators.factor_graph import Factor, FactorGraph 27 | 28 | __all__ = [ 29 | "linear_least_squares", 30 | "weighted_least_squares", 31 | "iterative_least_squares", 32 | "robust_least_squares", 33 | "KalmanFilter", 34 | "ExtendedKalmanFilter", 35 | "UnscentedKalmanFilter", 36 | "ParticleFilter", 37 | "Factor", 38 | "FactorGraph", 39 | ] 40 | 41 | -------------------------------------------------------------------------------- /core/coords/__init__.py: -------------------------------------------------------------------------------- 1 | """Coordinate systems and transformations for indoor positioning. 2 | 3 | This module provides functions and classes for working with different 4 | coordinate frames and transformations commonly used in navigation: 5 | - LLH (Latitude, Longitude, Height) geodetic coordinates 6 | - ECEF (Earth-Centered Earth-Fixed) Cartesian coordinates 7 | - ENU (East-North-Up) local tangent plane coordinates 8 | - NED (North-East-Down) local tangent plane coordinates 9 | - Rotation representations (quaternions, matrices, Euler angles) 10 | 11 | Reference: Chapter 2 - Coordinate Systems 12 | """ 13 | 14 | from core.coords.frames import Frame, FrameType 15 | from core.coords.rotations import ( 16 | euler_to_quat, 17 | euler_to_rotation_matrix, 18 | quat_to_euler, 19 | quat_to_rotation_matrix, 20 | rotation_matrix_to_euler, 21 | rotation_matrix_to_quat, 22 | ) 23 | from core.coords.transforms import ecef_to_enu, ecef_to_llh, enu_to_ecef, llh_to_ecef 24 | 25 | __all__ = [ 26 | # Frames 27 | "Frame", 28 | "FrameType", 29 | # Transforms 30 | "llh_to_ecef", 31 | "ecef_to_llh", 32 | "ecef_to_enu", 33 | "enu_to_ecef", 34 | # Rotations 35 | "euler_to_quat", 36 | "euler_to_rotation_matrix", 37 | "quat_to_euler", 38 | "quat_to_rotation_matrix", 39 | "rotation_matrix_to_euler", 40 | "rotation_matrix_to_quat", 41 | ] 42 | -------------------------------------------------------------------------------- /data/sim/ch7_slam_2d_square/ground_truth_poses.txt: -------------------------------------------------------------------------------- 1 | # x (m), y (m), yaw (rad) 2 | 0.000000 0.000000 0.000000 3 | 2.000000 0.000000 0.000000 4 | 4.000000 0.000000 0.000000 5 | 6.000000 0.000000 0.000000 6 | 8.000000 0.000000 0.000000 7 | 10.000000 0.000000 0.000000 8 | 12.000000 0.000000 0.000000 9 | 14.000000 0.000000 0.000000 10 | 16.000000 0.000000 0.000000 11 | 18.000000 0.000000 0.000000 12 | 20.000000 0.000000 1.570796 13 | 20.000000 2.000000 1.570796 14 | 20.000000 4.000000 1.570796 15 | 20.000000 6.000000 1.570796 16 | 20.000000 8.000000 1.570796 17 | 20.000000 10.000000 1.570796 18 | 20.000000 12.000000 1.570796 19 | 20.000000 14.000000 1.570796 20 | 20.000000 16.000000 1.570796 21 | 20.000000 18.000000 1.570796 22 | 20.000000 20.000000 3.141593 23 | 18.000000 20.000000 3.141593 24 | 16.000000 20.000000 3.141593 25 | 14.000000 20.000000 3.141593 26 | 12.000000 20.000000 3.141593 27 | 10.000000 20.000000 3.141593 28 | 8.000000 20.000000 3.141593 29 | 6.000000 20.000000 3.141593 30 | 4.000000 20.000000 3.141593 31 | 2.000000 20.000000 3.141593 32 | 0.000000 20.000000 -1.570796 33 | 0.000000 18.000000 -1.570796 34 | 0.000000 16.000000 -1.570796 35 | 0.000000 14.000000 -1.570796 36 | 0.000000 12.000000 -1.570796 37 | 0.000000 10.000000 -1.570796 38 | 0.000000 8.000000 -1.570796 39 | 0.000000 6.000000 -1.570796 40 | 0.000000 4.000000 -1.570796 41 | 0.000000 2.000000 -1.570796 42 | 0.000000 0.000000 0.000000 43 | -------------------------------------------------------------------------------- /data/sim/ch7_slam_2d_high_drift/ground_truth_poses.txt: -------------------------------------------------------------------------------- 1 | # x (m), y (m), yaw (rad) 2 | 0.000000 0.000000 0.000000 3 | 2.000000 0.000000 0.000000 4 | 4.000000 0.000000 0.000000 5 | 6.000000 0.000000 0.000000 6 | 8.000000 0.000000 0.000000 7 | 10.000000 0.000000 0.000000 8 | 12.000000 0.000000 0.000000 9 | 14.000000 0.000000 0.000000 10 | 16.000000 0.000000 0.000000 11 | 18.000000 0.000000 0.000000 12 | 20.000000 0.000000 1.570796 13 | 20.000000 2.000000 1.570796 14 | 20.000000 4.000000 1.570796 15 | 20.000000 6.000000 1.570796 16 | 20.000000 8.000000 1.570796 17 | 20.000000 10.000000 1.570796 18 | 20.000000 12.000000 1.570796 19 | 20.000000 14.000000 1.570796 20 | 20.000000 16.000000 1.570796 21 | 20.000000 18.000000 1.570796 22 | 20.000000 20.000000 3.141593 23 | 18.000000 20.000000 3.141593 24 | 16.000000 20.000000 3.141593 25 | 14.000000 20.000000 3.141593 26 | 12.000000 20.000000 3.141593 27 | 10.000000 20.000000 3.141593 28 | 8.000000 20.000000 3.141593 29 | 6.000000 20.000000 3.141593 30 | 4.000000 20.000000 3.141593 31 | 2.000000 20.000000 3.141593 32 | 0.000000 20.000000 -1.570796 33 | 0.000000 18.000000 -1.570796 34 | 0.000000 16.000000 -1.570796 35 | 0.000000 14.000000 -1.570796 36 | 0.000000 12.000000 -1.570796 37 | 0.000000 10.000000 -1.570796 38 | 0.000000 8.000000 -1.570796 39 | 0.000000 6.000000 -1.570796 40 | 0.000000 4.000000 -1.570796 41 | 0.000000 2.000000 -1.570796 42 | 0.000000 0.000000 0.000000 43 | -------------------------------------------------------------------------------- /data/sim/ch7_slam_2d_square/odometry_poses.txt: -------------------------------------------------------------------------------- 1 | # x (m), y (m), yaw (rad) - with cumulative drift 2 | 0.000000 0.000000 0.000000 3 | 2.030472 -0.103998 0.015009 4 | 4.127221 -0.267651 -0.011035 5 | 6.139533 -0.321483 -0.011371 6 | 8.055105 -0.255320 0.004185 7 | 10.061219 -0.134199 0.013535 8 | 11.974615 -0.071420 -0.005642 9 | 14.062398 -0.088193 -0.009339 10 | 15.995363 0.016013 -0.012430 11 | 17.951941 -0.043524 -0.001784 12 | 19.988556 -0.005884 1.577629 13 | 20.014068 2.208507 1.567384 14 | 19.959018 4.127328 1.589963 15 | 20.004904 6.117179 1.573474 16 | 19.925050 8.182032 1.584337 17 | 19.875657 10.114989 1.586671 18 | 19.756430 12.135220 1.591143 19 | 19.707603 14.202546 1.596925 20 | 19.799368 16.268777 1.590532 21 | 19.824704 18.222620 1.585029 22 | 19.880687 20.373129 -3.107995 23 | 18.048883 20.345069 -3.104740 24 | 15.994279 20.198145 -3.088873 25 | 14.029445 20.140763 -3.071713 26 | 12.044503 20.129710 -3.094379 27 | 10.140921 19.989997 -3.091530 28 | 8.072327 19.929131 -3.088360 29 | 6.011043 19.850277 -3.079224 30 | 4.078733 19.765981 -3.086859 31 | 2.203797 19.614486 -3.096247 32 | 0.206784 19.475744 -1.516520 33 | 0.309059 17.411714 -1.524986 34 | 0.231726 15.414049 -1.553928 35 | 0.163519 13.544903 -1.545933 36 | 0.173186 11.635101 -1.519948 37 | 0.346684 9.677014 -1.538621 38 | 0.315410 7.695527 -1.545401 39 | 0.195652 5.607782 -1.536713 40 | 0.205236 3.583159 -1.565634 41 | 0.162650 1.575699 -1.560980 42 | 0.342473 -0.424817 0.005029 43 | -------------------------------------------------------------------------------- /data/sim/ch7_slam_2d_high_drift/odometry_poses.txt: -------------------------------------------------------------------------------- 1 | # x (m), y (m), yaw (rad) - with cumulative drift 2 | 0.000000 0.000000 0.000000 3 | 2.091415 -0.311995 0.037523 4 | 4.393935 -0.811281 -0.027586 5 | 6.428895 -0.962341 -0.028426 6 | 8.179776 -0.748200 0.010463 7 | 10.195936 -0.388913 0.033839 8 | 11.933408 -0.219409 -0.014106 9 | 14.196507 -0.266312 -0.023349 10 | 16.000301 0.058426 -0.031075 11 | 17.867617 -0.105310 -0.004460 12 | 19.977782 0.009100 1.587878 13 | 20.054553 2.653291 1.562266 14 | 19.884745 4.410672 1.618714 15 | 20.042341 6.386304 1.577490 16 | 19.804675 8.579940 1.604648 17 | 19.674134 10.376899 1.610482 18 | 19.330958 12.430507 1.621662 19 | 19.198667 14.630300 1.636118 20 | 19.491969 16.843552 1.620134 21 | 19.591723 18.709630 1.606377 22 | 19.764207 21.165803 -3.057598 23 | 18.265910 21.140478 -3.049460 24 | 16.118902 20.727831 -3.009793 25 | 14.221730 20.616245 -2.966894 26 | 12.242171 20.655478 -3.023558 27 | 10.547567 20.304329 -3.016437 28 | 8.341685 20.155987 -3.008510 29 | 6.161038 19.957687 -2.985671 30 | 4.364556 19.785539 -3.004758 31 | 2.758577 19.416934 -3.028228 32 | 0.784005 19.046968 -1.435106 33 | 1.052284 16.863574 -1.456270 34 | 0.775218 14.842589 -1.528626 35 | 0.543897 13.228211 -1.508637 36 | 0.538031 11.496146 -1.443676 37 | 0.997507 9.646351 -1.490357 38 | 0.869175 7.691348 -1.507308 39 | 0.494908 5.410916 -1.485587 40 | 0.493591 3.331941 -1.557890 41 | 0.360847 1.308420 -1.546256 42 | 0.890472 -0.685740 0.012572 43 | -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_linear/gdop_toa.txt: -------------------------------------------------------------------------------- 1 | # GDOP for TOA 2 | 1.765605 3 | 1.524199 4 | 1.367414 5 | 1.275865 6 | 1.233912 7 | 1.233912 8 | 1.275865 9 | 1.367414 10 | 1.524199 11 | 1.765605 12 | 1.697353 13 | 1.408814 14 | 1.239249 15 | 1.149007 16 | 1.109244 17 | 1.109244 18 | 1.149007 19 | 1.239249 20 | 1.408814 21 | 1.697353 22 | 1.688628 23 | 1.312696 24 | 1.135574 25 | 1.056805 26 | 1.021823 27 | 1.021823 28 | 1.056805 29 | 1.135574 30 | 1.312696 31 | 1.688628 32 | 1.860551 33 | 1.242966 34 | 1.098185 35 | 1.043118 36 | 1.015731 37 | 1.015731 38 | 1.043118 39 | 1.098185 40 | 1.242966 41 | 1.860551 42 | 3.602541 43 | 1.235299 44 | 1.695226 45 | 1.355997 46 | 1.509667 47 | 1.509667 48 | 1.355997 49 | 1.695226 50 | 1.235299 51 | 3.602541 52 | 3.602541 53 | 1.235299 54 | 1.695226 55 | 1.355997 56 | 1.509667 57 | 1.509667 58 | 1.355997 59 | 1.695226 60 | 1.235299 61 | 3.602541 62 | 1.860551 63 | 1.242966 64 | 1.098185 65 | 1.043118 66 | 1.015731 67 | 1.015731 68 | 1.043118 69 | 1.098185 70 | 1.242966 71 | 1.860551 72 | 1.688628 73 | 1.312696 74 | 1.135574 75 | 1.056805 76 | 1.021823 77 | 1.021823 78 | 1.056805 79 | 1.135574 80 | 1.312696 81 | 1.688628 82 | 1.697353 83 | 1.408814 84 | 1.239249 85 | 1.149007 86 | 1.109244 87 | 1.109244 88 | 1.149007 89 | 1.239249 90 | 1.408814 91 | 1.697353 92 | 1.765605 93 | 1.524199 94 | 1.367414 95 | 1.275865 96 | 1.233912 97 | 1.233912 98 | 1.275865 99 | 1.367414 100 | 1.524199 101 | 1.765605 102 | -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_nlos/gdop_tdoa.txt: -------------------------------------------------------------------------------- 1 | # GDOP for TDOA 2 | 0.963207 3 | 0.948623 4 | 0.944755 5 | 0.940264 6 | 0.933374 7 | 0.923061 8 | 0.908303 9 | 0.888009 10 | 0.861387 11 | 0.835657 12 | 0.948623 13 | 0.899390 14 | 0.880971 15 | 0.872030 16 | 0.864644 17 | 0.855389 18 | 0.843076 19 | 0.828909 20 | 0.819078 21 | 0.834559 22 | 0.944755 23 | 0.880971 24 | 0.851165 25 | 0.837301 26 | 0.829588 27 | 0.823081 28 | 0.816447 29 | 0.812367 30 | 0.819494 31 | 0.856653 32 | 0.940264 33 | 0.872030 34 | 0.837301 35 | 0.821677 36 | 0.815509 37 | 0.813192 38 | 0.813086 39 | 0.818102 40 | 0.836662 41 | 0.883740 42 | 0.933374 43 | 0.864644 44 | 0.829588 45 | 0.815509 46 | 0.812982 47 | 0.815978 48 | 0.822355 49 | 0.834582 50 | 0.860243 51 | 0.911940 52 | 0.923061 53 | 0.855389 54 | 0.823081 55 | 0.813192 56 | 0.815978 57 | 0.824974 58 | 0.837465 59 | 0.855267 60 | 0.885289 61 | 0.939350 62 | 0.908303 63 | 0.843076 64 | 0.816447 65 | 0.813086 66 | 0.822355 67 | 0.837465 68 | 0.855308 69 | 0.877091 70 | 0.909324 71 | 0.964484 72 | 0.888009 73 | 0.828909 74 | 0.812367 75 | 0.818102 76 | 0.834582 77 | 0.855267 78 | 0.877091 79 | 0.900716 80 | 0.932236 81 | 0.986180 82 | 0.861387 83 | 0.819078 84 | 0.819494 85 | 0.836662 86 | 0.860243 87 | 0.885289 88 | 0.909324 89 | 0.932236 90 | 0.958411 91 | 1.004231 92 | 0.835657 93 | 0.834559 94 | 0.856653 95 | 0.883740 96 | 0.911940 97 | 0.939350 98 | 0.964484 99 | 0.986180 100 | 1.004231 101 | 1.026427 102 | -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_nlos/gdop_toa.txt: -------------------------------------------------------------------------------- 1 | # GDOP for TOA 2 | 1.086117 3 | 1.055847 4 | 1.035641 5 | 1.025067 6 | 1.020593 7 | 1.020593 8 | 1.025067 9 | 1.035641 10 | 1.055847 11 | 1.086117 12 | 1.055847 13 | 1.041383 14 | 1.025473 15 | 1.016013 16 | 1.011983 17 | 1.011983 18 | 1.016013 19 | 1.025473 20 | 1.041383 21 | 1.055847 22 | 1.035641 23 | 1.025473 24 | 1.013888 25 | 1.006969 26 | 1.004211 27 | 1.004211 28 | 1.006969 29 | 1.013888 30 | 1.025473 31 | 1.035641 32 | 1.025067 33 | 1.016013 34 | 1.006969 35 | 1.002211 36 | 1.000720 37 | 1.000720 38 | 1.002211 39 | 1.006969 40 | 1.016013 41 | 1.025067 42 | 1.020593 43 | 1.011983 44 | 1.004211 45 | 1.000720 46 | 1.000031 47 | 1.000031 48 | 1.000720 49 | 1.004211 50 | 1.011983 51 | 1.020593 52 | 1.020593 53 | 1.011983 54 | 1.004211 55 | 1.000720 56 | 1.000031 57 | 1.000031 58 | 1.000720 59 | 1.004211 60 | 1.011983 61 | 1.020593 62 | 1.025067 63 | 1.016013 64 | 1.006969 65 | 1.002211 66 | 1.000720 67 | 1.000720 68 | 1.002211 69 | 1.006969 70 | 1.016013 71 | 1.025067 72 | 1.035641 73 | 1.025473 74 | 1.013888 75 | 1.006969 76 | 1.004211 77 | 1.004211 78 | 1.006969 79 | 1.013888 80 | 1.025473 81 | 1.035641 82 | 1.055847 83 | 1.041383 84 | 1.025473 85 | 1.016013 86 | 1.011983 87 | 1.011983 88 | 1.016013 89 | 1.025473 90 | 1.041383 91 | 1.055847 92 | 1.086117 93 | 1.055847 94 | 1.035641 95 | 1.025067 96 | 1.020593 97 | 1.020593 98 | 1.025067 99 | 1.035641 100 | 1.055847 101 | 1.086117 102 | -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_optimal/gdop_toa.txt: -------------------------------------------------------------------------------- 1 | # GDOP for TOA 2 | 1.009356 3 | 1.000098 4 | 1.004413 5 | 1.022430 6 | 1.068818 7 | 1.068818 8 | 1.022430 9 | 1.004413 10 | 1.000098 11 | 1.009356 12 | 1.000098 13 | 1.006021 14 | 1.018285 15 | 1.030268 16 | 1.039772 17 | 1.039772 18 | 1.030268 19 | 1.018285 20 | 1.006021 21 | 1.000098 22 | 1.004413 23 | 1.018285 24 | 1.022078 25 | 1.018267 26 | 1.014444 27 | 1.014444 28 | 1.018267 29 | 1.022078 30 | 1.018285 31 | 1.004413 32 | 1.022430 33 | 1.030268 34 | 1.018267 35 | 1.007227 36 | 1.002668 37 | 1.002668 38 | 1.007227 39 | 1.018267 40 | 1.030268 41 | 1.022430 42 | 1.068818 43 | 1.039772 44 | 1.014444 45 | 1.002668 46 | 1.000121 47 | 1.000121 48 | 1.002668 49 | 1.014444 50 | 1.039772 51 | 1.068818 52 | 1.068818 53 | 1.039772 54 | 1.014444 55 | 1.002668 56 | 1.000121 57 | 1.000121 58 | 1.002668 59 | 1.014444 60 | 1.039772 61 | 1.068818 62 | 1.022430 63 | 1.030268 64 | 1.018267 65 | 1.007227 66 | 1.002668 67 | 1.002668 68 | 1.007227 69 | 1.018267 70 | 1.030268 71 | 1.022430 72 | 1.004413 73 | 1.018285 74 | 1.022078 75 | 1.018267 76 | 1.014444 77 | 1.014444 78 | 1.018267 79 | 1.022078 80 | 1.018285 81 | 1.004413 82 | 1.000098 83 | 1.006021 84 | 1.018285 85 | 1.030268 86 | 1.039772 87 | 1.039772 88 | 1.030268 89 | 1.018285 90 | 1.006021 91 | 1.000098 92 | 1.009356 93 | 1.000098 94 | 1.004413 95 | 1.022430 96 | 1.068818 97 | 1.068818 98 | 1.022430 99 | 1.004413 100 | 1.000098 101 | 1.009356 102 | -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_square/gdop_tdoa.txt: -------------------------------------------------------------------------------- 1 | # GDOP for TDOA 2 | 0.963207 3 | 0.948623 4 | 0.944755 5 | 0.940264 6 | 0.933374 7 | 0.923061 8 | 0.908303 9 | 0.888009 10 | 0.861387 11 | 0.835657 12 | 0.948623 13 | 0.899390 14 | 0.880971 15 | 0.872030 16 | 0.864644 17 | 0.855389 18 | 0.843076 19 | 0.828909 20 | 0.819078 21 | 0.834559 22 | 0.944755 23 | 0.880971 24 | 0.851165 25 | 0.837301 26 | 0.829588 27 | 0.823081 28 | 0.816447 29 | 0.812367 30 | 0.819494 31 | 0.856653 32 | 0.940264 33 | 0.872030 34 | 0.837301 35 | 0.821677 36 | 0.815509 37 | 0.813192 38 | 0.813086 39 | 0.818102 40 | 0.836662 41 | 0.883740 42 | 0.933374 43 | 0.864644 44 | 0.829588 45 | 0.815509 46 | 0.812982 47 | 0.815978 48 | 0.822355 49 | 0.834582 50 | 0.860243 51 | 0.911940 52 | 0.923061 53 | 0.855389 54 | 0.823081 55 | 0.813192 56 | 0.815978 57 | 0.824974 58 | 0.837465 59 | 0.855267 60 | 0.885289 61 | 0.939350 62 | 0.908303 63 | 0.843076 64 | 0.816447 65 | 0.813086 66 | 0.822355 67 | 0.837465 68 | 0.855308 69 | 0.877091 70 | 0.909324 71 | 0.964484 72 | 0.888009 73 | 0.828909 74 | 0.812367 75 | 0.818102 76 | 0.834582 77 | 0.855267 78 | 0.877091 79 | 0.900716 80 | 0.932236 81 | 0.986180 82 | 0.861387 83 | 0.819078 84 | 0.819494 85 | 0.836662 86 | 0.860243 87 | 0.885289 88 | 0.909324 89 | 0.932236 90 | 0.958411 91 | 1.004231 92 | 0.835657 93 | 0.834559 94 | 0.856653 95 | 0.883740 96 | 0.911940 97 | 0.939350 98 | 0.964484 99 | 0.986180 100 | 1.004231 101 | 1.026427 102 | -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_square/gdop_toa.txt: -------------------------------------------------------------------------------- 1 | # GDOP for TOA 2 | 1.086117 3 | 1.055847 4 | 1.035641 5 | 1.025067 6 | 1.020593 7 | 1.020593 8 | 1.025067 9 | 1.035641 10 | 1.055847 11 | 1.086117 12 | 1.055847 13 | 1.041383 14 | 1.025473 15 | 1.016013 16 | 1.011983 17 | 1.011983 18 | 1.016013 19 | 1.025473 20 | 1.041383 21 | 1.055847 22 | 1.035641 23 | 1.025473 24 | 1.013888 25 | 1.006969 26 | 1.004211 27 | 1.004211 28 | 1.006969 29 | 1.013888 30 | 1.025473 31 | 1.035641 32 | 1.025067 33 | 1.016013 34 | 1.006969 35 | 1.002211 36 | 1.000720 37 | 1.000720 38 | 1.002211 39 | 1.006969 40 | 1.016013 41 | 1.025067 42 | 1.020593 43 | 1.011983 44 | 1.004211 45 | 1.000720 46 | 1.000031 47 | 1.000031 48 | 1.000720 49 | 1.004211 50 | 1.011983 51 | 1.020593 52 | 1.020593 53 | 1.011983 54 | 1.004211 55 | 1.000720 56 | 1.000031 57 | 1.000031 58 | 1.000720 59 | 1.004211 60 | 1.011983 61 | 1.020593 62 | 1.025067 63 | 1.016013 64 | 1.006969 65 | 1.002211 66 | 1.000720 67 | 1.000720 68 | 1.002211 69 | 1.006969 70 | 1.016013 71 | 1.025067 72 | 1.035641 73 | 1.025473 74 | 1.013888 75 | 1.006969 76 | 1.004211 77 | 1.004211 78 | 1.006969 79 | 1.013888 80 | 1.025473 81 | 1.035641 82 | 1.055847 83 | 1.041383 84 | 1.025473 85 | 1.016013 86 | 1.011983 87 | 1.011983 88 | 1.016013 89 | 1.025473 90 | 1.041383 91 | 1.055847 92 | 1.086117 93 | 1.055847 94 | 1.035641 95 | 1.025067 96 | 1.020593 97 | 1.020593 98 | 1.025067 99 | 1.035641 100 | 1.055847 101 | 1.086117 102 | -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_optimal/gdop_tdoa.txt: -------------------------------------------------------------------------------- 1 | # GDOP for TDOA 2 | 1.929617 3 | 1.574967 4 | 1.292274 5 | 1.045160 6 | 0.854587 7 | 0.897288 8 | 1.135522 9 | 1.399700 10 | 1.704507 11 | 2.114918 12 | 1.570130 13 | 1.276146 14 | 1.065239 15 | 0.910452 16 | 0.824590 17 | 0.841400 18 | 0.953125 19 | 1.124264 20 | 1.356730 21 | 1.703265 22 | 1.346460 23 | 1.114187 24 | 0.966553 25 | 0.868466 26 | 0.817643 27 | 0.821058 28 | 0.877340 29 | 0.980488 30 | 1.141050 31 | 1.415877 32 | 1.184081 33 | 1.017964 34 | 0.921039 35 | 0.856465 36 | 0.819992 37 | 0.814104 38 | 0.839928 39 | 0.897638 40 | 0.995838 41 | 1.185976 42 | 1.052638 43 | 0.965638 44 | 0.902807 45 | 0.855209 46 | 0.824252 47 | 0.812988 48 | 0.823627 49 | 0.856429 50 | 0.911228 51 | 0.998807 52 | 1.052638 53 | 0.965638 54 | 0.902807 55 | 0.855209 56 | 0.824252 57 | 0.812988 58 | 0.823627 59 | 0.856429 60 | 0.911228 61 | 0.998807 62 | 1.184081 63 | 1.017964 64 | 0.921039 65 | 0.856465 66 | 0.819992 67 | 0.814104 68 | 0.839928 69 | 0.897638 70 | 0.995838 71 | 1.185976 72 | 1.346460 73 | 1.114187 74 | 0.966553 75 | 0.868466 76 | 0.817643 77 | 0.821058 78 | 0.877340 79 | 0.980488 80 | 1.141050 81 | 1.415877 82 | 1.570130 83 | 1.276146 84 | 1.065239 85 | 0.910452 86 | 0.824590 87 | 0.841400 88 | 0.953125 89 | 1.124264 90 | 1.356730 91 | 1.703265 92 | 1.929617 93 | 1.574967 94 | 1.292274 95 | 1.045160 96 | 0.854587 97 | 0.897288 98 | 1.135522 99 | 1.399700 100 | 1.704507 101 | 2.114918 102 | -------------------------------------------------------------------------------- /core/fusion/__init__.py: -------------------------------------------------------------------------------- 1 | """Sensor fusion utilities for Chapter 8. 2 | 3 | This package provides practical multi-sensor fusion tools including: 4 | - Time-stamped measurement types and temporal synchronization 5 | - Innovation monitoring and covariance tuning (Eqs. 8.5-8.7) 6 | - Chi-square gating for outlier rejection (Eqs. 8.8-8.9) 7 | 8 | These utilities support the Chapter 8 examples demonstrating loosely vs 9 | tightly coupled fusion, observability, tuning, calibration, and temporal 10 | synchronization. 11 | 12 | Author: Navigation Engineer 13 | References: Chapter 8 - Sensor Fusion 14 | """ 15 | 16 | from core.fusion.gating import ( 17 | chi_square_bounds, 18 | chi_square_gate, 19 | chi_square_threshold, 20 | mahalanobis_distance_squared, 21 | ) 22 | from core.fusion.tuning import ( 23 | cauchy_weight, 24 | compute_normalized_innovation, 25 | huber_weight, 26 | innovation, 27 | innovation_covariance, 28 | scale_measurement_covariance, 29 | ) 30 | from core.fusion.types import StampedMeasurement, TimeSyncModel 31 | 32 | __all__ = [ 33 | # Types 34 | "StampedMeasurement", 35 | "TimeSyncModel", 36 | # Tuning (Eqs. 8.5-8.7) 37 | "innovation", 38 | "innovation_covariance", 39 | "scale_measurement_covariance", 40 | "huber_weight", 41 | "cauchy_weight", 42 | "compute_normalized_innovation", 43 | # Gating (Eqs. 8.8-8.9) 44 | "mahalanobis_distance_squared", 45 | "chi_square_gate", 46 | "chi_square_threshold", 47 | "chi_square_bounds", 48 | ] 49 | 50 | 51 | -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_linear/gdop_tdoa.txt: -------------------------------------------------------------------------------- 1 | # GDOP for TDOA 2 | 13.478779 3 | 9.419627 4 | 7.044358 5 | 5.761716 6 | 5.185018 7 | 5.104877 8 | 5.496242 9 | 6.522922 10 | 8.525830 11 | 12.038347 12 | 12.979866 13 | 8.117202 14 | 5.541073 15 | 4.299860 16 | 3.812428 17 | 3.748056 18 | 4.061412 19 | 5.018741 20 | 7.138306 21 | 11.268468 22 | 14.114403 23 | 7.394225 24 | 4.345791 25 | 3.111365 26 | 2.770831 27 | 2.743369 28 | 2.918740 29 | 3.806369 30 | 6.232586 31 | 11.807016 32 | 21.082071 33 | 8.040761 34 | 3.634683 35 | 2.182282 36 | 2.069779 37 | 2.147986 38 | 2.043574 39 | 2.999252 40 | 6.370327 41 | 16.871023 42 | 111.018215 43 | 18.400430 44 | 5.495730 45 | 1.833657 46 | 2.189097 47 | 2.703565 48 | 1.613249 49 | 4.142486 50 | 13.559058 51 | 85.552129 52 | 111.018215 53 | 18.400430 54 | 5.495730 55 | 1.833657 56 | 2.189097 57 | 2.703565 58 | 1.613249 59 | 4.142486 60 | 13.559058 61 | 85.552129 62 | 21.082071 63 | 8.040761 64 | 3.634683 65 | 2.182282 66 | 2.069779 67 | 2.147986 68 | 2.043574 69 | 2.999252 70 | 6.370327 71 | 16.871023 72 | 14.114403 73 | 7.394225 74 | 4.345791 75 | 3.111365 76 | 2.770831 77 | 2.743369 78 | 2.918740 79 | 3.806369 80 | 6.232586 81 | 11.807016 82 | 12.979866 83 | 8.117202 84 | 5.541073 85 | 4.299860 86 | 3.812428 87 | 3.748056 88 | 4.061412 89 | 5.018741 90 | 7.138306 91 | 11.268468 92 | 13.478779 93 | 9.419627 94 | 7.044358 95 | 5.761716 96 | 5.185018 97 | 5.104877 98 | 5.496242 99 | 6.522922 100 | 8.525830 101 | 12.038347 102 | -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_linear/gdop_aoa.txt: -------------------------------------------------------------------------------- 1 | # GDOP for AOA 2 | 19.098663 3 | 15.460987 4 | 13.349344 5 | 12.235752 6 | 11.749730 7 | 11.749730 8 | 12.235752 9 | 13.349344 10 | 15.460987 11 | 19.098663 12 | 15.485954 13 | 11.768584 14 | 9.912269 15 | 9.097623 16 | 8.743174 17 | 8.743174 18 | 9.097623 19 | 9.912269 20 | 11.768584 21 | 15.485954 22 | 12.584852 23 | 8.495963 24 | 6.927231 25 | 6.535293 26 | 6.287098 27 | 6.287098 28 | 6.535293 29 | 6.927231 30 | 8.495963 31 | 12.584852 32 | 11.125334 33 | 5.748936 34 | 4.425606 35 | 4.474437 36 | 4.281529 37 | 4.281529 38 | 4.474437 39 | 4.425606 40 | 5.748936 41 | 11.125334 42 | 18.243835 43 | 4.129109 44 | 4.003045 45 | 3.417930 46 | 3.732064 47 | 3.732064 48 | 3.417930 49 | 4.003045 50 | 4.129109 51 | 18.243835 52 | 18.243835 53 | 4.129109 54 | 4.003045 55 | 3.417930 56 | 3.732064 57 | 3.732064 58 | 3.417930 59 | 4.003045 60 | 4.129109 61 | 18.243835 62 | 11.125334 63 | 5.748936 64 | 4.425606 65 | 4.474437 66 | 4.281529 67 | 4.281529 68 | 4.474437 69 | 4.425606 70 | 5.748936 71 | 11.125334 72 | 12.584852 73 | 8.495963 74 | 6.927231 75 | 6.535293 76 | 6.287098 77 | 6.287098 78 | 6.535293 79 | 6.927231 80 | 8.495963 81 | 12.584852 82 | 15.485954 83 | 11.768584 84 | 9.912269 85 | 9.097623 86 | 8.743174 87 | 8.743174 88 | 9.097623 89 | 9.912269 90 | 11.768584 91 | 15.485954 92 | 19.098663 93 | 15.460987 94 | 13.349344 95 | 12.235752 96 | 11.749730 97 | 11.749730 98 | 12.235752 99 | 13.349344 100 | 15.460987 101 | 19.098663 102 | -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_optimal/gdop_aoa.txt: -------------------------------------------------------------------------------- 1 | # GDOP for AOA 2 | 11.239342 3 | 11.474054 4 | 12.220895 5 | 12.806550 6 | 12.105891 7 | 12.105891 8 | 12.806550 9 | 12.220895 10 | 11.474054 11 | 11.239342 12 | 11.474054 13 | 12.563953 14 | 13.411266 15 | 12.372784 16 | 10.714102 17 | 10.714102 18 | 12.372784 19 | 13.411266 20 | 12.563953 21 | 11.474054 22 | 12.220895 23 | 13.411266 24 | 12.688731 25 | 11.012282 26 | 9.970553 27 | 9.970553 28 | 11.012282 29 | 12.688731 30 | 13.411266 31 | 12.220895 32 | 12.806550 33 | 12.372784 34 | 11.012282 35 | 10.145574 36 | 9.817492 37 | 9.817492 38 | 10.145574 39 | 11.012282 40 | 12.372784 41 | 12.806550 42 | 12.105891 43 | 10.714102 44 | 9.970553 45 | 9.817492 46 | 9.933968 47 | 9.933968 48 | 9.817492 49 | 9.970553 50 | 10.714102 51 | 12.105891 52 | 12.105891 53 | 10.714102 54 | 9.970553 55 | 9.817492 56 | 9.933968 57 | 9.933968 58 | 9.817492 59 | 9.970553 60 | 10.714102 61 | 12.105891 62 | 12.806550 63 | 12.372784 64 | 11.012282 65 | 10.145574 66 | 9.817492 67 | 9.817492 68 | 10.145574 69 | 11.012282 70 | 12.372784 71 | 12.806550 72 | 12.220895 73 | 13.411266 74 | 12.688731 75 | 11.012282 76 | 9.970553 77 | 9.970553 78 | 11.012282 79 | 12.688731 80 | 13.411266 81 | 12.220895 82 | 11.474054 83 | 12.563953 84 | 13.411266 85 | 12.372784 86 | 10.714102 87 | 10.714102 88 | 12.372784 89 | 13.411266 90 | 12.563953 91 | 11.474054 92 | 11.239342 93 | 11.474054 94 | 12.220895 95 | 12.806550 96 | 12.105891 97 | 12.105891 98 | 12.806550 99 | 12.220895 100 | 11.474054 101 | 11.239342 102 | -------------------------------------------------------------------------------- /core/rf/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | RF (Radio Frequency) positioning module. 3 | 4 | This module implements RF signal measurement models and positioning algorithms 5 | for Chapter 4 of "Principles of Indoor Positioning and Indoor Navigation". 6 | 7 | Submodules: 8 | measurement_models: TOA, TDOA, AOA, RSS measurement functions 9 | positioning: Positioning algorithms (LS, I-WLS, closed-form) 10 | dop: Dilution of Precision utilities 11 | """ 12 | 13 | from core.rf.dop import compute_dop, compute_dop_map, compute_geometry_matrix 14 | from core.rf.measurement_models import ( 15 | SPEED_OF_LIGHT, 16 | aoa_azimuth, 17 | aoa_elevation, 18 | aoa_measurement_vector, 19 | rss_pathloss, 20 | rss_to_distance, 21 | tdoa_measurement_vector, 22 | tdoa_range_difference, 23 | toa_range, 24 | two_way_toa_range, 25 | ) 26 | from core.rf.positioning import ( 27 | AOAPositioner, 28 | TDOAPositioner, 29 | TOAPositioner, 30 | toa_solve_with_clock_bias, 31 | ) 32 | 33 | __all__ = [ 34 | # Constants 35 | "SPEED_OF_LIGHT", 36 | # Measurement models 37 | "toa_range", 38 | "two_way_toa_range", 39 | "rss_pathloss", 40 | "rss_to_distance", 41 | "tdoa_range_difference", 42 | "tdoa_measurement_vector", 43 | "aoa_azimuth", 44 | "aoa_elevation", 45 | "aoa_measurement_vector", 46 | # Positioning 47 | "TOAPositioner", 48 | "TDOAPositioner", 49 | "AOAPositioner", 50 | "toa_solve_with_clock_bias", 51 | # DOP 52 | "compute_geometry_matrix", 53 | "compute_dop", 54 | "compute_dop_map", 55 | ] 56 | 57 | -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_nlos/gdop_aoa.txt: -------------------------------------------------------------------------------- 1 | # GDOP for AOA 2 | 16.634800 3 | 16.051395 4 | 16.181101 5 | 16.503358 6 | 16.736541 7 | 16.736541 8 | 16.503358 9 | 16.181101 10 | 16.051395 11 | 16.634800 12 | 16.051395 13 | 14.768127 14 | 14.503938 15 | 14.665747 16 | 14.848659 17 | 14.848659 18 | 14.665747 19 | 14.503938 20 | 14.768127 21 | 16.051395 22 | 16.181101 23 | 14.503938 24 | 13.909474 25 | 13.909915 26 | 14.041570 27 | 14.041570 28 | 13.909915 29 | 13.909474 30 | 14.503938 31 | 16.181101 32 | 16.503358 33 | 14.665747 34 | 13.909915 35 | 13.836624 36 | 13.956759 37 | 13.956759 38 | 13.836624 39 | 13.909915 40 | 14.665747 41 | 16.503358 42 | 16.736541 43 | 14.848659 44 | 14.041570 45 | 13.956759 46 | 14.089172 47 | 14.089172 48 | 13.956759 49 | 14.041570 50 | 14.848659 51 | 16.736541 52 | 16.736541 53 | 14.848659 54 | 14.041570 55 | 13.956759 56 | 14.089172 57 | 14.089172 58 | 13.956759 59 | 14.041570 60 | 14.848659 61 | 16.736541 62 | 16.503358 63 | 14.665747 64 | 13.909915 65 | 13.836624 66 | 13.956759 67 | 13.956759 68 | 13.836624 69 | 13.909915 70 | 14.665747 71 | 16.503358 72 | 16.181101 73 | 14.503938 74 | 13.909474 75 | 13.909915 76 | 14.041570 77 | 14.041570 78 | 13.909915 79 | 13.909474 80 | 14.503938 81 | 16.181101 82 | 16.051395 83 | 14.768127 84 | 14.503938 85 | 14.665747 86 | 14.848659 87 | 14.848659 88 | 14.665747 89 | 14.503938 90 | 14.768127 91 | 16.051395 92 | 16.634800 93 | 16.051395 94 | 16.181101 95 | 16.503358 96 | 16.736541 97 | 16.736541 98 | 16.503358 99 | 16.181101 100 | 16.051395 101 | 16.634800 102 | -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_square/gdop_aoa.txt: -------------------------------------------------------------------------------- 1 | # GDOP for AOA 2 | 16.634800 3 | 16.051395 4 | 16.181101 5 | 16.503358 6 | 16.736541 7 | 16.736541 8 | 16.503358 9 | 16.181101 10 | 16.051395 11 | 16.634800 12 | 16.051395 13 | 14.768127 14 | 14.503938 15 | 14.665747 16 | 14.848659 17 | 14.848659 18 | 14.665747 19 | 14.503938 20 | 14.768127 21 | 16.051395 22 | 16.181101 23 | 14.503938 24 | 13.909474 25 | 13.909915 26 | 14.041570 27 | 14.041570 28 | 13.909915 29 | 13.909474 30 | 14.503938 31 | 16.181101 32 | 16.503358 33 | 14.665747 34 | 13.909915 35 | 13.836624 36 | 13.956759 37 | 13.956759 38 | 13.836624 39 | 13.909915 40 | 14.665747 41 | 16.503358 42 | 16.736541 43 | 14.848659 44 | 14.041570 45 | 13.956759 46 | 14.089172 47 | 14.089172 48 | 13.956759 49 | 14.041570 50 | 14.848659 51 | 16.736541 52 | 16.736541 53 | 14.848659 54 | 14.041570 55 | 13.956759 56 | 14.089172 57 | 14.089172 58 | 13.956759 59 | 14.041570 60 | 14.848659 61 | 16.736541 62 | 16.503358 63 | 14.665747 64 | 13.909915 65 | 13.836624 66 | 13.956759 67 | 13.956759 68 | 13.836624 69 | 13.909915 70 | 14.665747 71 | 16.503358 72 | 16.181101 73 | 14.503938 74 | 13.909474 75 | 13.909915 76 | 14.041570 77 | 14.041570 78 | 13.909915 79 | 13.909474 80 | 14.503938 81 | 16.181101 82 | 16.051395 83 | 14.768127 84 | 14.503938 85 | 14.665747 86 | 14.848659 87 | 14.848659 88 | 14.665747 89 | 14.503938 90 | 14.768127 91 | 16.051395 92 | 16.634800 93 | 16.051395 94 | 16.181101 95 | 16.503358 96 | 16.736541 97 | 16.736541 98 | 16.503358 99 | 16.181101 100 | 16.051395 101 | 16.634800 102 | -------------------------------------------------------------------------------- /data/sim/ch2_coords_san_francisco/rotation_matrices.txt: -------------------------------------------------------------------------------- 1 | # 3x3 rotation matrices (flattened row-major) 2 | -0.889533 -0.438715 -0.127513 0.370738 -0.856251 0.359704 -0.266991 0.272695 0.924312 3 | 0.490682 0.871117 -0.019655 -0.858023 0.486988 0.163215 0.151751 -0.063222 0.986395 4 | -0.270835 0.831906 -0.484335 -0.838293 -0.451153 -0.306147 -0.473195 0.323099 0.819569 5 | -0.350001 -0.935891 0.040093 0.846985 -0.297890 0.440317 -0.400145 0.188070 0.896947 6 | 0.473298 0.733490 0.487834 -0.832697 0.553213 -0.023906 -0.287411 -0.394903 0.872609 7 | 0.320243 0.776348 -0.542888 -0.893659 0.437732 0.098812 0.314352 0.453514 0.833972 8 | -0.759856 -0.618260 0.200931 0.649157 -0.738177 0.183547 0.034843 0.269905 0.962256 9 | -0.211799 -0.895533 0.391360 0.862415 -0.359643 -0.356227 0.459762 0.262066 0.848493 10 | -0.384864 0.787810 0.480869 -0.852316 -0.503278 0.142370 0.354171 -0.355059 0.865157 11 | 0.626924 -0.774801 0.081541 0.755426 0.630146 0.179574 -0.190517 -0.050981 0.980359 12 | 0.299452 -0.952774 -0.050499 0.919814 0.274220 0.280617 -0.253517 -0.130481 0.958490 13 | 0.881589 0.161314 0.443598 0.040810 0.910221 -0.412107 -0.470251 0.381412 0.795857 14 | 0.226113 0.955925 -0.187298 -0.957064 0.253817 0.140018 0.181386 0.147596 0.972273 15 | -0.505164 0.834456 -0.220211 -0.852361 -0.442423 0.278822 0.135239 0.328550 0.934754 16 | -0.277852 0.958372 0.065733 -0.960095 -0.279319 0.014102 0.031876 -0.059192 0.997738 17 | 0.181823 0.958927 0.217713 -0.929984 0.095757 0.354909 0.319484 -0.267000 0.909198 18 | -0.895163 -0.233990 0.379383 0.236351 -0.970799 -0.041081 0.377917 0.052893 0.924327 19 | -0.907872 0.365505 0.205363 -0.418474 -0.819760 -0.390990 0.025439 -0.440908 0.897192 20 | 0.612478 -0.785424 0.089332 0.738439 0.528155 -0.419237 0.282098 0.322740 0.903471 21 | 0.740200 -0.634555 0.222359 0.648700 0.760947 0.012118 -0.176893 0.135275 0.974890 22 | -------------------------------------------------------------------------------- /core/estimators/base.py: -------------------------------------------------------------------------------- 1 | """ 2 | Base classes for state estimators. 3 | 4 | This module defines abstract base classes and common interfaces for all 5 | state estimation algorithms. 6 | """ 7 | 8 | from abc import ABC, abstractmethod 9 | from typing import Optional, Tuple 10 | 11 | import numpy as np 12 | 13 | 14 | class StateEstimator(ABC): 15 | """Abstract base class for state estimators.""" 16 | 17 | def __init__(self, state_dim: int): 18 | """ 19 | Initialize state estimator. 20 | 21 | Args: 22 | state_dim: Dimension of the state vector. 23 | """ 24 | self.state_dim = state_dim 25 | self.state: Optional[np.ndarray] = None 26 | self.covariance: Optional[np.ndarray] = None 27 | 28 | @abstractmethod 29 | def predict(self, u: Optional[np.ndarray] = None) -> None: 30 | """ 31 | Perform prediction step (time update). 32 | 33 | Args: 34 | u: Optional control input vector. 35 | """ 36 | pass 37 | 38 | @abstractmethod 39 | def update(self, z: np.ndarray) -> None: 40 | """ 41 | Perform measurement update (correction step). 42 | 43 | Args: 44 | z: Measurement vector. 45 | """ 46 | pass 47 | 48 | def get_state(self) -> Tuple[np.ndarray, np.ndarray]: 49 | """ 50 | Get current state estimate and covariance. 51 | 52 | Returns: 53 | Tuple of (state_vector, covariance_matrix). 54 | """ 55 | if self.state is None or self.covariance is None: 56 | raise RuntimeError("Estimator not initialized. Call predict() first.") 57 | return self.state.copy(), self.covariance.copy() 58 | 59 | 60 | class BatchEstimator(ABC): 61 | """Abstract base class for batch estimation algorithms.""" 62 | 63 | @abstractmethod 64 | def estimate( 65 | self, measurements: np.ndarray, *args, **kwargs 66 | ) -> Tuple[np.ndarray, np.ndarray]: 67 | """ 68 | Compute batch estimate from measurements. 69 | 70 | Args: 71 | measurements: Matrix of measurements (m × n). 72 | *args: Additional positional arguments. 73 | **kwargs: Additional keyword arguments. 74 | 75 | Returns: 76 | Tuple of (state_estimate, covariance_matrix). 77 | """ 78 | pass 79 | 80 | 81 | 82 | -------------------------------------------------------------------------------- /ch8_sensor_fusion/figs/lc_tc_comparison.json: -------------------------------------------------------------------------------- 1 | { 2 | "dataset": { 3 | "path": "data/sim/ch8_fusion_2d_imu_uwb", 4 | "config": { 5 | "dataset_info": { 6 | "description": "2D IMU + UWB fusion dataset for Chapter 8", 7 | "seed": 42, 8 | "duration_sec": 60.0, 9 | "imu_samples": 6000, 10 | "uwb_samples": 600 11 | }, 12 | "trajectory": { 13 | "type": "rectangular_walk", 14 | "width_m": 20.0, 15 | "height_m": 15.0, 16 | "speed_m_s": 1.0 17 | }, 18 | "imu": { 19 | "rate_hz": 100.0, 20 | "dt_sec": 0.01, 21 | "accel_noise_std_m_s2": 0.1, 22 | "gyro_noise_std_rad_s": 0.01, 23 | "accel_bias_m_s2": [ 24 | 0.0, 25 | 0.0 26 | ], 27 | "gyro_bias_rad_s": 0.0 28 | }, 29 | "uwb": { 30 | "rate_hz": 10.0, 31 | "n_anchors": 4, 32 | "range_noise_std_m": 0.05, 33 | "nlos_anchors": [], 34 | "nlos_bias_m": 0.5, 35 | "dropout_rate": 0.05 36 | }, 37 | "temporal_calibration": { 38 | "time_offset_sec": 0.0, 39 | "clock_drift": 0.0, 40 | "note": "Use TimeSyncModel to apply these during fusion" 41 | }, 42 | "coordinate_frame": { 43 | "description": "ENU (East-North-Up)", 44 | "origin": "Bottom-left corner (0, 0)", 45 | "units": "meters" 46 | } 47 | }, 48 | "n_imu_samples": 6000, 49 | "n_uwb_epochs": 600, 50 | "duration": 59.99 51 | }, 52 | "lc_fusion": { 53 | "rmse_2d": 12.895864923933225, 54 | "rmse_x": 17.091896971141036, 55 | "rmse_y": 6.3618961167691435, 56 | "max_error": 40.826098558030516, 57 | "mean_error": 11.747015377252849, 58 | "final_error": 39.11215035164275, 59 | "n_updates": 176, 60 | "n_rejected": 408, 61 | "n_failed": 16, 62 | "acceptance_rate": 30.136986301369863 63 | }, 64 | "tc_fusion": { 65 | "rmse_2d": 12.352023398920757, 66 | "rmse_x": 16.519044783820114, 67 | "rmse_y": 5.680327765641458, 68 | "max_error": 38.99335819329376, 69 | "mean_error": 11.24805877346406, 70 | "final_error": 37.39237045662993, 71 | "n_updates": 748, 72 | "n_rejected": 1523, 73 | "acceptance_rate": 32.937032144429764 74 | }, 75 | "comparison": { 76 | "rmse_difference": 0.5438415250124677, 77 | "better_rmse": "TC", 78 | "update_ratio": 0.23529411764705882 79 | } 80 | } -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_nlos/ground_truth_positions.txt: -------------------------------------------------------------------------------- 1 | # x (m), y (m) 2 | 2.000000 2.000000 3 | 3.777778 2.000000 4 | 5.555556 2.000000 5 | 7.333333 2.000000 6 | 9.111111 2.000000 7 | 10.888889 2.000000 8 | 12.666667 2.000000 9 | 14.444444 2.000000 10 | 16.222222 2.000000 11 | 18.000000 2.000000 12 | 2.000000 3.777778 13 | 3.777778 3.777778 14 | 5.555556 3.777778 15 | 7.333333 3.777778 16 | 9.111111 3.777778 17 | 10.888889 3.777778 18 | 12.666667 3.777778 19 | 14.444444 3.777778 20 | 16.222222 3.777778 21 | 18.000000 3.777778 22 | 2.000000 5.555556 23 | 3.777778 5.555556 24 | 5.555556 5.555556 25 | 7.333333 5.555556 26 | 9.111111 5.555556 27 | 10.888889 5.555556 28 | 12.666667 5.555556 29 | 14.444444 5.555556 30 | 16.222222 5.555556 31 | 18.000000 5.555556 32 | 2.000000 7.333333 33 | 3.777778 7.333333 34 | 5.555556 7.333333 35 | 7.333333 7.333333 36 | 9.111111 7.333333 37 | 10.888889 7.333333 38 | 12.666667 7.333333 39 | 14.444444 7.333333 40 | 16.222222 7.333333 41 | 18.000000 7.333333 42 | 2.000000 9.111111 43 | 3.777778 9.111111 44 | 5.555556 9.111111 45 | 7.333333 9.111111 46 | 9.111111 9.111111 47 | 10.888889 9.111111 48 | 12.666667 9.111111 49 | 14.444444 9.111111 50 | 16.222222 9.111111 51 | 18.000000 9.111111 52 | 2.000000 10.888889 53 | 3.777778 10.888889 54 | 5.555556 10.888889 55 | 7.333333 10.888889 56 | 9.111111 10.888889 57 | 10.888889 10.888889 58 | 12.666667 10.888889 59 | 14.444444 10.888889 60 | 16.222222 10.888889 61 | 18.000000 10.888889 62 | 2.000000 12.666667 63 | 3.777778 12.666667 64 | 5.555556 12.666667 65 | 7.333333 12.666667 66 | 9.111111 12.666667 67 | 10.888889 12.666667 68 | 12.666667 12.666667 69 | 14.444444 12.666667 70 | 16.222222 12.666667 71 | 18.000000 12.666667 72 | 2.000000 14.444444 73 | 3.777778 14.444444 74 | 5.555556 14.444444 75 | 7.333333 14.444444 76 | 9.111111 14.444444 77 | 10.888889 14.444444 78 | 12.666667 14.444444 79 | 14.444444 14.444444 80 | 16.222222 14.444444 81 | 18.000000 14.444444 82 | 2.000000 16.222222 83 | 3.777778 16.222222 84 | 5.555556 16.222222 85 | 7.333333 16.222222 86 | 9.111111 16.222222 87 | 10.888889 16.222222 88 | 12.666667 16.222222 89 | 14.444444 16.222222 90 | 16.222222 16.222222 91 | 18.000000 16.222222 92 | 2.000000 18.000000 93 | 3.777778 18.000000 94 | 5.555556 18.000000 95 | 7.333333 18.000000 96 | 9.111111 18.000000 97 | 10.888889 18.000000 98 | 12.666667 18.000000 99 | 14.444444 18.000000 100 | 16.222222 18.000000 101 | 18.000000 18.000000 102 | -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_linear/ground_truth_positions.txt: -------------------------------------------------------------------------------- 1 | # x (m), y (m) 2 | 2.000000 2.000000 3 | 3.777778 2.000000 4 | 5.555556 2.000000 5 | 7.333333 2.000000 6 | 9.111111 2.000000 7 | 10.888889 2.000000 8 | 12.666667 2.000000 9 | 14.444444 2.000000 10 | 16.222222 2.000000 11 | 18.000000 2.000000 12 | 2.000000 3.777778 13 | 3.777778 3.777778 14 | 5.555556 3.777778 15 | 7.333333 3.777778 16 | 9.111111 3.777778 17 | 10.888889 3.777778 18 | 12.666667 3.777778 19 | 14.444444 3.777778 20 | 16.222222 3.777778 21 | 18.000000 3.777778 22 | 2.000000 5.555556 23 | 3.777778 5.555556 24 | 5.555556 5.555556 25 | 7.333333 5.555556 26 | 9.111111 5.555556 27 | 10.888889 5.555556 28 | 12.666667 5.555556 29 | 14.444444 5.555556 30 | 16.222222 5.555556 31 | 18.000000 5.555556 32 | 2.000000 7.333333 33 | 3.777778 7.333333 34 | 5.555556 7.333333 35 | 7.333333 7.333333 36 | 9.111111 7.333333 37 | 10.888889 7.333333 38 | 12.666667 7.333333 39 | 14.444444 7.333333 40 | 16.222222 7.333333 41 | 18.000000 7.333333 42 | 2.000000 9.111111 43 | 3.777778 9.111111 44 | 5.555556 9.111111 45 | 7.333333 9.111111 46 | 9.111111 9.111111 47 | 10.888889 9.111111 48 | 12.666667 9.111111 49 | 14.444444 9.111111 50 | 16.222222 9.111111 51 | 18.000000 9.111111 52 | 2.000000 10.888889 53 | 3.777778 10.888889 54 | 5.555556 10.888889 55 | 7.333333 10.888889 56 | 9.111111 10.888889 57 | 10.888889 10.888889 58 | 12.666667 10.888889 59 | 14.444444 10.888889 60 | 16.222222 10.888889 61 | 18.000000 10.888889 62 | 2.000000 12.666667 63 | 3.777778 12.666667 64 | 5.555556 12.666667 65 | 7.333333 12.666667 66 | 9.111111 12.666667 67 | 10.888889 12.666667 68 | 12.666667 12.666667 69 | 14.444444 12.666667 70 | 16.222222 12.666667 71 | 18.000000 12.666667 72 | 2.000000 14.444444 73 | 3.777778 14.444444 74 | 5.555556 14.444444 75 | 7.333333 14.444444 76 | 9.111111 14.444444 77 | 10.888889 14.444444 78 | 12.666667 14.444444 79 | 14.444444 14.444444 80 | 16.222222 14.444444 81 | 18.000000 14.444444 82 | 2.000000 16.222222 83 | 3.777778 16.222222 84 | 5.555556 16.222222 85 | 7.333333 16.222222 86 | 9.111111 16.222222 87 | 10.888889 16.222222 88 | 12.666667 16.222222 89 | 14.444444 16.222222 90 | 16.222222 16.222222 91 | 18.000000 16.222222 92 | 2.000000 18.000000 93 | 3.777778 18.000000 94 | 5.555556 18.000000 95 | 7.333333 18.000000 96 | 9.111111 18.000000 97 | 10.888889 18.000000 98 | 12.666667 18.000000 99 | 14.444444 18.000000 100 | 16.222222 18.000000 101 | 18.000000 18.000000 102 | -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_optimal/ground_truth_positions.txt: -------------------------------------------------------------------------------- 1 | # x (m), y (m) 2 | 2.000000 2.000000 3 | 3.777778 2.000000 4 | 5.555556 2.000000 5 | 7.333333 2.000000 6 | 9.111111 2.000000 7 | 10.888889 2.000000 8 | 12.666667 2.000000 9 | 14.444444 2.000000 10 | 16.222222 2.000000 11 | 18.000000 2.000000 12 | 2.000000 3.777778 13 | 3.777778 3.777778 14 | 5.555556 3.777778 15 | 7.333333 3.777778 16 | 9.111111 3.777778 17 | 10.888889 3.777778 18 | 12.666667 3.777778 19 | 14.444444 3.777778 20 | 16.222222 3.777778 21 | 18.000000 3.777778 22 | 2.000000 5.555556 23 | 3.777778 5.555556 24 | 5.555556 5.555556 25 | 7.333333 5.555556 26 | 9.111111 5.555556 27 | 10.888889 5.555556 28 | 12.666667 5.555556 29 | 14.444444 5.555556 30 | 16.222222 5.555556 31 | 18.000000 5.555556 32 | 2.000000 7.333333 33 | 3.777778 7.333333 34 | 5.555556 7.333333 35 | 7.333333 7.333333 36 | 9.111111 7.333333 37 | 10.888889 7.333333 38 | 12.666667 7.333333 39 | 14.444444 7.333333 40 | 16.222222 7.333333 41 | 18.000000 7.333333 42 | 2.000000 9.111111 43 | 3.777778 9.111111 44 | 5.555556 9.111111 45 | 7.333333 9.111111 46 | 9.111111 9.111111 47 | 10.888889 9.111111 48 | 12.666667 9.111111 49 | 14.444444 9.111111 50 | 16.222222 9.111111 51 | 18.000000 9.111111 52 | 2.000000 10.888889 53 | 3.777778 10.888889 54 | 5.555556 10.888889 55 | 7.333333 10.888889 56 | 9.111111 10.888889 57 | 10.888889 10.888889 58 | 12.666667 10.888889 59 | 14.444444 10.888889 60 | 16.222222 10.888889 61 | 18.000000 10.888889 62 | 2.000000 12.666667 63 | 3.777778 12.666667 64 | 5.555556 12.666667 65 | 7.333333 12.666667 66 | 9.111111 12.666667 67 | 10.888889 12.666667 68 | 12.666667 12.666667 69 | 14.444444 12.666667 70 | 16.222222 12.666667 71 | 18.000000 12.666667 72 | 2.000000 14.444444 73 | 3.777778 14.444444 74 | 5.555556 14.444444 75 | 7.333333 14.444444 76 | 9.111111 14.444444 77 | 10.888889 14.444444 78 | 12.666667 14.444444 79 | 14.444444 14.444444 80 | 16.222222 14.444444 81 | 18.000000 14.444444 82 | 2.000000 16.222222 83 | 3.777778 16.222222 84 | 5.555556 16.222222 85 | 7.333333 16.222222 86 | 9.111111 16.222222 87 | 10.888889 16.222222 88 | 12.666667 16.222222 89 | 14.444444 16.222222 90 | 16.222222 16.222222 91 | 18.000000 16.222222 92 | 2.000000 18.000000 93 | 3.777778 18.000000 94 | 5.555556 18.000000 95 | 7.333333 18.000000 96 | 9.111111 18.000000 97 | 10.888889 18.000000 98 | 12.666667 18.000000 99 | 14.444444 18.000000 100 | 16.222222 18.000000 101 | 18.000000 18.000000 102 | -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_square/ground_truth_positions.txt: -------------------------------------------------------------------------------- 1 | # x (m), y (m) 2 | 2.000000 2.000000 3 | 3.777778 2.000000 4 | 5.555556 2.000000 5 | 7.333333 2.000000 6 | 9.111111 2.000000 7 | 10.888889 2.000000 8 | 12.666667 2.000000 9 | 14.444444 2.000000 10 | 16.222222 2.000000 11 | 18.000000 2.000000 12 | 2.000000 3.777778 13 | 3.777778 3.777778 14 | 5.555556 3.777778 15 | 7.333333 3.777778 16 | 9.111111 3.777778 17 | 10.888889 3.777778 18 | 12.666667 3.777778 19 | 14.444444 3.777778 20 | 16.222222 3.777778 21 | 18.000000 3.777778 22 | 2.000000 5.555556 23 | 3.777778 5.555556 24 | 5.555556 5.555556 25 | 7.333333 5.555556 26 | 9.111111 5.555556 27 | 10.888889 5.555556 28 | 12.666667 5.555556 29 | 14.444444 5.555556 30 | 16.222222 5.555556 31 | 18.000000 5.555556 32 | 2.000000 7.333333 33 | 3.777778 7.333333 34 | 5.555556 7.333333 35 | 7.333333 7.333333 36 | 9.111111 7.333333 37 | 10.888889 7.333333 38 | 12.666667 7.333333 39 | 14.444444 7.333333 40 | 16.222222 7.333333 41 | 18.000000 7.333333 42 | 2.000000 9.111111 43 | 3.777778 9.111111 44 | 5.555556 9.111111 45 | 7.333333 9.111111 46 | 9.111111 9.111111 47 | 10.888889 9.111111 48 | 12.666667 9.111111 49 | 14.444444 9.111111 50 | 16.222222 9.111111 51 | 18.000000 9.111111 52 | 2.000000 10.888889 53 | 3.777778 10.888889 54 | 5.555556 10.888889 55 | 7.333333 10.888889 56 | 9.111111 10.888889 57 | 10.888889 10.888889 58 | 12.666667 10.888889 59 | 14.444444 10.888889 60 | 16.222222 10.888889 61 | 18.000000 10.888889 62 | 2.000000 12.666667 63 | 3.777778 12.666667 64 | 5.555556 12.666667 65 | 7.333333 12.666667 66 | 9.111111 12.666667 67 | 10.888889 12.666667 68 | 12.666667 12.666667 69 | 14.444444 12.666667 70 | 16.222222 12.666667 71 | 18.000000 12.666667 72 | 2.000000 14.444444 73 | 3.777778 14.444444 74 | 5.555556 14.444444 75 | 7.333333 14.444444 76 | 9.111111 14.444444 77 | 10.888889 14.444444 78 | 12.666667 14.444444 79 | 14.444444 14.444444 80 | 16.222222 14.444444 81 | 18.000000 14.444444 82 | 2.000000 16.222222 83 | 3.777778 16.222222 84 | 5.555556 16.222222 85 | 7.333333 16.222222 86 | 9.111111 16.222222 87 | 10.888889 16.222222 88 | 12.666667 16.222222 89 | 14.444444 16.222222 90 | 16.222222 16.222222 91 | 18.000000 16.222222 92 | 2.000000 18.000000 93 | 3.777778 18.000000 94 | 5.555556 18.000000 95 | 7.333333 18.000000 96 | 9.111111 18.000000 97 | 10.888889 18.000000 98 | 12.666667 18.000000 99 | 14.444444 18.000000 100 | 16.222222 18.000000 101 | 18.000000 18.000000 102 | -------------------------------------------------------------------------------- /core/coords/frames.py: -------------------------------------------------------------------------------- 1 | """Coordinate frame definitions for indoor positioning and navigation. 2 | 3 | This module defines the coordinate frames used in indoor positioning: 4 | - ENU (East-North-Up): Local tangent plane with origin at reference point 5 | - NED (North-East-Down): Local tangent plane (aerospace convention) 6 | - ECEF (Earth-Centered Earth-Fixed): Global Cartesian frame 7 | - LLH (Latitude-Longitude-Height): Geodetic coordinates 8 | - Body: Vehicle/sensor frame (forward-right-down) 9 | - Map: World frame for indoor environments 10 | 11 | Reference: Chapter 2, Section 2.2 - Coordinate Frames 12 | """ 13 | 14 | from enum import Enum 15 | from typing import NamedTuple 16 | 17 | 18 | class FrameType(Enum): 19 | """Enumeration of coordinate frame types. 20 | 21 | Attributes: 22 | ENU: East-North-Up local tangent plane frame. 23 | NED: North-East-Down local tangent plane frame. 24 | ECEF: Earth-Centered Earth-Fixed Cartesian frame. 25 | LLH: Latitude-Longitude-Height geodetic frame. 26 | BODY: Vehicle/sensor body frame (forward-right-down). 27 | MAP: World frame for indoor positioning. 28 | """ 29 | 30 | ENU = "enu" 31 | NED = "ned" 32 | ECEF = "ecef" 33 | LLH = "llh" 34 | BODY = "body" 35 | MAP = "map" 36 | 37 | 38 | class Frame(NamedTuple): 39 | """Representation of a coordinate frame. 40 | 41 | Attributes: 42 | frame_type: Type of coordinate frame. 43 | description: Human-readable description of the frame. 44 | """ 45 | 46 | frame_type: FrameType 47 | description: str 48 | 49 | def __repr__(self) -> str: 50 | """Return string representation of frame.""" 51 | return f"Frame({self.frame_type.value}: {self.description})" 52 | 53 | 54 | # Common frame definitions 55 | FRAME_ENU = Frame( 56 | FrameType.ENU, 57 | "East-North-Up local tangent plane (x=East, y=North, z=Up)", 58 | ) 59 | 60 | FRAME_NED = Frame( 61 | FrameType.NED, 62 | "North-East-Down local tangent plane (x=North, y=East, z=Down)", 63 | ) 64 | 65 | FRAME_ECEF = Frame( 66 | FrameType.ECEF, 67 | "Earth-Centered Earth-Fixed (x=0°E 0°N, y=90°E 0°N, z=North Pole)", 68 | ) 69 | 70 | FRAME_LLH = Frame( 71 | FrameType.LLH, 72 | "Latitude-Longitude-Height geodetic coordinates", 73 | ) 74 | 75 | FRAME_BODY = Frame( 76 | FrameType.BODY, 77 | "Body frame (x=forward, y=right, z=down)", 78 | ) 79 | 80 | FRAME_MAP = Frame( 81 | FrameType.MAP, 82 | "Map/world frame for indoor positioning", 83 | ) 84 | -------------------------------------------------------------------------------- /docs/CH2_QUICK_REFERENCE.md: -------------------------------------------------------------------------------- 1 | # Chapter 2: Quick Reference Guide 2 | 3 | ## Equation → Code Mapping 4 | 5 | ### Coordinate Transformations 6 | 7 | | Equation | Description | Function | File | Tests Pass | 8 | |----------|-------------|----------|------|------------| 9 | | **Eq. (2.1)** | LLH → ECEF | `llh_to_ecef()` | `core/coords/transforms.py` | ✅ 5/5 | 10 | | **Eq. (2.2)** | ECEF → LLH | `ecef_to_llh()` | `core/coords/transforms.py` | ✅ 8/8 | 11 | | **Eq. (2.3)** | ECEF → ENU | `ecef_to_enu()` | `core/coords/transforms.py` | ✅ 9/9 | 12 | | **Eq. (2.4)** | ENU → ECEF | `enu_to_ecef()` | `core/coords/transforms.py` | ✅ 7/7 | 13 | 14 | ### Rotation Representations 15 | 16 | | Equation | Description | Function | File | Tests Pass | 17 | |----------|-------------|----------|------|------------| 18 | | **Eq. (2.5)** | Euler → Rotation Matrix | `euler_to_rotation_matrix()` | `core/coords/rotations.py` | ✅ 11/11 | 19 | | **Eq. (2.6)** | Rotation Matrix → Euler | `rotation_matrix_to_euler()` | `core/coords/rotations.py` | ✅ 11/11 | 20 | | **Eq. (2.7)** | Euler → Quaternion | `euler_to_quat()` | `core/coords/rotations.py` | ✅ 10/10 | 21 | | **Eq. (2.8)** | Quaternion → Euler | `quat_to_euler()` | `core/coords/rotations.py` | ✅ 9/9 | 22 | | **Eq. (2.9)** | Quaternion → Rotation Matrix | `quat_to_rotation_matrix()` | `core/coords/rotations.py` | ✅ 9/9 | 23 | | **Eq. (2.10)** | Rotation Matrix → Quaternion | `rotation_matrix_to_quat()` | `core/coords/rotations.py` | ✅ 10/10 | 24 | 25 | --- 26 | 27 | ## Summary Statistics 28 | 29 | | Metric | Value | 30 | |--------|-------| 31 | | **Total Equations Mapped** | 10 | 32 | | **Functions Implemented** | 10 | 33 | | **Test Cases** | 47 | 34 | | **Subtests** | 27 | 35 | | **Pass Rate** | 100% ✅ | 36 | | **Documentation Files** | 4 | 37 | 38 | --- 39 | 40 | ## Quick Commands 41 | 42 | ```bash 43 | # Run all Chapter 2 tests 44 | pytest tests/core/coords/ -v 45 | 46 | # Run examples 47 | python ch2_coords/example_coordinate_transforms.py 48 | 49 | # Search for equation 50 | grep -r "Eq. (2.1)" core/ ch2_coords/ 51 | 52 | # View documentation 53 | cat ch2_coords/README.md 54 | cat docs/ch2_equation_mapping.md 55 | ``` 56 | 57 | --- 58 | 59 | ## Documentation Files 60 | 61 | 1. **`ch2_coords/README.md`** - User guide with examples 62 | 2. **`docs/equation_index.yml`** - Machine-readable mapping 63 | 3. **`docs/ch2_equation_mapping.md`** - Detailed technical reference 64 | 4. **`CHAPTER_2_MAPPING_SUMMARY.md`** - Executive summary 65 | 66 | --- 67 | 68 | **Last Updated**: December 11, 2025 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /core/fingerprinting/__init__.py: -------------------------------------------------------------------------------- 1 | """Fingerprint-based localization algorithms for Chapter 5. 2 | 3 | This module implements deterministic and probabilistic fingerprinting methods 4 | for indoor positioning, as described in Chapter 5 of the book: 5 | Principles of Indoor Positioning and Indoor Navigation. 6 | 7 | Main components: 8 | - FingerprintDatabase: Core data structure for radio maps 9 | - load/save functions: I/O utilities for databases 10 | - validate_database: Data quality checks 11 | - nn_localize, knn_localize: Deterministic fingerprinting (Eqs. 5.1-5.2) 12 | - map_localize, posterior_mean_localize: Probabilistic (Eqs. 5.3-5.5) 13 | 14 | Example usage: 15 | >>> from core.fingerprinting import ( 16 | ... FingerprintDatabase, 17 | ... load_fingerprint_database, 18 | ... nn_localize, 19 | ... knn_localize, 20 | ... fit_gaussian_naive_bayes, 21 | ... map_localize 22 | ... ) 23 | >>> db = load_fingerprint_database('data/sim/ch5_wifi_fingerprint_grid') 24 | >>> z_query = np.array([-50, -60, -70]) 25 | >>> 26 | >>> # Deterministic 27 | >>> x_hat_nn = nn_localize(z_query, db, floor_id=0) 28 | >>> 29 | >>> # Probabilistic 30 | >>> model = fit_gaussian_naive_bayes(db) 31 | >>> x_hat_map = map_localize(z_query, model, floor_id=0) 32 | 33 | Author: Navigation Engineer 34 | Date: 2024 35 | """ 36 | 37 | from .dataset import ( 38 | load_fingerprint_database, 39 | print_database_summary, 40 | save_fingerprint_database, 41 | validate_database, 42 | ) 43 | from .deterministic import distance, knn_localize, nn_localize, pairwise_distances 44 | from .pattern_recognition import LinearRegressionLocalizer 45 | from .probabilistic import ( 46 | NaiveBayesFingerprintModel, 47 | fit_gaussian_naive_bayes, 48 | log_likelihood, 49 | log_posterior, 50 | map_localize, 51 | posterior_mean_localize, 52 | ) 53 | from .types import Fingerprint, FingerprintDatabase, Location 54 | 55 | __all__ = [ 56 | # Core types 57 | "FingerprintDatabase", 58 | "Location", 59 | "Fingerprint", 60 | # Dataset I/O 61 | "load_fingerprint_database", 62 | "save_fingerprint_database", 63 | "validate_database", 64 | "print_database_summary", 65 | # Deterministic methods 66 | "distance", 67 | "pairwise_distances", 68 | "nn_localize", 69 | "knn_localize", 70 | # Probabilistic methods 71 | "NaiveBayesFingerprintModel", 72 | "fit_gaussian_naive_bayes", 73 | "log_likelihood", 74 | "log_posterior", 75 | "map_localize", 76 | "posterior_mean_localize", 77 | # Pattern recognition methods 78 | "LinearRegressionLocalizer", 79 | ] 80 | 81 | __version__ = "0.1.0" 82 | 83 | -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_linear/tdoa_diffs.txt: -------------------------------------------------------------------------------- 1 | # TDOA range differences relative to beacon 0 (m) 2 | -1.948892 -4.690255 -7.865520 3 | -1.128680 -3.431959 -6.700428 4 | -0.162062 -2.086446 -4.965126 5 | 0.554921 -0.677411 -3.062811 6 | 1.484437 0.994469 -1.035060 7 | 2.148511 2.312231 1.030484 8 | 2.405339 3.653479 3.035925 9 | 2.817217 4.754801 4.968226 10 | 3.093376 5.553817 6.435806 11 | 3.392019 6.031154 7.857760 12 | -2.100880 -5.294975 -8.761426 13 | -1.257646 -3.938687 -7.607597 14 | -0.240088 -2.460532 -5.544011 15 | 0.786922 -0.612344 -3.594441 16 | 1.681719 1.073649 -1.327146 17 | 2.418701 2.896804 1.275242 18 | 3.021804 4.433085 3.569053 19 | 3.181388 5.433926 5.889828 20 | 3.412762 6.259782 7.283704 21 | 3.494251 6.643834 8.884970 22 | -2.584036 -6.046639 -9.563075 23 | -1.745691 -5.111284 -8.571492 24 | -0.473191 -2.935911 -6.351438 25 | 1.023774 -0.902271 -4.321759 26 | 2.197310 1.472404 -1.497096 27 | 2.923611 3.664949 1.250482 28 | 3.270183 5.260915 4.331419 29 | 3.368956 6.364815 6.609092 30 | 3.833064 6.918848 8.638024 31 | 3.820447 7.403802 9.887900 32 | -3.229294 -7.137772 -10.985486 33 | -2.524237 -6.027045 -9.774749 34 | -0.506866 -3.860136 -7.778621 35 | 1.437787 -1.085858 -4.714459 36 | 3.036480 1.787661 -1.611278 37 | 3.314220 4.267110 1.627501 38 | 3.674847 6.338588 4.880950 39 | 3.571754 7.094154 7.767229 40 | 3.806465 7.367872 9.745026 41 | 3.719165 7.778630 10.857872 42 | -3.911130 -7.998479 -11.732833 43 | -3.382510 -7.246261 -11.422823 44 | -0.838751 -4.520122 -8.579989 45 | 2.224512 -1.300118 -5.002547 46 | 3.679760 2.231840 -1.649641 47 | 3.773945 5.659470 1.847688 48 | 3.805467 7.571288 5.272263 49 | 3.887973 8.014735 8.671454 50 | 3.939516 7.984961 11.181681 51 | 3.970839 7.937366 11.823637 52 | -3.885561 -7.881502 -11.914908 53 | -3.324588 -7.369850 -11.403549 54 | -0.675813 -4.589050 -8.715840 55 | 2.254959 -1.277207 -5.101203 56 | 3.789079 1.987785 -1.706629 57 | 3.977254 5.419653 1.781697 58 | 3.917498 7.604231 5.289206 59 | 4.231676 7.840630 8.496905 60 | 4.189641 7.956924 11.304525 61 | 3.878388 8.021430 11.871495 62 | -3.240731 -7.186682 -11.079938 63 | -2.282978 -5.729674 -9.791821 64 | -0.451226 -3.737333 -7.738170 65 | 1.423649 -1.197605 -4.819008 66 | 2.853901 1.898179 -1.623431 67 | 3.353268 4.488502 1.734868 68 | 3.737590 6.324737 4.853771 69 | 3.923888 7.059918 7.506303 70 | 3.764398 7.720802 10.012356 71 | 3.965946 7.719879 10.740011 72 | -2.591880 -6.049182 -9.924169 73 | -1.655398 -4.892134 -8.575517 74 | -0.263942 -3.013680 -6.539361 75 | 0.936969 -0.920800 -4.162547 76 | 2.219314 1.358453 -1.473065 77 | 2.630053 3.656528 1.581084 78 | 3.485887 5.249217 4.359639 79 | 3.473367 6.369293 6.645053 80 | 3.652830 6.883347 8.485875 81 | 3.751661 7.032542 9.618963 82 | -2.217425 -5.213134 -8.660199 83 | -1.188399 -4.152619 -7.574953 84 | -0.293983 -2.672046 -5.736692 85 | 0.853942 -0.582599 -3.798225 86 | 1.708421 1.199714 -1.230033 87 | 2.509374 2.983444 1.319274 88 | 2.718857 4.456323 3.657899 89 | 3.212442 5.372010 5.732711 90 | 3.426016 6.257070 7.388750 91 | 3.616226 6.714040 8.847892 92 | -1.764762 -4.524765 -7.801622 93 | -1.008730 -3.401826 -6.642024 94 | -0.210076 -2.150375 -5.040083 95 | 0.642543 -0.595426 -3.023317 96 | 1.497113 1.055451 -1.159451 97 | 2.095435 2.356342 1.043565 98 | 2.517540 3.912213 3.082224 99 | 2.771948 4.768177 4.847120 100 | 3.178965 5.567980 6.615579 101 | 3.336303 6.058910 8.000934 102 | -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_nlos/tdoa_diffs.txt: -------------------------------------------------------------------------------- 1 | # TDOA range differences relative to beacon 0 (m) 2 | -15.477447 -22.757635 -15.269559 3 | -12.156444 -19.919990 -14.213520 4 | -8.624426 -17.137902 -12.891975 5 | -5.306436 -14.491377 -11.770278 6 | -1.675107 -11.702499 -10.817594 7 | 1.839826 -9.271795 -9.999748 8 | 5.094851 -6.726246 -9.278455 9 | 8.611465 -4.291896 -8.534963 10 | 12.028185 -2.055088 -8.055083 11 | 15.356095 -0.093362 -7.365618 12 | -14.110419 -20.009814 -12.047248 13 | -11.278021 -17.452772 -11.432585 14 | -8.180621 -14.918883 -10.229157 15 | -4.982894 -12.225862 -9.537850 16 | -1.712269 -9.793102 -8.838970 17 | 1.658263 -7.145597 -7.967661 18 | 5.099415 -4.562802 -7.404738 19 | 8.194020 -2.255176 -6.644579 20 | 11.322767 0.064394 -6.490411 21 | 14.069215 2.014349 -5.738958 22 | -12.924199 -17.151624 -8.425910 23 | -10.494244 -15.217425 -8.228239 24 | -7.728924 -12.387096 -7.328738 25 | -4.668945 -10.024897 -7.136757 26 | -1.547631 -7.417640 -6.478750 27 | 1.579222 -4.805633 -6.039160 28 | 4.606132 -2.352682 -5.233036 29 | 7.465859 0.086383 -4.984395 30 | 10.603223 2.260757 -4.491067 31 | 13.008335 4.437655 -4.168121 32 | -11.832059 -14.530585 -5.289534 33 | -9.759879 -12.391570 -4.909724 34 | -6.975828 -9.984480 -4.717664 35 | -4.347658 -7.522242 -4.180994 36 | -1.271893 -5.053702 -3.896583 37 | 1.291042 -2.706063 -3.570178 38 | 4.247472 0.019678 -3.194987 39 | 6.765906 2.300112 -2.937304 40 | 9.494115 4.436687 -2.867718 41 | 11.652246 6.705747 -2.634092 42 | -10.880785 -11.856943 -1.636276 43 | -8.726439 -9.566984 -1.746906 44 | -6.435914 -7.223925 -1.442348 45 | -4.021565 -5.007298 -1.172551 46 | -1.397993 -2.447599 -1.204310 47 | 1.163333 0.136386 -1.111800 48 | 3.751275 2.445320 -1.090613 49 | 6.317825 4.987226 -1.030188 50 | 8.697602 7.125352 -1.088726 51 | 10.828583 9.078170 -0.878669 52 | -9.974968 -9.134216 1.667645 53 | -7.938327 -7.096083 1.597048 54 | -5.731083 -4.728853 1.527695 55 | -3.659360 -2.451508 1.593430 56 | -1.177153 -0.177496 1.364380 57 | 1.255122 2.410727 1.224846 58 | 3.531548 5.011142 1.127452 59 | 6.119634 7.377121 0.817370 60 | 8.217537 9.691804 0.898418 61 | 9.855885 11.767983 0.894691 62 | -9.194668 -6.783482 5.060854 63 | -7.328710 -4.346480 5.010834 64 | -5.301105 -2.218101 4.585445 65 | -3.373403 -0.091516 4.245322 66 | -1.122714 2.539801 3.906006 67 | 0.998333 4.998314 3.688415 68 | 3.321821 7.548300 3.331913 69 | 5.498958 9.909452 2.826263 70 | 7.262138 12.537336 2.957422 71 | 9.250200 14.443008 2.395231 72 | -8.495584 -4.235311 8.568310 73 | -6.765845 -2.212436 8.191681 74 | -4.851972 0.105922 7.721829 75 | -3.136668 2.336028 7.020991 76 | -0.983733 4.739705 6.358470 77 | 0.743770 7.457242 6.020845 78 | 3.202753 10.015072 5.555554 79 | 4.902567 12.661647 4.954651 80 | 6.784883 15.011094 4.504516 81 | 8.503089 16.985252 4.045339 82 | -7.995682 -2.018225 12.195010 83 | -6.180460 -0.067602 11.227477 84 | -4.596409 2.089156 10.435822 85 | -2.725964 4.721083 9.365475 86 | -0.955377 7.087690 8.743060 87 | 1.018747 9.695769 8.100908 88 | 2.606559 12.377637 7.411398 89 | 4.586969 14.902462 6.779751 90 | 6.307706 17.660784 6.185422 91 | 7.959908 19.994304 5.902431 92 | -7.356047 0.035272 15.359025 93 | -5.805212 2.114125 14.080148 94 | -4.235981 4.228206 12.899565 95 | -2.569986 6.612454 11.939890 96 | -0.782183 9.171248 10.751027 97 | 0.906502 11.585081 9.945852 98 | 2.558224 14.554334 9.140862 99 | 4.129736 17.151517 8.337510 100 | 5.882493 19.963057 7.897389 101 | 7.363110 22.561812 7.467703 102 | -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_square/tdoa_diffs.txt: -------------------------------------------------------------------------------- 1 | # TDOA range differences relative to beacon 0 (m) 2 | -15.477447 -22.757635 -15.269559 3 | -12.156444 -19.919990 -14.213520 4 | -8.624426 -17.137902 -12.891975 5 | -5.306436 -14.491377 -11.770278 6 | -1.675107 -11.702499 -10.817594 7 | 1.839826 -9.271795 -9.999748 8 | 5.094851 -6.726246 -9.278455 9 | 8.611465 -4.291896 -8.534963 10 | 12.028185 -2.055088 -8.055083 11 | 15.356095 -0.093362 -7.365618 12 | -14.110419 -20.009814 -12.047248 13 | -11.278021 -17.452772 -11.432585 14 | -8.180621 -14.918883 -10.229157 15 | -4.982894 -12.225862 -9.537850 16 | -1.712269 -9.793102 -8.838970 17 | 1.658263 -7.145597 -7.967661 18 | 5.099415 -4.562802 -7.404738 19 | 8.194020 -2.255176 -6.644579 20 | 11.322767 0.064394 -6.490411 21 | 14.069215 2.014349 -5.738958 22 | -12.924199 -17.151624 -8.425910 23 | -10.494244 -15.217425 -8.228239 24 | -7.728924 -12.387096 -7.328738 25 | -4.668945 -10.024897 -7.136757 26 | -1.547631 -7.417640 -6.478750 27 | 1.579222 -4.805633 -6.039160 28 | 4.606132 -2.352682 -5.233036 29 | 7.465859 0.086383 -4.984395 30 | 10.603223 2.260757 -4.491067 31 | 13.008335 4.437655 -4.168121 32 | -11.832059 -14.530585 -5.289534 33 | -9.759879 -12.391570 -4.909724 34 | -6.975828 -9.984480 -4.717664 35 | -4.347658 -7.522242 -4.180994 36 | -1.271893 -5.053702 -3.896583 37 | 1.291042 -2.706063 -3.570178 38 | 4.247472 0.019678 -3.194987 39 | 6.765906 2.300112 -2.937304 40 | 9.494115 4.436687 -2.867718 41 | 11.652246 6.705747 -2.634092 42 | -10.880785 -11.856943 -1.636276 43 | -8.726439 -9.566984 -1.746906 44 | -6.435914 -7.223925 -1.442348 45 | -4.021565 -5.007298 -1.172551 46 | -1.397993 -2.447599 -1.204310 47 | 1.163333 0.136386 -1.111800 48 | 3.751275 2.445320 -1.090613 49 | 6.317825 4.987226 -1.030188 50 | 8.697602 7.125352 -1.088726 51 | 10.828583 9.078170 -0.878669 52 | -9.974968 -9.134216 1.667645 53 | -7.938327 -7.096083 1.597048 54 | -5.731083 -4.728853 1.527695 55 | -3.659360 -2.451508 1.593430 56 | -1.177153 -0.177496 1.364380 57 | 1.255122 2.410727 1.224846 58 | 3.531548 5.011142 1.127452 59 | 6.119634 7.377121 0.817370 60 | 8.217537 9.691804 0.898418 61 | 9.855885 11.767983 0.894691 62 | -9.194668 -6.783482 5.060854 63 | -7.328710 -4.346480 5.010834 64 | -5.301105 -2.218101 4.585445 65 | -3.373403 -0.091516 4.245322 66 | -1.122714 2.539801 3.906006 67 | 0.998333 4.998314 3.688415 68 | 3.321821 7.548300 3.331913 69 | 5.498958 9.909452 2.826263 70 | 7.262138 12.537336 2.957422 71 | 9.250200 14.443008 2.395231 72 | -8.495584 -4.235311 8.568310 73 | -6.765845 -2.212436 8.191681 74 | -4.851972 0.105922 7.721829 75 | -3.136668 2.336028 7.020991 76 | -0.983733 4.739705 6.358470 77 | 0.743770 7.457242 6.020845 78 | 3.202753 10.015072 5.555554 79 | 4.902567 12.661647 4.954651 80 | 6.784883 15.011094 4.504516 81 | 8.503089 16.985252 4.045339 82 | -7.995682 -2.018225 12.195010 83 | -6.180460 -0.067602 11.227477 84 | -4.596409 2.089156 10.435822 85 | -2.725964 4.721083 9.365475 86 | -0.955377 7.087690 8.743060 87 | 1.018747 9.695769 8.100908 88 | 2.606559 12.377637 7.411398 89 | 4.586969 14.902462 6.779751 90 | 6.307706 17.660784 6.185422 91 | 7.959908 19.994304 5.902431 92 | -7.356047 0.035272 15.359025 93 | -5.805212 2.114125 14.080148 94 | -4.235981 4.228206 12.899565 95 | -2.569986 6.612454 11.939890 96 | -0.782183 9.171248 10.751027 97 | 0.906502 11.585081 9.945852 98 | 2.558224 14.554334 9.140862 99 | 4.129736 17.151517 8.337510 100 | 5.882493 19.963057 7.897389 101 | 7.363110 22.561812 7.467703 102 | -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_optimal/tdoa_diffs.txt: -------------------------------------------------------------------------------- 1 | # TDOA range differences relative to beacon 0 (m) 2 | -0.195104 11.321286 11.464288 3 | -1.043456 9.277329 11.455939 4 | -1.975475 6.808593 11.679433 5 | -3.299004 4.046475 11.713196 6 | -4.442274 1.393662 11.352046 7 | -5.800242 -1.555191 9.902741 8 | -7.471480 -4.242252 7.427269 9 | -8.866947 -6.808354 4.827938 10 | -10.240311 -9.248426 2.142639 11 | -11.377753 -11.544866 -0.020544 12 | 0.964740 12.456405 10.221249 13 | 0.035687 10.241663 9.976457 14 | -1.061064 7.469955 10.094224 15 | -2.341595 4.601650 9.503983 16 | -3.755197 1.389736 8.563834 17 | -5.217574 -1.573709 7.196722 18 | -6.691968 -4.473114 4.952133 19 | -8.496510 -7.424461 2.654553 20 | -10.086274 -10.030939 -0.205017 21 | -11.600244 -12.542121 -2.211097 22 | 2.037755 13.689688 9.052502 23 | 1.027001 10.772237 8.462291 24 | -0.109679 8.181854 8.288669 25 | -1.302384 4.835371 7.123867 26 | -2.705459 1.623674 6.062621 27 | -4.308165 -1.575651 4.336653 28 | -6.138720 -4.833497 2.559752 29 | -8.151549 -7.911780 -0.032853 30 | -9.720158 -10.943067 -2.425610 31 | -11.563073 -13.484801 -4.793034 32 | 3.218267 14.741469 7.276798 33 | 2.121192 11.756684 6.881659 34 | 1.288209 8.553159 6.027187 35 | -0.082225 5.161423 5.225630 36 | -1.326727 1.671558 3.834444 37 | -3.345757 -1.948338 2.111764 38 | -5.159153 -5.121514 0.082053 39 | -7.494718 -8.593951 -2.337660 40 | -9.547719 -11.963848 -5.082040 41 | -11.831227 -14.770296 -7.579714 42 | 4.475887 15.685614 6.003792 43 | 3.721286 12.473237 5.128932 44 | 2.681456 9.039275 4.445040 45 | 1.372959 5.311442 3.464247 46 | -0.085154 1.837299 1.879270 47 | -1.920247 -1.634355 0.089518 48 | -3.979752 -5.340541 -2.096381 49 | -6.223547 -8.711973 -4.530246 50 | -8.705202 -12.320379 -7.308691 51 | -11.341057 -15.858637 -9.952148 52 | 5.888360 15.802591 4.434813 53 | 5.287439 12.349648 3.639976 54 | 4.468059 8.970347 2.685523 55 | 3.120734 5.334353 1.648264 56 | 1.794907 1.593245 0.051541 57 | 0.053803 -1.874172 -1.747213 58 | -2.150394 -5.307598 -3.796765 59 | -4.256179 -8.886079 -6.328461 60 | -6.946846 -12.348417 -8.694078 61 | -10.046604 -15.774574 -11.291194 62 | 7.335753 14.692560 3.053423 63 | 6.857503 12.054055 2.369535 64 | 6.192602 8.675962 1.218884 65 | 5.044828 5.049676 -0.020110 66 | 3.801503 1.782076 -1.488517 67 | 2.004101 -1.726946 -3.091678 68 | 0.044781 -5.135365 -5.086318 69 | -2.293830 -8.628187 -7.447340 70 | -5.094733 -11.610919 -9.309762 71 | -7.455523 -14.829046 -11.826498 72 | 8.801959 13.687145 1.919359 73 | 8.503436 10.991388 1.072124 74 | 8.097733 8.104085 0.102584 75 | 7.136936 4.816843 -1.243045 76 | 6.162097 1.509723 -2.758899 77 | 4.243828 -1.584072 -4.178297 78 | 2.603109 -4.845196 -5.938153 79 | -0.048975 -7.907302 -7.995054 80 | -2.514250 -10.978568 -9.963902 81 | -4.859811 -13.856060 -11.834019 82 | 10.088649 12.538246 1.082022 83 | 10.200267 10.027731 -0.086232 84 | 9.872008 7.258441 -1.085423 85 | 9.541220 4.631395 -2.515596 86 | 8.637119 1.515802 -3.704666 87 | 7.238712 -1.487070 -5.124859 88 | 4.820880 -4.449875 -6.774815 89 | 2.521511 -7.486377 -8.489530 90 | 0.022312 -10.033651 -10.195304 91 | -2.237814 -12.471915 -11.488629 92 | 11.440531 11.486776 0.076682 93 | 11.585848 9.307462 -0.995011 94 | 11.643377 6.744663 -2.062389 95 | 11.651743 4.128460 -3.110435 96 | 11.403702 1.454644 -4.605645 97 | 9.979981 -1.511080 -5.917477 98 | 7.503845 -3.983518 -7.389559 99 | 4.754650 -6.794978 -8.960034 100 | 2.354632 -9.234263 -10.186942 101 | 0.018036 -11.517110 -11.328875 102 | -------------------------------------------------------------------------------- /data/sim/ch3_estimator_nonlinear/time.txt: -------------------------------------------------------------------------------- 1 | # time (s) 2 | 0.000 3 | 0.100 4 | 0.200 5 | 0.300 6 | 0.400 7 | 0.500 8 | 0.600 9 | 0.700 10 | 0.800 11 | 0.900 12 | 1.000 13 | 1.100 14 | 1.200 15 | 1.300 16 | 1.400 17 | 1.500 18 | 1.600 19 | 1.700 20 | 1.800 21 | 1.900 22 | 2.000 23 | 2.100 24 | 2.200 25 | 2.300 26 | 2.400 27 | 2.500 28 | 2.600 29 | 2.700 30 | 2.800 31 | 2.900 32 | 3.000 33 | 3.100 34 | 3.200 35 | 3.300 36 | 3.400 37 | 3.500 38 | 3.600 39 | 3.700 40 | 3.800 41 | 3.900 42 | 4.000 43 | 4.100 44 | 4.200 45 | 4.300 46 | 4.400 47 | 4.500 48 | 4.600 49 | 4.700 50 | 4.800 51 | 4.900 52 | 5.000 53 | 5.100 54 | 5.200 55 | 5.300 56 | 5.400 57 | 5.500 58 | 5.600 59 | 5.700 60 | 5.800 61 | 5.900 62 | 6.000 63 | 6.100 64 | 6.200 65 | 6.300 66 | 6.400 67 | 6.500 68 | 6.600 69 | 6.700 70 | 6.800 71 | 6.900 72 | 7.000 73 | 7.100 74 | 7.200 75 | 7.300 76 | 7.400 77 | 7.500 78 | 7.600 79 | 7.700 80 | 7.800 81 | 7.900 82 | 8.000 83 | 8.100 84 | 8.200 85 | 8.300 86 | 8.400 87 | 8.500 88 | 8.600 89 | 8.700 90 | 8.800 91 | 8.900 92 | 9.000 93 | 9.100 94 | 9.200 95 | 9.300 96 | 9.400 97 | 9.500 98 | 9.600 99 | 9.700 100 | 9.800 101 | 9.900 102 | 10.000 103 | 10.100 104 | 10.200 105 | 10.300 106 | 10.400 107 | 10.500 108 | 10.600 109 | 10.700 110 | 10.800 111 | 10.900 112 | 11.000 113 | 11.100 114 | 11.200 115 | 11.300 116 | 11.400 117 | 11.500 118 | 11.600 119 | 11.700 120 | 11.800 121 | 11.900 122 | 12.000 123 | 12.100 124 | 12.200 125 | 12.300 126 | 12.400 127 | 12.500 128 | 12.600 129 | 12.700 130 | 12.800 131 | 12.900 132 | 13.000 133 | 13.100 134 | 13.200 135 | 13.300 136 | 13.400 137 | 13.500 138 | 13.600 139 | 13.700 140 | 13.800 141 | 13.900 142 | 14.000 143 | 14.100 144 | 14.200 145 | 14.300 146 | 14.400 147 | 14.500 148 | 14.600 149 | 14.700 150 | 14.800 151 | 14.900 152 | 15.000 153 | 15.100 154 | 15.200 155 | 15.300 156 | 15.400 157 | 15.500 158 | 15.600 159 | 15.700 160 | 15.800 161 | 15.900 162 | 16.000 163 | 16.100 164 | 16.200 165 | 16.300 166 | 16.400 167 | 16.500 168 | 16.600 169 | 16.700 170 | 16.800 171 | 16.900 172 | 17.000 173 | 17.100 174 | 17.200 175 | 17.300 176 | 17.400 177 | 17.500 178 | 17.600 179 | 17.700 180 | 17.800 181 | 17.900 182 | 18.000 183 | 18.100 184 | 18.200 185 | 18.300 186 | 18.400 187 | 18.500 188 | 18.600 189 | 18.700 190 | 18.800 191 | 18.900 192 | 19.000 193 | 19.100 194 | 19.200 195 | 19.300 196 | 19.400 197 | 19.500 198 | 19.600 199 | 19.700 200 | 19.800 201 | 19.900 202 | 20.000 203 | 20.100 204 | 20.200 205 | 20.300 206 | 20.400 207 | 20.500 208 | 20.600 209 | 20.700 210 | 20.800 211 | 20.900 212 | 21.000 213 | 21.100 214 | 21.200 215 | 21.300 216 | 21.400 217 | 21.500 218 | 21.600 219 | 21.700 220 | 21.800 221 | 21.900 222 | 22.000 223 | 22.100 224 | 22.200 225 | 22.300 226 | 22.400 227 | 22.500 228 | 22.600 229 | 22.700 230 | 22.800 231 | 22.900 232 | 23.000 233 | 23.100 234 | 23.200 235 | 23.300 236 | 23.400 237 | 23.500 238 | 23.600 239 | 23.700 240 | 23.800 241 | 23.900 242 | 24.000 243 | 24.100 244 | 24.200 245 | 24.300 246 | 24.400 247 | 24.500 248 | 24.600 249 | 24.700 250 | 24.800 251 | 24.900 252 | 25.000 253 | 25.100 254 | 25.200 255 | 25.300 256 | 25.400 257 | 25.500 258 | 25.600 259 | 25.700 260 | 25.800 261 | 25.900 262 | 26.000 263 | 26.100 264 | 26.200 265 | 26.300 266 | 26.400 267 | 26.500 268 | 26.600 269 | 26.700 270 | 26.800 271 | 26.900 272 | 27.000 273 | 27.100 274 | 27.200 275 | 27.300 276 | 27.400 277 | 27.500 278 | 27.600 279 | 27.700 280 | 27.800 281 | 27.900 282 | 28.000 283 | 28.100 284 | 28.200 285 | 28.300 286 | 28.400 287 | 28.500 288 | 28.600 289 | 28.700 290 | 28.800 291 | 28.900 292 | 29.000 293 | 29.100 294 | 29.200 295 | 29.300 296 | 29.400 297 | 29.500 298 | 29.600 299 | 29.700 300 | 29.800 301 | 29.900 302 | -------------------------------------------------------------------------------- /data/sim/ch3_estimator_high_nonlinear/time.txt: -------------------------------------------------------------------------------- 1 | # time (s) 2 | 0.000 3 | 0.100 4 | 0.200 5 | 0.300 6 | 0.400 7 | 0.500 8 | 0.600 9 | 0.700 10 | 0.800 11 | 0.900 12 | 1.000 13 | 1.100 14 | 1.200 15 | 1.300 16 | 1.400 17 | 1.500 18 | 1.600 19 | 1.700 20 | 1.800 21 | 1.900 22 | 2.000 23 | 2.100 24 | 2.200 25 | 2.300 26 | 2.400 27 | 2.500 28 | 2.600 29 | 2.700 30 | 2.800 31 | 2.900 32 | 3.000 33 | 3.100 34 | 3.200 35 | 3.300 36 | 3.400 37 | 3.500 38 | 3.600 39 | 3.700 40 | 3.800 41 | 3.900 42 | 4.000 43 | 4.100 44 | 4.200 45 | 4.300 46 | 4.400 47 | 4.500 48 | 4.600 49 | 4.700 50 | 4.800 51 | 4.900 52 | 5.000 53 | 5.100 54 | 5.200 55 | 5.300 56 | 5.400 57 | 5.500 58 | 5.600 59 | 5.700 60 | 5.800 61 | 5.900 62 | 6.000 63 | 6.100 64 | 6.200 65 | 6.300 66 | 6.400 67 | 6.500 68 | 6.600 69 | 6.700 70 | 6.800 71 | 6.900 72 | 7.000 73 | 7.100 74 | 7.200 75 | 7.300 76 | 7.400 77 | 7.500 78 | 7.600 79 | 7.700 80 | 7.800 81 | 7.900 82 | 8.000 83 | 8.100 84 | 8.200 85 | 8.300 86 | 8.400 87 | 8.500 88 | 8.600 89 | 8.700 90 | 8.800 91 | 8.900 92 | 9.000 93 | 9.100 94 | 9.200 95 | 9.300 96 | 9.400 97 | 9.500 98 | 9.600 99 | 9.700 100 | 9.800 101 | 9.900 102 | 10.000 103 | 10.100 104 | 10.200 105 | 10.300 106 | 10.400 107 | 10.500 108 | 10.600 109 | 10.700 110 | 10.800 111 | 10.900 112 | 11.000 113 | 11.100 114 | 11.200 115 | 11.300 116 | 11.400 117 | 11.500 118 | 11.600 119 | 11.700 120 | 11.800 121 | 11.900 122 | 12.000 123 | 12.100 124 | 12.200 125 | 12.300 126 | 12.400 127 | 12.500 128 | 12.600 129 | 12.700 130 | 12.800 131 | 12.900 132 | 13.000 133 | 13.100 134 | 13.200 135 | 13.300 136 | 13.400 137 | 13.500 138 | 13.600 139 | 13.700 140 | 13.800 141 | 13.900 142 | 14.000 143 | 14.100 144 | 14.200 145 | 14.300 146 | 14.400 147 | 14.500 148 | 14.600 149 | 14.700 150 | 14.800 151 | 14.900 152 | 15.000 153 | 15.100 154 | 15.200 155 | 15.300 156 | 15.400 157 | 15.500 158 | 15.600 159 | 15.700 160 | 15.800 161 | 15.900 162 | 16.000 163 | 16.100 164 | 16.200 165 | 16.300 166 | 16.400 167 | 16.500 168 | 16.600 169 | 16.700 170 | 16.800 171 | 16.900 172 | 17.000 173 | 17.100 174 | 17.200 175 | 17.300 176 | 17.400 177 | 17.500 178 | 17.600 179 | 17.700 180 | 17.800 181 | 17.900 182 | 18.000 183 | 18.100 184 | 18.200 185 | 18.300 186 | 18.400 187 | 18.500 188 | 18.600 189 | 18.700 190 | 18.800 191 | 18.900 192 | 19.000 193 | 19.100 194 | 19.200 195 | 19.300 196 | 19.400 197 | 19.500 198 | 19.600 199 | 19.700 200 | 19.800 201 | 19.900 202 | 20.000 203 | 20.100 204 | 20.200 205 | 20.300 206 | 20.400 207 | 20.500 208 | 20.600 209 | 20.700 210 | 20.800 211 | 20.900 212 | 21.000 213 | 21.100 214 | 21.200 215 | 21.300 216 | 21.400 217 | 21.500 218 | 21.600 219 | 21.700 220 | 21.800 221 | 21.900 222 | 22.000 223 | 22.100 224 | 22.200 225 | 22.300 226 | 22.400 227 | 22.500 228 | 22.600 229 | 22.700 230 | 22.800 231 | 22.900 232 | 23.000 233 | 23.100 234 | 23.200 235 | 23.300 236 | 23.400 237 | 23.500 238 | 23.600 239 | 23.700 240 | 23.800 241 | 23.900 242 | 24.000 243 | 24.100 244 | 24.200 245 | 24.300 246 | 24.400 247 | 24.500 248 | 24.600 249 | 24.700 250 | 24.800 251 | 24.900 252 | 25.000 253 | 25.100 254 | 25.200 255 | 25.300 256 | 25.400 257 | 25.500 258 | 25.600 259 | 25.700 260 | 25.800 261 | 25.900 262 | 26.000 263 | 26.100 264 | 26.200 265 | 26.300 266 | 26.400 267 | 26.500 268 | 26.600 269 | 26.700 270 | 26.800 271 | 26.900 272 | 27.000 273 | 27.100 274 | 27.200 275 | 27.300 276 | 27.400 277 | 27.500 278 | 27.600 279 | 27.700 280 | 27.800 281 | 27.900 282 | 28.000 283 | 28.100 284 | 28.200 285 | 28.300 286 | 28.400 287 | 28.500 288 | 28.600 289 | 28.700 290 | 28.800 291 | 28.900 292 | 29.000 293 | 29.100 294 | 29.200 295 | 29.300 296 | 29.400 297 | 29.500 298 | 29.600 299 | 29.700 300 | 29.800 301 | 29.900 302 | -------------------------------------------------------------------------------- /core/utils/angles.py: -------------------------------------------------------------------------------- 1 | """ 2 | Angle wrapping and manipulation utilities. 3 | 4 | Provides functions for handling angular quantities and ensuring they remain 5 | within proper bounds (typically [-π, π] for radians). 6 | 7 | Critical for: 8 | - Bearing measurements in positioning systems 9 | - Heading angles in navigation 10 | - Angular innovations in Kalman filters 11 | """ 12 | 13 | import numpy as np 14 | from typing import Union 15 | 16 | 17 | def wrap_angle(angle: float) -> float: 18 | """ 19 | Wrap angle to [-π, π] range. 20 | 21 | This is critical for bearing measurements and angular innovations 22 | in Kalman filters. Without wrapping, angles near ±180° can cause 23 | large incorrect innovations (e.g., -179° vs +179° = 358° error 24 | instead of 2° error). 25 | 26 | Args: 27 | angle: Angle in radians (can be any value) 28 | 29 | Returns: 30 | Wrapped angle in range [-π, π] 31 | 32 | Example: 33 | >>> wrap_angle(3.5 * np.pi) # 630° -> -90° 34 | -1.5707963267948966 35 | >>> wrap_angle(-3.5 * np.pi) # -630° -> 90° 36 | 1.5707963267948966 37 | 38 | References: 39 | Used in Extended Kalman Filter bearing measurement updates 40 | """ 41 | # Use atan2 trick for robust wrapping 42 | return np.arctan2(np.sin(angle), np.cos(angle)) 43 | 44 | 45 | def wrap_angle_array(angles: np.ndarray) -> np.ndarray: 46 | """ 47 | Wrap array of angles to [-π, π] range. 48 | 49 | Vectorized version of wrap_angle() for efficiency. 50 | 51 | Args: 52 | angles: Array of angles in radians 53 | 54 | Returns: 55 | Array of wrapped angles in range [-π, π] 56 | 57 | Example: 58 | >>> angles = np.array([0, np.pi/2, np.pi, -np.pi, 3*np.pi]) 59 | >>> wrap_angle_array(angles) 60 | array([ 0. , 1.57079633, 3.14159265, -3.14159265, -3.14159265]) 61 | """ 62 | return np.arctan2(np.sin(angles), np.cos(angles)) 63 | 64 | 65 | def angle_diff(angle1: Union[float, np.ndarray], 66 | angle2: Union[float, np.ndarray]) -> Union[float, np.ndarray]: 67 | """ 68 | Compute the shortest angular difference between two angles. 69 | 70 | Returns angle1 - angle2, wrapped to [-π, π]. This is the innovation 71 | for bearing measurements in EKF/UKF. 72 | 73 | Args: 74 | angle1: First angle in radians (measured) 75 | angle2: Second angle in radians (predicted) 76 | 77 | Returns: 78 | Shortest signed difference angle1 - angle2 in [-π, π] 79 | 80 | Example: 81 | >>> angle_diff(np.pi - 0.1, -np.pi + 0.1) # Nearly opposite 82 | -0.2 83 | >>> angle_diff(0.1, -0.1) # Small difference 84 | 0.2 85 | 86 | Notes: 87 | This is critical for EKF bearing updates: 88 | innovation = angle_diff(measured_bearing, predicted_bearing) 89 | 90 | Without this, bearings near ±180° would have huge innovations: 91 | measured = +179°, predicted = -179° -> innovation = 358° (WRONG!) 92 | with angle_diff: innovation = 2° (CORRECT!) 93 | 94 | References: 95 | EKF bearing measurement update (Chapter 3) 96 | """ 97 | if isinstance(angle1, np.ndarray) or isinstance(angle2, np.ndarray): 98 | return wrap_angle_array(np.asarray(angle1) - np.asarray(angle2)) 99 | else: 100 | return wrap_angle(angle1 - angle2) 101 | 102 | 103 | def degrees_to_radians(degrees: Union[float, np.ndarray]) -> Union[float, np.ndarray]: 104 | """Convert degrees to radians.""" 105 | return np.deg2rad(degrees) 106 | 107 | 108 | def radians_to_degrees(radians: Union[float, np.ndarray]) -> Union[float, np.ndarray]: 109 | """Convert radians to degrees.""" 110 | return np.rad2deg(radians) 111 | 112 | 113 | 114 | -------------------------------------------------------------------------------- /docs/guides/README.md: -------------------------------------------------------------------------------- 1 | # User Guides 2 | 3 | This directory contains user-facing guides and decision-making tools to help you use the IPIN-Examples repository effectively. These guides are intended for practitioners, researchers, and students who want to apply indoor positioning techniques. 4 | 5 | --- 6 | 7 | ## Available Guides 8 | 9 | ### Chapter 3: State Estimation 10 | 11 | #### [`ch3_estimator_selection.md`](./ch3_estimator_selection.md) 12 | **Comprehensive guide for choosing the right state estimator** 13 | 14 | This 500+ line guide helps you select the appropriate estimator for your specific indoor positioning scenario. 15 | 16 | **Contents:** 17 | - Quick selection table (scenario → recommended estimator) 18 | - Detailed analysis of each estimator (LS, KF, EKF, UKF, PF, FGO) 19 | - When to use / pros / cons / complexity for each method 20 | - Decision tree flowchart 21 | - Accuracy vs speed trade-offs 22 | - Common pitfalls and solutions 23 | - Real performance data from examples 24 | - Code examples for each estimator 25 | 26 | **Target audience:** 27 | - Practitioners selecting an estimator for their application 28 | - Students learning about state estimation trade-offs 29 | - Researchers comparing different methods 30 | 31 | **Start here if:** You're asking "Which estimator should I use for my indoor positioning problem?" 32 | 33 | --- 34 | 35 | ## Coming Soon 36 | 37 | Additional guides to be added: 38 | 39 | - **Ch4 RF Positioning Guide** - Selecting TOA/TDOA/AOA methods 40 | - **Ch5 Fingerprinting Guide** - Deterministic vs probabilistic methods 41 | - **Ch6 Dead Reckoning Guide** - IMU calibration and integration best practices 42 | - **Ch7 SLAM Guide** - When to use bundle adjustment vs pose graph optimization 43 | - **Ch8 Sensor Fusion Guide** - Loosely coupled vs tightly coupled fusion 44 | 45 | --- 46 | 47 | ## How to Use These Guides 48 | 49 | 1. **Browse by chapter** - Find the guide for your topic of interest 50 | 2. **Use quick reference tables** - Get fast answers to common questions 51 | 3. **Read detailed sections** - Deep dive into specific methods 52 | 4. **Follow decision trees** - Systematic approach to selection 53 | 5. **Study code examples** - See how to implement in practice 54 | 55 | --- 56 | 57 | ## Related Resources 58 | 59 | ### For Theory and Equations 60 | → See chapter-specific documentation in `docs/`: 61 | - `docs/ch2_equation_mapping.md` - Coordinate transformation equations 62 | - `docs/CH2_QUICK_REFERENCE.md` - Quick reference for Ch2 63 | - `docs/ch7_slam.md` - SLAM concepts and equations 64 | - `docs/ch8_fusion_api_reference.md` - Sensor fusion API 65 | - etc. 66 | 67 | ### For Implementation Details 68 | → See engineering documentation in [`docs/engineering/`](../engineering/) 69 | 70 | ### For Running Examples 71 | → See README files in each chapter directory: 72 | - `ch2_coords/README.md` 73 | - `ch3_estimators/README.md` 74 | - `ch4_rf_point_positioning/README.md` 75 | - `ch5_fingerprinting/README.md` 76 | - `ch6_dead_reckoning/README.md` 77 | - `ch7_slam/README.md` 78 | - `ch8_sensor_fusion/README.md` 79 | 80 | ### For Notebooks and Interactive Learning 81 | → See `notebooks/` directory 82 | 83 | --- 84 | 85 | ## Contributing 86 | 87 | To add a new user guide: 88 | 89 | 1. Focus on **practical decision-making** (not theory or implementation) 90 | 2. Include **quick reference tables** for fast lookup 91 | 3. Provide **real examples** and performance data 92 | 4. Use **clear sections** and navigation 93 | 5. Target **users/practitioners**, not developers 94 | 6. Add the guide to this README with a clear description 95 | 96 | --- 97 | 98 | ## Feedback 99 | 100 | If you have suggestions for new guides or improvements to existing ones, please open an issue or submit a pull request. 101 | 102 | --- 103 | 104 | **Note:** These are user-facing guides. For technical/engineering documentation, see the [`docs/engineering/`](../engineering/) directory. 105 | 106 | 107 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | requires = ["setuptools>=61.0", "wheel"] 3 | build-backend = "setuptools.build_meta" 4 | 5 | [tool.setuptools.packages.find] 6 | include = ["core*", "ch*"] 7 | exclude = ["references*", "data*", "notebooks*"] 8 | 9 | [project] 10 | name = "ipin-examples" 11 | version = "0.1.0" 12 | description = "Code examples for IPIN (Indoor Positioning and Indoor Navigation) book" 13 | readme = "README.md" 14 | requires-python = ">=3.8" 15 | license = "PolyForm-Noncommercial-1.0.0" 16 | authors = [ 17 | {name = "IPIN Book Examples", email = "ipin@example.com"} 18 | ] 19 | keywords = ["indoor positioning", "navigation", "SLAM", "sensor fusion"] 20 | classifiers = [ 21 | "Development Status :: 3 - Alpha", 22 | "Intended Audience :: Education", 23 | "Intended Audience :: Science/Research", 24 | "Programming Language :: Python :: 3", 25 | "Programming Language :: Python :: 3.8", 26 | "Programming Language :: Python :: 3.9", 27 | "Programming Language :: Python :: 3.10", 28 | "Programming Language :: Python :: 3.11", 29 | "Topic :: Scientific/Engineering", 30 | ] 31 | 32 | dependencies = [ 33 | "numpy>=1.20.0", 34 | "scipy>=1.7.0", 35 | "matplotlib>=3.3.0", 36 | "jupyter>=1.0.0", 37 | "tqdm>=4.62.0", 38 | ] 39 | 40 | [project.optional-dependencies] 41 | dev = [ 42 | "pytest>=7.0.0", 43 | "pytest-cov>=3.0.0", 44 | "black>=22.0.0", 45 | "flake8>=4.0.0", 46 | "mypy>=0.950", 47 | "ruff>=0.1.0", 48 | "pydocstyle>=6.0.0", 49 | "pylint>=2.12.0", 50 | ] 51 | 52 | [tool.black] 53 | line-length = 88 54 | target-version = ['py38', 'py39', 'py310', 'py311'] 55 | include = '\.pyi?$' 56 | extend-exclude = ''' 57 | /( 58 | # directories 59 | \.eggs 60 | | \.git 61 | | \.hg 62 | | \.mypy_cache 63 | | \.tox 64 | | \.venv 65 | | build 66 | | dist 67 | )/ 68 | ''' 69 | 70 | [tool.ruff] 71 | line-length = 88 72 | target-version = "py38" 73 | select = [ 74 | "E", # pycodestyle errors 75 | "W", # pycodestyle warnings 76 | "F", # pyflakes 77 | "I", # isort 78 | "B", # flake8-bugbear 79 | "C4", # flake8-comprehensions 80 | "UP", # pyupgrade 81 | ] 82 | ignore = [ 83 | "E501", # line too long (handled by black) 84 | "B008", # do not perform function calls in argument defaults 85 | "C901", # too complex 86 | ] 87 | 88 | [tool.ruff.per-file-ignores] 89 | "__init__.py" = ["F401"] # unused imports in __init__.py 90 | 91 | [tool.mypy] 92 | python_version = "3.8" 93 | warn_return_any = true 94 | warn_unused_configs = true 95 | disallow_untyped_defs = true 96 | disallow_incomplete_defs = true 97 | check_untyped_defs = true 98 | disallow_untyped_decorators = true 99 | no_implicit_optional = true 100 | warn_redundant_casts = true 101 | warn_unused_ignores = true 102 | warn_no_return = true 103 | warn_unreachable = true 104 | strict_equality = true 105 | show_error_codes = true 106 | 107 | [[tool.mypy.overrides]] 108 | module = [ 109 | "tests.*", 110 | ] 111 | disallow_untyped_defs = false 112 | 113 | [tool.pytest.ini_options] 114 | testpaths = ["tests"] 115 | python_files = ["test_*.py", "*_test.py"] 116 | python_classes = ["Test*"] 117 | python_functions = ["test_*"] 118 | addopts = [ 119 | "--strict-markers", 120 | "--strict-config", 121 | "--verbose", 122 | ] 123 | markers = [ 124 | "slow: marks tests as slow (deselect with '-m \"not slow\"')", 125 | "integration: marks tests as integration tests", 126 | ] 127 | 128 | [tool.coverage.run] 129 | source = ["ipin_examples", "core"] 130 | omit = [ 131 | "*/tests/*", 132 | "*/test_*.py", 133 | "*/__pycache__/*", 134 | ] 135 | 136 | [tool.coverage.report] 137 | exclude_lines = [ 138 | "pragma: no cover", 139 | "def __repr__", 140 | "raise AssertionError", 141 | "raise NotImplementedError", 142 | "if __name__ == .__main__.:", 143 | "if TYPE_CHECKING:", 144 | "@abstractmethod", 145 | ] 146 | 147 | [tool.pydocstyle] 148 | convention = "google" 149 | match = '(?!test_).*\.py' 150 | match-dir = '(?!\.git|\.venv|build|dist)' 151 | 152 | -------------------------------------------------------------------------------- /docs/engineering/ch3_bugfix_summary.md: -------------------------------------------------------------------------------- 1 | # Bug Fix Summary: Robust Least Squares Example 2 | 3 | ## Issue Discovered 4 | 5 | The user identified that the **Robust LS example performance matched Standard LS** instead of showing improvement, indicating the documentation in README.md was incorrect. 6 | 7 | ## Root Cause Analysis 8 | 9 | After extensive testing, we discovered: 10 | 11 | 1. **Insufficient Redundancy**: The original example used only **4 anchors** for 2D positioning 12 | - 2D positioning requires 2 unknowns (x, y) 13 | - With 4 measurements, only 2 degrees of freedom for redundancy 14 | - This is insufficient for robust methods to isolate outliers 15 | 16 | 2. **Outlier Distribution**: With minimal redundancy, the 3.0-5.0m outlier gets distributed across the geometric solution 17 | - All residuals become similar after least squares fitting 18 | - Robust weighting cannot distinguish the corrupted measurement 19 | - Result: All weights ≈ 1.0 (no downweighting) 20 | 21 | 3. **False Documentation**: README.md claimed: 22 | - "Robust LS error: 0.15 m" (FALSE - actual: 1.57 m, same as standard LS) 23 | - "Outlier weight: 0.12" (FALSE - actual: 1.0, no downweighting) 24 | - "Standard LS error: 1.23 m" (INCORRECT - actual: 1.57 m) 25 | 26 | ## Solution Implemented 27 | 28 | ### Code Changes (`example_least_squares.py`): 29 | 30 | 1. **Increased anchors from 4 to 8**: 31 | ```python 32 | anchors = np.array([ 33 | [0.0, 0.0], [10.0, 0.0], [0.0, 10.0], [10.0, 10.0], # Corners 34 | [5.0, 0.0], [5.0, 10.0], [0.0, 5.0], [10.0, 5.0] # Midpoints 35 | ]) 36 | ``` 37 | 38 | 2. **Used proper iterative robust LS**: 39 | - Re-linearization at each IRLS iteration 40 | - Compute weights based on ACTUAL measurement residuals (not linearized) 41 | - MAD-based robust scale estimation 42 | 43 | 3. **Increased outlier size from 3.0m to 5.0m** for clearer demonstration 44 | 45 | ### Documentation Updates (`README.md`): 46 | 47 | 1. **Updated Example 4 expected output** with correct values: 48 | ``` 49 | Standard LS error: 1.29 m (corrupted by outlier) 50 | Huber LS error: 0.08 m (93.5% improvement) 51 | Cauchy LS error: 0.03 m (97.4% improvement) 52 | Tukey LS error: 0.04 m (97.2% improvement) 53 | ``` 54 | 55 | 2. **Added "Important Notes on Robust Estimation" section** explaining: 56 | - Minimum 6-8 anchors recommended for 2D robust positioning 57 | - Why insufficient redundancy causes failure 58 | - Example showing 4-anchor limitation 59 | 60 | 3. **Updated example descriptions** to clarify anchor requirements 61 | 62 | ## Verification Results 63 | 64 | ### Before Fix (4 anchors): 65 | ``` 66 | Standard LS error: 1.57 m 67 | Huber LS error: 1.57 m ❌ IDENTICAL (NO IMPROVEMENT) 68 | Outlier weight: 1.0000 ❌ NOT DOWNWEIGHTED 69 | ``` 70 | 71 | ### After Fix (8 anchors): 72 | ``` 73 | Standard LS error: 1.29 m (corrupted) 74 | Huber LS error: 0.08 m ✅ 93.5% improvement 75 | Cauchy LS error: 0.03 m ✅ 97.4% improvement 76 | Tukey LS error: 0.04 m ✅ 97.2% improvement 77 | Outlier weights: 0.025, 0.0016, 0.0 ✅ PROPERLY REJECTED 78 | ``` 79 | 80 | ## Lessons Learned 81 | 82 | 1. **Robust estimation requires adequate redundancy** - general rule: 83 | - 2D: Use 6-8+ anchors (not just 4) 84 | - 3D: Use 8-10+ anchors (not just 5) 85 | 86 | 2. **Documentation must match reality** - fake/aspirational results mislead users 87 | 88 | 3. **Geometric configuration matters** - with minimal measurements, outliers distribute across solution space and cannot be isolated 89 | 90 | 4. **Always test before documenting** - the original example was never validated 91 | 92 | ## Files Modified 93 | 94 | - ✅ `ch3_estimators/example_least_squares.py` - Fixed robust LS example 95 | - ✅ `ch3_estimators/README.md` - Corrected documentation and added guidelines 96 | - ✅ Visualization updated to show 8-anchor scenario 97 | - ✅ Debug files cleaned up 98 | 99 | ## Status: RESOLVED ✅ 100 | 101 | The robust least squares example now works correctly and demonstrates proper outlier rejection as intended. 102 | 103 | -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_linear/toa_ranges.txt: -------------------------------------------------------------------------------- 1 | # ranges to 4 beacons (m) 2 | 8.276683 9.896002 12.881294 16.218572 3 | 8.080865 9.052440 11.584644 14.654376 4 | 8.272086 8.349672 10.229989 13.121018 5 | 8.585289 8.089328 9.374527 11.783143 6 | 9.505006 8.098661 8.592769 10.579669 7 | 10.493422 8.478112 8.226286 9.406754 8 | 11.759665 9.215394 8.113527 8.647536 9 | 13.172085 10.335381 8.334190 8.195509 10 | 14.655700 11.516573 9.112375 7.993237 11 | 16.033968 12.768432 10.129923 8.210585 12 | 6.579194 8.667618 11.718363 15.175840 13 | 6.244117 7.541522 10.447126 13.798421 14 | 6.477235 6.662937 8.810984 12.055846 15 | 7.085607 6.176540 7.736242 10.607772 16 | 8.046920 6.497443 6.873183 9.381207 17 | 9.192640 6.953312 6.359145 8.036634 18 | 10.826890 7.793777 6.245971 7.087418 19 | 12.034957 8.821248 6.850252 6.586086 20 | 13.714398 10.294863 7.553279 6.366937 21 | 15.287033 11.869368 8.511205 6.538815 22 | 4.851328 7.491049 10.960833 14.580097 23 | 4.390926 6.161833 9.467137 12.932314 24 | 4.533032 4.925612 8.041331 11.222007 25 | 5.456870 4.469639 6.522178 9.783302 26 | 6.820376 4.682500 5.316370 8.233339 27 | 8.236748 5.224521 4.469987 6.892337 28 | 9.766197 6.449691 4.464949 5.545207 29 | 11.313558 7.652734 5.105116 4.881539 30 | 13.063488 9.519783 6.248014 4.493905 31 | 14.570706 11.021410 7.447734 4.990839 32 | 3.484456 6.629659 10.279557 14.150334 33 | 2.706347 5.001027 8.685234 12.671370 34 | 3.030115 3.698892 7.081227 10.802784 35 | 4.162896 2.736236 5.522984 8.993288 36 | 5.820355 3.121654 3.910996 7.186657 37 | 7.307595 3.975476 2.941308 5.792570 38 | 9.143660 5.397760 2.801744 4.198282 39 | 10.862448 6.775074 3.487868 2.938991 40 | 12.502835 8.779203 5.003036 2.592170 41 | 14.290988 10.245395 6.613375 3.320225 42 | 2.169711 6.098352 10.075621 14.160256 43 | 0.951123 4.509876 8.477829 12.261441 44 | 1.632656 2.393746 6.393720 10.436334 45 | 3.468349 1.173028 4.716643 8.818517 46 | 5.182152 1.405876 2.944601 6.989030 47 | 7.039555 2.839408 1.389356 4.988749 48 | 8.954173 4.794057 1.055154 3.496325 49 | 10.413364 6.595240 2.763938 1.694598 50 | 12.280326 8.426602 4.278598 0.822133 51 | 13.949534 9.870782 5.982863 2.213402 52 | 2.227139 6.121140 10.069070 14.231698 53 | 0.949675 4.319889 8.193577 12.344521 54 | 1.817816 2.511075 6.524443 10.336719 55 | 3.342603 1.182551 4.950298 8.594470 56 | 5.059723 1.380556 2.970490 7.027260 57 | 7.000113 3.098842 1.467726 5.019270 58 | 8.783513 4.774733 1.049713 3.594934 59 | 10.618466 6.401400 2.359766 1.952707 60 | 12.312459 8.322581 4.165335 0.986165 61 | 13.989144 10.224211 6.048069 2.355424 62 | 3.164137 6.564149 10.259207 14.217471 63 | 2.703147 4.922436 8.512162 12.593330 64 | 3.212382 3.565383 6.930839 10.731586 65 | 4.043572 2.666067 5.296597 8.835611 66 | 5.869860 2.791293 3.840455 7.442864 67 | 7.265420 3.775165 2.957464 5.729848 68 | 9.095906 5.451570 2.634721 4.156796 69 | 10.860197 6.812393 3.392789 3.187384 70 | 12.472578 8.554532 4.994968 2.645983 71 | 14.295960 10.474416 6.629442 3.407335 72 | 5.046613 7.602721 10.968697 14.823598 73 | 4.442024 6.310029 9.435973 13.006367 74 | 4.759927 5.111443 7.649736 11.338481 75 | 5.593484 4.622899 6.554442 9.726602 76 | 6.710416 4.612374 5.260628 8.222573 77 | 8.201074 5.303919 4.569467 6.894641 78 | 9.707199 6.587193 4.677905 5.521962 79 | 11.435878 7.757680 5.169216 4.672621 80 | 12.811525 9.413209 6.032071 4.305767 81 | 14.683877 10.971062 7.366010 4.946142 82 | 6.558308 8.738610 11.666667 15.203248 83 | 6.170416 7.478683 10.315065 13.683736 84 | 6.398976 6.843987 8.895843 12.363434 85 | 6.996619 6.356575 7.893529 10.812611 86 | 8.035682 6.508868 6.843183 9.324312 87 | 9.341887 6.745069 6.311862 8.146327 88 | 10.702565 7.484718 6.296123 6.693994 89 | 12.381696 8.957872 6.725963 6.575407 90 | 13.807346 10.314668 7.491242 6.215571 91 | 15.364827 11.824115 8.490773 6.558700 92 | 8.333267 10.147927 12.985685 16.255996 93 | 7.930157 9.004389 11.535311 14.607925 94 | 8.099757 8.199615 10.168317 13.054127 95 | 8.728280 7.816934 9.225186 11.576516 96 | 9.440546 8.076653 8.604240 10.501533 97 | 10.471389 8.451860 8.131051 9.397775 98 | 11.809522 9.302747 8.039565 8.711139 99 | 13.201734 10.417334 8.357389 8.130143 100 | 14.548967 11.615450 9.070212 7.987961 101 | 16.096845 12.746640 10.000851 8.325704 102 | -------------------------------------------------------------------------------- /tests/docs/test_ch6_examples.py: -------------------------------------------------------------------------------- 1 | """ 2 | Test Ch6 dataset code examples to ensure they work correctly. 3 | 4 | This script extracts and tests Python code blocks from Ch6 dataset READMEs. 5 | 6 | Location: tests/docs/ 7 | Purpose: Documentation validation (not unit tests for core/ modules) 8 | 9 | Note: Unit tests for core/sensors/ are in tests/core/sensors/ 10 | """ 11 | 12 | import io 13 | import re 14 | import sys 15 | from pathlib import Path 16 | from contextlib import redirect_stdout, redirect_stderr 17 | 18 | 19 | def extract_python_blocks(readme_path): 20 | """Extract Python code blocks from README.""" 21 | with open(readme_path, 'r', encoding='utf-8') as f: 22 | content = f.read() 23 | 24 | # Find all Python code blocks 25 | pattern = r'```python\n(.*?)\n```' 26 | blocks = re.findall(pattern, content, re.DOTALL) 27 | return blocks 28 | 29 | 30 | def test_code_block(code, dataset_name, block_num): 31 | """Test a single code block.""" 32 | # Skip blocks that are just examples (contain plt.show() or are incomplete) 33 | if 'plt.show()' in code or '# ... rest is' in code or '...' in code: 34 | return True, "Skipped (plotting or incomplete example)" 35 | 36 | # Create a safe execution environment 37 | exec_globals = { 38 | '__name__': '__main__', 39 | '__file__': f'test_{dataset_name}.py' 40 | } 41 | 42 | try: 43 | # Redirect output 44 | stdout = io.StringIO() 45 | stderr = io.StringIO() 46 | 47 | with redirect_stdout(stdout), redirect_stderr(stderr): 48 | exec(code, exec_globals) 49 | 50 | return True, "OK" 51 | except Exception as e: 52 | return False, str(e) 53 | 54 | 55 | def main(): 56 | """Main test execution.""" 57 | print("\n" + "=" * 70) 58 | print("Testing Ch6 Dataset Code Examples") 59 | print("=" * 70) 60 | 61 | # Ch6 datasets to test 62 | datasets = [ 63 | 'ch6_strapdown_basic', 64 | 'ch6_foot_zupt_walk', 65 | 'ch6_wheel_odom_square', 66 | 'ch6_pdr_corridor_walk', 67 | 'ch6_env_sensors_heading_altitude', 68 | ] 69 | 70 | total_tests = 0 71 | total_passed = 0 72 | total_skipped = 0 73 | total_failed = 0 74 | 75 | for dataset in datasets: 76 | readme_path = Path(f"data/sim/{dataset}/README.md") 77 | 78 | if not readme_path.exists(): 79 | print(f"\n[X] {dataset}: README not found") 80 | continue 81 | 82 | print(f"\n\nTesting: {dataset}") 83 | print("-" * 70) 84 | 85 | blocks = extract_python_blocks(readme_path) 86 | print(f"Found {len(blocks)} Python code blocks") 87 | 88 | for i, block in enumerate(blocks, 1): 89 | total_tests += 1 90 | success, message = test_code_block(block, dataset, i) 91 | 92 | if success: 93 | if "Skipped" in message: 94 | status = "[SKIP]" 95 | total_skipped += 1 96 | else: 97 | status = "[OK]" 98 | total_passed += 1 99 | else: 100 | status = "[FAIL]" 101 | total_failed += 1 102 | 103 | # Truncate message for display 104 | display_msg = message[:50] + "..." if len(message) > 50 else message 105 | print(f" Block {i:2d}: {status} {display_msg}") 106 | 107 | # Summary 108 | print("\n" + "=" * 70) 109 | print("TEST SUMMARY") 110 | print("=" * 70) 111 | print(f"Total tests: {total_tests}") 112 | print(f"Passed: {total_passed} (executable)") 113 | print(f"Skipped: {total_skipped} (plotting/incomplete)") 114 | print(f"Failed: {total_failed}") 115 | print() 116 | 117 | if total_failed == 0: 118 | print("[OK] All testable code examples work correctly!") 119 | return 0 120 | else: 121 | print(f"[FAIL] {total_failed} code examples failed") 122 | return 1 123 | 124 | 125 | if __name__ == "__main__": 126 | sys.exit(main()) 127 | 128 | 129 | -------------------------------------------------------------------------------- /core/slam/__init__.py: -------------------------------------------------------------------------------- 1 | """SLAM algorithms and geometry for Chapter 7. 2 | 3 | This module implements minimal, reusable building blocks for Chapter 7 4 | (SLAM Technologies) of the book: Principles of Indoor Positioning and 5 | Indoor Navigation. 6 | 7 | This is NOT a full SLAM framework. Instead, it provides: 8 | - Geometry & residual models (ICP, NDT, LOAM, camera) 9 | - Factor graph factors for pose graph and bundle adjustment 10 | - SE(2) operations for 2D transformations 11 | 12 | The nonlinear solvers live in core/estimators (Gauss-Newton / LM / FGO). 13 | Chapter examples in ch7_slam/ wire datasets + factors + solver + plots. 14 | 15 | Main components: 16 | - Pose2, CameraIntrinsics: Core data structures 17 | - se2_compose, se2_inverse, se2_apply: SE(2) operations 18 | - icp_point_to_point: ICP scan matching (Section 7.2.1) 19 | - ndt_align: NDT alignment (Section 7.2.2) 20 | - create_pose_graph: Pose graph optimization (Section 7.3) 21 | - project_point, distort_normalized: Camera projection (Section 7.4) 22 | - create_reprojection_factor: Visual SLAM bundle adjustment (Eqs. 7.68-7.70) 23 | - create_odometry_factor, create_loop_closure_factor, create_prior_factor: Factor constructors 24 | 25 | Example usage: 26 | >>> from core.slam import Pose2, se2_compose, se2_inverse, se2_apply 27 | >>> import numpy as np 28 | >>> 29 | >>> # Create poses 30 | >>> p1 = Pose2(x=0.0, y=0.0, yaw=0.0) 31 | >>> p2 = Pose2(x=1.0, y=0.0, yaw=np.pi/2) 32 | >>> 33 | >>> # Compose poses 34 | >>> p_composed = se2_compose(p1.to_array(), p2.to_array()) 35 | >>> 36 | >>> # Transform points 37 | >>> points = np.array([[1, 0], [0, 1]]) 38 | >>> points_transformed = se2_apply(p2.to_array(), points) 39 | 40 | References: 41 | Chapter 7: SLAM Technologies 42 | - Section 7.2: LiDAR SLAM (ICP, NDT, LOAM) 43 | - Section 7.4: Visual SLAM (camera model, bundle adjustment) 44 | 45 | Author: Navigation Engineer 46 | Date: 2024 47 | """ 48 | 49 | from . import camera 50 | from .camera import ( 51 | compute_reprojection_error, 52 | distort_normalized, 53 | project_point, 54 | undistort_normalized, 55 | unproject_pixel, 56 | ) 57 | from .factors import ( 58 | create_landmark_factor, 59 | create_loop_closure_factor, 60 | create_odometry_factor, 61 | create_pose_graph, 62 | create_prior_factor, 63 | create_reprojection_factor, 64 | ) 65 | from .ndt import ( 66 | build_ndt_map, 67 | ndt_align, 68 | ndt_covariance, 69 | ndt_gradient, 70 | ndt_score, 71 | ) 72 | from .scan_matching import ( 73 | align_svd, 74 | compute_icp_covariance, 75 | compute_icp_residual, 76 | find_correspondences, 77 | icp_point_to_point, 78 | ) 79 | from .se2 import ( 80 | se2_apply, 81 | se2_compose, 82 | se2_from_matrix, 83 | se2_inverse, 84 | se2_relative, 85 | se2_to_matrix, 86 | wrap_angle, 87 | ) 88 | from .types import CameraIntrinsics, PointCloud2D, PointCloud3D, Pose2, VoxelGrid 89 | 90 | __all__ = [ 91 | # Core types 92 | "Pose2", 93 | "CameraIntrinsics", 94 | "PointCloud2D", 95 | "PointCloud3D", 96 | "VoxelGrid", 97 | # SE(2) operations 98 | "se2_compose", 99 | "se2_inverse", 100 | "se2_apply", 101 | "se2_relative", 102 | "se2_to_matrix", 103 | "se2_from_matrix", 104 | "wrap_angle", 105 | # ICP scan matching 106 | "find_correspondences", 107 | "compute_icp_residual", 108 | "align_svd", 109 | "icp_point_to_point", 110 | "compute_icp_covariance", 111 | # NDT alignment 112 | "build_ndt_map", 113 | "ndt_score", 114 | "ndt_gradient", 115 | "ndt_align", 116 | "ndt_covariance", 117 | # Camera model and projection 118 | "camera", 119 | "distort_normalized", 120 | "undistort_normalized", 121 | "project_point", 122 | "unproject_pixel", 123 | "compute_reprojection_error", 124 | # Pose graph factors 125 | "create_odometry_factor", 126 | "create_loop_closure_factor", 127 | "create_prior_factor", 128 | "create_landmark_factor", 129 | "create_reprojection_factor", 130 | "create_pose_graph", 131 | ] 132 | 133 | __version__ = "0.1.0" 134 | 135 | -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_linear/aoa_angles.txt: -------------------------------------------------------------------------------- 1 | # AOA angles from 4 beacons (rad) 2 | -1.826814 -2.214884 -2.496629 -2.591750 3 | -1.567903 -2.058161 -2.376345 -2.585798 4 | -1.363710 -1.792585 -2.263102 -2.505837 5 | -1.150061 -1.634978 -2.122101 -2.388069 6 | -0.980236 -1.483655 -1.928494 -2.298125 7 | -0.854206 -1.203794 -1.683976 -2.111628 8 | -0.728065 -1.037750 -1.463553 -1.980501 9 | -0.695379 -0.875678 -1.290636 -1.762408 10 | -0.630078 -0.817871 -1.119985 -1.529071 11 | -0.552308 -0.686575 -0.897963 -1.386113 12 | -1.881032 -2.282102 -2.593348 -2.759095 13 | -1.628827 -2.199339 -2.507388 -2.718754 14 | -1.224105 -1.930664 -2.408278 -2.678730 15 | -1.084525 -1.713683 -2.272755 -2.535897 16 | -0.908440 -1.319790 -2.034138 -2.377729 17 | -0.750476 -1.178908 -1.792114 -2.252443 18 | -0.584054 -0.912329 -1.410453 -2.056209 19 | -0.575928 -0.799088 -1.174006 -1.829549 20 | -0.472594 -0.677246 -1.017142 -1.565751 21 | -0.399441 -0.509915 -0.808971 -1.284091 22 | -1.928136 -2.533828 -2.733400 -2.885279 23 | -1.657840 -2.349040 -2.676649 -2.796112 24 | -1.275017 -2.086494 -2.525921 -2.678929 25 | -0.935609 -1.728985 -2.372476 -2.687126 26 | -0.704696 -1.329214 -2.074106 -2.513706 27 | -0.540592 -0.978559 -1.854536 -2.442308 28 | -0.563445 -0.769280 -1.415745 -2.203965 29 | -0.404462 -0.640502 -1.079628 -1.862091 30 | -0.359123 -0.493229 -0.835378 -1.486295 31 | -0.362272 -0.420561 -0.678460 -1.166034 32 | -2.203406 -2.683040 -2.859740 -3.033352 33 | -1.709146 -2.526756 -2.815114 -2.897226 34 | -1.047872 -2.318068 -2.735870 -2.856714 35 | -0.674342 -1.769386 -2.592539 -2.813709 36 | -0.435175 -1.231935 -2.439864 -2.828154 37 | -0.385802 -0.729384 -1.941085 -2.655880 38 | -0.312243 -0.500954 -1.335097 -2.470955 39 | -0.259923 -0.385436 -0.790828 -2.052526 40 | -0.227313 -0.285570 -0.503250 -1.535902 41 | -0.206858 -0.297944 -0.441063 -0.912359 42 | -2.734939 -2.955608 -3.039554 -3.082763 43 | -1.804149 -2.934999 -3.022945 -3.098083 44 | -0.552729 -2.780684 -3.018736 -3.066617 45 | -0.252815 -2.164269 -2.950176 -3.019113 46 | -0.159396 -0.684733 -2.827247 -3.024044 47 | -0.153438 -0.350946 -2.570333 -2.988373 48 | -0.105212 -0.160617 -0.915265 -2.857661 49 | -0.035899 -0.152514 -0.297980 -2.617857 50 | -0.050348 -0.126497 -0.167430 -1.409393 51 | -0.056307 -0.123860 -0.122405 -0.395093 52 | 2.687336 2.951074 3.021912 3.075718 53 | 1.834919 2.940658 2.983354 3.066620 54 | 0.531833 2.708700 2.964162 3.046436 55 | 0.217933 2.222990 3.016946 2.981729 56 | 0.152031 0.719228 2.821187 2.991047 57 | 0.078626 0.314079 2.438696 2.924629 58 | 0.080576 0.204667 0.908674 2.866621 59 | 0.074064 0.127075 0.342141 2.583582 60 | 0.067643 0.129166 0.147084 1.331556 61 | 0.033071 0.094848 0.189403 0.406922 62 | 2.231125 2.705122 2.791464 2.980767 63 | 1.667471 2.572450 2.856483 2.948597 64 | 1.027894 2.323707 2.740674 2.924847 65 | 0.713591 1.807221 2.586464 2.841106 66 | 0.505380 1.139874 2.395751 2.764905 67 | 0.289721 0.693177 1.933373 2.711710 68 | 0.291948 0.528855 1.331337 2.494001 69 | 0.253436 0.424833 0.891597 2.116893 70 | 0.254471 0.281477 0.593162 1.509982 71 | 0.191140 0.241188 0.373549 0.986002 72 | 2.007508 2.506152 2.677897 2.832408 73 | 1.582971 2.325286 2.619997 2.749177 74 | 1.235480 2.044142 2.500015 2.751301 75 | 0.920241 1.699514 2.389407 2.650150 76 | 0.765931 1.285253 2.073286 2.503636 77 | 0.533599 0.981163 1.789494 2.394630 78 | 0.470595 0.765588 1.434664 2.325260 79 | 0.412045 0.604238 1.079702 1.922305 80 | 0.377779 0.448701 0.796829 1.500422 81 | 0.306967 0.410511 0.633939 1.146966 82 | 1.866738 2.250689 2.525513 2.694290 83 | 1.623211 2.113391 2.507384 2.674279 84 | 1.288250 1.918889 2.387596 2.623701 85 | 1.067890 1.647268 2.218459 2.498975 86 | 0.898775 1.434764 2.062971 2.417806 87 | 0.751725 1.178025 1.757606 2.246051 88 | 0.582094 0.902438 1.511902 2.045710 89 | 0.536042 0.720218 1.187532 1.789868 90 | 0.434536 0.686372 0.960204 1.485640 91 | 0.369206 0.568155 0.793015 1.242945 92 | 1.820005 2.218862 2.495606 2.620377 93 | 1.624965 2.069643 2.326789 2.612375 94 | 1.400380 1.879206 2.259948 2.502262 95 | 1.217461 1.661016 2.081405 2.413108 96 | 1.036267 1.457175 1.920893 2.255096 97 | 0.863714 1.309609 1.660725 2.190704 98 | 0.784942 1.020229 1.485552 1.928169 99 | 0.621770 0.900593 1.320293 1.860904 100 | 0.565310 0.722897 1.031292 1.565828 101 | 0.574270 0.692004 0.961282 1.369170 102 | -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_nlos/aoa_angles.txt: -------------------------------------------------------------------------------- 1 | # AOA angles from 4 beacons (rad) 2 | 0.774359 3.030349 -2.385971 -1.429442 3 | 0.517563 3.017181 -2.310746 -1.387691 4 | 0.360594 3.078764 -2.261228 -1.289311 5 | 0.292197 3.003950 -2.207234 -1.175817 6 | 0.238121 2.909079 -2.125998 -1.118642 7 | 0.187330 2.945971 -2.014544 -0.999061 8 | 0.173956 2.880312 -1.933570 -0.972503 9 | 0.095844 2.813036 -1.886548 -0.894115 10 | 0.072155 2.608522 -1.812481 -0.823345 11 | 0.077495 2.344360 -1.652121 -0.845693 12 | 1.084660 2.990631 -2.416451 -1.483854 13 | 0.763067 2.880450 -2.369801 -1.390053 14 | 0.698890 2.900250 -2.332830 -1.315272 15 | 0.470158 2.815595 -2.292191 -1.163210 16 | 0.367743 2.881945 -2.190625 -1.029789 17 | 0.318063 2.705753 -2.127144 -0.973615 18 | 0.328463 2.680864 -1.941750 -0.901463 19 | 0.217165 2.513184 -1.878287 -0.857079 20 | 0.227098 2.326760 -1.842140 -0.816051 21 | 0.225658 2.104380 -1.698858 -0.757790 22 | 1.290755 2.812443 -2.475380 -1.484293 23 | 0.936534 2.793165 -2.444709 -1.318278 24 | 0.744503 2.761565 -2.344272 -1.143280 25 | 0.640018 2.718964 -2.282610 -1.120398 26 | 0.558610 2.666419 -2.143689 -0.953139 27 | 0.504151 2.609893 -2.172295 -0.941298 28 | 0.323737 2.484993 -2.034417 -0.840546 29 | 0.365033 2.319441 -1.949645 -0.740018 30 | 0.319595 2.170296 -1.850950 -0.692949 31 | 0.244492 1.914015 -1.749294 -0.694336 32 | 1.315435 2.795045 -2.507136 -1.494176 33 | 1.039893 2.768552 -2.465797 -1.251398 34 | 0.917315 2.666486 -2.408292 -1.122565 35 | 0.785797 2.663186 -2.326287 -1.016615 36 | 0.723420 2.492966 -2.324556 -1.003131 37 | 0.576229 2.479920 -2.169854 -0.855902 38 | 0.511052 2.374387 -2.104872 -0.789501 39 | 0.459838 2.226032 -1.946106 -0.673573 40 | 0.412064 2.074542 -1.800577 -0.711184 41 | 0.368239 1.799707 -1.750237 -0.598271 42 | 1.343140 2.711926 -2.584168 -1.393725 43 | 1.189365 2.628951 -2.539478 -1.265941 44 | 0.989651 2.590993 -2.509844 -1.108945 45 | 0.900875 2.568061 -2.428329 -0.957826 46 | 0.798193 2.434858 -2.340348 -0.884829 47 | 0.671628 2.303747 -2.371020 -0.804370 48 | 0.620555 2.276109 -2.151463 -0.686740 49 | 0.611740 2.102911 -1.991783 -0.641368 50 | 0.533991 1.945048 -1.864675 -0.674733 51 | 0.475673 1.751677 -1.727771 -0.520911 52 | 1.353115 2.554110 -2.704044 -1.357179 53 | 1.255995 2.556997 -2.680403 -1.180111 54 | 1.111706 2.411512 -2.619220 -1.033489 55 | 0.935430 2.440216 -2.454457 -0.950745 56 | 0.853894 2.400681 -2.466757 -0.807620 57 | 0.735700 2.283118 -2.384350 -0.741515 58 | 0.688439 2.179939 -2.267126 -0.637930 59 | 0.635120 2.032583 -2.124988 -0.601602 60 | 0.586200 1.926218 -2.024266 -0.506001 61 | 0.513707 1.758637 -1.744556 -0.479876 62 | 1.431022 2.510139 -2.844243 -1.277148 63 | 1.294483 2.472829 -2.688520 -1.073282 64 | 1.142639 2.432638 -2.680389 -0.889231 65 | 1.084850 2.347640 -2.652779 -0.787386 66 | 0.971728 2.244735 -2.549317 -0.685063 67 | 0.781114 2.142115 -2.496099 -0.541693 68 | 0.778847 2.105301 -2.350675 -0.497647 69 | 0.723375 2.016619 -2.156380 -0.451760 70 | 0.702593 1.828499 -2.016646 -0.402234 71 | 0.616126 1.707983 -1.881723 -0.328169 72 | 1.447067 2.467456 -2.887698 -1.227028 73 | 1.277204 2.408828 -2.837689 -1.017264 74 | 1.204981 2.326697 -2.812247 -0.773369 75 | 1.093960 2.270542 -2.719435 -0.665923 76 | 1.058247 2.176190 -2.743700 -0.612553 77 | 0.885471 2.120276 -2.620311 -0.502996 78 | 0.847621 2.045154 -2.480503 -0.302367 79 | 0.795122 1.938458 -2.344446 -0.352340 80 | 0.756500 1.779754 -2.182178 -0.350363 81 | 0.675815 1.700670 -1.919962 -0.300343 82 | 1.433071 2.320770 -2.994199 -1.112976 83 | 1.358713 2.302590 -2.899191 -0.781818 84 | 1.203281 2.272047 -2.871925 -0.577783 85 | 1.135138 2.203469 -2.847584 -0.495643 86 | 1.074707 2.202630 -2.750143 -0.382250 87 | 0.996770 2.124434 -2.738434 -0.346361 88 | 0.867283 1.970501 -2.618057 -0.306742 89 | 0.842061 1.853105 -2.553346 -0.281716 90 | 0.749040 1.838156 -2.370588 -0.278256 91 | 0.684479 1.705020 -2.068259 -0.223727 92 | 1.464369 2.360759 -3.002181 -0.787468 93 | 1.390320 2.317517 -3.062027 -0.436553 94 | 1.293062 2.258905 -2.992974 -0.331250 95 | 1.225377 2.191082 -3.002457 -0.249317 96 | 1.136218 2.139224 -2.956386 -0.242695 97 | 1.030581 2.124721 -2.973585 -0.130266 98 | 0.997111 1.935179 -2.877444 -0.194020 99 | 0.862686 1.878079 -2.749995 -0.039527 100 | 0.823046 1.728867 -2.708576 -0.099866 101 | 0.840522 1.698717 -2.322207 -0.067305 102 | -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_square/aoa_angles.txt: -------------------------------------------------------------------------------- 1 | # AOA angles from 4 beacons (rad) 2 | 0.774359 3.030349 -2.385971 -1.429442 3 | 0.517563 3.017181 -2.310746 -1.387691 4 | 0.360594 3.078764 -2.261228 -1.289311 5 | 0.292197 3.003950 -2.207234 -1.175817 6 | 0.238121 2.909079 -2.125998 -1.118642 7 | 0.187330 2.945971 -2.014544 -0.999061 8 | 0.173956 2.880312 -1.933570 -0.972503 9 | 0.095844 2.813036 -1.886548 -0.894115 10 | 0.072155 2.608522 -1.812481 -0.823345 11 | 0.077495 2.344360 -1.652121 -0.845693 12 | 1.084660 2.990631 -2.416451 -1.483854 13 | 0.763067 2.880450 -2.369801 -1.390053 14 | 0.698890 2.900250 -2.332830 -1.315272 15 | 0.470158 2.815595 -2.292191 -1.163210 16 | 0.367743 2.881945 -2.190625 -1.029789 17 | 0.318063 2.705753 -2.127144 -0.973615 18 | 0.328463 2.680864 -1.941750 -0.901463 19 | 0.217165 2.513184 -1.878287 -0.857079 20 | 0.227098 2.326760 -1.842140 -0.816051 21 | 0.225658 2.104380 -1.698858 -0.757790 22 | 1.290755 2.812443 -2.475380 -1.484293 23 | 0.936534 2.793165 -2.444709 -1.318278 24 | 0.744503 2.761565 -2.344272 -1.143280 25 | 0.640018 2.718964 -2.282610 -1.120398 26 | 0.558610 2.666419 -2.143689 -0.953139 27 | 0.504151 2.609893 -2.172295 -0.941298 28 | 0.323737 2.484993 -2.034417 -0.840546 29 | 0.365033 2.319441 -1.949645 -0.740018 30 | 0.319595 2.170296 -1.850950 -0.692949 31 | 0.244492 1.914015 -1.749294 -0.694336 32 | 1.315435 2.795045 -2.507136 -1.494176 33 | 1.039893 2.768552 -2.465797 -1.251398 34 | 0.917315 2.666486 -2.408292 -1.122565 35 | 0.785797 2.663186 -2.326287 -1.016615 36 | 0.723420 2.492966 -2.324556 -1.003131 37 | 0.576229 2.479920 -2.169854 -0.855902 38 | 0.511052 2.374387 -2.104872 -0.789501 39 | 0.459838 2.226032 -1.946106 -0.673573 40 | 0.412064 2.074542 -1.800577 -0.711184 41 | 0.368239 1.799707 -1.750237 -0.598271 42 | 1.343140 2.711926 -2.584168 -1.393725 43 | 1.189365 2.628951 -2.539478 -1.265941 44 | 0.989651 2.590993 -2.509844 -1.108945 45 | 0.900875 2.568061 -2.428329 -0.957826 46 | 0.798193 2.434858 -2.340348 -0.884829 47 | 0.671628 2.303747 -2.371020 -0.804370 48 | 0.620555 2.276109 -2.151463 -0.686740 49 | 0.611740 2.102911 -1.991783 -0.641368 50 | 0.533991 1.945048 -1.864675 -0.674733 51 | 0.475673 1.751677 -1.727771 -0.520911 52 | 1.353115 2.554110 -2.704044 -1.357179 53 | 1.255995 2.556997 -2.680403 -1.180111 54 | 1.111706 2.411512 -2.619220 -1.033489 55 | 0.935430 2.440216 -2.454457 -0.950745 56 | 0.853894 2.400681 -2.466757 -0.807620 57 | 0.735700 2.283118 -2.384350 -0.741515 58 | 0.688439 2.179939 -2.267126 -0.637930 59 | 0.635120 2.032583 -2.124988 -0.601602 60 | 0.586200 1.926218 -2.024266 -0.506001 61 | 0.513707 1.758637 -1.744556 -0.479876 62 | 1.431022 2.510139 -2.844243 -1.277148 63 | 1.294483 2.472829 -2.688520 -1.073282 64 | 1.142639 2.432638 -2.680389 -0.889231 65 | 1.084850 2.347640 -2.652779 -0.787386 66 | 0.971728 2.244735 -2.549317 -0.685063 67 | 0.781114 2.142115 -2.496099 -0.541693 68 | 0.778847 2.105301 -2.350675 -0.497647 69 | 0.723375 2.016619 -2.156380 -0.451760 70 | 0.702593 1.828499 -2.016646 -0.402234 71 | 0.616126 1.707983 -1.881723 -0.328169 72 | 1.447067 2.467456 -2.887698 -1.227028 73 | 1.277204 2.408828 -2.837689 -1.017264 74 | 1.204981 2.326697 -2.812247 -0.773369 75 | 1.093960 2.270542 -2.719435 -0.665923 76 | 1.058247 2.176190 -2.743700 -0.612553 77 | 0.885471 2.120276 -2.620311 -0.502996 78 | 0.847621 2.045154 -2.480503 -0.302367 79 | 0.795122 1.938458 -2.344446 -0.352340 80 | 0.756500 1.779754 -2.182178 -0.350363 81 | 0.675815 1.700670 -1.919962 -0.300343 82 | 1.433071 2.320770 -2.994199 -1.112976 83 | 1.358713 2.302590 -2.899191 -0.781818 84 | 1.203281 2.272047 -2.871925 -0.577783 85 | 1.135138 2.203469 -2.847584 -0.495643 86 | 1.074707 2.202630 -2.750143 -0.382250 87 | 0.996770 2.124434 -2.738434 -0.346361 88 | 0.867283 1.970501 -2.618057 -0.306742 89 | 0.842061 1.853105 -2.553346 -0.281716 90 | 0.749040 1.838156 -2.370588 -0.278256 91 | 0.684479 1.705020 -2.068259 -0.223727 92 | 1.464369 2.360759 -3.002181 -0.787468 93 | 1.390320 2.317517 -3.062027 -0.436553 94 | 1.293062 2.258905 -2.992974 -0.331250 95 | 1.225377 2.191082 -3.002457 -0.249317 96 | 1.136218 2.139224 -2.956386 -0.242695 97 | 1.030581 2.124721 -2.973585 -0.130266 98 | 0.997111 1.935179 -2.877444 -0.194020 99 | 0.862686 1.878079 -2.749995 -0.039527 100 | 0.823046 1.728867 -2.708576 -0.099866 101 | 0.840522 1.698717 -2.322207 -0.067305 102 | -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_optimal/aoa_angles.txt: -------------------------------------------------------------------------------- 1 | # AOA angles from 4 beacons (rad) 2 | -2.734407 -1.989607 -1.355595 2.927311 3 | -2.652776 -1.905359 -1.136070 2.806825 4 | -2.620763 -1.738110 -0.977995 2.700858 5 | -2.552332 -1.698915 -0.852080 2.506196 6 | -2.485924 -1.671003 -0.731713 1.972602 7 | -2.415357 -1.500991 -0.608807 1.180265 8 | -2.295389 -1.418746 -0.539214 0.628587 9 | -2.219527 -1.311726 -0.522177 0.423290 10 | -2.062490 -1.284147 -0.492964 0.324953 11 | -1.848937 -1.164406 -0.388892 0.184684 12 | -2.808010 -1.973037 -1.268153 2.664686 13 | -2.797669 -1.969395 -1.038748 2.547882 14 | -2.633133 -1.823742 -0.876483 2.362676 15 | -2.690525 -1.769874 -0.762071 2.168484 16 | -2.647762 -1.551238 -0.627847 1.831156 17 | -2.558299 -1.558838 -0.563755 1.345730 18 | -2.399362 -1.392903 -0.402997 0.962529 19 | -2.338294 -1.334617 -0.384291 0.690720 20 | -2.118152 -1.233975 -0.408800 0.515002 21 | -1.863011 -1.065959 -0.338213 0.416887 22 | -2.834007 -2.106371 -1.157974 2.483521 23 | -2.911267 -1.996024 -0.896910 2.409448 24 | -2.883989 -1.882150 -0.662818 2.305880 25 | -2.812450 -1.762655 -0.536760 1.998932 26 | -2.743021 -1.635653 -0.380779 1.784372 27 | -2.655375 -1.493473 -0.426285 1.395682 28 | -2.686322 -1.396504 -0.331295 1.133608 29 | -2.468992 -1.309050 -0.310174 0.941436 30 | -2.285643 -1.161726 -0.291755 0.763398 31 | -2.048525 -1.067342 -0.282982 0.588896 32 | -2.983623 -2.093785 -0.906045 2.319664 33 | -3.033874 -1.975881 -0.601805 2.303961 34 | -2.964183 -1.913576 -0.434137 2.150561 35 | -2.933698 -1.731903 -0.318863 1.948952 36 | -2.855710 -1.696786 -0.328421 1.635525 37 | -2.873330 -1.484700 -0.215668 1.454998 38 | -2.806566 -1.345108 -0.216776 1.217922 39 | -2.704017 -1.226436 -0.144539 1.072277 40 | -2.539427 -1.086140 -0.102860 0.818935 41 | -2.232934 -1.044821 -0.169917 0.756883 42 | -3.103821 -2.165523 -0.404842 2.286773 43 | -3.075227 -2.090845 -0.220133 2.140882 44 | -3.113715 -1.946183 -0.172865 2.014714 45 | -3.063745 -1.760939 -0.117429 1.875800 46 | -3.047346 -1.662240 -0.081407 1.657275 47 | -3.069454 -1.541792 -0.184934 1.454572 48 | -3.023974 -1.303021 -0.058030 1.309395 49 | -2.933935 -1.198720 -0.010670 1.121542 50 | -2.888250 -1.070457 -0.014674 0.888046 51 | -2.716269 -0.972368 -0.024669 0.873374 52 | 3.056217 -2.334791 0.387199 2.201962 53 | 3.105997 -2.163410 0.180541 2.087569 54 | 3.092819 -2.108762 0.118291 1.948065 55 | 3.028863 -1.846834 0.184199 1.753310 56 | 3.039981 -1.623563 0.075346 1.630026 57 | 2.994642 -1.457964 0.053296 1.444572 58 | 2.999339 -1.269620 0.051440 1.316256 59 | 2.972099 -1.126943 0.054831 1.144408 60 | 2.905545 -0.950144 -0.005672 1.057389 61 | 2.693033 -0.844050 0.091668 0.925861 62 | 3.011342 -2.417892 0.837769 2.161509 63 | 2.992200 -2.280237 0.643174 2.049221 64 | 2.944205 -2.104697 0.438941 1.941485 65 | 2.972946 -1.928122 0.312789 1.776304 66 | 2.925914 -1.727551 0.284309 1.633502 67 | 2.777249 -1.502415 0.207956 1.551741 68 | 2.786271 -1.212316 0.213016 1.390449 69 | 2.697530 -0.993440 0.245308 1.251362 70 | 2.566585 -0.899326 0.192772 1.136518 71 | 2.217216 -0.761362 0.102403 1.066187 72 | 2.913379 -2.532497 1.102471 2.074801 73 | 2.836398 -2.418011 0.840259 1.933899 74 | 2.844452 -2.275035 0.636913 1.881324 75 | 2.797083 -2.038489 0.553691 1.735765 76 | 2.804257 -1.770016 0.379959 1.567267 77 | 2.648382 -1.425399 0.361243 1.478117 78 | 2.593472 -1.118702 0.350214 1.499199 79 | 2.476576 -0.895567 0.310248 1.287132 80 | 2.304299 -0.775705 0.253206 1.143633 81 | 1.993220 -0.614701 0.238462 1.064028 82 | 2.793716 -2.787739 1.200317 1.999871 83 | 2.792053 -2.649542 1.038744 1.940632 84 | 2.697278 -2.463340 0.855801 1.857601 85 | 2.673890 -2.215723 0.707776 1.713775 86 | 2.638096 -1.761210 0.656680 1.636344 87 | 2.559548 -1.297807 0.529247 1.503640 88 | 2.397402 -0.980990 0.504446 1.390974 89 | 2.298408 -0.752133 0.397817 1.277478 90 | 2.080093 -0.507094 0.351862 1.155084 91 | 1.832776 -0.429624 0.322257 1.095791 92 | 2.727598 -2.892049 1.354572 1.986951 93 | 2.709838 -2.817370 1.086514 1.953962 94 | 2.657433 -2.706875 0.974841 1.827174 95 | 2.619733 -2.491013 0.811383 1.734810 96 | 2.541955 -1.964636 0.724112 1.593529 97 | 2.424866 -1.067221 0.585556 1.572837 98 | 2.352266 -0.665994 0.561213 1.386299 99 | 2.145919 -0.414938 0.551834 1.426785 100 | 1.997722 -0.359802 0.404270 1.260779 101 | 1.870899 -0.227715 0.452212 1.195925 102 | -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_optimal/toa_ranges.txt: -------------------------------------------------------------------------------- 1 | # ranges to 4 beacons (m) 2 | 19.728187 19.593717 8.321256 8.340268 3 | 18.165358 19.051709 8.959848 6.582503 4 | 16.634128 18.525127 9.696993 4.838500 5 | 14.900093 18.258057 10.965444 3.321939 6 | 13.523438 18.043803 12.212008 2.210995 7 | 12.060977 17.994420 13.661263 2.102052 8 | 10.817675 18.150224 15.067268 3.314203 9 | 9.755679 18.603138 16.480939 4.919391 10 | 8.895199 19.089759 18.154117 6.525903 11 | 8.155663 19.659899 19.827638 8.210585 12 | 19.088548 18.111352 6.476337 8.702518 13 | 17.392522 17.396594 7.415181 7.362773 14 | 15.791135 16.797813 8.194397 5.731511 15 | 14.139195 16.358645 9.575835 4.562935 16 | 12.535912 16.423350 11.046087 3.979218 17 | 10.942766 16.339714 12.579785 3.865280 18 | 9.775280 16.455938 14.100560 4.652728 19 | 8.219031 16.683219 15.892713 6.005435 20 | 7.278750 17.358250 17.408352 7.420010 21 | 6.502339 18.179169 18.912466 8.850187 22 | 18.518194 16.536123 4.891371 9.631387 23 | 16.760964 15.759180 5.953654 8.268569 24 | 14.936972 14.966040 7.327507 6.985840 25 | 13.325082 14.664008 8.652748 6.205888 26 | 11.808148 14.573041 10.152872 5.661393 27 | 10.175913 14.395463 11.649753 5.745332 28 | 8.601386 14.693783 13.394550 6.152062 29 | 7.077391 14.937073 15.145545 7.287318 30 | 5.891334 15.900851 16.937776 8.385386 31 | 4.755883 16.590109 18.521514 9.856950 32 | 18.347582 15.045223 3.263440 10.751175 33 | 16.470376 14.119627 4.665534 9.778992 34 | 14.631442 13.505144 6.269258 8.598302 35 | 12.838472 12.931824 7.951279 7.728774 36 | 11.266077 12.930583 9.472821 7.186657 37 | 9.413924 12.741781 11.263084 7.414636 38 | 7.879147 12.967246 12.997332 7.732666 39 | 6.245363 13.224461 14.558889 8.426795 40 | 4.617231 14.247782 16.449152 9.533631 41 | 3.372616 14.877416 18.243928 10.839438 42 | 18.003010 13.544635 2.224827 12.256931 43 | 16.281435 12.736391 4.088642 11.039997 44 | 14.312812 11.553696 5.514479 10.091461 45 | 12.716350 11.272582 7.353083 9.599723 46 | 10.919432 10.908069 9.076421 9.197399 47 | 9.247924 10.741969 10.891549 8.955288 48 | 7.629050 11.254154 12.641860 9.539846 49 | 5.557380 11.850776 14.634663 10.040314 50 | 3.906767 12.697762 16.210380 10.938947 51 | 2.109978 13.343122 17.939312 12.149631 52 | 18.060438 12.180518 2.218277 13.715276 53 | 16.279987 11.038173 3.804391 12.631308 54 | 14.497973 10.047359 5.645202 11.615512 55 | 12.590604 9.564777 7.586739 11.093004 56 | 10.797003 9.112008 9.102310 11.006370 57 | 9.208482 9.230662 10.969920 10.756550 58 | 7.458391 9.517502 12.636420 11.355783 59 | 5.762482 10.033270 14.230491 11.922088 60 | 3.938900 11.085510 16.097117 12.611210 61 | 2.149589 12.309647 18.004517 13.678558 62 | 18.027262 10.850791 3.243091 14.947236 63 | 16.467176 9.545984 4.492462 14.196004 64 | 14.813708 8.522882 6.118870 13.375857 65 | 12.719147 7.720463 7.724892 12.712290 66 | 11.315582 7.289413 9.402280 12.753672 67 | 9.371748 7.230661 11.279240 12.662722 68 | 7.831392 7.879865 12.830309 12.832372 69 | 6.243112 8.413027 14.463809 13.523942 70 | 4.586973 9.528058 16.441084 14.082497 71 | 3.377588 10.977514 18.259996 15.055471 72 | 18.713479 9.875747 4.899235 16.646936 73 | 16.812062 8.521234 5.922490 15.728764 74 | 15.163867 7.153709 6.935912 15.100477 75 | 13.461695 6.291144 8.685012 14.675313 76 | 11.698188 5.657363 10.097129 14.496179 77 | 10.140240 5.629309 11.749233 14.593188 78 | 8.542388 6.305159 13.607506 14.654942 79 | 7.199711 7.043856 15.209645 15.076562 80 | 5.639372 8.408135 16.721833 15.583390 81 | 4.869055 9.767712 18.439790 16.584302 82 | 19.067662 8.941889 6.424641 17.970381 83 | 17.318822 7.238423 7.283120 17.343420 84 | 15.712876 5.991896 8.279257 17.026065 85 | 14.050207 4.722885 9.733122 16.583569 86 | 12.524674 4.069162 11.016087 16.287936 87 | 11.092014 3.765858 12.532502 16.340586 88 | 9.650955 4.331085 14.150712 16.075098 89 | 8.565770 5.832877 15.768423 16.981723 90 | 7.371698 7.282723 17.346315 17.363977 91 | 6.580133 8.893461 18.892034 18.110527 92 | 19.784771 8.394139 8.425648 19.829196 93 | 18.014650 6.494304 8.910515 19.045405 94 | 16.461799 4.708204 9.635321 18.438475 95 | 15.043084 3.122538 10.816103 17.978438 96 | 13.458977 2.188496 12.223478 17.966157 97 | 12.038944 2.134869 13.566028 17.926372 98 | 10.867531 3.374451 14.993305 18.240931 99 | 9.785328 5.018226 16.504138 18.520891 100 | 8.788466 6.679282 18.111954 19.029981 101 | 8.218541 8.186602 19.698566 19.777209 102 | -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_square/toa_ranges.txt: -------------------------------------------------------------------------------- 1 | # ranges to 4 beacons (m) 2 | 2.858899 18.006772 25.530889 18.204827 3 | 4.352309 16.351648 24.344119 18.438912 4 | 6.026845 14.566796 23.036205 18.802626 5 | 7.519792 12.885187 22.122996 19.425112 6 | 9.339709 11.092908 21.124441 20.196907 7 | 11.007151 9.300527 20.324041 20.950715 8 | 12.788717 7.554934 19.522304 21.990968 9 | 14.598103 5.967150 18.806905 23.124715 10 | 16.393120 4.319183 18.458700 24.221546 11 | 18.020222 2.790611 18.240693 25.420218 12 | 4.317972 18.415935 24.171980 16.200439 13 | 5.360512 16.678293 23.077605 16.739804 14 | 6.781834 14.908068 21.573934 17.045590 15 | 8.275976 13.136726 20.540129 17.741551 16 | 9.857884 11.702395 19.550898 18.703995 17 | 11.435309 9.956420 18.644216 19.522207 18 | 13.375929 8.265204 17.790896 20.610248 19 | 14.807844 6.581503 17.312241 21.893381 20 | 16.655781 5.326240 16.690051 23.082435 21 | 18.358749 4.366120 16.212406 24.234458 22 | 5.882205 18.862089 23.096695 14.473810 23 | 6.659248 17.178709 21.841600 14.857382 24 | 7.680969 15.329282 20.640454 15.347245 25 | 9.101422 13.806909 19.289356 16.242852 26 | 10.718449 12.325514 18.104486 17.113065 27 | 12.262827 10.594990 16.966649 18.208058 28 | 13.857808 9.205354 16.170158 19.201273 29 | 15.438796 7.681070 15.508787 20.600264 30 | 17.205414 6.891550 15.048032 21.764923 31 | 18.720009 5.982826 14.563184 23.196162 32 | 7.752292 19.500260 21.940206 12.722218 33 | 8.279642 17.809964 20.623054 13.379641 34 | 9.143013 16.280751 19.318469 13.854725 35 | 10.265046 14.623831 18.061517 14.561973 36 | 11.751143 13.360815 16.683146 15.402750 37 | 13.048636 11.739695 15.655521 16.731290 38 | 14.712346 10.393821 14.689339 17.842904 39 | 16.282327 9.000801 13.701789 19.063403 40 | 17.795846 8.384564 13.227233 20.497925 41 | 19.475789 7.497115 12.871059 21.996990 42 | 9.309116 20.207413 21.073490 11.203105 43 | 9.898140 18.800822 19.745569 11.532540 44 | 10.512339 16.870591 17.977205 12.178375 45 | 11.714264 15.665019 16.669737 13.234434 46 | 12.879379 14.180855 15.321266 14.240926 47 | 14.291451 12.701916 14.164335 15.200133 48 | 15.845143 11.739219 13.072092 16.750171 49 | 17.009053 10.761077 12.387136 17.991929 50 | 18.631543 10.019734 11.489425 19.443759 51 | 20.095890 9.159394 10.988416 21.062064 52 | 11.109543 21.092951 20.204189 9.531548 53 | 11.559032 19.542985 18.529167 9.953281 54 | 12.250446 17.998974 17.096874 10.525813 55 | 13.020836 16.775102 15.802832 11.578070 56 | 14.069789 15.356854 14.145837 12.966317 57 | 15.453328 14.274189 12.929867 14.029336 58 | 16.775045 13.152214 11.634333 15.748220 59 | 18.225208 12.120184 10.430017 17.238984 60 | 19.595827 11.578053 9.713822 18.675640 61 | 20.998251 11.255821 9.310624 20.341336 62 | 12.654393 22.008342 19.346264 7.566935 63 | 13.245257 20.510278 17.671077 8.332786 64 | 13.956609 19.159490 16.155834 9.152197 65 | 14.411155 17.830701 14.558091 10.138864 66 | 15.708019 16.606067 13.036992 11.751586 67 | 16.582074 15.446754 11.764306 13.092954 68 | 17.941630 14.713064 10.256883 14.524379 69 | 19.292323 13.669450 8.975380 16.299550 70 | 20.544493 13.128707 8.250350 17.772834 71 | 22.054353 12.948556 7.664707 19.510508 72 | 14.755148 23.214960 18.863361 6.039654 73 | 14.922318 21.900771 17.236570 6.719463 74 | 15.527109 20.466656 15.297316 7.844474 75 | 16.237303 19.340355 13.941435 9.186884 76 | 17.015083 18.120089 12.184043 10.695706 77 | 18.091854 17.080982 10.659534 12.345661 78 | 19.178996 16.342124 9.383846 13.797843 79 | 20.512658 15.405261 7.953642 15.439804 80 | 21.527318 14.996948 6.620116 17.002918 81 | 23.074379 14.610135 5.803802 18.910268 82 | 16.367601 24.326160 18.281051 4.157333 83 | 16.600520 22.900848 16.660151 5.311410 84 | 17.132404 21.879842 14.868070 6.924348 85 | 17.740543 20.680406 13.333771 8.392835 86 | 18.589105 19.726088 11.508630 9.904642 87 | 19.596825 18.490634 9.854475 11.619631 88 | 20.615249 17.509701 8.287494 12.853179 89 | 21.945306 17.146956 6.759122 15.091978 90 | 23.034122 16.659754 5.314305 16.645675 91 | 24.275776 16.391382 4.121458 18.415110 92 | 18.197826 25.603772 18.290207 2.959908 93 | 18.319233 24.189947 16.408436 4.274829 94 | 18.787765 22.913528 14.477744 5.802487 95 | 19.498120 21.799303 12.787146 7.383148 96 | 20.121755 21.037159 11.169652 9.272264 97 | 20.951377 20.120781 9.382300 10.975476 98 | 22.025083 19.477624 7.613004 12.868062 99 | 23.124540 18.982352 5.896856 14.562560 100 | 24.172736 18.535692 4.298905 16.329920 101 | 25.428174 18.051161 2.829278 18.190264 102 | -------------------------------------------------------------------------------- /data/sim/ch4_rf_2d_nlos/toa_ranges.txt: -------------------------------------------------------------------------------- 1 | # ranges to 4 beacons (m) 2 | 2.858899 18.806772 26.330889 18.204827 3 | 4.352309 17.151648 25.144119 18.438912 4 | 6.026845 15.366796 23.836205 18.802626 5 | 7.519792 13.685187 22.922996 19.425112 6 | 9.339709 11.892908 21.924441 20.196907 7 | 11.007151 10.100527 21.124041 20.950715 8 | 12.788717 8.354934 20.322304 21.990968 9 | 14.598103 6.767150 19.606905 23.124715 10 | 16.393120 5.119183 19.258700 24.221546 11 | 18.020222 3.590611 19.040693 25.420218 12 | 4.317972 19.215935 24.971980 16.200439 13 | 5.360512 17.478293 23.877605 16.739804 14 | 6.781834 15.708068 22.373934 17.045590 15 | 8.275976 13.936726 21.340129 17.741551 16 | 9.857884 12.502395 20.350898 18.703995 17 | 11.435309 10.756420 19.444216 19.522207 18 | 13.375929 9.065204 18.590896 20.610248 19 | 14.807844 7.381503 18.112241 21.893381 20 | 16.655781 6.126240 17.490051 23.082435 21 | 18.358749 5.166120 17.012406 24.234458 22 | 5.882205 19.662089 23.896695 14.473810 23 | 6.659248 17.978709 22.641600 14.857382 24 | 7.680969 16.129282 21.440454 15.347245 25 | 9.101422 14.606909 20.089356 16.242852 26 | 10.718449 13.125514 18.904486 17.113065 27 | 12.262827 11.394990 17.766649 18.208058 28 | 13.857808 10.005354 16.970158 19.201273 29 | 15.438796 8.481070 16.308787 20.600264 30 | 17.205414 7.691550 15.848032 21.764923 31 | 18.720009 6.782826 15.363184 23.196162 32 | 7.752292 20.300260 22.740206 12.722218 33 | 8.279642 18.609964 21.423054 13.379641 34 | 9.143013 17.080751 20.118469 13.854725 35 | 10.265046 15.423831 18.861517 14.561973 36 | 11.751143 14.160815 17.483146 15.402750 37 | 13.048636 12.539695 16.455521 16.731290 38 | 14.712346 11.193821 15.489339 17.842904 39 | 16.282327 9.800801 14.501789 19.063403 40 | 17.795846 9.184564 14.027233 20.497925 41 | 19.475789 8.297115 13.671059 21.996990 42 | 9.309116 21.007413 21.873490 11.203105 43 | 9.898140 19.600822 20.545569 11.532540 44 | 10.512339 17.670591 18.777205 12.178375 45 | 11.714264 16.465019 17.469737 13.234434 46 | 12.879379 14.980855 16.121266 14.240926 47 | 14.291451 13.501916 14.964335 15.200133 48 | 15.845143 12.539219 13.872092 16.750171 49 | 17.009053 11.561077 13.187136 17.991929 50 | 18.631543 10.819734 12.289425 19.443759 51 | 20.095890 9.959394 11.788416 21.062064 52 | 11.109543 21.892951 21.004189 9.531548 53 | 11.559032 20.342985 19.329167 9.953281 54 | 12.250446 18.798974 17.896874 10.525813 55 | 13.020836 17.575102 16.602832 11.578070 56 | 14.069789 16.156854 14.945837 12.966317 57 | 15.453328 15.074189 13.729867 14.029336 58 | 16.775045 13.952214 12.434333 15.748220 59 | 18.225208 12.920184 11.230017 17.238984 60 | 19.595827 12.378053 10.513822 18.675640 61 | 20.998251 12.055821 10.110624 20.341336 62 | 12.654393 22.808342 20.146264 7.566935 63 | 13.245257 21.310278 18.471077 8.332786 64 | 13.956609 19.959490 16.955834 9.152197 65 | 14.411155 18.630701 15.358091 10.138864 66 | 15.708019 17.406067 13.836992 11.751586 67 | 16.582074 16.246754 12.564306 13.092954 68 | 17.941630 15.513064 11.056883 14.524379 69 | 19.292323 14.469450 9.775380 16.299550 70 | 20.544493 13.928707 9.050350 17.772834 71 | 22.054353 13.748556 8.464707 19.510508 72 | 14.755148 24.014960 19.663361 6.039654 73 | 14.922318 22.700771 18.036570 6.719463 74 | 15.527109 21.266656 16.097316 7.844474 75 | 16.237303 20.140355 14.741435 9.186884 76 | 17.015083 18.920089 12.984043 10.695706 77 | 18.091854 17.880982 11.459534 12.345661 78 | 19.178996 17.142124 10.183846 13.797843 79 | 20.512658 16.205261 8.753642 15.439804 80 | 21.527318 15.796948 7.420116 17.002918 81 | 23.074379 15.410135 6.603802 18.910268 82 | 16.367601 25.126160 19.081051 4.157333 83 | 16.600520 23.700848 17.460151 5.311410 84 | 17.132404 22.679842 15.668070 6.924348 85 | 17.740543 21.480406 14.133771 8.392835 86 | 18.589105 20.526088 12.308630 9.904642 87 | 19.596825 19.290634 10.654475 11.619631 88 | 20.615249 18.309701 9.087494 12.853179 89 | 21.945306 17.946956 7.559122 15.091978 90 | 23.034122 17.459754 6.114305 16.645675 91 | 24.275776 17.191382 4.921458 18.415110 92 | 18.197826 26.403772 19.090207 2.959908 93 | 18.319233 24.989947 17.208436 4.274829 94 | 18.787765 23.713528 15.277744 5.802487 95 | 19.498120 22.599303 13.587146 7.383148 96 | 20.121755 21.837159 11.969652 9.272264 97 | 20.951377 20.920781 10.182300 10.975476 98 | 22.025083 20.277624 8.413004 12.868062 99 | 23.124540 19.782352 6.696856 14.562560 100 | 24.172736 19.335692 5.098905 16.329920 101 | 25.428174 18.851161 3.629278 18.190264 102 | -------------------------------------------------------------------------------- /docs/engineering/README.md: -------------------------------------------------------------------------------- 1 | # Engineering Documentation 2 | 3 | This directory contains technical implementation notes, bug fixes, and engineering decisions made during the development of the IPIN-Examples repository. These documents are intended for developers, maintainers, and contributors who need to understand the technical details and evolution of the codebase. 4 | 5 | --- 6 | 7 | ## Documents Overview 8 | 9 | ### Chapter 3 Estimators - Production Ready Improvements 10 | 11 | The following documents detail the comprehensive improvements made to the ch3_estimators module to make it production-ready: 12 | 13 | #### [`complete_implementation_summary.md`](./complete_implementation_summary.md) 14 | **Master overview of all ch3 improvements** 15 | - Complete checklist of all fixes (Must Fix + Should Fix) 16 | - New code statistics (~4650 lines total) 17 | - Key features and examples 18 | - Testing & verification procedures 19 | - Integration with Ch8 principles 20 | - **Read this first** for a high-level understanding of all improvements 21 | 22 | #### [`ch3_implementation_summary.md`](./ch3_implementation_summary.md) 23 | **Initial implementation summary** 24 | - Overview of the three critical fixes 25 | - New utility modules (angles, geometry, observability) 26 | - Applied improvements to examples 27 | - Testing results 28 | - ~900 lines of new production-quality utilities 29 | 30 | #### [`ch3_production_fixes.md`](./ch3_production_fixes.md) 31 | **Detailed technical documentation of critical fixes** 32 | - Fix #1: Angle wrapping in bearing measurements 33 | - Fix #2: Standardized singularity handling in Jacobians 34 | - Fix #3: Observability checks for degenerate geometries 35 | - Complete API reference 36 | - Before/after comparisons 37 | - Performance impact analysis 38 | - ~650 lines of comprehensive technical documentation 39 | 40 | #### [`ch3_robustness_improvements.md`](./ch3_robustness_improvements.md) 41 | **Robustness enhancements for production use** 42 | - Input validation and error handling 43 | - Shared motion/measurement models module 44 | - Unit tests for Jacobian correctness 45 | - Code reuse and maintainability improvements 46 | - ~2000 lines of new production code 47 | 48 | #### [`ch3_bugfix_summary.md`](./ch3_bugfix_summary.md) 49 | **Specific bug fix: Robust Least Squares** 50 | - Issue: Robust LS performance matched Standard LS 51 | - Root cause: Insufficient redundancy (only 4 anchors) 52 | - Solution: Increased to 8 anchors, proper IRLS implementation 53 | - Before: 1.57m error, After: 0.08m error (93.5% improvement) 54 | - Lessons learned about robust estimation requirements 55 | 56 | --- 57 | 58 | ## When to Read These Documents 59 | 60 | ### You're implementing a new feature 61 | → Read `ch3_production_fixes.md` and `ch3_robustness_improvements.md` to understand best practices 62 | 63 | ### You found a bug or unexpected behavior 64 | → Check `ch3_bugfix_summary.md` for similar issues and solutions 65 | 66 | ### You're onboarding to the project 67 | → Start with `complete_implementation_summary.md` for the big picture 68 | 69 | ### You need to understand a specific utility function 70 | → Use `ch3_production_fixes.md` for detailed API documentation 71 | 72 | ### You want to add tests 73 | → See `ch3_robustness_improvements.md` for testing patterns 74 | 75 | --- 76 | 77 | ## Related Code 78 | 79 | These documents describe improvements to: 80 | 81 | - **Core utilities:** `core/utils/` (angles, geometry, observability) 82 | - **Shared models:** `core/models/` (motion and measurement models) 83 | - **Examples:** `ch3_estimators/example_*.py` 84 | - **Tests:** `tests/test_jacobians.py` 85 | 86 | --- 87 | 88 | ## Document Status 89 | 90 | ✅ **All improvements complete and tested** (as of the last update) 91 | 92 | - Production fixes: Complete 93 | - Robustness improvements: Complete 94 | - Documentation: Complete 95 | - Tests: Complete (15 passing tests) 96 | 97 | --- 98 | 99 | ## Contributing 100 | 101 | When adding new features or fixes to ch3 estimators (or other modules): 102 | 103 | 1. Document the **why** (what problem does it solve?) 104 | 2. Document the **what** (what was implemented?) 105 | 3. Document the **how** (API usage and examples) 106 | 4. Add tests to verify correctness 107 | 5. Update relevant engineering documentation 108 | 109 | --- 110 | 111 | ## Questions? 112 | 113 | For questions about: 114 | - **Usage:** See the [user guides](../guides/) directory 115 | - **Theory:** See the chapter-specific docs in `docs/` 116 | - **Implementation:** Refer to the documents in this directory 117 | - **Code:** Read the inline documentation in the source files 118 | 119 | --- 120 | 121 | **Note:** These are engineering/technical documents. For user-facing guides and tutorials, see the [`docs/guides/`](../guides/) directory. 122 | 123 | 124 | -------------------------------------------------------------------------------- /docs/README.md: -------------------------------------------------------------------------------- 1 | # Documentation 2 | 3 | Welcome to the IPIN-Examples documentation. This directory contains various types of documentation to help you understand and use the code effectively. 4 | 5 | --- 6 | 7 | ## 📚 Documentation Types 8 | 9 | ### 🎯 [User Guides](./guides/) 10 | **For practitioners, researchers, and students** 11 | 12 | Decision-making tools and practical guides to help you apply indoor positioning techniques: 13 | 14 | - **[Ch3 Estimator Selection Guide](./guides/ch3_estimator_selection.md)** - Comprehensive guide for choosing the right state estimator (LS, KF, EKF, UKF, PF, FGO) 15 | 16 | → *Start here if you're asking: "Which method should I use for my application?"* 17 | 18 | --- 19 | 20 | ### 🔧 [Engineering Documentation](./engineering/) 21 | **For developers, maintainers, and contributors** 22 | 23 | Technical implementation notes, bug fixes, and engineering decisions: 24 | 25 | - **[Complete Implementation Summary](./engineering/complete_implementation_summary.md)** - Master overview of ch3 improvements 26 | - **[Ch3 Production Fixes](./engineering/ch3_production_fixes.md)** - Critical fixes (angle wrapping, singularity handling, observability) 27 | - **[Ch3 Robustness Improvements](./engineering/ch3_robustness_improvements.md)** - Input validation, shared models, tests 28 | - **[Ch3 Bugfix Summary](./engineering/ch3_bugfix_summary.md)** - Robust LS fix details 29 | 30 | → *Read these if you're contributing code or need to understand implementation details* 31 | 32 | --- 33 | 34 | ### 📖 Chapter-Specific References 35 | **Technical references and equation mappings** 36 | 37 | Detailed documentation for each chapter of the book: 38 | 39 | - **[ch2_equation_mapping.md](./ch2_equation_mapping.md)** - Coordinate transformation equations 40 | - **[CH2_QUICK_REFERENCE.md](./CH2_QUICK_REFERENCE.md)** - Quick reference for Ch2 41 | - **[ch7_slam.md](./ch7_slam.md)** - SLAM concepts and equations 42 | - **[ch8_fusion_api_reference.md](./ch8_fusion_api_reference.md)** - Sensor fusion API reference 43 | - **[ch8_lc_tc_comparison_guide.md](./ch8_lc_tc_comparison_guide.md)** - Loosely vs tightly coupled fusion comparison 44 | - **[ch8_comparison_tool_summary.md](./ch8_comparison_tool_summary.md)** - Ch8 comparison tool documentation 45 | 46 | --- 47 | 48 | ### 🗂️ Additional Documentation 49 | 50 | - **[data_simulation_guide.md](./data_simulation_guide.md)** - How to generate and use simulated datasets 51 | - **[equation_index.yml](./equation_index.yml)** - Searchable index of equations 52 | 53 | --- 54 | 55 | ## 🧭 Quick Navigation 56 | 57 | **I want to...** 58 | 59 | | Goal | Where to Go | 60 | |------|-------------| 61 | | Choose which estimator to use | [`guides/ch3_estimator_selection.md`](./guides/ch3_estimator_selection.md) | 62 | | Understand a code implementation | [`engineering/`](./engineering/) directory | 63 | | Map equations to code | Chapter-specific docs (e.g., `ch2_equation_mapping.md`) | 64 | | Generate datasets | [`data_simulation_guide.md`](./data_simulation_guide.md) | 65 | | Run examples | See README files in each `ch*_*/` folder | 66 | | Understand the book structure | Main [`README.md`](../README.md) at repository root | 67 | 68 | --- 69 | 70 | ## 📝 Documentation Structure 71 | 72 | ``` 73 | docs/ 74 | ├── README.md # This file (navigation hub) 75 | │ 76 | ├── guides/ # User-facing guides 77 | │ ├── README.md # Guide index 78 | │ └── ch3_estimator_selection.md # Estimator selection guide 79 | │ 80 | ├── engineering/ # Technical/engineering docs 81 | │ ├── README.md # Engineering doc index 82 | │ ├── complete_implementation_summary.md 83 | │ ├── ch3_implementation_summary.md 84 | │ ├── ch3_production_fixes.md 85 | │ ├── ch3_robustness_improvements.md 86 | │ └── ch3_bugfix_summary.md 87 | │ 88 | └── [chapter-specific docs] # Ch2, Ch7, Ch8 references 89 | ├── ch2_equation_mapping.md 90 | ├── CH2_QUICK_REFERENCE.md 91 | ├── ch7_slam.md 92 | ├── ch8_fusion_api_reference.md 93 | ├── ch8_lc_tc_comparison_guide.md 94 | ├── ch8_comparison_tool_summary.md 95 | ├── data_simulation_guide.md 96 | └── equation_index.yml 97 | ``` 98 | 99 | --- 100 | 101 | ## 🤝 Contributing Documentation 102 | 103 | When adding new documentation: 104 | 105 | ### For User Guides (`guides/`) 106 | - Focus on **practical decision-making** 107 | - Include **quick reference tables** 108 | - Provide **real examples** and performance data 109 | - Target **users/practitioners** 110 | 111 | ### For Engineering Docs (`engineering/`) 112 | - Focus on **implementation details** 113 | - Explain **why** decisions were made 114 | - Document **bugs** and their fixes 115 | - Target **developers/maintainers** 116 | 117 | ### For Chapter References 118 | - Map **equations to code** 119 | - Provide **API references** 120 | - Include **usage examples** 121 | - Maintain **consistency** with book notation 122 | 123 | --- 124 | 125 | ## 📚 Related Resources 126 | 127 | - **Main README:** [`../README.md`](../README.md) - Repository overview and setup 128 | - **Jupyter Notebooks:** [`../notebooks/`](../notebooks/) - Interactive tutorials 129 | - **Chapter READMEs:** `../ch*_*/README.md` - Chapter-specific examples and usage 130 | - **Tests:** [`../tests/`](../tests/) - Unit tests showing API usage 131 | 132 | --- 133 | 134 | **Questions or suggestions?** Open an issue or submit a pull request! 135 | 136 | 137 | -------------------------------------------------------------------------------- /core/eval/metrics.py: -------------------------------------------------------------------------------- 1 | """ 2 | Evaluation Metrics for Indoor Positioning. 3 | 4 | This module provides functions to compute error metrics and consistency 5 | statistics for positioning algorithms. 6 | 7 | Author: Navigation Engineering Team 8 | Date: December 2025 9 | """ 10 | 11 | from typing import Dict, Optional, Union 12 | 13 | import numpy as np 14 | 15 | 16 | def compute_position_errors( 17 | truth: np.ndarray, estimated: np.ndarray 18 | ) -> np.ndarray: 19 | """ 20 | Compute position errors between true and estimated positions. 21 | 22 | Args: 23 | truth: True positions, shape (N, 2) or (N, 3) 24 | estimated: Estimated positions, shape (N, 2) or (N, 3) 25 | 26 | Returns: 27 | errors: Position error vectors, shape (N, 2) or (N, 3) 28 | 29 | Raises: 30 | ValueError: If inputs have incompatible shapes 31 | """ 32 | truth = np.asarray(truth) 33 | estimated = np.asarray(estimated) 34 | 35 | if truth.shape != estimated.shape: 36 | raise ValueError( 37 | f"Shape mismatch: truth {truth.shape} vs estimated {estimated.shape}" 38 | ) 39 | 40 | return estimated - truth 41 | 42 | 43 | def compute_rmse(errors: np.ndarray, axis: Optional[int] = None) -> Union[float, np.ndarray]: 44 | """ 45 | Compute Root Mean Square Error (RMSE). 46 | 47 | Args: 48 | errors: Error vectors, shape (N, d) or (N,) 49 | axis: Axis along which to compute RMSE 50 | None: scalar RMSE across all dimensions 51 | 0: per-dimension RMSE 52 | 1: per-sample RMSE 53 | 54 | Returns: 55 | rmse: RMSE value(s) 56 | """ 57 | errors = np.asarray(errors) 58 | 59 | if axis is None: 60 | # Scalar RMSE across all dimensions 61 | return np.sqrt(np.mean(errors**2)) 62 | else: 63 | # Per-axis or per-sample RMSE 64 | return np.sqrt(np.mean(errors**2, axis=axis)) 65 | 66 | 67 | def compute_error_stats(errors: np.ndarray) -> Dict[str, float]: 68 | """ 69 | Compute error statistics. 70 | 71 | Args: 72 | errors: Error vectors, shape (N, d) or (N,) 73 | 74 | Returns: 75 | stats: Dictionary with keys: 76 | - 'mean': Mean error 77 | - 'median': Median error 78 | - 'std': Standard deviation 79 | - 'rmse': Root mean square error 80 | - 'p50': 50th percentile (median) 81 | - 'p75': 75th percentile 82 | - 'p90': 90th percentile 83 | - 'p95': 95th percentile 84 | - 'max': Maximum error 85 | """ 86 | errors = np.asarray(errors) 87 | 88 | # Compute error magnitudes if multi-dimensional 89 | if errors.ndim > 1: 90 | error_magnitudes = np.linalg.norm(errors, axis=1) 91 | else: 92 | error_magnitudes = np.abs(errors) 93 | 94 | stats = { 95 | "mean": float(np.mean(error_magnitudes)), 96 | "median": float(np.median(error_magnitudes)), 97 | "std": float(np.std(error_magnitudes)), 98 | "rmse": float(np.sqrt(np.mean(error_magnitudes**2))), 99 | "p50": float(np.percentile(error_magnitudes, 50)), 100 | "p75": float(np.percentile(error_magnitudes, 75)), 101 | "p90": float(np.percentile(error_magnitudes, 90)), 102 | "p95": float(np.percentile(error_magnitudes, 95)), 103 | "max": float(np.max(error_magnitudes)), 104 | } 105 | 106 | return stats 107 | 108 | 109 | def compute_nees( 110 | truth: np.ndarray, estimated: np.ndarray, covariance: np.ndarray 111 | ) -> np.ndarray: 112 | """ 113 | Compute Normalized Estimation Error Squared (NEES). 114 | 115 | NEES is a consistency metric for filter performance: 116 | NEES = (x_true - x_est)^T P^{-1} (x_true - x_est) 117 | 118 | For consistent estimators, NEES follows chi-squared distribution 119 | with n degrees of freedom (state dimension). 120 | 121 | Args: 122 | truth: True states, shape (N, n) 123 | estimated: Estimated states, shape (N, n) 124 | covariance: Estimation covariances, shape (N, n, n) 125 | 126 | Returns: 127 | nees: NEES values, shape (N,) 128 | 129 | Raises: 130 | ValueError: If inputs have incompatible shapes 131 | """ 132 | truth = np.asarray(truth) 133 | estimated = np.asarray(estimated) 134 | covariance = np.asarray(covariance) 135 | 136 | if truth.shape != estimated.shape: 137 | raise ValueError("truth and estimated must have same shape") 138 | 139 | N, n = truth.shape 140 | 141 | if covariance.shape != (N, n, n): 142 | raise ValueError( 143 | f"covariance must have shape ({N}, {n}, {n}), " 144 | f"got {covariance.shape}" 145 | ) 146 | 147 | nees = np.zeros(N) 148 | for i in range(N): 149 | error = estimated[i] - truth[i] 150 | try: 151 | P_inv = np.linalg.inv(covariance[i]) 152 | nees[i] = error @ P_inv @ error 153 | except np.linalg.LinAlgError: 154 | nees[i] = np.nan 155 | 156 | return nees 157 | 158 | 159 | def compute_nis(innovation: np.ndarray, S: np.ndarray) -> np.ndarray: 160 | """ 161 | Compute Normalized Innovation Squared (NIS). 162 | 163 | NIS is a consistency metric for measurement updates: 164 | NIS = nu^T S^{-1} nu 165 | 166 | where nu is the innovation and S is the innovation covariance. 167 | 168 | For consistent estimators, NIS follows chi-squared distribution 169 | with m degrees of freedom (measurement dimension). 170 | 171 | Args: 172 | innovation: Innovation vectors, shape (N, m) 173 | S: Innovation covariances, shape (N, m, m) 174 | 175 | Returns: 176 | nis: NIS values, shape (N,) 177 | 178 | Raises: 179 | ValueError: If inputs have incompatible shapes 180 | """ 181 | innovation = np.asarray(innovation) 182 | S = np.asarray(S) 183 | 184 | if innovation.ndim == 1: 185 | innovation = innovation.reshape(-1, 1) 186 | 187 | N, m = innovation.shape 188 | 189 | if S.shape != (N, m, m): 190 | raise ValueError( 191 | f"S must have shape ({N}, {m}, {m}), got {S.shape}" 192 | ) 193 | 194 | nis = np.zeros(N) 195 | for i in range(N): 196 | try: 197 | S_inv = np.linalg.inv(S[i]) 198 | nis[i] = innovation[i] @ S_inv @ innovation[i] 199 | except np.linalg.LinAlgError: 200 | nis[i] = np.nan 201 | 202 | return nis 203 | 204 | 205 | 206 | -------------------------------------------------------------------------------- /notebooks/README.md: -------------------------------------------------------------------------------- 1 | # Jupyter Notebooks 2 | 3 | This directory will contain interactive Jupyter notebooks for each chapter of *Principles of Indoor Positioning and Indoor Navigation*. 4 | 5 | ## Notebooks 6 | 7 | | Notebook | Chapter | Description | Status | 8 | |----------|---------|-------------|--------| 9 | | `ch2_coordinate_systems.ipynb` | 2 | LLH/ECEF/ENU transforms, rotations | ✅ Available | 10 | | `ch3_state_estimation.ipynb` | 3 | LS, WLS, Kalman Filter | ✅ Available | 11 | | `ch4_rf_positioning.ipynb` | 4 | TOA, TDOA, AOA, RSS positioning | ✅ Available | 12 | | `ch5_fingerprinting.ipynb` | 5 | NN, k-NN, Bayesian fingerprinting | ✅ Available | 13 | | `ch6_dead_reckoning.ipynb` | 6 | IMU strapdown, PDR, environmental sensors | ✅ Available | 14 | | `ch7_slam.ipynb` | 7 | Pose graph SLAM, ICP scan matching | ✅ Available | 15 | | `ch8_sensor_fusion.ipynb` | 8 | TC/LC fusion, chi-square gating | ✅ Available | 16 | 17 | ## 🚀 Quick Start with Google Colab 18 | 19 | 1. Open a notebook in Google Colab 20 | 2. **Set the `GITHUB_REPO` variable** in the setup cell to your repository URL: 21 | ```python 22 | GITHUB_REPO = "https://github.com/YOUR_USERNAME/IPIN-Examples.git" 23 | ``` 24 | 3. Run the setup cell - it will clone and install dependencies automatically 25 | 26 | **Or run locally:** 27 | ```bash 28 | cd IPIN-Examples 29 | jupyter notebook notebooks/ 30 | ``` 31 | 32 | ## ⚠️ Troubleshooting 33 | 34 | If you encounter import errors: 35 | 36 | 1. **Restart the kernel** (Kernel → Restart & Clear Output) 37 | 2. **Re-run the setup cell** (first code cell in each notebook) 38 | 3. See `TROUBLESHOOTING.md` for detailed solutions 39 | 40 | **Recent Fix (Dec 2025)**: Chapter 6 notebook import issue resolved. If you see `NameError: name 'euler_to_quat' is not defined`, make sure you have the latest version and restart your kernel. 41 | 42 | ## Important Resources 43 | 44 | In addition to notebooks, please explore the extensive resources already available: 45 | 46 | 1. **Example Scripts** - Each `ch*_*/` directory contains runnable Python examples (24+ scripts total) 47 | 2. **Chapter READMEs** - Comprehensive documentation in each chapter folder with equation-to-code mappings 48 | 3. **Documentation** - Additional guides in `docs/` folder 49 | 4. **Simulated Datasets** - Pre-generated datasets in `data/sim/` for testing and experimentation 50 | 51 | ## Running Example Scripts 52 | 53 | ### Chapter 2: Coordinate Systems 54 | 55 | ```bash 56 | python ch2_coords/example_coordinate_transforms.py 57 | ``` 58 | 59 | ### Chapter 3: State Estimation 60 | 61 | ```bash 62 | python ch3_estimators/example_least_squares.py 63 | python ch3_estimators/example_kalman_1d.py 64 | python ch3_estimators/example_ekf_range_bearing.py 65 | python ch3_estimators/example_comparison.py 66 | ``` 67 | 68 | ### Chapter 4: RF Point Positioning 69 | 70 | ```bash 71 | python ch4_rf_point_positioning/example_toa_positioning.py 72 | python ch4_rf_point_positioning/example_tdoa_positioning.py 73 | python ch4_rf_point_positioning/example_aoa_positioning.py 74 | python ch4_rf_point_positioning/example_comparison.py 75 | ``` 76 | 77 | ### Chapter 5: Fingerprinting 78 | 79 | ```bash 80 | python ch5_fingerprinting/example_deterministic.py 81 | python ch5_fingerprinting/example_probabilistic.py 82 | python ch5_fingerprinting/example_pattern_recognition.py 83 | python ch5_fingerprinting/example_comparison.py 84 | ``` 85 | 86 | ### Chapter 6: Dead Reckoning 87 | 88 | ```bash 89 | python ch6_dead_reckoning/example_imu_strapdown.py 90 | python ch6_dead_reckoning/example_pdr.py 91 | python ch6_dead_reckoning/example_wheel_odometry.py 92 | python ch6_dead_reckoning/example_zupt.py 93 | python ch6_dead_reckoning/example_allan_variance.py 94 | python ch6_dead_reckoning/example_environment.py 95 | python ch6_dead_reckoning/example_comparison.py 96 | ``` 97 | 98 | ### Chapter 7: SLAM 99 | 100 | ```bash 101 | python ch7_slam/example_pose_graph_slam.py 102 | python ch7_slam/example_bundle_adjustment.py 103 | ``` 104 | 105 | ### Chapter 8: Sensor Fusion 106 | 107 | ```bash 108 | python -m ch8_sensor_fusion.lc_uwb_imu_ekf 109 | python -m ch8_sensor_fusion.tc_uwb_imu_ekf 110 | python -m ch8_sensor_fusion.compare_lc_tc 111 | python -m ch8_sensor_fusion.observability_demo 112 | python -m ch8_sensor_fusion.temporal_calibration_demo 113 | python -m ch8_sensor_fusion.tuning_robust_demo 114 | ``` 115 | 116 | ## Available Documentation 117 | 118 | The `docs/` folder contains additional guides and references: 119 | 120 | | Document | Description | 121 | |----------|-------------| 122 | | `equation_index.yml` | Maps book equations to code implementations | 123 | | `ch2_equation_mapping.md` | Chapter 2 equation-to-code mappings | 124 | | `CH2_QUICK_REFERENCE.md` | Quick reference for coordinate transforms | 125 | | `ch7_slam.md` | SLAM algorithms documentation | 126 | | `ch8_fusion_api_reference.md` | Sensor fusion API reference | 127 | | `ch8_lc_tc_comparison_guide.md` | Loosely vs Tightly Coupled fusion comparison | 128 | | `data_simulation_guide.md` | Guide for generating simulated datasets | 129 | 130 | ## Simulated Datasets 131 | 132 | The `data/sim/` folder contains pre-generated datasets for each chapter: 133 | 134 | | Dataset Folder | Description | 135 | |----------------|-------------| 136 | | `ch2_coordinate_transforms/` | Coordinate system test data | 137 | | `ch3_estimator_comparison/` | State estimation scenarios | 138 | | `ch4_rf_2d_positioning/` | RF positioning with anchors | 139 | | `ch5_wifi_fingerprint_*/` | Wi-Fi fingerprinting datasets (dense/sparse) | 140 | | `ch6_pdr_*/` | PDR datasets (indoor/outdoor, consumer/tactical) | 141 | | `ch6_strapdown_*/` | IMU strapdown navigation data | 142 | | `ch6_wheel_odom_*/` | Wheel odometry datasets | 143 | | `ch6_zupt_*/` | Zero-velocity update scenarios | 144 | | `ch6_env_*/` | Environmental sensor data (barometer, magnetometer) | 145 | | `ch7_slam_2d/` | 2D SLAM test environments | 146 | | `ch8_fusion_2d_imu_uwb_*/` | IMU-UWB fusion datasets | 147 | 148 | Run the dataset generation scripts in `scripts/` to create custom datasets. 149 | 150 | ## Contributing 151 | 152 | To contribute a notebook: 153 | 154 | 1. Use the naming convention `ch{N}_{topic}.ipynb` 155 | 2. Include markdown cells explaining the algorithms and equations 156 | 3. Reference book equations using format: `Eq. (X.Y)` 157 | 4. Generate visualizations that can be saved as static images 158 | 5. Ensure the notebook runs without errors using `jupyter nbconvert --execute` 159 | 160 | ## Dependencies 161 | 162 | Notebooks will require: 163 | - `jupyter` or `jupyterlab` 164 | - All dependencies in `pyproject.toml` 165 | - Optionally: `ipywidgets` for interactive demos 166 | 167 | ```bash 168 | pip install jupyter 169 | jupyter notebook notebooks/ 170 | ``` 171 | --------------------------------------------------------------------------------