diff --git a/CLAUDE.md b/CLAUDE.md index 2032283..7536204 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -1475,7 +1475,7 @@ lines). Two traps found doing it: single constant, so one file compiled differently while printing exactly the same thing. Compare output, not code objects. -### The other four linters are configured and the repo does not pass them +### The other four linters: black passes now, mypy is the gap pyflakes is the exception, not the rule. The README used to tell readers to run `black .`, `ruff check .`, `mypy .` and `pylint`, as though the repository @@ -1491,17 +1491,23 @@ was checked to differ only by trailing whitespace before the change was believed — 3961 removed lines against 3961 added, zero differing by anything else. That took the count to 1879 at no semantic risk. -**Ruff refused the other 907, and was right to.** They sit inside docstrings, -where whitespace is string content rather than layout — and here that content is -*printed*, since every example now passes `description=__doc__` to argparse. A -tool declining an unsafe fix is not an obstacle to route around with -`--unsafe-fixes`. +**Ruff refused the other 907 and was right to** — they sit inside string +literals, where whitespace is content rather than layout. Reaching for +`--unsafe-fixes` there would have been wrong. -What remains is mostly not lint: 727 are `List[int]`-for-`list[int]` style -modernisations that only became legal when the floor moved to 3.10. The ~200 -after that are the ones with content, and **B905 is the group to read first** — -41 `zip()` calls with no `strict=`, which truncate to the shorter argument -without saying so. +**Black cleared 889 of those 907, and that is the interesting part.** Black +knows which triple-quoted strings are *docstrings* and normalises those, where +ruff could only see a string and had to stop. So the answer to a tool declining +an unsafe fix was a tool that could tell the difference, not overriding the +first one. The twelve that survive both are in argparse `epilog=` strings, which +are not docstrings and whose blank lines get printed. + +Running black took ruff from 1879 to **951** and made `black --check` pass on +all 299 files. What remains is mostly not lint: 727 are +`List[int]`-for-`list[int]` modernisations that only became legal when the floor +moved to 3.10. The ~140 after that are the ones with content, and **B905 is the +group to read first** — 41 `zip()` calls with no `strict=`, which truncate to +the shorter argument without saying so. `tests/test_lint_debt_only_shrinks.py` records the count **per rule**, not as a total: a total lets ten fixed W293 pay for ten new B905, which is the opposite @@ -1509,9 +1515,16 @@ of what a ratchet is for. It fails in both directions — a rule that grows, and baseline left above the real count, because a stale number hides the debt it exists to expose. -Black is untouched deliberately. It would reformat 237 files, which is a diff -nobody can review and a `git blame` nobody can read, for no defect fixed. Worth -doing when no other session is mid-flight, in its own commit, and not before. +**mypy is now the honest remaining gap**, at 406 errors in `core/` alone, and +it is the one of the four that would be red on arrival and stay red. Nothing +about the black run touched it; formatting does not change types. + +The black run itself was 241 files, and it changed `.py` only — no figure or +data byte moved. **The full suite reads 3404 passed / 21 skipped on both sides +of it**, measured on trees verified identical beforehand. What made that safe to +*believe* was not the argument that black is a formatter and therefore harmless; +it was that the README transcripts and the figure gate would have said +otherwise if it were not. ## Parallel sessions diff --git a/README.md b/README.md index d39f429..910355f 100644 --- a/README.md +++ b/README.md @@ -213,22 +213,22 @@ runs in CI on every pull request: pytest ``` -### The linters are configured, and the repository does not pass them +### Where the linters stand -This is worth stating plainly, because the section used to imply otherwise and -a reader who ran these got thousands of complaints and reasonably concluded -they had broken something. Measured over `core/`, the chapters, `scripts/`, -`tools/` and `tests/`: +Stated plainly, because this section used to imply the repository passed all of +them and a reader who ran them got thousands of complaints. Measured over +`core/`, the chapters, `scripts/`, `tools/` and `tests/`: -| Tool | Today | -|---|---| -| `ruff check` | **1879 findings.** 907 are whitespace inside docstrings that ruff will not safely fix — there it is string content, not layout. 727 are annotation modernisations (`List[int]` → `list[int]`) that only became available when the floor moved to 3.10. | -| `black --check` | 237 of 293 files would be reformatted | -| `mypy` | 404 errors in `core/` alone | +| Tool | Today | Was | +|---|---|---| +| `black --check` | **passes** — 299 files unchanged | 237 of 288 reformatted | +| `ruff check` | **951 findings.** 727 are annotation modernisations (`List[int]` → `list[int]`) that only became available when the floor moved to 3.10; the ~140 after that are the ones with content, `zip()` without `strict=` first | 5836 | +| `mypy` | 406 errors in `core/` alone — **the remaining gap** | 404 | `tests/test_lint_debt_only_shrinks.py` records the ruff count per rule and fails -if any of them grows, so the number can only go down from here. It is not -pass/fail on the linters themselves, which would be red on arrival and stay red. +both when one grows and when a baseline sits above the real count, so the number +can only go down. It is not pass/fail on the linters themselves; mypy would be +red on arrival and stay red. ```bash ruff check core ch*_* tests diff --git a/ch2_coords/__init__.py b/ch2_coords/__init__.py index 243f03a..453a0cd 100644 --- a/ch2_coords/__init__.py +++ b/ch2_coords/__init__.py @@ -5,4 +5,3 @@ Reference: Chapter 2 of the IPIN book """ - diff --git a/ch2_coords/example_attitude_visualization.py b/ch2_coords/example_attitude_visualization.py index a5955c0..64722c6 100644 --- a/ch2_coords/example_attitude_visualization.py +++ b/ch2_coords/example_attitude_visualization.py @@ -105,18 +105,34 @@ def _style_3d(ax, elev: float = VIEW_ELEV, azim: float = VIEW_AZIM) -> None: def _draw_reference(ax, axis_names=("X", "Y", "Z")) -> None: """Draw the faint unrotated frame, without labels, behind the subject.""" - plot_frame_3d(ax, None, alpha=0.22, linewidth=1.4, linestyle="--", - axis_names=axis_names, show_labels=False) + plot_frame_3d( + ax, + None, + alpha=0.22, + linewidth=1.4, + linestyle="--", + axis_names=axis_names, + show_labels=False, + ) def _draw_rotation_axis(ax, axis: str, radius: float = 1.45) -> None: """Mark the axis a rotation happens about, as a dashed grey line.""" - direction = {"x": np.array([1.0, 0.0, 0.0]), - "y": np.array([0.0, 1.0, 0.0]), - "z": np.array([0.0, 0.0, 1.0])}[axis] + direction = { + "x": np.array([1.0, 0.0, 0.0]), + "y": np.array([0.0, 1.0, 0.0]), + "z": np.array([0.0, 0.0, 1.0]), + }[axis] span = np.outer(np.array([-radius, radius]), direction) - ax.plot(span[:, 0], span[:, 1], span[:, 2], - color="0.25", linestyle=":", linewidth=2.0, zorder=0) + ax.plot( + span[:, 0], + span[:, 1], + span[:, 2], + color="0.25", + linestyle=":", + linewidth=2.0, + zorder=0, + ) def plot_euler_convention(angle_deg: float = 35.0) -> plt.Figure: @@ -133,14 +149,26 @@ def plot_euler_convention(angle_deg: float = 35.0) -> plt.Figure: """ angle = np.deg2rad(angle_deg) panels = [ - (f"Yaw psi = {angle_deg:g} deg, about Z\nEq. (2.14)", - euler_to_rotation_matrix(0.0, 0.0, angle), YAW_AXIS), - (f"Roll phi = {angle_deg:g} deg, about Y\nEq. (2.15)", - euler_to_rotation_matrix(angle, 0.0, 0.0), ROLL_AXIS), - (f"Pitch theta = {angle_deg:g} deg, about X\nEq. (2.16)", - euler_to_rotation_matrix(0.0, angle, 0.0), PITCH_AXIS), - ("Composed C = Rx(theta) Ry(phi) Rz(psi)\nEq. (2.17)", - euler_to_rotation_matrix(angle, angle, angle), None), + ( + f"Yaw psi = {angle_deg:g} deg, about Z\nEq. (2.14)", + euler_to_rotation_matrix(0.0, 0.0, angle), + YAW_AXIS, + ), + ( + f"Roll phi = {angle_deg:g} deg, about Y\nEq. (2.15)", + euler_to_rotation_matrix(angle, 0.0, 0.0), + ROLL_AXIS, + ), + ( + f"Pitch theta = {angle_deg:g} deg, about X\nEq. (2.16)", + euler_to_rotation_matrix(0.0, angle, 0.0), + PITCH_AXIS, + ), + ( + "Composed C = Rx(theta) Ry(phi) Rz(psi)\nEq. (2.17)", + euler_to_rotation_matrix(angle, angle, angle), + None, + ), ] fig = plt.figure(figsize=(14, 4.2)) @@ -162,9 +190,9 @@ def plot_euler_convention(angle_deg: float = 35.0) -> plt.Figure: return fig -def plot_passive_vs_active(roll_deg: float = 0.0, - pitch_deg: float = 0.0, - yaw_deg: float = 50.0) -> plt.Figure: +def plot_passive_vs_active( + roll_deg: float = 0.0, pitch_deg: float = 0.0, yaw_deg: float = 50.0 +) -> plt.Figure: """Figure 2: the passive/active transpose trap. Chapter 2's C is passive: ``x_new = C x_old`` rotates the *coordinates*. @@ -188,10 +216,8 @@ def plot_passive_vs_active(roll_deg: float = 0.0, fig = plt.figure(figsize=(11, 4.6)) for index, (title, matrix) in enumerate( [ - ("Passive: C, Eq. (2.21)\n'x_new = C x_old' -- rotates coordinates", - C), - ("Active: C^T, Ch. 6 Eq. (6.13)\nbody-to-map -- rotates the vector", - C.T), + ("Passive: C, Eq. (2.21)\n'x_new = C x_old' -- rotates coordinates", C), + ("Active: C^T, Ch. 6 Eq. (6.13)\nbody-to-map -- rotates the vector", C.T), ], start=1, ): @@ -254,7 +280,8 @@ def plot_gimbal_lock() -> plt.Figure: _style_3d(ax, **gimbal_view) ax.set_title( f"roll = 90, yaw = {yaw_deg:g}, pitch = {pitch_deg:g}\n" - "(identical to its neighbour -- that is the lock)", fontsize=9 + "(identical to its neighbour -- that is the lock)", + fontsize=9, ) # Show numerically that the recovery collapses to a single angle. @@ -299,9 +326,7 @@ def plot_frame_chain() -> plt.Figure: """ # ENU->NED as an explicit matrix, obtained by mapping the basis vectors # through the library function rather than hard-coding it here. - C_enu_to_ned = np.column_stack( - [enu_to_ned(basis) for basis in np.eye(3)] - ) + C_enu_to_ned = np.column_stack([enu_to_ned(basis) for basis in np.eye(3)]) attitude = euler_to_rotation_matrix( np.deg2rad(15.0), np.deg2rad(10.0), np.deg2rad(40.0) @@ -310,8 +335,11 @@ def plot_frame_chain() -> plt.Figure: panels = [ ("ENU (local tangent)\nEast, North, Up", np.eye(3), ("E", "N", "U")), ("NED, Eq. (2.5)\nNorth, East, Down", C_enu_to_ned, ("N", "E", "D")), - ("Body, Eqs. (2.6)/(2.7)\nroll 15, pitch 10, yaw 40", - attitude, ("x", "y", "z")), + ( + "Body, Eqs. (2.6)/(2.7)\nroll 15, pitch 10, yaw 40", + attitude, + ("x", "y", "z"), + ), ] fig = plt.figure(figsize=(12, 4.4)) diff --git a/ch2_coords/example_coordinate_transforms.py b/ch2_coords/example_coordinate_transforms.py index 77fa607..aeda06b 100644 --- a/ch2_coords/example_coordinate_transforms.py +++ b/ch2_coords/example_coordinate_transforms.py @@ -53,33 +53,33 @@ def load_dataset(data_dir: str) -> dict: """Load coordinate transforms dataset. - + Args: data_dir: Path to dataset directory (e.g., 'data/sim/ch2_coords_san_francisco') - + Returns: Dictionary with loaded data arrays and config """ path = Path(data_dir) data = { - 'llh': np.loadtxt(path / 'llh_coordinates.txt'), - 'ecef': np.loadtxt(path / 'ecef_coordinates.txt'), - 'enu': np.loadtxt(path / 'enu_coordinates.txt'), - 'reference_llh': np.loadtxt(path / 'reference_llh.txt'), - 'euler_angles': np.loadtxt(path / 'euler_angles.txt'), - 'quaternions': np.loadtxt(path / 'quaternions.txt'), + "llh": np.loadtxt(path / "llh_coordinates.txt"), + "ecef": np.loadtxt(path / "ecef_coordinates.txt"), + "enu": np.loadtxt(path / "enu_coordinates.txt"), + "reference_llh": np.loadtxt(path / "reference_llh.txt"), + "euler_angles": np.loadtxt(path / "euler_angles.txt"), + "quaternions": np.loadtxt(path / "quaternions.txt"), } - with open(path / 'config.json') as f: - data['config'] = json.load(f) + with open(path / "config.json") as f: + data["config"] = json.load(f) return data def run_with_dataset(data_dir: str) -> None: """Run coordinate transform examples using pre-generated dataset. - + Args: data_dir: Path to dataset directory """ @@ -90,7 +90,7 @@ def run_with_dataset(data_dir: str) -> None: # Load dataset data = load_dataset(data_dir) - config = data['config'] + config = data["config"] print("\nDataset Info:") print(f" Location: {config.get('location', 'Unknown')}") @@ -100,17 +100,23 @@ def run_with_dataset(data_dir: str) -> None: print("\n1. LLH to ECEF Transformation (Dataset Verification)") print("-" * 70) - llh_sample = data['llh'][0] - ecef_dataset = data['ecef'][0] + llh_sample = data["llh"][0] + ecef_dataset = data["ecef"][0] - print(f"Dataset LLH: lat={np.rad2deg(llh_sample[0]):.6f}°, " - f"lon={np.rad2deg(llh_sample[1]):.6f}°, h={llh_sample[2]:.2f}m") - print(f"Dataset ECEF: [{ecef_dataset[0]:,.2f}, {ecef_dataset[1]:,.2f}, {ecef_dataset[2]:,.2f}] m") + print( + f"Dataset LLH: lat={np.rad2deg(llh_sample[0]):.6f}°, " + f"lon={np.rad2deg(llh_sample[1]):.6f}°, h={llh_sample[2]:.2f}m" + ) + print( + f"Dataset ECEF: [{ecef_dataset[0]:,.2f}, {ecef_dataset[1]:,.2f}, {ecef_dataset[2]:,.2f}] m" + ) # Verify our transform matches ecef_computed = llh_to_ecef(llh_sample[0], llh_sample[1], llh_sample[2]) diff = np.linalg.norm(ecef_computed - ecef_dataset) - print(f"Computed ECEF: [{ecef_computed[0]:,.2f}, {ecef_computed[1]:,.2f}, {ecef_computed[2]:,.2f}] m") + print( + f"Computed ECEF: [{ecef_computed[0]:,.2f}, {ecef_computed[1]:,.2f}, {ecef_computed[2]:,.2f}] m" + ) print(f"Difference: {diff:.6e} m (should be ~0)") # Example 2: Round-trip LLH -> ECEF -> LLH @@ -121,8 +127,8 @@ def run_with_dataset(data_dir: str) -> None: errors_lon = [] errors_h = [] - for i in range(min(10, len(data['llh']))): - llh_orig = data['llh'][i] + for i in range(min(10, len(data["llh"]))): + llh_orig = data["llh"][i] ecef = llh_to_ecef(llh_orig[0], llh_orig[1], llh_orig[2]) llh_recovered = ecef_to_llh(ecef[0], ecef[1], ecef[2]) @@ -131,49 +137,65 @@ def run_with_dataset(data_dir: str) -> None: errors_h.append(np.abs(llh_recovered[2] - llh_orig[2])) print("Round-trip errors (10 samples):") - print(f" Latitude: {np.max(errors_lat):.2e} rad = {np.rad2deg(np.max(errors_lat)) * 3600:.2e} arcsec") - print(f" Longitude: {np.max(errors_lon):.2e} rad = {np.rad2deg(np.max(errors_lon)) * 3600:.2e} arcsec") + print( + f" Latitude: {np.max(errors_lat):.2e} rad = {np.rad2deg(np.max(errors_lat)) * 3600:.2e} arcsec" + ) + print( + f" Longitude: {np.max(errors_lon):.2e} rad = {np.rad2deg(np.max(errors_lon)) * 3600:.2e} arcsec" + ) print(f" Height: {np.max(errors_h):.2e} m") # Example 3: ENU Frame print("\n3. Local ENU Frame") print("-" * 70) - ref_llh = data['reference_llh'] + ref_llh = data["reference_llh"] if ref_llh.ndim == 1: lat_ref, lon_ref, _ = ref_llh[0], ref_llh[1], ref_llh[2] else: lat_ref, lon_ref, _ = ref_llh[0, 0], ref_llh[0, 1], ref_llh[0, 2] - print(f"Reference point: lat={np.rad2deg(lat_ref):.6f}°, lon={np.rad2deg(lon_ref):.6f}°") + print( + f"Reference point: lat={np.rad2deg(lat_ref):.6f}°, lon={np.rad2deg(lon_ref):.6f}°" + ) # Show first few ENU coordinates print("\nSample ENU coordinates (from dataset):") - for i in range(min(5, len(data['enu']))): - enu = data['enu'][i] + for i in range(min(5, len(data["enu"]))): + enu = data["enu"][i] print(f" Point {i}: E={enu[0]:.2f}m, N={enu[1]:.2f}m, U={enu[2]:.2f}m") # Example 4: Rotation Representations print("\n4. Rotation Representations") print("-" * 70) - euler_sample = data['euler_angles'][0] - quat_sample = data['quaternions'][0] + euler_sample = data["euler_angles"][0] + quat_sample = data["quaternions"][0] - print(f"Dataset Euler: roll={np.rad2deg(euler_sample[0]):.2f}°, " - f"pitch={np.rad2deg(euler_sample[1]):.2f}°, yaw={np.rad2deg(euler_sample[2]):.2f}°") - print(f"Dataset Quaternion: [{quat_sample[0]:.4f}, {quat_sample[1]:.4f}, " - f"{quat_sample[2]:.4f}, {quat_sample[3]:.4f}]") + print( + f"Dataset Euler: roll={np.rad2deg(euler_sample[0]):.2f}°, " + f"pitch={np.rad2deg(euler_sample[1]):.2f}°, yaw={np.rad2deg(euler_sample[2]):.2f}°" + ) + print( + f"Dataset Quaternion: [{quat_sample[0]:.4f}, {quat_sample[1]:.4f}, " + f"{quat_sample[2]:.4f}, {quat_sample[3]:.4f}]" + ) # Convert and verify quat_computed = euler_to_quat(euler_sample[0], euler_sample[1], euler_sample[2]) - R_from_euler = euler_to_rotation_matrix(euler_sample[0], euler_sample[1], euler_sample[2]) + R_from_euler = euler_to_rotation_matrix( + euler_sample[0], euler_sample[1], euler_sample[2] + ) R_from_quat = quat_to_rotation_matrix(quat_sample) - print(f"\nComputed Quaternion: [{quat_computed[0]:.4f}, {quat_computed[1]:.4f}, " - f"{quat_computed[2]:.4f}, {quat_computed[3]:.4f}]") + print( + f"\nComputed Quaternion: [{quat_computed[0]:.4f}, {quat_computed[1]:.4f}, " + f"{quat_computed[2]:.4f}, {quat_computed[3]:.4f}]" + ) print(f"Quaternion norm: {np.linalg.norm(quat_computed):.6f} (should be 1.0)") - print(f"Rotation matrix determinant: {np.linalg.det(R_from_euler):.6f} (should be 1.0)") + print( + f"Rotation matrix determinant: {np.linalg.det(R_from_euler):.6f} (should be 1.0)" + ) # Example 5: Apply the coordinate transform (passive: x_new = C @ x_old) print("\n5. Applying the Coordinate Transform (x_new = C @ x_old)") @@ -258,12 +280,21 @@ def run_with_inline_data() -> None: # Define target points relative to reference, each with the ENU it is named # for. Printing the two together is what makes the section check itself. targets = [ - ("100m East", lat_ref, lon_ref + dlon_100_east, 0.0, - np.array([100.0, 0.0, 0.0])), - ("100m North", lat_ref + dlat_100_north, lon_ref, 0.0, - np.array([0.0, 100.0, 0.0])), - ("50m Up", lat_ref, lon_ref, 50.0, - np.array([0.0, 0.0, 50.0])), + ( + "100m East", + lat_ref, + lon_ref + dlon_100_east, + 0.0, + np.array([100.0, 0.0, 0.0]), + ), + ( + "100m North", + lat_ref + dlat_100_north, + lon_ref, + 0.0, + np.array([0.0, 100.0, 0.0]), + ), + ("50m Up", lat_ref, lon_ref, 50.0, np.array([0.0, 0.0, 50.0])), ] for name, lat_tgt, lon_tgt, height_tgt, enu_named in targets: @@ -275,8 +306,10 @@ def run_with_inline_data() -> None: print(f"\nTarget: {name}") print(f" ENU: [{enu[0]:.2f}, {enu[1]:.2f}, {enu[2]:.2f}] m") - print(f" Error vs. the named offset: " - f"{np.linalg.norm(enu - enu_named) * 1e3:.2f} mm") + print( + f" Error vs. the named offset: " + f"{np.linalg.norm(enu - enu_named) * 1e3:.2f} mm" + ) # Example 4: Rotation representations print("\n4. Rotation Representations") @@ -323,14 +356,18 @@ def run_with_inline_data() -> None: euler_from_quat = quat_to_euler(q) print(f"Quaternion: {q}") - print(f"Euler from quat_to_euler: " - f"[{np.rad2deg(euler_from_quat[0]):.1f}°, " - f"{np.rad2deg(euler_from_quat[1]):.1f}°, " - f"{np.rad2deg(euler_from_quat[2]):.1f}°]") - print(f"Original Euler: " - f"[{np.rad2deg(roll):.1f}°, " - f"{np.rad2deg(pitch):.1f}°, " - f"{np.rad2deg(yaw):.1f}°]") + print( + f"Euler from quat_to_euler: " + f"[{np.rad2deg(euler_from_quat[0]):.1f}°, " + f"{np.rad2deg(euler_from_quat[1]):.1f}°, " + f"{np.rad2deg(euler_from_quat[2]):.1f}°]" + ) + print( + f"Original Euler: " + f"[{np.rad2deg(roll):.1f}°, " + f"{np.rad2deg(pitch):.1f}°, " + f"{np.rad2deg(yaw):.1f}°]" + ) # Round-trip check: Euler -> Quat -> Euler q_rt = euler_to_quat(roll, pitch, yaw) @@ -340,11 +377,11 @@ def run_with_inline_data() -> None: # a yaw of 200 deg gets it back as -160 and a raw subtraction would call # that 2pi of error and print FAIL for a perfect round-trip. The Ch2 # dataset generator had exactly this and reported 360 deg as its accuracy. - rt_error = np.max(np.abs( - angle_diff(np.array([roll, pitch, yaw]), euler_rt) - )) - print(f"\nRound-trip Euler->Quat->Euler error: {rt_error:.2e} rad " - f"({'PASS' if rt_error < 1e-9 else 'FAIL'})") + rt_error = np.max(np.abs(angle_diff(np.array([roll, pitch, yaw]), euler_rt))) + print( + f"\nRound-trip Euler->Quat->Euler error: {rt_error:.2e} rad " + f"({'PASS' if rt_error < 1e-9 else 'FAIL'})" + ) # Example 7: Round-trip rotation conversions (matrix path) print("\n7. Round-trip Rotation Conversions (Matrix Path)") @@ -353,11 +390,15 @@ def run_with_inline_data() -> None: R_from_euler = euler_to_rotation_matrix(roll, pitch, yaw) euler_recovered = rotation_matrix_to_euler(R_from_euler) - print(f"Original Euler: [{np.rad2deg(roll):.1f}°, " - f"{np.rad2deg(pitch):.1f}°, {np.rad2deg(yaw):.1f}°]") - print(f"Recovered Euler: [{np.rad2deg(euler_recovered[0]):.1f}°, " - f"{np.rad2deg(euler_recovered[1]):.1f}°, " - f"{np.rad2deg(euler_recovered[2]):.1f}°]") + print( + f"Original Euler: [{np.rad2deg(roll):.1f}°, " + f"{np.rad2deg(pitch):.1f}°, {np.rad2deg(yaw):.1f}°]" + ) + print( + f"Recovered Euler: [{np.rad2deg(euler_recovered[0]):.1f}°, " + f"{np.rad2deg(euler_recovered[1]):.1f}°, " + f"{np.rad2deg(euler_recovered[2]):.1f}°]" + ) # Example 8: Coordinate frame conversions print("\n8. Practical Indoor Positioning Scenario") @@ -383,13 +424,17 @@ def run_with_inline_data() -> None: print(f"\n{name}:") print(f" ENU: [{enu_pos[0]:.1f}, {enu_pos[1]:.1f}, {enu_pos[2]:.1f}] m") - print(f" LLH: [{np.rad2deg(llh[0]):.6f}°, " - f"{np.rad2deg(llh[1]):.6f}°, {llh[2]:.2f} m]") + print( + f" LLH: [{np.rad2deg(llh[0]):.6f}°, " + f"{np.rad2deg(llh[1]):.6f}°, {llh[2]:.2f} m]" + ) print("\n" + "=" * 70) print("Examples completed successfully!") print("=" * 70) - print("\nTip: Run with --data ch2_coords_san_francisco to use pre-generated dataset") + print( + "\nTip: Run with --data ch2_coords_san_francisco to use pre-generated dataset" + ) def main() -> None: @@ -407,13 +452,13 @@ def main() -> None: # Specify full path to dataset python example_coordinate_transforms.py --data data/sim/ch2_coords_san_francisco - """ + """, ) parser.add_argument( "--data", type=str, default=None, - help="Dataset name or path (e.g., 'ch2_coords_san_francisco' or full path)" + help="Dataset name or path (e.g., 'ch2_coords_san_francisco' or full path)", ) args = parser.parse_args() @@ -425,7 +470,9 @@ def main() -> None: # Try prepending data/sim/ data_path = resolve_data_path(Path("data/sim") / args.data) if not data_path.exists(): - print(f"Error: Dataset not found at '{args.data}' or 'data/sim/{args.data}'") + print( + f"Error: Dataset not found at '{args.data}' or 'data/sim/{args.data}'" + ) print("Available datasets:") sim_dir = resolve_data_path(Path("data/sim")) if sim_dir.exists(): diff --git a/ch3_estimators/__init__.py b/ch3_estimators/__init__.py index 9c8236d..1219e20 100644 --- a/ch3_estimators/__init__.py +++ b/ch3_estimators/__init__.py @@ -4,6 +4,3 @@ This package contains example scripts demonstrating various state estimation algorithms for indoor positioning as described in Chapter 3 of the IPIN book. """ - - - diff --git a/ch3_estimators/example_comparison.py b/ch3_estimators/example_comparison.py index bf5908f..4bb3b1a 100644 --- a/ch3_estimators/example_comparison.py +++ b/ch3_estimators/example_comparison.py @@ -76,33 +76,32 @@ def setup_scenario(seed=42): n_steps = 30 # Landmark/anchor positions - anchors = np.array([ - [0.0, 0.0], - [20.0, 0.0], - [20.0, 20.0], - [0.0, 20.0], - ]) + anchors = np.array( + [ + [0.0, 0.0], + [20.0, 0.0], + [20.0, 20.0], + [0.0, 20.0], + ] + ) # Generate true trajectory (constant velocity with process noise) print("\n--- Setting up scenario ---") true_x0 = np.array([10.0, 10.0, 1.0, 0.5]) # [x, y, vx, vy] def process_model_true(x, u, dt): - F = np.array([ - [1, 0, dt, 0], - [0, 1, 0, dt], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) + F = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) return F @ x q = 0.5 - Q = q * np.array([ - [dt**3/3, 0, dt**2/2, 0], - [0, dt**3/3, 0, dt**2/2], - [dt**2/2, 0, dt, 0], - [0, dt**2/2, 0, dt] - ]) + Q = q * np.array( + [ + [dt**3 / 3, 0, dt**2 / 2, 0], + [0, dt**3 / 3, 0, dt**2 / 2], + [dt**2 / 2, 0, dt, 0], + [0, dt**2 / 2, 0, dt], + ] + ) true_states = [true_x0.copy()] true_state = true_x0.copy() @@ -133,21 +132,11 @@ def run_ekf(dt, n_steps, anchors, measurements, Q, range_std): print("\nRunning EKF...") def process_model(x, u, dt): - F = np.array([ - [1, 0, dt, 0], - [0, 1, 0, dt], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) + F = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) return F @ x def process_jacobian(x, u, dt): - return np.array([ - [1, 0, dt, 0], - [0, 1, 0, dt], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) + return np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) def measurement_model(x): ranges = [] @@ -165,7 +154,7 @@ def measurement_jacobian(x): if r < 1e-6: H.append([0, 0, 0, 0]) else: - H.append([dx/r, dy/r, 0, 0]) + H.append([dx / r, dy / r, 0, 0]) return np.array(H) def Q_func(dt): @@ -178,9 +167,14 @@ def R_func(): P0 = np.diag([4.0, 4.0, 2.0, 2.0]) ekf = ExtendedKalmanFilter( - process_model, process_jacobian, - measurement_model, measurement_jacobian, - Q_func, R_func, x0, P0 + process_model, + process_jacobian, + measurement_model, + measurement_jacobian, + Q_func, + R_func, + x0, + P0, ) estimates = [x0.copy()] @@ -203,12 +197,7 @@ def run_ukf(dt, n_steps, anchors, measurements, Q, range_std): print("Running UKF...") def process_model(x, u, dt): - F = np.array([ - [1, 0, dt, 0], - [0, 1, 0, dt], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) + F = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) return F @ x def measurement_model(x): @@ -228,8 +217,7 @@ def R_func(): P0 = np.diag([4.0, 4.0, 2.0, 2.0]) ukf = UnscentedKalmanFilter( - process_model, measurement_model, - Q_func, R_func, x0, P0 + process_model, measurement_model, Q_func, R_func, x0, P0 ) estimates = [x0.copy()] @@ -283,12 +271,7 @@ def process_model_with_noise(x, u, dt): Implements Eq. (3.33): Sample from transition prior. """ - F = np.array([ - [1, 0, dt, 0], - [0, 1, 0, dt], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) + F = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) process_noise = np.random.multivariate_normal(np.zeros(4), Q) return F @ x + process_noise @@ -302,16 +285,16 @@ def likelihood_func(z, x): Uses Gaussian likelihood for range measurements. """ # Predicted ranges from particle state - predicted_ranges = np.array([ - np.linalg.norm(x[:2] - anchor) for anchor in anchors - ]) + predicted_ranges = np.array( + [np.linalg.norm(x[:2] - anchor) for anchor in anchors] + ) # Gaussian likelihood: p(z | x) = N(z; h(x), R) residual = z - predicted_ranges - mahalanobis_sq = np.sum((residual / range_std)**2) + mahalanobis_sq = np.sum((residual / range_std) ** 2) likelihood = np.exp(-0.5 * mahalanobis_sq) # Normalize by Gaussian constant (optional for relative weights) - likelihood /= (range_std * np.sqrt(2 * np.pi))**len(anchors) + likelihood /= (range_std * np.sqrt(2 * np.pi)) ** len(anchors) return likelihood x0 = np.array([10.0, 10.0, 0.0, 0.0]) @@ -320,15 +303,20 @@ def likelihood_func(z, x): # Initialize Particle Filter with N particles # Particles are drawn from N(x0, P0) pf = ParticleFilter( - process_model_with_noise, likelihood_func, - n_particles, x0, P0, - resample_threshold=0.5 # Resample when N_eff < 0.5 * N + process_model_with_noise, + likelihood_func, + n_particles, + x0, + P0, + resample_threshold=0.5, # Resample when N_eff < 0.5 * N ) estimates = [x0.copy()] start_time = time.time() - for z in tqdm(measurements, desc=f"PF filtering ({n_particles} particles)", unit="step"): + for z in tqdm( + measurements, desc=f"PF filtering ({n_particles} particles)", unit="step" + ): # SIR Algorithm per time step: # Step 1: PROPAGATE - pf.predict() propagates all particles through # process model with noise [Eq. 3.33] @@ -376,22 +364,13 @@ def prior_jacobian(x_vars): Q_inv = np.linalg.inv(Q) for i in tqdm(range(n_steps), desc="Adding process factors", unit="factor"): + def process_residual(x_vars, i=i, dt=dt): - F = np.array([ - [1, 0, dt, 0], - [0, 1, 0, dt], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) + F = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) return x_vars[1] - F @ x_vars[0] def process_jacobian(x_vars, dt=dt): - F = np.array([ - [1, 0, dt, 0], - [0, 1, 0, dt], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) + F = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) return [-F, np.eye(4)] process_factor = Factor([i, i + 1], process_residual, process_jacobian, Q_inv) @@ -400,7 +379,13 @@ def process_jacobian(x_vars, dt=dt): # Add measurement factors R_inv = np.linalg.inv(np.diag([range_std**2] * len(anchors))) - for i, z in tqdm(enumerate(measurements), desc="Adding measurement factors", unit="factor", total=len(measurements)): + for i, z in tqdm( + enumerate(measurements), + desc="Adding measurement factors", + unit="factor", + total=len(measurements), + ): + def meas_residual(x_vars, z=z): x = x_vars[0] predicted_ranges = [] @@ -419,7 +404,7 @@ def meas_jacobian(x_vars): if r < 1e-6: H.append([0, 0, 0, 0]) else: - H.append([dx/r, dy/r, 0, 0]) + H.append([dx / r, dy / r, 0, 0]) return [np.array(H)] meas_factor = Factor([i + 1], meas_residual, meas_jacobian, R_inv) @@ -428,15 +413,14 @@ def meas_jacobian(x_vars): # Optimize print(" Optimizing factor graph (up to 10 Gauss-Newton iterations)...") start_time = time.time() - optimized_vars, costs = graph.optimize( - method="gauss_newton", max_iterations=10 - ) + optimized_vars, costs = graph.optimize(method="gauss_newton", max_iterations=10) elapsed_time = time.time() - start_time # optimize() returns the cost at each iteration, so this is the number of # iterations actually taken rather than the 10 it was allowed. n_iterations = len(costs) - print(f" [OK] FGO completed in {elapsed_time:.4f}s " - f"({n_iterations} iterations)") + print( + f" [OK] FGO completed in {elapsed_time:.4f}s " f"({n_iterations} iterations)" + ) # Extract estimates estimates = [] @@ -481,10 +465,10 @@ def model_evaluation_counts(n_steps, n_states, n_particles, fgo_iterations): n_sigma = 2 * n_states + 1 return { - 'EKF': n_steps * 2, - 'UKF': n_steps * 2 * n_sigma, - 'PF': n_steps * 2 * n_particles, - 'FGO': fgo_iterations * 2 * n_steps, + "EKF": n_steps * 2, + "UKF": n_steps * 2 * n_sigma, + "PF": n_steps * 2 * n_particles, + "FGO": fgo_iterations * 2 * n_steps, } @@ -521,22 +505,26 @@ def main(): results = {} print("\n[1/4] Extended Kalman Filter (EKF)") - results['EKF'], results['EKF_time'] = run_ekf(dt, n_steps, anchors, measurements, Q, range_std) + results["EKF"], results["EKF_time"] = run_ekf( + dt, n_steps, anchors, measurements, Q, range_std + ) print("\n[2/4] Unscented Kalman Filter (UKF)") - results['UKF'], results['UKF_time'] = run_ukf(dt, n_steps, anchors, measurements, Q, range_std) + results["UKF"], results["UKF_time"] = run_ukf( + dt, n_steps, anchors, measurements, Q, range_std + ) print("\n[3/4] Particle Filter (PF)") - results['PF'], results['PF_time'], n_particles = run_pf( + results["PF"], results["PF_time"], n_particles = run_pf( dt, n_steps, anchors, measurements, Q, range_std ) print("\n[4/4] Factor Graph Optimization (FGO)") - results['FGO'], results['FGO_time'], fgo_iterations = run_fgo( + results["FGO"], results["FGO_time"], fgo_iterations = run_fgo( dt, n_steps, anchors, measurements, Q, range_std ) - results['model_evaluations'] = model_evaluation_counts( + results["model_evaluations"] = model_evaluation_counts( n_steps=n_steps, n_states=true_states.shape[1], n_particles=n_particles, @@ -548,13 +536,13 @@ def main(): print("RESULTS") print("=" * 70) - for method in ['EKF', 'UKF', 'PF', 'FGO']: + for method in ["EKF", "UKF", "PF", "FGO"]: estimates = results[method] position_errors = np.linalg.norm(estimates[:, :2] - true_states[:, :2], axis=1) rmse = np.sqrt(np.mean(position_errors**2)) mean_error = np.mean(position_errors) max_error = np.max(position_errors) - comp_time = results[f'{method}_time'] + comp_time = results[f"{method}_time"] print(f"\n{method}:") print(f" RMSE: {rmse:.4f} m") @@ -585,7 +573,10 @@ def main(): scenario = setup_scenario(seed=seed) dt_r, n_r, anchors_r, truth_r, meas_r, Q_r, rstd_r = scenario for name, runner in ( - ("EKF", run_ekf), ("UKF", run_ukf), ("PF", run_pf), ("FGO", run_fgo) + ("EKF", run_ekf), + ("UKF", run_ukf), + ("PF", run_pf), + ("FGO", run_fgo), ): # Index rather than unpack: run_pf returns a third value # (n_particles) that the others do not, and only the estimates @@ -602,8 +593,10 @@ def main(): print(f"{'Method':<8} {'mean RMSE':<12} {'min':<9} {'max':<9} {'best of 4':<10}") for name, values in repeated.items(): values = np.asarray(values) - print(f"{name:<8} {values.mean():<12.3f} {values.min():<9.3f} " - f"{values.max():<9.3f} {best_counts[name]}/{n_seeds}") + print( + f"{name:<8} {values.mean():<12.3f} {values.min():<9.3f} " + f"{values.max():<9.3f} {best_counts[name]}/{n_seeds}" + ) print() print(" FGO wins every seed, which is what batch smoothing should do: it") @@ -622,7 +615,7 @@ def main(): # Panels 1-3 are the shared primitives drawn into this grid; only panel 4 # (timing bars) is specific to this comparison. - methods = ['EKF', 'UKF', 'PF', 'FGO'] + methods = ["EKF", "UKF", "PF", "FGO"] trajectories = {m: results[m][:, :2] for m in methods} errors = {m: results[m][:, :2] - true_states[:, :2] for m in methods} time_steps = np.arange(n_steps + 1) * dt @@ -643,9 +636,7 @@ def main(): ) # Plot 3: CDF of Errors - plot_error_cdf( - errors, title="Cumulative Distribution of Errors", ax=axes[1, 0] - ) + plot_error_cdf(errors, title="Cumulative Distribution of Errors", ax=axes[1, 0]) # Plot 4: Computational cost, counted rather than timed # @@ -654,26 +645,30 @@ def main(): # machine. Counting model evaluations is exact and reproducible, and it is # the quantity that actually separates these estimators. ax = axes[1, 1] - methods = ['EKF', 'UKF', 'PF', 'FGO'] - evaluations = [results['model_evaluations'][m] for m in methods] - colors = ['b', 'g', 'm', 'r'] - bars = ax.bar(methods, evaluations, color=colors, alpha=0.7, - edgecolor='black') + methods = ["EKF", "UKF", "PF", "FGO"] + evaluations = [results["model_evaluations"][m] for m in methods] + colors = ["b", "g", "m", "r"] + bars = ax.bar(methods, evaluations, color=colors, alpha=0.7, edgecolor="black") - ax.set_yscale('log') + ax.set_yscale("log") ax.set_ylabel("Model evaluations", fontsize=12) ax.set_title("Computational Cost", fontsize=14, fontweight="bold") - ax.grid(True, alpha=0.3, axis='y') + ax.grid(True, alpha=0.3, axis="y") # Add value labels on bars for bar, n_evals in zip(bars, evaluations): height = bar.get_height() - ax.text(bar.get_x() + bar.get_width() / 2., height, - f'{n_evals:,}', ha='center', va='bottom', fontsize=10) + ax.text( + bar.get_x() + bar.get_width() / 2.0, + height, + f"{n_evals:,}", + ha="center", + va="bottom", + fontsize=10, + ) plt.tight_layout() - paths = save_figure(fig, Path(__file__).parent / "figs", - "ch3_estimator_comparison") + paths = save_figure(fig, Path(__file__).parent / "figs", "ch3_estimator_comparison") print(f"[OK] Plot saved as: {paths[0]}") show_figures_if_requested() @@ -681,12 +676,11 @@ def main(): print("\n" + "=" * 70) print("COMPARISON COMPLETED") print("=" * 70) - print(f"Total execution time: {overall_time:.2f} seconds ({overall_time/60:.1f} minutes)") + print( + f"Total execution time: {overall_time:.2f} seconds ({overall_time/60:.1f} minutes)" + ) print("=" * 70) if __name__ == "__main__": main() - - - diff --git a/ch3_estimators/example_ekf_range_bearing.py b/ch3_estimators/example_ekf_range_bearing.py index 26566ca..a61a1c9 100644 --- a/ch3_estimators/example_ekf_range_bearing.py +++ b/ch3_estimators/example_ekf_range_bearing.py @@ -59,6 +59,7 @@ def create_range_bearing_innovation_func(n_landmarks: int): Returns: innovation_func(z, z_pred) -> innovation vector with wrapped bearings. """ + def innovation_func(z: np.ndarray, z_pred: np.ndarray) -> np.ndarray: innovation = np.zeros_like(z) for i in range(n_landmarks): @@ -73,33 +74,33 @@ def innovation_func(z: np.ndarray, z_pred: np.ndarray) -> np.ndarray: def load_estimator_dataset(data_dir: str) -> Dict: """Load estimator dataset from directory. - + Args: data_dir: Path to dataset directory (e.g., 'data/sim/ch3_estimator_nonlinear') - + Returns: Dictionary with time, ground truth, and measurements """ path = Path(data_dir) data = { - 't': np.loadtxt(path / 'time.txt'), - 'beacons': np.loadtxt(path / 'beacons.txt'), - 'true_states': np.loadtxt(path / 'ground_truth_states.txt'), - 'range_meas': np.loadtxt(path / 'range_measurements.txt'), - 'bearing_meas': np.loadtxt(path / 'bearing_measurements.txt'), + "t": np.loadtxt(path / "time.txt"), + "beacons": np.loadtxt(path / "beacons.txt"), + "true_states": np.loadtxt(path / "ground_truth_states.txt"), + "range_meas": np.loadtxt(path / "range_measurements.txt"), + "bearing_meas": np.loadtxt(path / "bearing_measurements.txt"), } # Load config - with open(path / 'config.json') as f: - data['config'] = json.load(f) + with open(path / "config.json") as f: + data["config"] = json.load(f) return data def run_with_dataset(data_dir: str) -> None: """Run EKF example using pre-generated dataset. - + Args: data_dir: Path to dataset directory """ @@ -110,13 +111,13 @@ def run_with_dataset(data_dir: str) -> None: # Load dataset data = load_estimator_dataset(data_dir) - config = data['config'] + config = data["config"] - t = data['t'] - landmarks = data['beacons'] - true_states = data['true_states'] - range_meas = data['range_meas'] - bearing_meas = data['bearing_meas'] + t = data["t"] + landmarks = data["beacons"] + true_states = data["true_states"] + range_meas = data["range_meas"] + bearing_meas = data["bearing_meas"] dt = t[1] - t[0] if len(t) > 1 else 0.5 n_steps = len(t) - 1 @@ -130,7 +131,9 @@ def run_with_dataset(data_dir: str) -> None: else: print(" [OK] Landmark geometry is valid") - is_obs, obs_msg = check_range_only_observability_2d(landmarks, initial_pos, warn=False) + is_obs, obs_msg = check_range_only_observability_2d( + landmarks, initial_pos, warn=False + ) if is_obs: print(" [OK] Position is observable from range measurements") else: @@ -140,26 +143,22 @@ def run_with_dataset(data_dir: str) -> None: print(f" Duration: {t[-1]:.1f} s ({n_steps} steps)") print(f" Time step: {dt:.2f} s") print(f" Landmarks: {len(landmarks)}") - print(f" Range noise: {config.get('measurements', {}).get('range_noise_std', 'N/A')} m") - print(f" Bearing noise: {np.rad2deg(config.get('measurements', {}).get('bearing_noise_std', 0)):.2f}°") + print( + f" Range noise: {config.get('measurements', {}).get('range_noise_std', 'N/A')} m" + ) + print( + f" Bearing noise: {np.rad2deg(config.get('measurements', {}).get('bearing_noise_std', 0)):.2f}°" + ) # Process model: constant velocity def process_model(x, u, dt_val): - F = np.array([ - [1, 0, dt_val, 0], - [0, 1, 0, dt_val], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) + F = np.array([[1, 0, dt_val, 0], [0, 1, 0, dt_val], [0, 0, 1, 0], [0, 0, 0, 1]]) return F @ x def process_jacobian(x, u, dt_val): - return np.array([ - [1, 0, dt_val, 0], - [0, 1, 0, dt_val], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) + return np.array( + [[1, 0, dt_val, 0], [0, 1, 0, dt_val], [0, 0, 1, 0], [0, 0, 0, 1]] + ) # Measurement model: range and bearing to landmarks def measurement_model(x): @@ -186,23 +185,25 @@ def measurement_jacobian(x): H.extend([[0, 0, 0, 0], [0, 0, 0, 0]]) else: # Range Jacobian: ∂r/∂[x,y] = [-dx/r, -dy/r] - H.append([-dx/r, -dy/r, 0, 0]) + H.append([-dx / r, -dy / r, 0, 0]) # Bearing Jacobian: ∂θ/∂[x,y] = [dy/r², -dx/r²] - H.append([dy/r_sq, -dx/r_sq, 0, 0]) + H.append([dy / r_sq, -dx / r_sq, 0, 0]) return np.array(H) # Noise covariances - q = config.get('process', {}).get('noise_std', 0.5) - range_std = config.get('measurements', {}).get('range_noise_std', 0.5) - bearing_std = config.get('measurements', {}).get('bearing_noise_std', 0.05) + q = config.get("process", {}).get("noise_std", 0.5) + range_std = config.get("measurements", {}).get("range_noise_std", 0.5) + bearing_std = config.get("measurements", {}).get("bearing_noise_std", 0.05) def Q_func(dt_val): - return q * np.array([ - [dt_val**3/3, 0, dt_val**2/2, 0], - [0, dt_val**3/3, 0, dt_val**2/2], - [dt_val**2/2, 0, dt_val, 0], - [0, dt_val**2/2, 0, dt_val] - ]) + return q * np.array( + [ + [dt_val**3 / 3, 0, dt_val**2 / 2, 0], + [0, dt_val**3 / 3, 0, dt_val**2 / 2], + [dt_val**2 / 2, 0, dt_val, 0], + [0, dt_val**2 / 2, 0, dt_val], + ] + ) def R_func(): R_diag = [] @@ -221,10 +222,15 @@ def R_func(): print("\nRunning Extended Kalman Filter...") print(" (Using angle wrapping for bearing innovations)") ekf = ExtendedKalmanFilter( - process_model, process_jacobian, - measurement_model, measurement_jacobian, - Q_func, R_func, x0_est, P0, - innovation_func=innovation_func + process_model, + process_jacobian, + measurement_model, + measurement_jacobian, + Q_func, + R_func, + x0_est, + P0, + innovation_func=innovation_func, ) estimates = [x0_est.copy()] @@ -234,7 +240,7 @@ def R_func(): # Form measurement from dataset z = [] for i in range(len(landmarks)): - z.extend([range_meas[k+1, i], bearing_meas[k+1, i]]) + z.extend([range_meas[k + 1, i], bearing_meas[k + 1, i]]) z = np.array(z) ekf.predict(dt=dt) @@ -258,16 +264,45 @@ def R_func(): # Visualization print("\nCreating visualization...") fig, axes = plt.subplots(2, 2, figsize=(14, 10)) - fig.suptitle('EKF Range-Bearing Positioning (Dataset)', fontsize=14, fontweight='bold') + fig.suptitle( + "EKF Range-Bearing Positioning (Dataset)", fontsize=14, fontweight="bold" + ) # Trajectory ax = axes[0, 0] - ax.scatter(landmarks[:, 0], landmarks[:, 1], s=200, c="red", marker="^", - label="Landmarks", zorder=3, edgecolors="black", linewidths=2) - ax.plot(true_states[:, 0], true_states[:, 1], "g-", linewidth=2, label="True Trajectory") + ax.scatter( + landmarks[:, 0], + landmarks[:, 1], + s=200, + c="red", + marker="^", + label="Landmarks", + zorder=3, + edgecolors="black", + linewidths=2, + ) + ax.plot( + true_states[:, 0], true_states[:, 1], "g-", linewidth=2, label="True Trajectory" + ) ax.plot(estimates[:, 0], estimates[:, 1], "b--", linewidth=2, label="EKF Estimate") - ax.scatter(true_states[0, 0], true_states[0, 1], s=150, c="green", marker="o", label="Start", zorder=3) - ax.scatter(true_states[-1, 0], true_states[-1, 1], s=150, c="orange", marker="s", label="End", zorder=3) + ax.scatter( + true_states[0, 0], + true_states[0, 1], + s=150, + c="green", + marker="o", + label="Start", + zorder=3, + ) + ax.scatter( + true_states[-1, 0], + true_states[-1, 1], + s=150, + c="orange", + marker="s", + label="End", + zorder=3, + ) ax.set_xlabel("X Position [m]") ax.set_ylabel("Y Position [m]") ax.set_title("2D Trajectory") @@ -305,8 +340,7 @@ def R_func(): plt.tight_layout() - paths = save_figure(fig, Path(__file__).parent / "figs", - "ch3_ekf_range_bearing") + paths = save_figure(fig, Path(__file__).parent / "figs", "ch3_ekf_range_bearing") print(f"Plot saved: {paths[0]}") show_figures_if_requested() @@ -334,12 +368,14 @@ def example_2d_range_bearing_positioning(): n_steps = int(t_max / dt) # Landmark positions (known) - landmarks = np.array([ - [0.0, 0.0], - [20.0, 0.0], - [20.0, 20.0], - [0.0, 20.0], - ]) + landmarks = np.array( + [ + [0.0, 0.0], + [20.0, 0.0], + [20.0, 20.0], + [0.0, 20.0], + ] + ) print("\nSimulation Parameters:") print(f" Time step: {dt} s") @@ -357,7 +393,9 @@ def example_2d_range_bearing_positioning(): else: print(" [OK] Landmark geometry is valid") - is_obs, obs_msg = check_range_only_observability_2d(landmarks, true_x0[:2], warn=False) + is_obs, obs_msg = check_range_only_observability_2d( + landmarks, true_x0[:2], warn=False + ) if is_obs: print(" [OK] Position is observable from range measurements") else: @@ -365,21 +403,11 @@ def example_2d_range_bearing_positioning(): # Process model: constant velocity in 2D def process_model(x, u, dt): - F = np.array([ - [1, 0, dt, 0], - [0, 1, 0, dt], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) + F = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) return F @ x def process_jacobian(x, u, dt): - return np.array([ - [1, 0, dt, 0], - [0, 1, 0, dt], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) + return np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) # Measurement model: range and bearing to all landmarks def measurement_model(x): @@ -418,13 +446,16 @@ def measurement_jacobian(x): # Process noise covariance q = 0.5 + def Q_func(dt): - return q * np.array([ - [dt**3/3, 0, dt**2/2, 0], - [0, dt**3/3, 0, dt**2/2], - [dt**2/2, 0, dt, 0], - [0, dt**2/2, 0, dt] - ]) + return q * np.array( + [ + [dt**3 / 3, 0, dt**2 / 2, 0], + [0, dt**3 / 3, 0, dt**2 / 2], + [dt**2 / 2, 0, dt, 0], + [0, dt**2 / 2, 0, dt], + ] + ) # Measurement noise covariance range_std = 0.5 @@ -469,10 +500,15 @@ def R_func(): print("\nRunning Extended Kalman Filter...") print(" (Using angle wrapping for bearing innovations)") ekf = ExtendedKalmanFilter( - process_model, process_jacobian, - measurement_model, measurement_jacobian, - Q_func, R_func, x0_est, P0, - innovation_func=innovation_func + process_model, + process_jacobian, + measurement_model, + measurement_jacobian, + Q_func, + R_func, + x0_est, + P0, + innovation_func=innovation_func, ) estimates = [x0_est.copy()] @@ -495,8 +531,12 @@ def R_func(): velocity_errors = np.linalg.norm(estimates[:, 2:] - true_states[:, 2:], axis=1) print("\nResults:") - print(f" Final true position: ({true_states[-1, 0]:.2f}, {true_states[-1, 1]:.2f}) m") - print(f" Final estimated position: ({estimates[-1, 0]:.2f}, {estimates[-1, 1]:.2f}) m") + print( + f" Final true position: ({true_states[-1, 0]:.2f}, {true_states[-1, 1]:.2f}) m" + ) + print( + f" Final estimated position: ({estimates[-1, 0]:.2f}, {estimates[-1, 1]:.2f}) m" + ) print(f" Final position error: {position_errors[-1]:.4f} m") print(f" Mean position error: {np.mean(position_errors[5:]):.4f} m") print(f" Final velocity error: {velocity_errors[-1]:.4f} m/s") @@ -506,14 +546,41 @@ def R_func(): fig, axes = plt.subplots(2, 2, figsize=(14, 10)) ax = axes[0, 0] - ax.scatter(landmarks[:, 0], landmarks[:, 1], s=200, c="red", marker="^", - label="Landmarks", zorder=3, edgecolors="black", linewidths=2) - ax.plot(true_states[:, 0], true_states[:, 1], "g-", linewidth=2, label="True Trajectory") + ax.scatter( + landmarks[:, 0], + landmarks[:, 1], + s=200, + c="red", + marker="^", + label="Landmarks", + zorder=3, + edgecolors="black", + linewidths=2, + ) + ax.plot( + true_states[:, 0], true_states[:, 1], "g-", linewidth=2, label="True Trajectory" + ) ax.plot(estimates[:, 0], estimates[:, 1], "b--", linewidth=2, label="EKF Estimate") - ax.scatter(true_states[0, 0], true_states[0, 1], s=150, c="green", marker="o", - label="Start", zorder=3, edgecolors="black") - ax.scatter(true_states[-1, 0], true_states[-1, 1], s=150, c="orange", marker="s", - label="End", zorder=3, edgecolors="black") + ax.scatter( + true_states[0, 0], + true_states[0, 1], + s=150, + c="green", + marker="o", + label="Start", + zorder=3, + edgecolors="black", + ) + ax.scatter( + true_states[-1, 0], + true_states[-1, 1], + s=150, + c="orange", + marker="s", + label="End", + zorder=3, + edgecolors="black", + ) P_final = covariances[-1] pos_cov = P_final[:2, :2] @@ -521,8 +588,17 @@ def R_func(): angle = np.arctan2(eigenvectors[1, 0], eigenvectors[0, 0]) width, height = 2 * 2 * np.sqrt(eigenvalues) from matplotlib.patches import Ellipse - ellipse = Ellipse(estimates[-1, :2], width, height, angle=np.rad2deg(angle), - facecolor="blue", alpha=0.2, edgecolor="blue", linewidth=2) + + ellipse = Ellipse( + estimates[-1, :2], + width, + height, + angle=np.rad2deg(angle), + facecolor="blue", + alpha=0.2, + edgecolor="blue", + linewidth=2, + ) ax.add_patch(ellipse) ax.set_xlabel("X Position [m]", fontsize=12) @@ -560,8 +636,7 @@ def R_func(): ax.grid(True, alpha=0.3) plt.tight_layout() - paths = save_figure(fig, Path(__file__).parent / "figs", - "ch3_ekf_range_bearing") + paths = save_figure(fig, Path(__file__).parent / "figs", "ch3_ekf_range_bearing") print(f"Plot saved as: {paths[0]}") show_figures_if_requested() @@ -583,11 +658,13 @@ def main(): # Run with high nonlinearity scenario python example_ekf_range_bearing.py --data ch3_estimator_high_nonlinear - """ + """, ) parser.add_argument( - "--data", type=str, default=None, - help="Dataset name or path (e.g., 'ch3_estimator_nonlinear' or full path)" + "--data", + type=str, + default=None, + help="Dataset name or path (e.g., 'ch3_estimator_nonlinear' or full path)", ) args = parser.parse_args() @@ -598,7 +675,9 @@ def main(): if not data_path.exists(): data_path = resolve_data_path(Path("data/sim") / args.data) if not data_path.exists(): - print(f"Error: Dataset not found at '{args.data}' or 'data/sim/{args.data}'") + print( + f"Error: Dataset not found at '{args.data}' or 'data/sim/{args.data}'" + ) print("\nAvailable datasets:") sim_dir = resolve_data_path(Path("data/sim")) if sim_dir.exists(): diff --git a/ch3_estimators/example_iekf_range_bearing.py b/ch3_estimators/example_iekf_range_bearing.py index beb1f62..45e8a5e 100644 --- a/ch3_estimators/example_iekf_range_bearing.py +++ b/ch3_estimators/example_iekf_range_bearing.py @@ -68,6 +68,7 @@ def create_range_bearing_innovation_func(n_landmarks: int): Returns: innovation_func(z, z_pred) -> innovation vector with wrapped bearings. """ + def innovation_func(z: np.ndarray, z_pred: np.ndarray) -> np.ndarray: innovation = np.zeros_like(z) for i in range(n_landmarks): @@ -88,12 +89,14 @@ def setup_high_nonlinearity_scenario(): highly nonlinear due to close proximity to landmarks. """ # Landmarks positioned to create high nonlinearity when close - landmarks = np.array([ - [0.0, 0.0], - [15.0, 0.0], - [15.0, 15.0], - [0.0, 15.0], - ]) + landmarks = np.array( + [ + [0.0, 0.0], + [15.0, 0.0], + [15.0, 15.0], + [0.0, 15.0], + ] + ) # Start position close to a landmark (high nonlinearity) true_x0 = np.array([2.0, 2.0, 0.8, 0.6]) @@ -106,21 +109,11 @@ def create_models(landmarks): # Process model: constant velocity in 2D def process_model(x, u, dt): - F = np.array([ - [1, 0, dt, 0], - [0, 1, 0, dt], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) + F = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) return F @ x def process_jacobian(x, u, dt): - return np.array([ - [1, 0, dt, 0], - [0, 1, 0, dt], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) + return np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) # Measurement model: range and bearing to all landmarks def measurement_model(x): @@ -128,7 +121,7 @@ def measurement_model(x): for lm in landmarks: dx = lm[0] - x[0] dy = lm[1] - x[1] - r = np.sqrt(dx ** 2 + dy ** 2) + r = np.sqrt(dx**2 + dy**2) theta = np.arctan2(dy, dx) measurements.extend([r, theta]) return np.array(measurements) @@ -138,8 +131,8 @@ def measurement_jacobian(x): for lm in landmarks: dx = lm[0] - x[0] dy = lm[1] - x[1] - r = np.sqrt(dx ** 2 + dy ** 2) - r_sq = max(r ** 2, 1e-12) + r = np.sqrt(dx**2 + dy**2) + r_sq = max(r**2, 1e-12) if r < 1e-6: # At landmark - singularity @@ -168,8 +161,9 @@ def example_iekf_vs_ekf_comparison(): # Setup scenario landmarks, true_x0 = setup_high_nonlinearity_scenario() - process_model, process_jacobian, measurement_model, measurement_jacobian = \ + process_model, process_jacobian, measurement_model, measurement_jacobian = ( create_models(landmarks) + ) # Simulation parameters dt = 0.5 @@ -186,12 +180,14 @@ def example_iekf_vs_ekf_comparison(): q = 0.3 # Process noise def Q_func(dt): - return q * np.array([ - [dt ** 3 / 3, 0, dt ** 2 / 2, 0], - [0, dt ** 3 / 3, 0, dt ** 2 / 2], - [dt ** 2 / 2, 0, dt, 0], - [0, dt ** 2 / 2, 0, dt] - ]) + return q * np.array( + [ + [dt**3 / 3, 0, dt**2 / 2, 0], + [0, dt**3 / 3, 0, dt**2 / 2], + [dt**2 / 2, 0, dt, 0], + [0, dt**2 / 2, 0, dt], + ] + ) range_std = 0.3 bearing_std = 0.08 # Higher bearing noise for more challenge @@ -199,7 +195,7 @@ def Q_func(dt): def R_func(): R_diag = [] for _ in landmarks: - R_diag.extend([range_std ** 2, bearing_std ** 2]) + R_diag.extend([range_std**2, bearing_std**2]) return np.diag(R_diag) print(f" Range noise: {range_std:.2f} m") @@ -217,19 +213,29 @@ def R_func(): # Create both filters ekf = ExtendedKalmanFilter( - process_model, process_jacobian, - measurement_model, measurement_jacobian, - Q_func, R_func, x0_est.copy(), P0.copy(), - innovation_func=innovation_func + process_model, + process_jacobian, + measurement_model, + measurement_jacobian, + Q_func, + R_func, + x0_est.copy(), + P0.copy(), + innovation_func=innovation_func, ) iekf = IteratedExtendedKalmanFilter( - process_model, process_jacobian, - measurement_model, measurement_jacobian, - Q_func, R_func, x0_est.copy(), P0.copy(), + process_model, + process_jacobian, + measurement_model, + measurement_jacobian, + Q_func, + R_func, + x0_est.copy(), + P0.copy(), max_iterations=5, convergence_tol=1e-6, - innovation_func=innovation_func + innovation_func=innovation_func, ) # Generate true trajectory @@ -291,16 +297,26 @@ def R_func(): print("=" * 50) print(f"\n{'Metric':<30} {'EKF':<15} {'IEKF':<15}") print("-" * 60) - print(f"{'Mean position error (m)':<30} {np.mean(ekf_pos_errors[5:]):<15.4f} " - f"{np.mean(iekf_pos_errors[5:]):<15.4f}") - print(f"{'RMSE position (m)':<30} {np.sqrt(np.mean(ekf_pos_errors**2)):<15.4f} " - f"{np.sqrt(np.mean(iekf_pos_errors**2)):<15.4f}") - print(f"{'Max position error (m)':<30} {np.max(ekf_pos_errors):<15.4f} " - f"{np.max(iekf_pos_errors):<15.4f}") - print(f"{'Final position error (m)':<30} {ekf_pos_errors[-1]:<15.4f} " - f"{iekf_pos_errors[-1]:<15.4f}") - print(f"{'Mean velocity error (m/s)':<30} {np.mean(ekf_vel_errors[5:]):<15.4f} " - f"{np.mean(iekf_vel_errors[5:]):<15.4f}") + print( + f"{'Mean position error (m)':<30} {np.mean(ekf_pos_errors[5:]):<15.4f} " + f"{np.mean(iekf_pos_errors[5:]):<15.4f}" + ) + print( + f"{'RMSE position (m)':<30} {np.sqrt(np.mean(ekf_pos_errors**2)):<15.4f} " + f"{np.sqrt(np.mean(iekf_pos_errors**2)):<15.4f}" + ) + print( + f"{'Max position error (m)':<30} {np.max(ekf_pos_errors):<15.4f} " + f"{np.max(iekf_pos_errors):<15.4f}" + ) + print( + f"{'Final position error (m)':<30} {ekf_pos_errors[-1]:<15.4f} " + f"{iekf_pos_errors[-1]:<15.4f}" + ) + print( + f"{'Mean velocity error (m/s)':<30} {np.mean(ekf_vel_errors[5:]):<15.4f} " + f"{np.mean(iekf_vel_errors[5:]):<15.4f}" + ) # Report the improvement where it exists, and say where it does not. # @@ -340,10 +356,16 @@ def R_func(): # useful sense at fixed noise: 5.66 m barely moves, because what matters # is whether the linearisation error is large *relative to* the noise # floor, and one doubling is not enough to clear 0.30 m / 0.08 rad. - early = (np.mean(ekf_pos_errors[1:2]) - np.mean(iekf_pos_errors[1:2])) / \ - np.mean(ekf_pos_errors[1:2]) * 100 - steady = (np.mean(ekf_pos_errors[5:]) - np.mean(iekf_pos_errors[5:])) / \ - np.mean(ekf_pos_errors[5:]) * 100 + early = ( + (np.mean(ekf_pos_errors[1:2]) - np.mean(iekf_pos_errors[1:2])) + / np.mean(ekf_pos_errors[1:2]) + * 100 + ) + steady = ( + (np.mean(ekf_pos_errors[5:]) - np.mean(iekf_pos_errors[5:])) + / np.mean(ekf_pos_errors[5:]) + * 100 + ) print(f"\nIEKF improvement, first update: {early:+.1f}%") print(f"IEKF improvement, steps 5+: {steady:+.1f}%") print(f"Mean IEKF iterations per update: {np.mean(iekf_iterations):.1f}") @@ -368,14 +390,16 @@ def R_func(): # Visualization print("\nCreating visualization...") fig, axes = plt.subplots(2, 2, figsize=(14, 10)) - fig.suptitle('IEKF vs EKF: High Nonlinearity Comparison (Section 3.2.3)', - fontsize=14, fontweight='bold') + fig.suptitle( + "IEKF vs EKF: High Nonlinearity Comparison (Section 3.2.3)", + fontsize=14, + fontweight="bold", + ) # Trajectory comparison (shared primitive; it supplies Start/End markers) plot_trajectory_2d( true_states[:, :2], - {"EKF Estimate": ekf_estimates[:, :2], - "IEKF Estimate": iekf_estimates[:, :2]}, + {"EKF Estimate": ekf_estimates[:, :2], "IEKF Estimate": iekf_estimates[:, :2]}, anchors_xy=landmarks[:, :2], title="2D Trajectory Comparison", axis_labels=("X Position [m]", "Y Position [m]"), @@ -394,17 +418,31 @@ def R_func(): ax=ax, title_fontweight="normal", ) - ax.axhline(y=np.mean(ekf_pos_errors[5:]), color="b", linestyle="--", alpha=0.5, - label=f"EKF mean: {np.mean(ekf_pos_errors[5:]):.2f} m") - ax.axhline(y=np.mean(iekf_pos_errors[5:]), color="m", linestyle="--", alpha=0.5, - label=f"IEKF mean: {np.mean(iekf_pos_errors[5:]):.2f} m") + ax.axhline( + y=np.mean(ekf_pos_errors[5:]), + color="b", + linestyle="--", + alpha=0.5, + label=f"EKF mean: {np.mean(ekf_pos_errors[5:]):.2f} m", + ) + ax.axhline( + y=np.mean(iekf_pos_errors[5:]), + color="m", + linestyle="--", + alpha=0.5, + label=f"IEKF mean: {np.mean(iekf_pos_errors[5:]):.2f} m", + ) ax.legend(fontsize=9) # IEKF iterations ax = axes[1, 0] ax.bar(time[1:], iekf_iterations, width=dt * 0.8, color="purple", alpha=0.7) - ax.axhline(y=np.mean(iekf_iterations), color="r", linestyle="--", - label=f"Mean: {np.mean(iekf_iterations):.1f}") + ax.axhline( + y=np.mean(iekf_iterations), + color="r", + linestyle="--", + label=f"Mean: {np.mean(iekf_iterations):.1f}", + ) ax.set_xlabel("Time [s]", fontsize=12) ax.set_ylabel("IEKF Iterations", fontsize=12) ax.set_title("IEKF Iterations per Update", fontsize=12) @@ -414,8 +452,8 @@ def R_func(): # Cumulative error ax = axes[1, 1] - ekf_cumulative = np.cumsum(ekf_pos_errors ** 2) - iekf_cumulative = np.cumsum(iekf_pos_errors ** 2) + ekf_cumulative = np.cumsum(ekf_pos_errors**2) + iekf_cumulative = np.cumsum(iekf_pos_errors**2) ax.plot(time, ekf_cumulative, "b-", linewidth=2, label="EKF") ax.plot(time, iekf_cumulative, "m-", linewidth=2, label="IEKF") ax.set_xlabel("Time [s]", fontsize=12) @@ -427,8 +465,9 @@ def R_func(): plt.tight_layout() # Save figure (svg + pdf + png via the shared layer) - paths = save_figure(fig, Path(__file__).parent / "figs", - "ch3_iekf_vs_ekf_comparison") + paths = save_figure( + fig, Path(__file__).parent / "figs", "ch3_iekf_vs_ekf_comparison" + ) print(f"Plot saved: {paths[0]}") show_figures_if_requested() @@ -461,7 +500,7 @@ def measurement_model(x): for lm in landmarks: dx = lm[0] - x[0] dy = lm[1] - x[1] - r = np.sqrt(dx ** 2 + dy ** 2) + r = np.sqrt(dx**2 + dy**2) theta = np.arctan2(dy, dx) measurements.extend([r, theta]) return np.array(measurements) @@ -471,8 +510,8 @@ def measurement_jacobian(x): for lm in landmarks: dx = lm[0] - x[0] dy = lm[1] - x[1] - r = np.sqrt(dx ** 2 + dy ** 2) - r_sq = max(r ** 2, 1e-12) + r = np.sqrt(dx**2 + dy**2) + r_sq = max(r**2, 1e-12) if r < 1e-6: H.extend([[0, 0, 0, 0], [0, 0, 0, 0]]) else: @@ -487,7 +526,7 @@ def measurement_jacobian(x): # Predicted state with error x_pred = np.array([4.0, 5.0, 0.0, 0.0]) P_pred = np.diag([2.0, 2.0, 1.0, 1.0]) - R = np.diag([0.3 ** 2, 0.05 ** 2, 0.3 ** 2, 0.05 ** 2]) + R = np.diag([0.3**2, 0.05**2, 0.3**2, 0.05**2]) print(f"\nTrue state: {true_state[:2]}") print(f"Predicted state: {x_pred[:2]}") @@ -516,8 +555,10 @@ def measurement_jacobian(x): step_size = np.linalg.norm(x_new - x_iter) residual_norm = np.linalg.norm(z_true - measurement_model(x_new)) - print(f"{j:<6} {x_iter[0]:<8.4f} {x_iter[1]:<8.4f} " - f"{step_size:<12.6f} {residual_norm:<12.6f}") + print( + f"{j:<6} {x_iter[0]:<8.4f} {x_iter[1]:<8.4f} " + f"{step_size:<12.6f} {residual_norm:<12.6f}" + ) if step_size < 1e-6: print(f"\nConverged at iteration {j + 1}") @@ -538,9 +579,10 @@ def main(): description="Chapter 3: Iterated Extended Kalman Filter Example (Section 3.2.3)" ) parser.add_argument( - "--demo", choices=["comparison", "convergence", "both"], + "--demo", + choices=["comparison", "convergence", "both"], default="both", - help="Which demo to run" + help="Which demo to run", ) args = parser.parse_args() @@ -567,4 +609,3 @@ def main(): if __name__ == "__main__": main() - diff --git a/ch3_estimators/example_kalman_1d.py b/ch3_estimators/example_kalman_1d.py index 179aac5..5906d5e 100644 --- a/ch3_estimators/example_kalman_1d.py +++ b/ch3_estimators/example_kalman_1d.py @@ -83,7 +83,9 @@ def example_1d_constant_velocity(): print("\nSimulation Parameters:") print(f" Time step: {dt} s") print(f" Duration: {t_max} s ({n_steps} steps)") - print(f" True initial state: position={true_x0[0]:.1f} m, velocity={true_x0[1]:.1f} m/s") + print( + f" True initial state: position={true_x0[0]:.1f} m, velocity={true_x0[1]:.1f} m/s" + ) print(f" Measurement noise: {measurement_std:.2f} m (std dev)") print(f" Process noise intensity: {q:.2f}") @@ -195,7 +197,12 @@ def example_1d_constant_velocity(): # Plot 3: Position Error ax = axes[1, 0] ax.plot(time, position_errors, "r-", linewidth=2, label="Position Error") - ax.axhline(y=measurement_std, color="k", linestyle="--", label=f"Measurement Noise ({measurement_std} m)") + ax.axhline( + y=measurement_std, + color="k", + linestyle="--", + label=f"Measurement Noise ({measurement_std} m)", + ) ax.set_xlabel("Time [s]", fontsize=12) ax.set_ylabel("Position Error [m]", fontsize=12) ax.set_title("Position Estimation Error", fontsize=14, fontweight="bold") @@ -241,6 +248,3 @@ def main(): if __name__ == "__main__": main() - - - diff --git a/ch3_estimators/example_least_squares.py b/ch3_estimators/example_least_squares.py index 9bbbcc1..9c1176f 100644 --- a/ch3_estimators/example_least_squares.py +++ b/ch3_estimators/example_least_squares.py @@ -111,8 +111,9 @@ def jacobian(x: np.ndarray) -> np.ndarray: return h, jacobian -def compute_ranges(position: np.ndarray, anchors: np.ndarray, - noise_std: float = 0.0) -> np.ndarray: +def compute_ranges( + position: np.ndarray, anchors: np.ndarray, noise_std: float = 0.0 +) -> np.ndarray: """Compute ranges from position to anchors with optional noise. Args: @@ -207,13 +208,17 @@ def example_2_weighted_ls(): # Generate measurements with different noise levels np.random.seed(42) - y = np.array([ - compute_ranges(true_position, anchors[i:i + 1], noise_std=measurement_stds[i])[0] - for i in range(len(anchors)) - ]) + y = np.array( + [ + compute_ranges( + true_position, anchors[i : i + 1], noise_std=measurement_stds[i] + )[0] + for i in range(len(anchors)) + ] + ) # Weight matrix: W = diag(1/sigma^2) (book Section 3.1.1) - W = np.diag(1.0 / measurement_stds ** 2) + W = np.diag(1.0 / measurement_stds**2) # Linearization x0 = np.array([5.0, 5.0]) @@ -250,8 +255,10 @@ def example_2_weighted_ls(): # weight on anchor 0 than on any other. When anchor 0 draws an unlucky # error, WLS follows it. Weighting buys accuracy on average by trusting # the good sensor, and pays for it by depending on that sensor. - print(f"\nThis draw: WLS is {((error_ls - error_wls) / error_ls * 100):.1f}% " - f"better than LS") + print( + f"\nThis draw: WLS is {((error_ls - error_wls) / error_ls * 100):.1f}% " + f"better than LS" + ) print(" Over 5000 draws: ~14% better in RMS, ~9% in the per-draw median,") print(" and worse than plain LS on ~28% of them. A single draw cannot tell") print(" you which of those you are looking at.") @@ -340,14 +347,18 @@ def example_4_levenberg_marquardt(): print("\n--- Gauss-Newton from poor guess ---") result_gn = gauss_newton(h, jacobian, y, x0_poor, max_iter=50) error_gn = np.linalg.norm(result_gn.x - true_position) - print(f"Result: {result_gn.x}, error: {error_gn:.4f} m, " - f"iters: {result_gn.iterations}, converged: {result_gn.converged}") + print( + f"Result: {result_gn.x}, error: {error_gn:.4f} m, " + f"iters: {result_gn.iterations}, converged: {result_gn.converged}" + ) print("\n--- Levenberg-Marquardt from poor guess ---") result_lm = levenberg_marquardt(h, jacobian, y, x0_poor, max_iter=50, mu0=1e-3) error_lm = np.linalg.norm(result_lm.x - true_position) - print(f"Result: {result_lm.x}, error: {error_lm:.4f} m, " - f"iters: {result_lm.iterations}, converged: {result_lm.converged}") + print( + f"Result: {result_lm.x}, error: {error_lm:.4f} m, " + f"iters: {result_lm.iterations}, converged: {result_lm.converged}" + ) if error_lm < error_gn: print("\n[OK] LM converged better than GN from poor initial guess") @@ -377,10 +388,18 @@ def example_5_robust_ls(): print("=" * 70) # Use more anchors for robust estimation (need redundancy!) - anchors = np.array([ - [0.0, 0.0], [10.0, 0.0], [0.0, 10.0], [10.0, 10.0], # Corners - [5.0, 0.0], [5.0, 10.0], [0.0, 5.0], [10.0, 5.0] # Midpoints - ]) + anchors = np.array( + [ + [0.0, 0.0], + [10.0, 0.0], + [0.0, 10.0], + [10.0, 10.0], # Corners + [5.0, 0.0], + [5.0, 10.0], + [0.0, 5.0], + [10.0, 5.0], # Midpoints + ] + ) true_position = np.array([3.0, 4.0]) h, jacobian = create_range_model(anchors) @@ -413,7 +432,10 @@ def example_5_robust_ls(): results = {} for label, method in table_3_1_methods.items(): result = robust_gauss_newton( - h, jacobian, y, x0, + h, + jacobian, + y, + x0, loss=method, loss_param=1.5, max_iter=30, @@ -434,7 +456,9 @@ def example_5_robust_ls(): for label, res in results.items(): pos_str = f"[{res['position'][0]:.3f}, {res['position'][1]:.3f}]" - print(f"{label:<20} {pos_str:<25} {res['error']:<10.4f} {res['outlier_weight']:<10.4f}") + print( + f"{label:<20} {pos_str:<25} {res['error']:<10.4f} {res['outlier_weight']:<10.4f}" + ) print("\nKey insight from Table 3.1:") print(" - L2 is corrupted by the outlier (no downweighting)") @@ -456,10 +480,18 @@ def visualize_results(): h_4, jac_4 = create_range_model(anchors_4) # 8 anchors for robust example - anchors_8 = np.array([ - [0.0, 0.0], [10.0, 0.0], [0.0, 10.0], [10.0, 10.0], - [5.0, 0.0], [5.0, 10.0], [0.0, 5.0], [10.0, 5.0] - ]) + anchors_8 = np.array( + [ + [0.0, 0.0], + [10.0, 0.0], + [0.0, 10.0], + [10.0, 10.0], + [5.0, 0.0], + [5.0, 10.0], + [0.0, 5.0], + [10.0, 5.0], + ] + ) h_8, jac_8 = create_range_model(anchors_8) np.random.seed(42) @@ -500,33 +532,74 @@ def visualize_results(): ax = axes[0] # Anchors - ax.scatter(anchors_4[:, 0], anchors_4[:, 1], s=200, c="blue", marker="^", - label="Anchors", zorder=5) + ax.scatter( + anchors_4[:, 0], + anchors_4[:, 1], + s=200, + c="blue", + marker="^", + label="Anchors", + zorder=5, + ) # True position - ax.scatter(true_position[0], true_position[1], s=250, c="green", marker="*", - label="True Position", zorder=5) + ax.scatter( + true_position[0], + true_position[1], + s=250, + c="green", + marker="*", + label="True Position", + zorder=5, + ) # Estimates - ax.scatter(x0[0], x0[1], s=150, c="gray", marker="x", label="Initial Guess", zorder=4) - ax.scatter(pos_linear[0], pos_linear[1], s=150, c="orange", marker="o", - label="Linear LS (Eq. 3.2)", zorder=4) - ax.scatter(pos_gn[0], pos_gn[1], s=150, c="red", marker="s", - label="Gauss-Newton (Eq. 3.52)", zorder=4) - ax.scatter(pos_lm[0], pos_lm[1], s=150, c="purple", marker="D", - label="LM (Eq. 3.53)", zorder=4) + ax.scatter( + x0[0], x0[1], s=150, c="gray", marker="x", label="Initial Guess", zorder=4 + ) + ax.scatter( + pos_linear[0], + pos_linear[1], + s=150, + c="orange", + marker="o", + label="Linear LS (Eq. 3.2)", + zorder=4, + ) + ax.scatter( + pos_gn[0], + pos_gn[1], + s=150, + c="red", + marker="s", + label="Gauss-Newton (Eq. 3.52)", + zorder=4, + ) + ax.scatter( + pos_lm[0], + pos_lm[1], + s=150, + c="purple", + marker="D", + label="LM (Eq. 3.53)", + zorder=4, + ) # Range circles for i, anchor in enumerate(anchors_4): - circle = plt.Circle(anchor, y_clean[i], fill=False, - edgecolor="blue", alpha=0.3, linestyle="--") + circle = plt.Circle( + anchor, y_clean[i], fill=False, edgecolor="blue", alpha=0.3, linestyle="--" + ) ax.add_patch(circle) ax.set_xlabel("X (m)", fontsize=12) ax.set_ylabel("Y (m)", fontsize=12) - ax.set_title("On clean data all three solvers agree to centimetres\n" - "(inset: a 20 cm window on the same estimates)", - fontsize=12, fontweight="bold") + ax.set_title( + "On clean data all three solvers agree to centimetres\n" + "(inset: a 20 cm window on the same estimates)", + fontsize=12, + fontweight="bold", + ) ax.legend(fontsize=9, loc="upper right") ax.grid(True, alpha=0.3) ax.set_aspect("equal") @@ -540,15 +613,23 @@ def visualize_results(): # overview, and put the part worth seeing beside it. spread = 0.1 axins = ax.inset_axes([0.03, 0.03, 0.36, 0.36]) - axins.scatter(true_position[0], true_position[1], s=250, c="green", - marker="*", zorder=5) - axins.scatter(pos_linear[0], pos_linear[1], s=150, c="orange", marker="o", - zorder=4) + axins.scatter( + true_position[0], true_position[1], s=250, c="green", marker="*", zorder=5 + ) + axins.scatter(pos_linear[0], pos_linear[1], s=150, c="orange", marker="o", zorder=4) # Gauss-Newton and LM converge to the same point here, so LM is drawn as a # larger hollow marker around it: two rings mean they agree exactly. axins.scatter(pos_gn[0], pos_gn[1], s=110, c="red", marker="s", zorder=4) - axins.scatter(pos_lm[0], pos_lm[1], s=320, facecolors="none", - edgecolors="purple", marker="D", linewidths=2, zorder=4) + axins.scatter( + pos_lm[0], + pos_lm[1], + s=320, + facecolors="none", + edgecolors="purple", + marker="D", + linewidths=2, + zorder=4, + ) axins.set_xlim(true_position[0] - spread, true_position[0] + spread) axins.set_ylim(true_position[1] - spread, true_position[1] + spread) axins.set_xticks([]) @@ -560,26 +641,76 @@ def visualize_results(): ax = axes[1] # Anchors - ax.scatter(anchors_8[:, 0], anchors_8[:, 1], s=200, c="blue", marker="^", - label="Anchors (8)", zorder=5) + ax.scatter( + anchors_8[:, 0], + anchors_8[:, 1], + s=200, + c="blue", + marker="^", + label="Anchors (8)", + zorder=5, + ) # Mark outlier anchor - ax.scatter(anchors_8[2, 0], anchors_8[2, 1], s=350, facecolors="none", - edgecolors="red", linewidth=3, zorder=4, label="Outlier Anchor") + ax.scatter( + anchors_8[2, 0], + anchors_8[2, 1], + s=350, + facecolors="none", + edgecolors="red", + linewidth=3, + zorder=4, + label="Outlier Anchor", + ) # True position - ax.scatter(true_position[0], true_position[1], s=250, c="green", marker="*", - label="True Position", zorder=5) + ax.scatter( + true_position[0], + true_position[1], + s=250, + c="green", + marker="*", + label="True Position", + zorder=5, + ) # Table 3.1 estimator results - ax.scatter(pos_l2[0], pos_l2[1], s=150, c="orange", marker="o", - label="L2 (Table 3.1) - corrupted", zorder=4) - ax.scatter(pos_cauchy[0], pos_cauchy[1], s=150, c="cyan", marker="s", - label="Cauchy (Table 3.1)", zorder=4) - ax.scatter(pos_huber[0], pos_huber[1], s=150, c="magenta", marker="^", - label="Huber (Table 3.1)", zorder=4) - ax.scatter(pos_gm[0], pos_gm[1], s=150, c="purple", marker="D", - label="G-M (Table 3.1)", zorder=4) + ax.scatter( + pos_l2[0], + pos_l2[1], + s=150, + c="orange", + marker="o", + label="L2 (Table 3.1) - corrupted", + zorder=4, + ) + ax.scatter( + pos_cauchy[0], + pos_cauchy[1], + s=150, + c="cyan", + marker="s", + label="Cauchy (Table 3.1)", + zorder=4, + ) + ax.scatter( + pos_huber[0], + pos_huber[1], + s=150, + c="magenta", + marker="^", + label="Huber (Table 3.1)", + zorder=4, + ) + ax.scatter( + pos_gm[0], + pos_gm[1], + s=150, + c="purple", + marker="D", + label="G-M (Table 3.1)", + zorder=4, + ) # Range circles (show first 4 anchors only to reduce clutter) for i in range(4): @@ -587,15 +718,25 @@ def visualize_results(): color = "red" if i == 2 else "blue" alpha = 0.6 if i == 2 else 0.2 lw = 2.5 if i == 2 else 1 - circle = plt.Circle(anchor, y_outlier[i], fill=False, - edgecolor=color, alpha=alpha, linestyle="--", linewidth=lw) + circle = plt.Circle( + anchor, + y_outlier[i], + fill=False, + edgecolor=color, + alpha=alpha, + linestyle="--", + linewidth=lw, + ) ax.add_patch(circle) ax.set_xlabel("X (m)", fontsize=12) ax.set_ylabel("Y (m)", fontsize=12) - ax.set_title("One corrupted anchor drags plain least squares off the truth\n" - "while every robust loss in Table 3.1 ignores it", - fontsize=12, fontweight="bold") + ax.set_title( + "One corrupted anchor drags plain least squares off the truth\n" + "while every robust loss in Table 3.1 ignores it", + fontsize=12, + fontweight="bold", + ) ax.legend(fontsize=9, loc="upper right") ax.grid(True, alpha=0.3) ax.set_aspect("equal") @@ -605,8 +746,9 @@ def visualize_results(): plt.tight_layout() # Save to figs directory (svg + pdf + png via the shared layer) - paths = save_figure(fig, Path(__file__).parent / "figs", - "ch3_least_squares_examples") + paths = save_figure( + fig, Path(__file__).parent / "figs", "ch3_least_squares_examples" + ) print(f"\nPlot saved as: {paths[0]}") show_figures_if_requested() diff --git a/ch3_estimators/example_particle_bimodal.py b/ch3_estimators/example_particle_bimodal.py index ab3e413..c502eb0 100644 --- a/ch3_estimators/example_particle_bimodal.py +++ b/ch3_estimators/example_particle_bimodal.py @@ -140,9 +140,7 @@ def process_model(x, u, dt): anchors = _anchors_at(step) particle_filter.likelihood_func = _make_likelihood(anchors) - ranges = np.array( - [np.linalg.norm(truth[step][:2] - a) for a in anchors] - ) + ranges = np.array([np.linalg.norm(truth[step][:2] - a) for a in anchors]) ranges = ranges + np.random.normal(0.0, RANGE_STD, len(anchors)) particle_filter.predict(dt=DT) @@ -169,7 +167,8 @@ def process_model(x, u, dt): error_mean.append(np.linalg.norm(mean_xy - truth_xy)) error_best_mode.append( min(np.linalg.norm(m - truth_xy) for m in modes) - if modes else np.linalg.norm(mean_xy - truth_xy) + if modes + else np.linalg.norm(mean_xy - truth_xy) ) finally: np.random.set_state(rng_state) @@ -202,19 +201,58 @@ def _draw_frame(axes, scenario, index): # --- particle cloud above = cloud[cloud[:, 1] > 0] below = cloud[cloud[:, 1] <= 0] - axes[0].scatter(above[:, 0], above[:, 1], s=5, c="tab:blue", alpha=0.35, - label=f"particles above baseline ({len(above)})") - axes[0].scatter(below[:, 0], below[:, 1], s=5, c="tab:purple", alpha=0.35, - label=f"particles below ({len(below)})") - axes[0].plot(truth[: step + 1, 0], truth[: step + 1, 1], "k-", - linewidth=2.0, label="true trajectory") - axes[0].scatter(*truth[step][:2], s=90, marker="*", c="lime", - edgecolors="black", zorder=6, label="true position") - axes[0].scatter(*scenario["means"][index], s=70, marker="X", c="red", - edgecolors="black", zorder=6, label="posterior mean") - axes[0].scatter(anchors[:, 0], anchors[:, 1], s=150, marker="^", c="orange", - edgecolors="black", linewidths=1.5, zorder=5, - label=f"{len(anchors)} anchors") + axes[0].scatter( + above[:, 0], + above[:, 1], + s=5, + c="tab:blue", + alpha=0.35, + label=f"particles above baseline ({len(above)})", + ) + axes[0].scatter( + below[:, 0], + below[:, 1], + s=5, + c="tab:purple", + alpha=0.35, + label=f"particles below ({len(below)})", + ) + axes[0].plot( + truth[: step + 1, 0], + truth[: step + 1, 1], + "k-", + linewidth=2.0, + label="true trajectory", + ) + axes[0].scatter( + *truth[step][:2], + s=90, + marker="*", + c="lime", + edgecolors="black", + zorder=6, + label="true position", + ) + axes[0].scatter( + *scenario["means"][index], + s=70, + marker="X", + c="red", + edgecolors="black", + zorder=6, + label="posterior mean", + ) + axes[0].scatter( + anchors[:, 0], + anchors[:, 1], + s=150, + marker="^", + c="orange", + edgecolors="black", + linewidths=1.5, + zorder=5, + label=f"{len(anchors)} anchors", + ) axes[0].axhline(0.0, color="0.6", linestyle=":", linewidth=1.2) axes[0].set_xlim(-6, 26) axes[0].set_ylim(-16, 22) @@ -232,28 +270,47 @@ def _draw_frame(axes, scenario, index): ) # --- how the two modes share the particles - axes[1].plot(steps[: index + 1], scenario["fraction_above"][: index + 1], - "-", color="tab:blue", linewidth=1.8) + axes[1].plot( + steps[: index + 1], + scenario["fraction_above"][: index + 1], + "-", + color="tab:blue", + linewidth=1.8, + ) axes[1].axhline(0.5, color="0.6", linestyle=":", linewidth=1.2) - axes[1].axvline(THIRD_ANCHOR_STEP, color="tab:green", linestyle="--", - linewidth=1.4, label="third anchor appears") + axes[1].axvline( + THIRD_ANCHOR_STEP, + color="tab:green", + linestyle="--", + linewidth=1.4, + label="third anchor appears", + ) axes[1].set_xlim(steps[0], steps[-1]) axes[1].set_ylim(-0.05, 1.05) axes[1].grid(alpha=0.3) axes[1].set_xlabel("step") axes[1].set_ylabel("fraction of particles above baseline") axes[1].legend(fontsize=8, loc="lower right") - axes[1].set_title( - "two live hypotheses, then one", fontsize=10 - ) + axes[1].set_title("two live hypotheses, then one", fontsize=10) # --- the punchline: the mean is not where the target is - axes[2].plot(steps[: index + 1], scenario["error_mean"][: index + 1], - "-", color="red", linewidth=1.8, label="posterior mean") - axes[2].plot(steps[: index + 1], scenario["error_best_mode"][: index + 1], - "-", color="tab:blue", linewidth=1.8, label="nearest mode") - axes[2].axvline(THIRD_ANCHOR_STEP, color="tab:green", linestyle="--", - linewidth=1.4) + axes[2].plot( + steps[: index + 1], + scenario["error_mean"][: index + 1], + "-", + color="red", + linewidth=1.8, + label="posterior mean", + ) + axes[2].plot( + steps[: index + 1], + scenario["error_best_mode"][: index + 1], + "-", + color="tab:blue", + linewidth=1.8, + label="nearest mode", + ) + axes[2].axvline(THIRD_ANCHOR_STEP, color="tab:green", linestyle="--", linewidth=1.4) axes[2].set_xlim(steps[0], steps[-1]) axes[2].set_ylim(0, max(scenario["error_mean"].max() * 1.15, 1.0)) axes[2].grid(alpha=0.3) @@ -306,11 +363,17 @@ def plot_bimodal_summary(scenario) -> plt.Figure: # complete run so the static figure shows the whole story at once. steps = scenario["steps"] axes[1].clear() - axes[1].plot(steps, scenario["fraction_above"], "-", color="tab:blue", - linewidth=1.8) + axes[1].plot( + steps, scenario["fraction_above"], "-", color="tab:blue", linewidth=1.8 + ) axes[1].axhline(0.5, color="0.6", linestyle=":", linewidth=1.2) - axes[1].axvline(THIRD_ANCHOR_STEP, color="tab:green", linestyle="--", - linewidth=1.4, label="third anchor appears") + axes[1].axvline( + THIRD_ANCHOR_STEP, + color="tab:green", + linestyle="--", + linewidth=1.4, + label="third anchor appears", + ) axes[1].set_xlim(steps[0], steps[-1]) axes[1].set_ylim(-0.05, 1.05) axes[1].grid(alpha=0.3) @@ -320,12 +383,23 @@ def plot_bimodal_summary(scenario) -> plt.Figure: axes[1].set_title("two live hypotheses, then one", fontsize=10) axes[2].clear() - axes[2].plot(steps, scenario["error_mean"], "-", color="red", - linewidth=1.8, label="posterior mean") - axes[2].plot(steps, scenario["error_best_mode"], "-", color="tab:blue", - linewidth=1.8, label="nearest mode") - axes[2].axvline(THIRD_ANCHOR_STEP, color="tab:green", linestyle="--", - linewidth=1.4) + axes[2].plot( + steps, + scenario["error_mean"], + "-", + color="red", + linewidth=1.8, + label="posterior mean", + ) + axes[2].plot( + steps, + scenario["error_best_mode"], + "-", + color="tab:blue", + linewidth=1.8, + label="nearest mode", + ) + axes[2].axvline(THIRD_ANCHOR_STEP, color="tab:green", linestyle="--", linewidth=1.4) axes[2].set_xlim(steps[0], steps[-1]) axes[2].set_ylim(0, scenario["error_mean"].max() * 1.15) axes[2].grid(alpha=0.3) @@ -352,44 +426,61 @@ def main() -> None: parser = argparse.ArgumentParser( description="Bimodal particle-filter posterior (Chapter 3)" ) - parser.add_argument("--out-dir", default=str(FIGS_DIR), - help="Output directory for figures") - parser.add_argument("--animate", action="store_true", default=False, - help="Also render the animation GIF (slower)") + parser.add_argument( + "--out-dir", default=str(FIGS_DIR), help="Output directory for figures" + ) + parser.add_argument( + "--animate", + action="store_true", + default=False, + help="Also render the animation GIF (slower)", + ) args = parser.parse_args() print("=" * 70) print("Chapter 3, Section 3.3: a posterior that is not Gaussian") print("=" * 70) - print(f"Two range-only anchors for steps 1-{THIRD_ANCHOR_STEP - 1}, " - f"a third from step {THIRD_ANCHOR_STEP}.\n") + print( + f"Two range-only anchors for steps 1-{THIRD_ANCHOR_STEP - 1}, " + f"a third from step {THIRD_ANCHOR_STEP}.\n" + ) scenario = run_bimodal_scenario() bimodal = scenario["steps"] < THIRD_ANCHOR_STEP resolved = ~bimodal print(f" While bimodal ({bimodal.sum()} steps):") - print(f" particles above baseline: " - f"{scenario['fraction_above'][bimodal].min():.2f} to " - f"{scenario['fraction_above'][bimodal].max():.2f}") + print( + f" particles above baseline: " + f"{scenario['fraction_above'][bimodal].min():.2f} to " + f"{scenario['fraction_above'][bimodal].max():.2f}" + ) print(f" mean error {scenario['error_mean'][bimodal].mean():.2f} m") - print(f" nearest-mode " - f"{scenario['error_best_mode'][bimodal].mean():.2f} m" - f" <- the mean is the misleading one") + print( + f" nearest-mode " + f"{scenario['error_best_mode'][bimodal].mean():.2f} m" + f" <- the mean is the misleading one" + ) print(f" After the third anchor ({resolved.sum()} steps):") print(f" mean error {scenario['error_mean'][resolved].mean():.2f} m") - print(f" nearest-mode " - f"{scenario['error_best_mode'][resolved].mean():.2f} m\n") + print( + f" nearest-mode " + f"{scenario['error_best_mode'][resolved].mean():.2f} m\n" + ) - paths = save_figure(plot_bimodal_summary(scenario), args.out_dir, - "ch3_particle_bimodal") - print(f" saved ch3_particle_bimodal: " - f"{', '.join(p.suffix.lstrip('.') for p in paths)}") + paths = save_figure( + plot_bimodal_summary(scenario), args.out_dir, "ch3_particle_bimodal" + ) + print( + f" saved ch3_particle_bimodal: " + f"{', '.join(p.suffix.lstrip('.') for p in paths)}" + ) if args.animate: fig, update, n_frames = animate_bimodal(scenario) - path = save_animation(fig, update, n_frames, args.out_dir, - "ch3_particle_bimodal", fps=4) + path = save_animation( + fig, update, n_frames, args.out_dir, "ch3_particle_bimodal", fps=4 + ) plt.close(fig) size_mb = path.stat().st_size / (1024 * 1024) print(f" saved {path.name}: {n_frames} frames, {size_mb:.2f} MB") diff --git a/ch4_rf_point_positioning/__init__.py b/ch4_rf_point_positioning/__init__.py index 615353b..85786a6 100644 --- a/ch4_rf_point_positioning/__init__.py +++ b/ch4_rf_point_positioning/__init__.py @@ -12,6 +12,3 @@ """ __version__ = "1.0.0" - - - diff --git a/ch4_rf_point_positioning/example_aoa_positioning.py b/ch4_rf_point_positioning/example_aoa_positioning.py index 196def5..a207a80 100644 --- a/ch4_rf_point_positioning/example_aoa_positioning.py +++ b/ch4_rf_point_positioning/example_aoa_positioning.py @@ -97,17 +97,15 @@ def demo_aoa_basic(): print(f"\nEstimated position: {estimated_position}") print(f"Converged: {info['converged']}") print(f"Iterations: {info['iterations']}") - print( - f"Position error: {np.linalg.norm(estimated_position - true_position):.6f} m" - ) + print(f"Position error: {np.linalg.norm(estimated_position - true_position):.6f} m") return anchors, true_position, aoa_measurements - # Seed for the Monte Carlo in Demo 2. SEED = 42 + def demo_aoa_with_noise(): """Demonstrate AOA positioning with measurement noise.""" print("\n" + "=" * 70) @@ -187,10 +185,14 @@ def demo_aoa_with_noise(): # Print results print("\n" + "-" * 70) - print(f"Median position error over {trials} draws per noise level " - f"(Eq. 4.64 geometry).") - print(f"{'Noise (deg)':<13} {'Median err (m)':<16} {'m/deg':<9} " - f"{'no-converge':<13} {'>100 m':<8}") + print( + f"Median position error over {trials} draws per noise level " + f"(Eq. 4.64 geometry)." + ) + print( + f"{'Noise (deg)':<13} {'Median err (m)':<16} {'m/deg':<9} " + f"{'no-converge':<13} {'>100 m':<8}" + ) print("-" * 70) for r in results: error_str = f"{r['error']:.4f}" if r["error"] != np.inf else "FAILED" @@ -279,9 +281,7 @@ def demo_minimum_anchors(): anchor_configs = { "2 anchors": np.array([[0, 0], [10, 0]], dtype=float), "3 anchors": np.array([[0, 0], [10, 0], [5, 10]], dtype=float), - "4 anchors": np.array( - [[0, 0], [10, 0], [10, 10], [0, 10]], dtype=float - ), + "4 anchors": np.array([[0, 0], [10, 0], [10, 10], [0, 10]], dtype=float), } print(f"\nTrue position (E, N): {true_position}") @@ -295,9 +295,7 @@ def demo_minimum_anchors(): # Try to solve try: positioner = AOAPositioner(anchors) - est_pos, info = positioner.solve( - aoa, initial_guess=np.array([5.0, 5.0]) - ) + est_pos, info = positioner.solve(aoa, initial_guess=np.array([5.0, 5.0])) if info["converged"]: error = np.linalg.norm(est_pos - true_position) @@ -333,9 +331,7 @@ def visualize_aoa_geometry(): # Solve positioner = AOAPositioner(anchors) - est_pos, info = positioner.solve( - aoa_noisy, initial_guess=np.array([6.0, 6.0]) - ) + est_pos, info = positioner.solve(aoa_noisy, initial_guess=np.array([6.0, 6.0])) # Plot fig, ax = plt.subplots(figsize=(10, 10)) @@ -471,14 +467,14 @@ def demo_closed_form_algorithms(): err_ple = np.linalg.norm(pos_ple - true_pos_2d) print("\nResults (perfect measurements):") - print(f" I-LS: pos={pos_ils}, error={err_ils:.6f} m, iters={info_ils['iterations']}") + print( + f" I-LS: pos={pos_ils}, error={err_ils:.6f} m, iters={info_ils['iterations']}" + ) print(f" PLE: pos={pos_ple}, error={err_ple:.6f} m (closed-form)") # === 3D Comparison === print("\n--- 3D Comparison (I-LS vs OVE vs PLE) ---") - anchors_3d = np.array( - [[0, 0, 5], [10, 0, 5], [10, 10, 5], [0, 10, 5]], dtype=float - ) + anchors_3d = np.array([[0, 0, 5], [10, 0, 5], [10, 10, 5], [0, 10, 5]], dtype=float) true_pos_3d = np.array([4.0, 6.0, 0.0]) # Generate angles @@ -521,8 +517,12 @@ def demo_closed_form_algorithms(): for _ in range(n_trials): # Add noise - elev_noisy = elevations + np.random.randn(len(elevations)) * np.deg2rad(noise_deg) - azim_noisy = azimuths_3d + np.random.randn(len(azimuths_3d)) * np.deg2rad(noise_deg) + elev_noisy = elevations + np.random.randn(len(elevations)) * np.deg2rad( + noise_deg + ) + azim_noisy = azimuths_3d + np.random.randn(len(azimuths_3d)) * np.deg2rad( + noise_deg + ) # Iterative LS (unweighted) aoa_noisy = np.zeros(2 * len(anchors_3d)) @@ -530,7 +530,9 @@ def demo_closed_form_algorithms(): aoa_noisy[2 * i] = elev_noisy[i] aoa_noisy[2 * i + 1] = azim_noisy[i] try: - pos, info = positioner_3d.solve(aoa_noisy, initial_guess=np.array([5.0, 5.0, 1.0])) + pos, info = positioner_3d.solve( + aoa_noisy, initial_guess=np.array([5.0, 5.0, 1.0]) + ) if info["converged"]: errors["I-LS"].append(np.linalg.norm(pos - true_pos_3d)) except Exception: @@ -554,7 +556,9 @@ def demo_closed_form_algorithms(): for method, errs in errors.items(): if errs: rmse = np.sqrt(np.mean(np.array(errs) ** 2)) - print(f" {method}: RMSE={rmse:.4f} m (success rate={100*len(errs)/n_trials:.0f}%)") + print( + f" {method}: RMSE={rmse:.4f} m (success rate={100*len(errs)/n_trials:.0f}%)" + ) else: print(f" {method}: No successful trials") @@ -594,7 +598,9 @@ def demo_geometry_sensitivity(): for name, anchors in geometries.items(): # Generate angles azimuths = np.array([aoa_azimuth(a, true_pos) for a in anchors]) - azimuths_noisy = azimuths + np.random.randn(len(azimuths)) * np.deg2rad(noise_deg) + azimuths_noisy = azimuths + np.random.randn(len(azimuths)) * np.deg2rad( + noise_deg + ) # Iterative LS (unweighted) aoa_meas = aoa_angle_vector(anchors, true_pos, include_elevation=False) @@ -639,9 +645,7 @@ def demo_ove_vs_ple_3d(): print("Demo 8: OVE vs PLE 3D Noise Sensitivity") print("=" * 70) - anchors_3d = np.array( - [[0, 0, 5], [10, 0, 5], [10, 10, 5], [0, 10, 5]], dtype=float - ) + anchors_3d = np.array([[0, 0, 5], [10, 0, 5], [10, 10, 5], [0, 10, 5]], dtype=float) true_pos = np.array([5.0, 5.0, 0.0]) elevations = np.array([aoa_elevation(a, true_pos) for a in anchors_3d]) @@ -727,6 +731,3 @@ def main(): if __name__ == "__main__": main() - - - diff --git a/ch4_rf_point_positioning/example_comparison.py b/ch4_rf_point_positioning/example_comparison.py index 9eaf851..49c6a6a 100644 --- a/ch4_rf_point_positioning/example_comparison.py +++ b/ch4_rf_point_positioning/example_comparison.py @@ -59,28 +59,28 @@ def load_rf_dataset(data_dir: str) -> Dict: """Load RF positioning dataset. - + Args: data_dir: Path to dataset directory (e.g., 'data/sim/ch4_rf_2d_square') - + Returns: Dictionary with beacons, positions, measurements, and config """ path = Path(data_dir) data = { - 'beacons': np.loadtxt(path / 'beacons.txt'), - 'positions': np.loadtxt(path / 'ground_truth_positions.txt'), - 'toa_ranges': np.loadtxt(path / 'toa_ranges.txt'), - 'tdoa_diffs': np.loadtxt(path / 'tdoa_diffs.txt'), - 'aoa_angles': np.loadtxt(path / 'aoa_angles.txt'), - 'gdop_toa': np.loadtxt(path / 'gdop_toa.txt'), - 'gdop_tdoa': np.loadtxt(path / 'gdop_tdoa.txt'), - 'gdop_aoa': np.loadtxt(path / 'gdop_aoa.txt'), + "beacons": np.loadtxt(path / "beacons.txt"), + "positions": np.loadtxt(path / "ground_truth_positions.txt"), + "toa_ranges": np.loadtxt(path / "toa_ranges.txt"), + "tdoa_diffs": np.loadtxt(path / "tdoa_diffs.txt"), + "aoa_angles": np.loadtxt(path / "aoa_angles.txt"), + "gdop_toa": np.loadtxt(path / "gdop_toa.txt"), + "gdop_tdoa": np.loadtxt(path / "gdop_tdoa.txt"), + "gdop_aoa": np.loadtxt(path / "gdop_aoa.txt"), } - with open(path / 'config.json') as f: - data['config'] = json.load(f) + with open(path / "config.json") as f: + data["config"] = json.load(f) return data @@ -106,14 +106,14 @@ def solve_every_method(data: dict, verbose: bool = True) -> dict[str, SolveOutco measurements were reported at 0.095 m there and 0.088 m here, and this docstring existed to explain the gap away. """ - beacons = data['beacons'] - truth = data['positions'] + beacons = data["beacons"] + truth = data["positions"] guess = np.mean(beacons, axis=0) solvers = { - 'TOA': (TOAPositioner(beacons, method="iterative_ls"), data['toa_ranges']), - 'TDOA': (TDOAPositioner(beacons, reference_idx=0), data['tdoa_diffs']), - 'AOA': (AOAPositioner(beacons), data['aoa_angles']), + "TOA": (TOAPositioner(beacons, method="iterative_ls"), data["toa_ranges"]), + "TDOA": (TDOAPositioner(beacons, reference_idx=0), data["tdoa_diffs"]), + "AOA": (AOAPositioner(beacons), data["aoa_angles"]), } outcomes = {} @@ -122,14 +122,18 @@ def solve_every_method(data: dict, verbose: bool = True) -> dict[str, SolveOutco if verbose: print(f"\n--- Running {method} Positioning ---") outcomes[method] = solve_batch( - solver, measurements, guess, truth, + solver, + measurements, + guess, + truth, progress=partial(tqdm, desc=method, disable=not verbose), ) return outcomes -def print_method_table(outcomes: dict[str, SolveOutcome], - gdop: dict[str, np.ndarray]) -> None: +def print_method_table( + outcomes: dict[str, SolveOutcome], gdop: dict[str, np.ndarray] +) -> None: """One row per method, always -- failures included, nothing omitted.""" print( f"A fix has failed if it raised, reported converged=False, never left " @@ -170,40 +174,44 @@ def run_with_dataset(data_dir: str, verbose: bool = True) -> dict: print("=" * 70) data = load_rf_dataset(data_dir) - config = data['config'] - beacons = data['beacons'] - positions = data['positions'] + config = data["config"] + beacons = data["beacons"] + positions = data["positions"] if verbose: print("\nDataset Info:") print(f" Geometry: {config.get('geometry', {}).get('type', 'unknown')}") print(f" Beacons: {len(beacons)}") print(f" Test points: {len(positions)}") - print(f" TOA noise: " - f"{config.get('measurements', {}).get('toa_noise_std_m', 'N/A')} m") - print(f" AOA noise: " - f"{config.get('measurements', {}).get('aoa_noise_std_deg', 'N/A')} deg") + print( + f" TOA noise: " + f"{config.get('measurements', {}).get('toa_noise_std_m', 'N/A')} m" + ) + print( + f" AOA noise: " + f"{config.get('measurements', {}).get('aoa_noise_std_deg', 'N/A')} deg" + ) outcomes = solve_every_method(data, verbose=verbose) results = { - 'outcomes': outcomes, - 'gdop': { - 'TOA': data['gdop_toa'], - 'TDOA': data['gdop_tdoa'], - 'AOA': data['gdop_aoa'], + "outcomes": outcomes, + "gdop": { + "TOA": data["gdop_toa"], + "TDOA": data["gdop_tdoa"], + "AOA": data["gdop_aoa"], }, - 'n_points': len(positions), - 'beacons': beacons, - 'positions': positions, - 'config': config, + "n_points": len(positions), + "beacons": beacons, + "positions": positions, + "config": config, } if verbose: print("\n" + "=" * 70) print("Results Summary") print("=" * 70) - print_method_table(outcomes, results['gdop']) + print_method_table(outcomes, results["gdop"]) return results @@ -212,9 +220,9 @@ def run_with_dataset(data_dir: str, verbose: bool = True) -> dict: #: figure's x-axis, so they name the layout rather than grade it: "Linear #: (poor)" prejudges a geometry that is the best of the three for AOA. GEOMETRIES = ( - ('ch4_rf_2d_square', 'Square (4 corners)'), - ('ch4_rf_2d_optimal', 'Optimal (circular)'), - ('ch4_rf_2d_linear', 'Collinear (4 in a row)'), + ("ch4_rf_2d_square", "Square (4 corners)"), + ("ch4_rf_2d_optimal", "Optimal (circular)"), + ("ch4_rf_2d_linear", "Collinear (4 in a row)"), ) @@ -254,7 +262,7 @@ def compare_geometries(verbose: bool = True) -> dict: all_results[geometry_label] = results if verbose: - print_method_table(results['outcomes'], results['gdop']) + print_method_table(results["outcomes"], results["gdop"]) if verbose and all_results: print_geometry_insight(all_results) @@ -274,7 +282,7 @@ def print_geometry_insight(all_results: dict) -> None: for label, results in all_results.items(): row = f"{label:<{width}}" for method in METHODS: - out = results['outcomes'][method] + out = results["outcomes"][method] cell = f"{out.median_m:.3f} [{out.n_failed}]" row += f"{cell:>18}" print(row) @@ -322,7 +330,10 @@ def generate_scenario(seed=42): def toa_positioning_test( - anchors, true_positions, noise_std=0.0, clock_bias_m=0.0, + anchors, + true_positions, + noise_std=0.0, + clock_bias_m=0.0, ): """Test TOA positioning (inline mode). @@ -494,7 +505,8 @@ def rss_positioning_test( for anchor in anchors: # Use simulate_rss_measurement for full fading model (Eq. 4.12) rss_meas, info = simulate_rss_measurement( - anchor, true_pos, + anchor, + true_pos, p_ref_dbm=p_ref_dbm, path_loss_exp=path_loss_exp, sigma_long_db=sigma_long_db, @@ -541,10 +553,10 @@ def run_inline_comparison(): print(" Area: 10m x 10m") # ---- Independent noise schedules per method ---- - toa_noise_levels = [0.0, 0.05, 0.1, 0.2, 0.5] # metres - tdoa_noise_levels = [0.0, 0.05, 0.1, 0.2, 0.5] # metres - aoa_noise_levels_deg = [0.0, 1.0, 3.0, 5.0, 10.0] # degrees - rss_noise_levels = [0.0, 2.0, 4.0, 6.0, 8.0] # dB + toa_noise_levels = [0.0, 0.05, 0.1, 0.2, 0.5] # metres + tdoa_noise_levels = [0.0, 0.05, 0.1, 0.2, 0.5] # metres + aoa_noise_levels_deg = [0.0, 1.0, 3.0, 5.0, 10.0] # degrees + rss_noise_levels = [0.0, 2.0, 4.0, 6.0, 8.0] # dB # Shared clock bias (metres) added to TOA; cancels in TDOA diffs. clock_bias_m = 1.5 @@ -582,7 +594,9 @@ def run_inline_comparison(): results["TOA"].append( toa_positioning_test( - anchors, true_positions, toa_noise, + anchors, + true_positions, + toa_noise, clock_bias_m=clock_bias_m, ) ) @@ -594,13 +608,12 @@ def run_inline_comparison(): ) # Same measurements, same seed offset, weighting switched off. results["AOA_unw"].append( - aoa_positioning_test( - anchors, true_positions, aoa_noise_rad, weighted=False - ) + aoa_positioning_test(anchors, true_positions, aoa_noise_rad, weighted=False) ) results["RSS"].append( rss_positioning_test( - anchors, true_positions, + anchors, + true_positions, sigma_long_db=rss_fading_db, sigma_short_linear=sigma_short_linear, n_samples_avg=n_samples_avg, @@ -615,8 +628,10 @@ def run_inline_comparison(): print("Results Summary (median error in metres)") print("=" * 70) print(f" Clock bias: {clock_bias_m} m (TOA only; cancels in TDOA)") - print(f" RSS config: Rayleigh short-term (sigma={sigma_short_linear}), " - f"{n_samples_avg} samples averaged") + print( + f" RSS config: Rayleigh short-term (sigma={sigma_short_linear}), " + f"{n_samples_avg} samples averaged" + ) print( f" AOA anchor {DEGRADED_ANCHOR} is {DEGRADED_ANCHOR_SCALE:.0f}x noisier " f"than the others; 'AOA unw' solves the same bearings unweighted" @@ -635,6 +650,7 @@ def run_inline_comparison(): # of one method against a handful of its own divergences. See the note # printed under the table. for i in range(n_levels): + def _median(arr): return np.median(arr) if len(arr) > 0 else np.nan @@ -691,56 +707,83 @@ def plot_dataset_results(results: Dict, output_file: str = None): fig, axes = plt.subplots(2, 2, figsize=(14, 10)) fig.suptitle("RF Positioning: Dataset Analysis", fontsize=16, fontweight="bold") - beacons = results['beacons'] - positions = results['positions'] + beacons = results["beacons"] + positions = results["positions"] # 1. Beacon geometry and test points ax1 = axes[0, 0] - ax1.scatter(beacons[:, 0], beacons[:, 1], s=200, c='red', marker='^', - label='Beacons', zorder=10, edgecolors='black', linewidths=2) - ax1.scatter(positions[:, 0], positions[:, 1], s=20, c='blue', alpha=0.5, label='Test Points') + ax1.scatter( + beacons[:, 0], + beacons[:, 1], + s=200, + c="red", + marker="^", + label="Beacons", + zorder=10, + edgecolors="black", + linewidths=2, + ) + ax1.scatter( + positions[:, 0], positions[:, 1], s=20, c="blue", alpha=0.5, label="Test Points" + ) for i, b in enumerate(beacons): - ax1.annotate(f'B{i}', (b[0], b[1]), xytext=(5, 5), textcoords='offset points', fontsize=10) - ax1.set_xlabel('X (m)') - ax1.set_ylabel('Y (m)') - ax1.set_title('Beacon Geometry & Test Points') + ax1.annotate( + f"B{i}", + (b[0], b[1]), + xytext=(5, 5), + textcoords="offset points", + fontsize=10, + ) + ax1.set_xlabel("X (m)") + ax1.set_ylabel("Y (m)") + ax1.set_title("Beacon Geometry & Test Points") ax1.legend() ax1.grid(True, alpha=0.3) - ax1.axis('equal') + ax1.axis("equal") # 2. Error CDF over the fixes that solved, labelled with the ones that # did not. A CDF drawn over the successes alone reads as the method's # accuracy; the label is what stops it reading that way. ax2 = axes[0, 1] - colors = {'TOA': 'blue', 'TDOA': 'red', 'AOA': 'green'} + colors = {"TOA": "blue", "TDOA": "red", "AOA": "green"} for method, color in colors.items(): - out = results['outcomes'][method] + out = results["outcomes"][method] errors = out.errors[out.solved] if len(errors) > 0: sorted_errors = np.sort(errors) cdf = np.arange(1, len(sorted_errors) + 1) / len(sorted_errors) - ax2.plot(sorted_errors, cdf, color=color, linewidth=2, - label=f'{method} ({out.n - out.n_failed}/{out.n} solved)') + ax2.plot( + sorted_errors, + cdf, + color=color, + linewidth=2, + label=f"{method} ({out.n - out.n_failed}/{out.n} solved)", + ) else: - ax2.plot([], [], color=color, linewidth=2, - label=f'{method} (0/{out.n} solved)') - ax2.set_xlabel('Position Error (m)') - ax2.set_ylabel('CDF') - ax2.set_title('Error CDF over solved fixes') + ax2.plot( + [], [], color=color, linewidth=2, label=f"{method} (0/{out.n} solved)" + ) + ax2.set_xlabel("Position Error (m)") + ax2.set_ylabel("CDF") + ax2.set_title("Error CDF over solved fixes") ax2.legend() ax2.grid(True, alpha=0.3) ax2.set_xlim(left=0) # 3. GDOP distribution ax3 = axes[1, 0] - gdop_data = [results['gdop']['TOA'], results['gdop']['TDOA'], results['gdop']['AOA']] - bp = ax3.boxplot(gdop_data, tick_labels=['TOA', 'TDOA', 'AOA'], patch_artist=True) - for patch, color in zip(bp['boxes'], ['blue', 'red', 'green']): + gdop_data = [ + results["gdop"]["TOA"], + results["gdop"]["TDOA"], + results["gdop"]["AOA"], + ] + bp = ax3.boxplot(gdop_data, tick_labels=["TOA", "TDOA", "AOA"], patch_artist=True) + for patch, color in zip(bp["boxes"], ["blue", "red", "green"]): patch.set_facecolor(color) patch.set_alpha(0.6) - ax3.set_ylabel('GDOP') - ax3.set_title('Geometric Dilution of Precision') - ax3.grid(True, alpha=0.3, axis='y') + ax3.set_ylabel("GDOP") + ax3.set_title("Geometric Dilution of Precision") + ax3.grid(True, alpha=0.3, axis="y") # 4. Error vs GDOP scatter. # Both sides are indexed by the same mask, so point i's error is paired @@ -749,14 +792,20 @@ def plot_dataset_results(results: Dict, output_file: str = None): # silently shifted the pairing whenever anything failed. ax4 = axes[1, 1] for method, color in colors.items(): - out = results['outcomes'][method] - gdop = results['gdop'][method] + out = results["outcomes"][method] + gdop = results["gdop"][method] if out.solved.any(): - ax4.scatter(gdop[out.solved], out.errors[out.solved], - alpha=0.5, label=method, color=color, s=20) - ax4.set_xlabel('GDOP') - ax4.set_ylabel('Position Error (m)') - ax4.set_title('Error vs GDOP (lower GDOP = better geometry)') + ax4.scatter( + gdop[out.solved], + out.errors[out.solved], + alpha=0.5, + label=method, + color=color, + s=20, + ) + ax4.set_xlabel("GDOP") + ax4.set_ylabel("Position Error (m)") + ax4.set_title("Error vs GDOP (lower GDOP = better geometry)") ax4.legend() ax4.grid(True, alpha=0.3) @@ -786,31 +835,40 @@ def plot_geometry_comparison(all_results: dict): and still not work -- which is exactly what the RMSE was hiding. """ fig, (ax_err, ax_fail) = plt.subplots(1, 2, figsize=(13, 5.5)) - fig.suptitle("Beacon geometry is method-specific", fontsize=15, - fontweight="bold") + fig.suptitle("Beacon geometry is method-specific", fontsize=15, fontweight="bold") labels = list(all_results) x = np.arange(len(labels)) width = 0.26 - colors = {'TOA': 'tab:blue', 'TDOA': 'tab:red', 'AOA': 'tab:green'} + colors = {"TOA": "tab:blue", "TDOA": "tab:red", "AOA": "tab:green"} for i, method in enumerate(METHODS): - medians = [all_results[g]['outcomes'][method].median_m for g in labels] + medians = [all_results[g]["outcomes"][method].median_m for g in labels] failures = [ - 100.0 * all_results[g]['outcomes'][method].n_failed - / all_results[g]['outcomes'][method].n + 100.0 + * all_results[g]["outcomes"][method].n_failed + / all_results[g]["outcomes"][method].n for g in labels ] offset = x + (i - 1) * width - bars = ax_err.bar(offset, medians, width, label=method, - color=colors[method], edgecolor="white", linewidth=0.8) + bars = ax_err.bar( + offset, + medians, + width, + label=method, + color=colors[method], + edgecolor="white", + linewidth=0.8, + ) for rect, median, failed in zip(bars, medians, failures, strict=True): ax_err.annotate( f"{median:.2f}", (rect.get_x() + rect.get_width() / 2, median), - textcoords="offset points", xytext=(0, 3), - ha="center", fontsize=8, + textcoords="offset points", + xytext=(0, 3), + ha="center", + fontsize=8, ) # Hatch the bars whose median is mostly failures, so a tall bar and # a broken method are distinguishable at a glance. @@ -818,8 +876,15 @@ def plot_geometry_comparison(all_results: dict): rect.set_hatch("///") rect.set_edgecolor("black") - ax_fail.bar(offset, failures, width, label=method, color=colors[method], - edgecolor="white", linewidth=0.8) + ax_fail.bar( + offset, + failures, + width, + label=method, + color=colors[method], + edgecolor="white", + linewidth=0.8, + ) ax_err.set_yscale("log") ax_err.set_ylabel("Median position error (m), log scale") @@ -829,12 +894,10 @@ def plot_geometry_comparison(all_results: dict): # Headroom for the legend, on a log axis, so it cannot sit over a bar. The # first draft put it at the default "best" location, which matplotlib chose # to be on top of the collinear group. - top = max( - all_results[g]['outcomes'][m].median_m for g in labels for m in METHODS - ) + top = max(all_results[g]["outcomes"][m].median_m for g in labels for m in METHODS) ax_err.set_ylim(top=top * 12) ax_err.legend(title="Method", loc="upper center", ncols=3, fontsize=9) - ax_err.grid(True, alpha=0.3, axis='y', which='both') + ax_err.grid(True, alpha=0.3, axis="y", which="both") ax_err.set_axisbelow(True) ax_fail.set_ylabel("Fixes that failed (%)") @@ -843,16 +906,19 @@ def plot_geometry_comparison(all_results: dict): ax_fail.set_xticklabels(labels, fontsize=9) ax_fail.set_ylim(0, 122) ax_fail.legend(title="Method", loc="upper center", ncols=3, fontsize=9) - ax_fail.grid(True, alpha=0.3, axis='y') + ax_fail.grid(True, alpha=0.3, axis="y") ax_fail.set_axisbelow(True) fig.text( - 0.5, 0.055, + 0.5, + 0.055, "Hatched bars are medians made mostly of failed fixes: on the collinear " "array the beacon centroid lies on the line of symmetry,\nso TOA and " "TDOA never leave it, and their 6.77 m is the distance from the seed to " "the truth. AOA is the best of the three there.", - ha="center", fontsize=8.5, style="italic", + ha="center", + fontsize=8.5, + style="italic", ) fig.tight_layout(rect=(0, 0.085, 1, 1)) return fig @@ -869,8 +935,12 @@ def plot_inline_comparison(noise_levels, results): # 1. RMSE vs Noise ax1 = axes[0, 0] for method, color in zip(methods, colors): - rmse_values = [np.sqrt(np.mean(e**2)) if len(e) > 0 else np.nan for e in results[method]] - ax1.plot(noise_levels, rmse_values, "o-", label=method, color=color, linewidth=2) + rmse_values = [ + np.sqrt(np.mean(e**2)) if len(e) > 0 else np.nan for e in results[method] + ] + ax1.plot( + noise_levels, rmse_values, "o-", label=method, color=color, linewidth=2 + ) ax1.set_xlabel("Measurement Noise (m)") ax1.set_ylabel("RMSE (m)") ax1.set_title("RMSE vs Measurement Noise") @@ -898,7 +968,7 @@ def plot_inline_comparison(noise_levels, results): data = [results[m][noise_idx] for m in methods if len(results[m][noise_idx]) > 0] labels = [m for m in methods if len(results[m][noise_idx]) > 0] bp = ax3.boxplot(data, tick_labels=labels, patch_artist=True, showfliers=False) - for patch, color in zip(bp["boxes"], colors[:len(data)]): + for patch, color in zip(bp["boxes"], colors[: len(data)]): patch.set_facecolor(color) patch.set_alpha(0.6) ax3.set_ylabel("Position Error (m)") @@ -918,8 +988,15 @@ def plot_inline_comparison(noise_levels, results): dashes = ["-", "--", "-.", ":"] for method, color, dash in zip(methods, colors, dashes): rates = [len(e) / total_points * 100 for e in results[method]] - ax4.plot(noise_levels, rates, dash, marker="o", label=method, - color=color, linewidth=2) + ax4.plot( + noise_levels, + rates, + dash, + marker="o", + label=method, + color=color, + linewidth=2, + ) ax4.set_xlabel("Measurement Noise (m)") ax4.set_ylabel("Success Rate (%)") ax4.set_title("Convergence Success Rate") @@ -949,19 +1026,24 @@ def main(): # Compare NLOS vs baseline python -m ch4_rf_point_positioning.example_comparison --data ch4_rf_2d_nlos - """ + """, ) parser.add_argument( - "--data", type=str, default=None, - help="Dataset name or path (e.g., 'ch4_rf_2d_square' or full path)" + "--data", + type=str, + default=None, + help="Dataset name or path (e.g., 'ch4_rf_2d_square' or full path)", ) parser.add_argument( - "--compare-geometry", action="store_true", - help="Compare positioning across different beacon geometries" + "--compare-geometry", + action="store_true", + help="Compare positioning across different beacon geometries", ) parser.add_argument( - "--output", type=str, default=None, - help="Output file for figure (default: ch4_rf_point_positioning/figs/ch4_rf_comparison.png)" + "--output", + type=str, + default=None, + help="Output file for figure (default: ch4_rf_point_positioning/figs/ch4_rf_comparison.png)", ) args = parser.parse_args() @@ -975,7 +1057,10 @@ def main(): if len(all_results) > 0: fig = plot_geometry_comparison(all_results) - output_file = args.output or "ch4_rf_point_positioning/figs/ch4_geometry_comparison.png" + output_file = ( + args.output + or "ch4_rf_point_positioning/figs/ch4_geometry_comparison.png" + ) paths = save_figure(fig, Path(output_file).parent, Path(output_file).stem) print(f"\n[OK] Figure saved: {paths[0]}") show_figures_if_requested() @@ -986,7 +1071,9 @@ def main(): if not data_path.exists(): data_path = resolve_data_path(Path("data/sim") / args.data) if not data_path.exists(): - print(f"Error: Dataset not found at '{args.data}' or 'data/sim/{args.data}'") + print( + f"Error: Dataset not found at '{args.data}' or 'data/sim/{args.data}'" + ) print("\nAvailable datasets:") sim_dir = resolve_data_path(Path("data/sim")) if sim_dir.exists(): @@ -997,7 +1084,9 @@ def main(): results = run_with_dataset(str(data_path), verbose=True) - output_file = args.output or "ch4_rf_point_positioning/figs/ch4_rf_comparison.png" + output_file = ( + args.output or "ch4_rf_point_positioning/figs/ch4_rf_comparison.png" + ) Path(output_file).parent.mkdir(parents=True, exist_ok=True) plot_dataset_results(results, output_file) show_figures_if_requested() @@ -1018,7 +1107,9 @@ def main(): fig = plot_inline_comparison(noise_levels, results) - output_file = args.output or "ch4_rf_point_positioning/figs/ch4_rf_comparison.png" + output_file = ( + args.output or "ch4_rf_point_positioning/figs/ch4_rf_comparison.png" + ) paths = save_figure(fig, Path(output_file).parent, Path(output_file).stem) print(f"[OK] Figure saved: {paths[0]}") show_figures_if_requested() diff --git a/ch4_rf_point_positioning/example_dop_geometry.py b/ch4_rf_point_positioning/example_dop_geometry.py index aa41818..d5ceb7d 100644 --- a/ch4_rf_point_positioning/example_dop_geometry.py +++ b/ch4_rf_point_positioning/example_dop_geometry.py @@ -82,9 +82,7 @@ def gdop_at(position, anchors=ANCHORS): """GDOP at a position for the given anchors, or inf if singular.""" try: - return compute_dop(compute_geometry_matrix(anchors, position, "toa"))[ - "GDOP" - ] + return compute_dop(compute_geometry_matrix(anchors, position, "toa"))["GDOP"] except (np.linalg.LinAlgError, ValueError): return np.inf @@ -170,29 +168,58 @@ def _draw_frame(axes, walk, index): # --- GDOP field with the receiver walking through it image = axes[0].imshow( - np.clip(walk["field"], 1.0, GDOP_CLIP), extent=FIELD_EXTENT, - origin="lower", cmap="jet", aspect="equal", vmin=1.0, vmax=GDOP_CLIP, + np.clip(walk["field"], 1.0, GDOP_CLIP), + extent=FIELD_EXTENT, + origin="lower", + cmap="jet", + aspect="equal", + vmin=1.0, + vmax=GDOP_CLIP, + ) + axes[0].plot( + ANCHORS[:, 0], + ANCHORS[:, 1], + "ws", + markersize=9, + markeredgecolor="black", + markeredgewidth=1.5, + label="anchors", + ) + axes[0].plot( + walk["walk"][: index + 1, 0], + walk["walk"][: index + 1, 1], + "-", + color="white", + linewidth=1.2, + alpha=0.7, ) - axes[0].plot(ANCHORS[:, 0], ANCHORS[:, 1], "ws", markersize=9, - markeredgecolor="black", markeredgewidth=1.5, label="anchors") - axes[0].plot(walk["walk"][: index + 1, 0], walk["walk"][: index + 1, 1], - "-", color="white", linewidth=1.2, alpha=0.7) # Monte-Carlo cloud of actual fixes, and the DOP-predicted 3-sigma ellipse. rng = np.random.default_rng(1000 + index) cloud = rng.multivariate_normal(position, walk["covariances"][index], 150) - axes[0].scatter(cloud[:, 0], cloud[:, 1], s=4, c="white", alpha=0.35, - zorder=4) + axes[0].scatter(cloud[:, 0], cloud[:, 1], s=4, c="white", alpha=0.35, zorder=4) eigenvalues, eigenvectors = np.linalg.eigh(walk["covariances"][index]) angle = np.degrees(np.arctan2(eigenvectors[1, -1], eigenvectors[0, -1])) ellipse = Ellipse( - position, 2 * 3 * np.sqrt(eigenvalues[-1]), - 2 * 3 * np.sqrt(eigenvalues[0]), angle=angle, facecolor="none", - edgecolor="white", linewidth=2.0, zorder=5, + position, + 2 * 3 * np.sqrt(eigenvalues[-1]), + 2 * 3 * np.sqrt(eigenvalues[0]), + angle=angle, + facecolor="none", + edgecolor="white", + linewidth=2.0, + zorder=5, ) axes[0].add_patch(ellipse) - axes[0].plot(*position, "o", color="magenta", markersize=8, - markeredgecolor="white", zorder=6, label="receiver") + axes[0].plot( + *position, + "o", + color="magenta", + markersize=8, + markeredgecolor="white", + zorder=6, + label="receiver", + ) axes[0].set_xlim(FIELD_EXTENT[0], FIELD_EXTENT[1]) axes[0].set_ylim(FIELD_EXTENT[2], FIELD_EXTENT[3]) @@ -208,10 +235,22 @@ def _draw_frame(axes, walk, index): # --- DOP prediction against Monte-Carlo truth steps = np.arange(1, index + 2) - axes[1].plot(steps, walk["predicted"][: index + 1], "-", color="tab:blue", - linewidth=2.0, label="predicted: GDOP x range_std") - axes[1].plot(steps, walk["mc_rms"][: index + 1], "o", color="tab:red", - markersize=5, label="measured: TOA solver RMS") + axes[1].plot( + steps, + walk["predicted"][: index + 1], + "-", + color="tab:blue", + linewidth=2.0, + label="predicted: GDOP x range_std", + ) + axes[1].plot( + steps, + walk["mc_rms"][: index + 1], + "o", + color="tab:red", + markersize=5, + label="measured: TOA solver RMS", + ) axes[1].set_xlim(0.5, N_STEPS + 0.5) axes[1].set_ylim(0, max(walk["predicted"].max(), walk["mc_rms"].max()) * 1.1) axes[1].grid(alpha=0.3) @@ -278,10 +317,22 @@ def plot_dop_summary(walk) -> plt.Figure: # Redraw the error panel over the whole walk. steps = np.arange(1, N_STEPS + 1) axes[1].clear() - axes[1].plot(steps, walk["predicted"], "-", color="tab:blue", - linewidth=2.0, label="predicted: GDOP x range_std") - axes[1].plot(steps, walk["mc_rms"], "o", color="tab:red", markersize=5, - label="measured: TOA solver RMS") + axes[1].plot( + steps, + walk["predicted"], + "-", + color="tab:blue", + linewidth=2.0, + label="predicted: GDOP x range_std", + ) + axes[1].plot( + steps, + walk["mc_rms"], + "o", + color="tab:red", + markersize=5, + label="measured: TOA solver RMS", + ) axes[1].set_xlim(0.5, N_STEPS + 0.5) axes[1].set_ylim(0, max(walk["predicted"].max(), walk["mc_rms"].max()) * 1.1) axes[1].grid(alpha=0.3) @@ -308,10 +359,15 @@ def main() -> None: parser = argparse.ArgumentParser( description="Dilution of precision and geometry (Chapter 4)" ) - parser.add_argument("--out-dir", default=str(FIGS_DIR), - help="Output directory for figures") - parser.add_argument("--animate", action="store_true", default=False, - help="Also render the DOP-geometry GIF (slower)") + parser.add_argument( + "--out-dir", default=str(FIGS_DIR), help="Output directory for figures" + ) + parser.add_argument( + "--animate", + action="store_true", + default=False, + help="Also render the DOP-geometry GIF (slower)", + ) args = parser.parse_args() print("=" * 70) @@ -321,23 +377,34 @@ def main() -> None: walk = run_walk() agreement = np.abs(walk["predicted"] - walk["mc_rms"]) / walk["predicted"] - print(f" Four anchors clustered in a {int(ANCHORS[:, 0].max())} m corner, " - f"range noise {RANGE_STD:.0f} m") - print(f" Receiver GDOP: {walk['gdop'][0]:.1f} beside the cluster " - f"-> {walk['gdop'][-1]:.1f} far down the corridor") - print(f" DOP prediction vs Monte-Carlo RMS: mean disagreement " - f"{agreement.mean() * 100:.1f}%") - print(f" position error grows {walk['mc_rms'][-1] / walk['mc_rms'][0]:.0f}x " - f"with an optimal solver and fixed noise -- pure geometry\n") + print( + f" Four anchors clustered in a {int(ANCHORS[:, 0].max())} m corner, " + f"range noise {RANGE_STD:.0f} m" + ) + print( + f" Receiver GDOP: {walk['gdop'][0]:.1f} beside the cluster " + f"-> {walk['gdop'][-1]:.1f} far down the corridor" + ) + print( + f" DOP prediction vs Monte-Carlo RMS: mean disagreement " + f"{agreement.mean() * 100:.1f}%" + ) + print( + f" position error grows {walk['mc_rms'][-1] / walk['mc_rms'][0]:.0f}x " + f"with an optimal solver and fixed noise -- pure geometry\n" + ) paths = save_figure(plot_dop_summary(walk), args.out_dir, "ch4_dop_geometry") - print(f" saved ch4_dop_geometry: " - f"{', '.join(p.suffix.lstrip('.') for p in paths)}") + print( + f" saved ch4_dop_geometry: " + f"{', '.join(p.suffix.lstrip('.') for p in paths)}" + ) if args.animate: fig, update, n_frames = animate_dop(walk) - path = save_animation(fig, update, n_frames, args.out_dir, - "ch4_dop_geometry", fps=4) + path = save_animation( + fig, update, n_frames, args.out_dir, "ch4_dop_geometry", fps=4 + ) plt.close(fig) size_mb = path.stat().st_size / (1024 * 1024) print(f" saved {path.name}: {n_frames} frames, {size_mb:.2f} MB") diff --git a/ch4_rf_point_positioning/example_initial_guess_basin.py b/ch4_rf_point_positioning/example_initial_guess_basin.py index 26e33de..c68ebed 100644 --- a/ch4_rf_point_positioning/example_initial_guess_basin.py +++ b/ch4_rf_point_positioning/example_initial_guess_basin.py @@ -108,8 +108,9 @@ def seed_grid(): axis = np.arange(GRID_MIN, GRID_MAX + GRID_STEP / 2, GRID_STEP) xx, yy = np.meshgrid(axis, axis) seeds = np.column_stack([xx.ravel(), yy.ravel()]) - assert np.min(np.linalg.norm(seeds - TRUTH, axis=1)) > STALL_M, ( - "a seed coincides with the target: it cannot move, and would be scored a stall") + assert ( + np.min(np.linalg.norm(seeds - TRUTH, axis=1)) > STALL_M + ), "a seed coincides with the target: it cannot move, and would be scored a stall" return axis, xx, yy, seeds @@ -161,8 +162,10 @@ def sweep(residual, verbose=True): } if verbose: n_bad = result["n"] - result["counts"][SOLVED] - print(f"\n residual={residual!r}: {n_bad}/{result['n']} seeds failed to reach " - f"the target") + print( + f"\n residual={residual!r}: {n_bad}/{result['n']} seeds failed to reach " + f"the target" + ) for c, name in LABELS.items(): if result["counts"][c]: print(f" {name:<24} {result['counts'][c]:>5}") @@ -187,7 +190,7 @@ def trace_worst(result): finite = np.where(np.isfinite(flat), flat, -np.inf) lying = result["claimed"].ravel() & (result["codes"].ravel() == DIVERGED) ranked = np.where(lying, finite, -np.inf) - if not np.isfinite(ranked).any(): # no silent failure: fall back to the worst + if not np.isfinite(ranked).any(): # no silent failure: fall back to the worst ranked = finite seed = result["seeds"][int(np.argmax(ranked))] _, info = AOAPositioner(ANCHORS).solve( @@ -199,14 +202,23 @@ def trace_worst(result): def plot_basin(ax, result): """One basin map: every seed coloured by what the solve did from there.""" cmap = ListedColormap(COLOURS) - ax.pcolormesh(result["xx"], result["yy"], result["codes"], cmap=cmap, - vmin=-0.5, vmax=len(COLOURS) - 0.5, shading="auto") + ax.pcolormesh( + result["xx"], + result["yy"], + result["codes"], + cmap=cmap, + vmin=-0.5, + vmax=len(COLOURS) - 0.5, + shading="auto", + ) ax.plot(ANCHORS[:, 0], ANCHORS[:, 1], "k^", ms=9, label="anchors") ax.plot(*TRUTH, "w*", ms=18, mec="k", mew=1.2, label="target") solved = result["counts"][SOLVED] - ax.set_title(f'residual="{result["residual"]}" ' - f'{result["n"] - solved}/{result["n"]} seeds fail', - fontsize=11) + ax.set_title( + f'residual="{result["residual"]}" ' + f'{result["n"] - solved}/{result["n"]} seeds fail', + fontsize=11, + ) ax.set_xlabel("initial guess x (m)") ax.set_ylabel("initial guess y (m)") ax.set_aspect("equal") @@ -223,11 +235,23 @@ def plot_failure_rates(ax, results): for k, r in enumerate(results): pos = np.arange(len(codes)) + k * width - 0.4 + width / 2 pct = [100 * r["counts"][c] / r["n"] for c in codes] - bars = ax.bar(pos, pct, width, label=f'residual="{r["residual"]}"', - color=SERIES[k], edgecolor="black", linewidth=0.5) + bars = ax.bar( + pos, + pct, + width, + label=f'residual="{r["residual"]}"', + color=SERIES[k], + edgecolor="black", + linewidth=0.5, + ) for b, v, c in zip(bars, pct, codes, strict=True): - ax.text(b.get_x() + b.get_width() / 2, max(v, 0) + 1.5, - f"{r['counts'][c]}", ha="center", fontsize=9) + ax.text( + b.get_x() + b.get_width() / 2, + max(v, 0) + 1.5, + f"{r['counts'][c]}", + ha="center", + fontsize=9, + ) ax.set_xticks(np.arange(len(codes))) ax.set_xticklabels([LABELS[c].replace(" ", "\n", 1) for c in codes], fontsize=9) ax.set_ylabel("% of seeds") @@ -240,12 +264,16 @@ def plot_trace(ax, seed, history, converged): """The walk to infinity, with the flag it set on arrival.""" d = np.linalg.norm(history - TRUTH, axis=1) ax.semilogy(np.arange(len(d)), np.maximum(d, 1e-12), "o-", color="#6A1B9A", ms=4) - ax.axhline(100.0, color="#C62828", ls="--", lw=1, - label="divergence threshold, 100 m") + ax.axhline( + 100.0, color="#C62828", ls="--", lw=1, label="divergence threshold, 100 m" + ) ax.set_xlabel("Gauss-Newton iteration") ax.set_ylabel("distance from the target (m)") - ax.set_title(f'seed ({seed[0]:.1f}, {seed[1]:.1f}) -> {d[-1]:.1e} m, ' - f'converged={converged}', fontsize=11) + ax.set_title( + f"seed ({seed[0]:.1f}, {seed[1]:.1f}) -> {d[-1]:.1e} m, " + f"converged={converged}", + fontsize=11, + ) ax.grid(True, which="both", alpha=0.3) ax.legend(fontsize=9, loc="lower right") @@ -261,14 +289,27 @@ def plot_summary(results, trace): # only the outcomes that actually occurred: an unused "solver raised" patch sat in a # grey almost identical to the tan bars, which is a second meaning for one colour seen = [c for c in LABELS if any(r["counts"][c] for r in results)] - handles = [Patch(facecolor=COLOURS[c], edgecolor="black", label=LABELS[c]) - for c in seen] - handles += [plt.Line2D([], [], color="k", marker="^", ls="", label="anchor"), - plt.Line2D([], [], color="w", marker="*", mec="k", ls="", ms=12, - label="target")] - fig.legend(handles=handles, loc="lower center", ncol=len(handles), frameon=False, fontsize=9) - fig.suptitle("An initial-guess problem that is not about the initial guess\n" - "AOA, four anchors, zero measurement noise", fontsize=13) + handles = [ + Patch(facecolor=COLOURS[c], edgecolor="black", label=LABELS[c]) for c in seen + ] + handles += [ + plt.Line2D([], [], color="k", marker="^", ls="", label="anchor"), + plt.Line2D( + [], [], color="w", marker="*", mec="k", ls="", ms=12, label="target" + ), + ] + fig.legend( + handles=handles, + loc="lower center", + ncol=len(handles), + frameon=False, + fontsize=9, + ) + fig.suptitle( + "An initial-guess problem that is not about the initial guess\n" + "AOA, four anchors, zero measurement noise", + fontsize=13, + ) fig.tight_layout(rect=(0, 0.045, 1, 0.96)) return fig @@ -276,17 +317,21 @@ def plot_summary(results, trace): def main() -> None: """Sweep both parameterisations and write the figure.""" parser = argparse.ArgumentParser( - description="Initial-guess basin for AOA positioning (Chapter 4)") - parser.add_argument("--out-dir", default=str(FIGS_DIR), - help="Output directory for figures") + description="Initial-guess basin for AOA positioning (Chapter 4)" + ) + parser.add_argument( + "--out-dir", default=str(FIGS_DIR), help="Output directory for figures" + ) args = parser.parse_args() axis, *_ = seed_grid() print("=" * 70) print("Chapter 4: the initial guess is not usually the problem") print("=" * 70) - print(f" {len(axis)}x{len(axis)} seeds over [{GRID_MIN:.0f}, {GRID_MAX:.0f}] m, " - f"target at ({TRUTH[0]:.1f}, {TRUTH[1]:.1f}), zero measurement noise") + print( + f" {len(axis)}x{len(axis)} seeds over [{GRID_MIN:.0f}, {GRID_MAX:.0f}] m, " + f"target at ({TRUTH[0]:.1f}, {TRUTH[1]:.1f}), zero measurement noise" + ) tan_r = sweep("tan") ang_r = sweep("angle") @@ -300,28 +345,49 @@ def main() -> None: print(f" {'':30}{'tan(psi)':>12}{'wrap(angle)':>14}") print(f" {'seeds that fail':30}{bad_tan:>12}{bad_ang:>14}") print(f" {' quiet: stalled or plausible':30}{quiet_tan:>12}{quiet_ang:>14}") - print(f" {' loud: walked off past 100 m':30}" - f"{tan_r['counts'][DIVERGED]:>12}{ang_r['counts'][DIVERGED]:>14}") - print(f" {'failures claiming converged':30}" - f"{tan_r['silent']:>12}{ang_r['silent']:>14}") - print(f"\n Overall {bad_tan / max(bad_ang, 1):.1f}x fewer failures -- but the honest " - f"statement is narrower:") - print(f" the wrapped-angle form removes the QUIET failures ({quiet_tan} -> " - f"{quiet_ang}), the ones") - print(" that look like answers. Seeds far outside the room still walk off under both,") - print(f" and {ang_r['silent']} of those still set converged=True. The residual fix " - f"makes the") - print(" solver honest, not safe -- the four-condition test is still what catches it.") + print( + f" {' loud: walked off past 100 m':30}" + f"{tan_r['counts'][DIVERGED]:>12}{ang_r['counts'][DIVERGED]:>14}" + ) + print( + f" {'failures claiming converged':30}" + f"{tan_r['silent']:>12}{ang_r['silent']:>14}" + ) + print( + f"\n Overall {bad_tan / max(bad_ang, 1):.1f}x fewer failures -- but the honest " + f"statement is narrower:" + ) + print( + f" the wrapped-angle form removes the QUIET failures ({quiet_tan} -> " + f"{quiet_ang}), the ones" + ) + print( + " that look like answers. Seeds far outside the room still walk off under both," + ) + print( + f" and {ang_r['silent']} of those still set converged=True. The residual fix " + f"makes the" + ) + print( + " solver honest, not safe -- the four-condition test is still what catches it." + ) seed, history, converged = trace_worst(tan_r) - print(f"\n Worst SILENT tan failure: seed ({seed[0]:.1f}, {seed[1]:.1f}) walked to " - f"{np.linalg.norm(history[-1] - TRUTH):.2e} m") + print( + f"\n Worst SILENT tan failure: seed ({seed[0]:.1f}, {seed[1]:.1f}) walked to " + f"{np.linalg.norm(history[-1] - TRUTH):.2e} m" + ) print(f" in {len(history) - 1} iterations and reported converged={converged}.") - paths = save_figure(plot_summary([tan_r, ang_r], (seed, history, converged)), - args.out_dir, "ch4_initial_guess_basin") - print(f"\n saved ch4_initial_guess_basin: " - f"{', '.join(p.suffix.lstrip('.') for p in paths)}") + paths = save_figure( + plot_summary([tan_r, ang_r], (seed, history, converged)), + args.out_dir, + "ch4_initial_guess_basin", + ) + print( + f"\n saved ch4_initial_guess_basin: " + f"{', '.join(p.suffix.lstrip('.') for p in paths)}" + ) plt.close("all") print(f"Figures written to {resolve_figs_dir(args.out_dir)}") show_figures_if_requested() diff --git a/ch4_rf_point_positioning/example_tdoa_positioning.py b/ch4_rf_point_positioning/example_tdoa_positioning.py index 240df24..099eb40 100644 --- a/ch4_rf_point_positioning/example_tdoa_positioning.py +++ b/ch4_rf_point_positioning/example_tdoa_positioning.py @@ -40,6 +40,7 @@ # Seed for the Monte Carlo in Demo 2. SEED = 42 + def demo_tdoa_basic(): """Demonstrate basic TDOA positioning with iterative LS. @@ -52,9 +53,7 @@ def demo_tdoa_basic(): print("=" * 70) # Setup anchors (5 anchors in a larger area) - anchors = np.array( - [[0, 0], [15, 0], [15, 15], [0, 15], [7.5, 7.5]], dtype=float - ) + anchors = np.array([[0, 0], [15, 0], [15, 15], [0, 15], [7.5, 7.5]], dtype=float) # True position true_position = np.array([5.0, 8.0]) @@ -81,9 +80,7 @@ def demo_tdoa_basic(): print(f"\nEstimated position: {estimated_position}") print(f"Converged: {info['converged']}") print(f"Iterations: {info['iterations']}") - print( - f"Position error: {np.linalg.norm(estimated_position - true_position):.6f} m" - ) + print(f"Position error: {np.linalg.norm(estimated_position - true_position):.6f} m") return anchors, true_position, tdoa_measurements @@ -95,9 +92,7 @@ def demo_tdoa_with_noise(): print("=" * 70) # Setup - anchors = np.array( - [[0, 0], [20, 0], [20, 20], [0, 20], [10, 10]], dtype=float - ) + anchors = np.array([[0, 0], [20, 0], [20, 20], [0, 20], [10, 10]], dtype=float) true_position = np.array([7.0, 12.0]) # Generate noiseless TDOA @@ -127,9 +122,7 @@ def demo_tdoa_with_noise(): for noise_std in noise_levels: errors, n_failed = [], 0 for _ in range(trials if noise_std > 0 else 1): - tdoa_noisy = ( - tdoa_true + rng.standard_normal(len(tdoa_true)) * noise_std - ) + tdoa_noisy = tdoa_true + rng.standard_normal(len(tdoa_true)) * noise_std positioner = TDOAPositioner(anchors, reference_idx=0) est_pos, info = positioner.solve( tdoa_noisy, initial_guess=np.array([10.0, 10.0]) @@ -162,8 +155,10 @@ def demo_tdoa_with_noise(): # Print results print("\n" + "-" * 70) print(f"Median position error over {trials} draws per noise level.") - print(f"{'Noise (m)':<13} {'Median err (m)':<16} {'err/noise':<11} " - f"{'no-converge':<13} {'>100 m':<8}") + print( + f"{'Noise (m)':<13} {'Median err (m)':<16} {'err/noise':<11} " + f"{'no-converge':<13} {'>100 m':<8}" + ) print("-" * 70) for r in results: error_str = f"{r['error']:.4f}" if r["error"] != np.inf else "FAILED" @@ -199,9 +194,7 @@ def demo_correlated_covariance(): print("=" * 70) # Setup: 5 anchors with heterogeneous noise levels - anchors = np.array( - [[0, 0], [20, 0], [20, 20], [0, 20], [10, 10]], dtype=float - ) + anchors = np.array([[0, 0], [20, 0], [20, 20], [0, 20], [10, 10]], dtype=float) # True position true_position = np.array([7.0, 12.0]) @@ -254,8 +247,10 @@ def demo_correlated_covariance(): # Compute noisy ranges noisy_ranges = np.array( - [np.linalg.norm(true_position - anchors[i]) + range_noise[i] - for i in range(len(anchors))] + [ + np.linalg.norm(true_position - anchors[i]) + range_noise[i] + for i in range(len(anchors)) + ] ) # Compute noisy TDOA (range differences relative to reference) @@ -267,26 +262,24 @@ def demo_correlated_covariance(): positioner = TDOAPositioner(anchors, reference_idx=0) try: est_identity, info_id = positioner.solve( - tdoa_noisy, initial_guess=np.array([10.0, 10.0]), + tdoa_noisy, + initial_guess=np.array([10.0, 10.0]), covariance=cov_identity, ) if info_id["converged"]: - errors_identity.append( - np.linalg.norm(est_identity - true_position) - ) + errors_identity.append(np.linalg.norm(est_identity - true_position)) except Exception: pass # Solve with correlated weighting try: est_correlated, info_corr = positioner.solve( - tdoa_noisy, initial_guess=np.array([10.0, 10.0]), + tdoa_noisy, + initial_guess=np.array([10.0, 10.0]), covariance=cov_correlated, ) if info_corr["converged"]: - errors_correlated.append( - np.linalg.norm(est_correlated - true_position) - ) + errors_correlated.append(np.linalg.norm(est_correlated - true_position)) except Exception: pass @@ -337,9 +330,7 @@ def demo_covariance_sensitivity(): print("=" * 70) # Setup: 4 anchors - anchors = np.array( - [[0, 0], [20, 0], [20, 20], [0, 20]], dtype=float - ) + anchors = np.array([[0, 0], [20, 0], [20, 20], [0, 20]], dtype=float) true_position = np.array([8.0, 12.0]) print(f"\nTrue position: {true_position}") @@ -366,14 +357,15 @@ def demo_covariance_sensitivity(): # Generate noisy ranges range_noise = np.random.randn(len(anchors)) * sigmas noisy_ranges = np.array( - [np.linalg.norm(true_position - anchors[i]) + range_noise[i] - for i in range(len(anchors))] + [ + np.linalg.norm(true_position - anchors[i]) + range_noise[i] + for i in range(len(anchors)) + ] ) # Compute noisy TDOA tdoa_noisy = np.array( - [noisy_ranges[i] - noisy_ranges[0] - for i in range(1, len(anchors))] + [noisy_ranges[i] - noisy_ranges[0] for i in range(1, len(anchors))] ) positioner = TDOAPositioner(anchors, reference_idx=0) @@ -381,7 +373,8 @@ def demo_covariance_sensitivity(): # Identity weighting try: est_id, info = positioner.solve( - tdoa_noisy, initial_guess=np.array([10.0, 10.0]), + tdoa_noisy, + initial_guess=np.array([10.0, 10.0]), covariance=cov_id, ) if info["converged"]: @@ -392,7 +385,8 @@ def demo_covariance_sensitivity(): # Correlated weighting try: est_corr, info = positioner.solve( - tdoa_noisy, initial_guess=np.array([10.0, 10.0]), + tdoa_noisy, + initial_guess=np.array([10.0, 10.0]), covariance=cov_corr, ) if info["converged"]: @@ -400,21 +394,25 @@ def demo_covariance_sensitivity(): except Exception: pass - rmse_id = np.sqrt(np.mean(np.array(errors_id)**2)) - rmse_corr = np.sqrt(np.mean(np.array(errors_corr)**2)) + rmse_id = np.sqrt(np.mean(np.array(errors_id) ** 2)) + rmse_corr = np.sqrt(np.mean(np.array(errors_corr) ** 2)) improvement = (rmse_id - rmse_corr) / rmse_id * 100 if rmse_id > 0 else 0 - results.append({ - "ref_sigma": ref_sigma, - "rmse_identity": rmse_id, - "rmse_correlated": rmse_corr, - "improvement": improvement, - }) + results.append( + { + "ref_sigma": ref_sigma, + "rmse_identity": rmse_id, + "rmse_correlated": rmse_corr, + "improvement": improvement, + } + ) # Print results print("\n" + "-" * 70) - print(f"{'Ref sigma (m)':<15} {'RMSE Id (m)':<15} " - f"{'RMSE Corr (m)':<15} {'Improvement':<15}") + print( + f"{'Ref sigma (m)':<15} {'RMSE Id (m)':<15} " + f"{'RMSE Corr (m)':<15} {'Improvement':<15}" + ) print("-" * 70) for r in results: @@ -470,7 +468,7 @@ def demo_visualize_covariance(): # Show diagonal derivation print("\nDiagonal derivation:") for i, anc_idx in enumerate(non_ref): - diag_val = sigmas[anc_idx]**2 + sigmas[ref_idx]**2 + diag_val = sigmas[anc_idx] ** 2 + sigmas[ref_idx] ** 2 print( f" var(d^{anc_idx},{ref_idx}) = " f"{sigmas[anc_idx]:.2f}^2 + {sigmas[ref_idx]:.2f}^2 = {diag_val:.4f}" @@ -479,24 +477,32 @@ def demo_visualize_covariance(): # Create figure try: fig, ax = plt.subplots(figsize=(8, 6)) - im = ax.imshow(cov, cmap='Blues') - ax.set_title('TDOA Covariance Matrix (Eq. 4.42)', fontsize=12) - ax.set_xlabel('TDOA measurement index') - ax.set_ylabel('TDOA measurement index') + im = ax.imshow(cov, cmap="Blues") + ax.set_title("TDOA Covariance Matrix (Eq. 4.42)", fontsize=12) + ax.set_xlabel("TDOA measurement index") + ax.set_ylabel("TDOA measurement index") # Add colorbar cbar = plt.colorbar(im, ax=ax) - cbar.set_label('Covariance (m^2)') + cbar.set_label("Covariance (m^2)") # Add annotations for i in range(len(cov)): for j in range(len(cov)): - ax.text(j, i, f'{cov[i, j]:.3f}', - ha='center', va='center', color='black', fontsize=9) + ax.text( + j, + i, + f"{cov[i, j]:.3f}", + ha="center", + va="center", + color="black", + fontsize=9, + ) plt.tight_layout() - paths = save_figure(fig, Path(__file__).parent / "figs", - "tdoa_covariance_matrix") + paths = save_figure( + fig, Path(__file__).parent / "figs", "tdoa_covariance_matrix" + ) print(f"\nFigure saved to: {paths[0]}") plt.close() except Exception as e: @@ -514,14 +520,10 @@ def demo_geometry_effect(): true_position = np.array([5.0, 5.0]) # Good geometry: anchors surrounding the target - good_anchors = np.array( - [[0, 0], [10, 0], [10, 10], [0, 10]], dtype=float - ) + good_anchors = np.array([[0, 0], [10, 0], [10, 10], [0, 10]], dtype=float) # Poor geometry: anchors on one side - poor_anchors = np.array( - [[0, 0], [2, 0], [4, 0], [6, 0]], dtype=float - ) + poor_anchors = np.array([[0, 0], [2, 0], [4, 0], [6, 0]], dtype=float) noise_std = 0.2 # meters @@ -605,8 +607,7 @@ def demo_fang_toa_solver(): - the range-weighted iterative solution (requires an initial guess) """ print("\n" + "=" * 70) - print("Demo 7: Fang's TOA Closed-Form vs Range-Weighted LS " - "(Eqs. 4.43-4.49)") + print("Demo 7: Fang's TOA Closed-Form vs Range-Weighted LS " "(Eqs. 4.43-4.49)") print("=" * 70) # Setup: 4 anchors in a square @@ -628,15 +629,17 @@ def demo_fang_toa_solver(): fang_error = np.linalg.norm(fang_pos - true_position) # Range-weighted iterative (W_ii = 1/d_i^2) - positioner = TOAPositioner(anchors, method='range_weighted') + positioner = TOAPositioner(anchors, method="range_weighted") rw_pos, rw_info = positioner.solve( ranges_true, initial_guess=np.array([10.0, 10.0]) ) rw_error = np.linalg.norm(rw_pos - true_position) print(f"Fang: position={fang_pos}, error={fang_error:.6f} m") - print(f"RW-LS: position={rw_pos}, error={rw_error:.6f} m, " - f"iters={rw_info['iterations']}") + print( + f"RW-LS: position={rw_pos}, error={rw_error:.6f} m, " + f"iters={rw_info['iterations']}" + ) # Test with noisy measurements print("\n--- Noisy Measurements (Monte Carlo) ---") @@ -664,28 +667,34 @@ def demo_fang_toa_solver(): i_pos, i_info = positioner.solve( ranges_noisy, initial_guess=np.array([10.0, 10.0]) ) - if i_info['converged']: + if i_info["converged"]: rw_errors.append(np.linalg.norm(i_pos - true_position)) except Exception: pass - fang_rmse = np.sqrt(np.mean(np.array(fang_errors)**2)) - rw_rmse = np.sqrt(np.mean(np.array(rw_errors)**2)) - - results.append({ - 'noise': noise_std, - 'fang_rmse': fang_rmse, - 'rw_rmse': rw_rmse, - 'fang_success': len(fang_errors), - 'rw_success': len(rw_errors), - }) + fang_rmse = np.sqrt(np.mean(np.array(fang_errors) ** 2)) + rw_rmse = np.sqrt(np.mean(np.array(rw_errors) ** 2)) + + results.append( + { + "noise": noise_std, + "fang_rmse": fang_rmse, + "rw_rmse": rw_rmse, + "fang_success": len(fang_errors), + "rw_success": len(rw_errors), + } + ) - print(f"\n{'Noise (m)':<12} {'Fang RMSE':<15} {'RW-LS RMSE':<15} " - f"{'Fang Success':<15} {'RW-LS Success':<15}") + print( + f"\n{'Noise (m)':<12} {'Fang RMSE':<15} {'RW-LS RMSE':<15} " + f"{'Fang Success':<15} {'RW-LS Success':<15}" + ) print("-" * 70) for r in results: - print(f"{r['noise']:<12.2f} {r['fang_rmse']:<15.4f} {r['rw_rmse']:<15.4f} " - f"{r['fang_success']:<15} {r['rw_success']:<15}") + print( + f"{r['noise']:<12.2f} {r['fang_rmse']:<15.4f} {r['rw_rmse']:<15.4f} " + f"{r['fang_success']:<15} {r['rw_success']:<15}" + ) print("\nKey Insights:") print(" - Fang's method is non-iterative (no initial guess required)") @@ -704,14 +713,11 @@ def demo_chan_tdoa_solver(): - the iterative TDOA solution: W = I here, W = Sigma^-1 under noise """ print("\n" + "=" * 70) - print("Demo 8: Chan's TDOA Closed-Form vs Iterative LS/WLS " - "(Eqs. 4.50-4.62)") + print("Demo 8: Chan's TDOA Closed-Form vs Iterative LS/WLS " "(Eqs. 4.50-4.62)") print("=" * 70) # Setup: 5 anchors for good geometry - anchors = np.array( - [[0, 0], [20, 0], [20, 20], [0, 20], [10, 10]], dtype=float - ) + anchors = np.array([[0, 0], [20, 0], [20, 20], [0, 20], [10, 10]], dtype=float) true_position = np.array([8.0, 12.0]) ref_idx = 0 @@ -722,8 +728,9 @@ def demo_chan_tdoa_solver(): # Compute true ranges and TDOA ranges_true = np.linalg.norm(anchors - true_position, axis=1) d_ref = ranges_true[ref_idx] - tdoa_true = np.array([ranges_true[i] - d_ref - for i in range(len(anchors)) if i != ref_idx]) + tdoa_true = np.array( + [ranges_true[i] - d_ref for i in range(len(anchors)) if i != ref_idx] + ) print(f"True reference distance: {d_ref:.4f} m") print(f"True TDOA measurements: {tdoa_true}") @@ -744,8 +751,10 @@ def demo_chan_tdoa_solver(): print(f"Chan: position={chan_pos}, error={chan_error:.6f} m") print(f" reference distance estimate={chan_info['reference_distance']:.4f} m") - print(f"I-LS: position={ils_pos}, error={ils_error:.6f} m, " - f"iters={ils_info['iterations']}") + print( + f"I-LS: position={ils_pos}, error={ils_error:.6f} m, " + f"iters={ils_info['iterations']}" + ) # Test with noisy measurements (correlated noise) print("\n--- Noisy Measurements (Monte Carlo with Correlated Noise) ---") @@ -767,8 +776,13 @@ def demo_chan_tdoa_solver(): ranges_noisy = ranges_true + np.random.randn(len(anchors)) * noise_std # Compute noisy TDOA - tdoa_noisy = np.array([ranges_noisy[i] - ranges_noisy[ref_idx] - for i in range(len(anchors)) if i != ref_idx]) + tdoa_noisy = np.array( + [ + ranges_noisy[i] - ranges_noisy[ref_idx] + for i in range(len(anchors)) + if i != ref_idx + ] + ) # Chan's method (with WLS using covariance) try: @@ -782,31 +796,42 @@ def demo_chan_tdoa_solver(): # I-WLS (with covariance) try: i_pos, i_info = positioner.solve( - tdoa_noisy, initial_guess=np.array([10.0, 10.0]), + tdoa_noisy, + initial_guess=np.array([10.0, 10.0]), covariance=cov, ) - if i_info['converged']: + if i_info["converged"]: iwls_errors.append(np.linalg.norm(i_pos - true_position)) except Exception: pass - chan_rmse = np.sqrt(np.mean(np.array(chan_errors)**2)) if chan_errors else np.inf - iwls_rmse = np.sqrt(np.mean(np.array(iwls_errors)**2)) if iwls_errors else np.inf + chan_rmse = ( + np.sqrt(np.mean(np.array(chan_errors) ** 2)) if chan_errors else np.inf + ) + iwls_rmse = ( + np.sqrt(np.mean(np.array(iwls_errors) ** 2)) if iwls_errors else np.inf + ) - results.append({ - 'noise': noise_std, - 'chan_rmse': chan_rmse, - 'iwls_rmse': iwls_rmse, - 'chan_success': len(chan_errors), - 'iwls_success': len(iwls_errors), - }) + results.append( + { + "noise": noise_std, + "chan_rmse": chan_rmse, + "iwls_rmse": iwls_rmse, + "chan_success": len(chan_errors), + "iwls_success": len(iwls_errors), + } + ) - print(f"\n{'Noise (m)':<12} {'Chan RMSE':<15} {'I-WLS RMSE':<15} " - f"{'Chan Success':<15} {'I-WLS Success':<15}") + print( + f"\n{'Noise (m)':<12} {'Chan RMSE':<15} {'I-WLS RMSE':<15} " + f"{'Chan Success':<15} {'I-WLS Success':<15}" + ) print("-" * 70) for r in results: - print(f"{r['noise']:<12.2f} {r['chan_rmse']:<15.4f} {r['iwls_rmse']:<15.4f} " - f"{r['chan_success']:<15} {r['iwls_success']:<15}") + print( + f"{r['noise']:<12.2f} {r['chan_rmse']:<15.4f} {r['iwls_rmse']:<15.4f} " + f"{r['chan_success']:<15} {r['iwls_success']:<15}" + ) print("\nKey Insights:") print(" - Chan's method is non-iterative, estimates position + ref distance") @@ -830,9 +855,7 @@ def demo_closed_form_comparison(): print("=" * 70) # Setup - anchors = np.array( - [[0, 0], [20, 0], [20, 20], [0, 20], [10, 10]], dtype=float - ) + anchors = np.array([[0, 0], [20, 0], [20, 20], [0, 20], [10, 10]], dtype=float) true_position = np.array([7.5, 11.0]) ref_idx = 0 @@ -853,7 +876,7 @@ def demo_closed_form_comparison(): tdoa_chan_err = [] tdoa_iwls_err = [] - toa_positioner = TOAPositioner(anchors, method='range_weighted') + toa_positioner = TOAPositioner(anchors, method="range_weighted") tdoa_positioner = TDOAPositioner(anchors, reference_idx=ref_idx) sigmas = np.ones(len(anchors)) * noise_std cov = build_tdoa_covariance(sigmas, ref_idx=ref_idx) @@ -863,8 +886,13 @@ def demo_closed_form_comparison(): for _ in range(n_trials): # Generate noisy ranges ranges_noisy = ranges_true + np.random.randn(len(anchors)) * noise_std - tdoa_noisy = np.array([ranges_noisy[i] - ranges_noisy[ref_idx] - for i in range(len(anchors)) if i != ref_idx]) + tdoa_noisy = np.array( + [ + ranges_noisy[i] - ranges_noisy[ref_idx] + for i in range(len(anchors)) + if i != ref_idx + ] + ) # TOA: Fang try: @@ -878,14 +906,16 @@ def demo_closed_form_comparison(): pos, info = toa_positioner.solve( ranges_noisy, initial_guess=np.array([10.0, 10.0]) ) - if info['converged']: + if info["converged"]: toa_rw_err.append(np.linalg.norm(pos - true_position)) except Exception: pass # TDOA: Chan try: - pos, _ = tdoa_chan_solver(anchors, tdoa_noisy, ref_idx=ref_idx, covariance=cov) + pos, _ = tdoa_chan_solver( + anchors, tdoa_noisy, ref_idx=ref_idx, covariance=cov + ) tdoa_chan_err.append(np.linalg.norm(pos - true_position)) except Exception: pass @@ -895,7 +925,7 @@ def demo_closed_form_comparison(): pos, info = tdoa_positioner.solve( tdoa_noisy, initial_guess=np.array([10.0, 10.0]), covariance=cov ) - if info['converged']: + if info["converged"]: tdoa_iwls_err.append(np.linalg.norm(pos - true_position)) except Exception: pass @@ -908,20 +938,30 @@ def stats(errors): return np.sqrt(np.mean(e**2)), np.mean(e), np.std(e), len(e) print("\n" + "-" * 80) - print(f"{'Method':<25} {'RMSE (m)':<12} {'Mean (m)':<12} {'Std (m)':<12} {'Success':<10}") + print( + f"{'Method':<25} {'RMSE (m)':<12} {'Mean (m)':<12} {'Std (m)':<12} {'Success':<10}" + ) print("-" * 80) rmse, mean, std, n = stats(toa_fang_err) - print(f"{'TOA Fang (closed-form)':<25} {rmse:<12.4f} {mean:<12.4f} {std:<12.4f} {n:<10}") + print( + f"{'TOA Fang (closed-form)':<25} {rmse:<12.4f} {mean:<12.4f} {std:<12.4f} {n:<10}" + ) rmse, mean, std, n = stats(toa_rw_err) - print(f"{'TOA RW-LS (iterative)':<25} {rmse:<12.4f} {mean:<12.4f} {std:<12.4f} {n:<10}") + print( + f"{'TOA RW-LS (iterative)':<25} {rmse:<12.4f} {mean:<12.4f} {std:<12.4f} {n:<10}" + ) rmse, mean, std, n = stats(tdoa_chan_err) - print(f"{'TDOA Chan (closed-form)':<25} {rmse:<12.4f} {mean:<12.4f} {std:<12.4f} {n:<10}") + print( + f"{'TDOA Chan (closed-form)':<25} {rmse:<12.4f} {mean:<12.4f} {std:<12.4f} {n:<10}" + ) rmse, mean, std, n = stats(tdoa_iwls_err) - print(f"{'TDOA I-WLS (iterative)':<25} {rmse:<12.4f} {mean:<12.4f} {std:<12.4f} {n:<10}") + print( + f"{'TDOA I-WLS (iterative)':<25} {rmse:<12.4f} {mean:<12.4f} {std:<12.4f} {n:<10}" + ) print("\nSummary:") print(" - Closed-form methods (Fang, Chan) don't need initial guess") @@ -937,39 +977,42 @@ def stats(errors): # TOA comparison ax1 = axes[0] data = [toa_fang_err, toa_rw_err] - tick_labels = ['Fang\n(closed-form)', 'RW-LS\n(iterative)'] + tick_labels = ["Fang\n(closed-form)", "RW-LS\n(iterative)"] bp = ax1.boxplot(data, tick_labels=tick_labels, patch_artist=True) - bp['boxes'][0].set_facecolor('lightblue') - bp['boxes'][1].set_facecolor('lightgreen') - ax1.set_ylabel('Position Error (m)') - ax1.set_title('TOA Positioning Comparison') + bp["boxes"][0].set_facecolor("lightblue") + bp["boxes"][1].set_facecolor("lightgreen") + ax1.set_ylabel("Position Error (m)") + ax1.set_title("TOA Positioning Comparison") ax1.grid(True, alpha=0.3) # TDOA comparison ax2 = axes[1] data = [tdoa_chan_err, tdoa_iwls_err] - tick_labels = ['Chan\n(closed-form)', 'I-WLS\n(iterative)'] + tick_labels = ["Chan\n(closed-form)", "I-WLS\n(iterative)"] bp = ax2.boxplot(data, tick_labels=tick_labels, patch_artist=True) - bp['boxes'][0].set_facecolor('lightyellow') - bp['boxes'][1].set_facecolor('lightcoral') - ax2.set_ylabel('Position Error (m)') - ax2.set_title('TDOA Positioning Comparison') + bp["boxes"][0].set_facecolor("lightyellow") + bp["boxes"][1].set_facecolor("lightcoral") + ax2.set_ylabel("Position Error (m)") + ax2.set_title("TDOA Positioning Comparison") ax2.grid(True, alpha=0.3) - plt.suptitle(f'Closed-Form vs Iterative Solvers (noise={noise_std}m)', fontsize=12) + plt.suptitle( + f"Closed-Form vs Iterative Solvers (noise={noise_std}m)", fontsize=12 + ) plt.tight_layout() - paths = save_figure(fig, Path(__file__).parent / "figs", - "closed_form_comparison") + paths = save_figure( + fig, Path(__file__).parent / "figs", "closed_form_comparison" + ) print(f"\nFigure saved: {paths[0]}") plt.close() except Exception as e: print(f"\nCould not save figure: {e}") return { - 'toa_fang': toa_fang_err, - 'toa_rw': toa_rw_err, - 'tdoa_chan': tdoa_chan_err, - 'tdoa_iwls': tdoa_iwls_err, + "toa_fang": toa_fang_err, + "toa_rw": toa_rw_err, + "tdoa_chan": tdoa_chan_err, + "tdoa_iwls": tdoa_iwls_err, } @@ -1005,6 +1048,3 @@ def main(): if __name__ == "__main__": main() - - - diff --git a/ch4_rf_point_positioning/example_toa_positioning.py b/ch4_rf_point_positioning/example_toa_positioning.py index 6df6d15..0004438 100644 --- a/ch4_rf_point_positioning/example_toa_positioning.py +++ b/ch4_rf_point_positioning/example_toa_positioning.py @@ -69,9 +69,7 @@ def example_toa_perfect(): # Solve using iterative LS (book default: Eq. 4.20) positioner = TOAPositioner(anchors, method="iterative_ls") - estimated_pos, info = positioner.solve( - ranges, initial_guess=np.array([6.0, 6.0]) - ) + estimated_pos, info = positioner.solve(ranges, initial_guess=np.array([6.0, 6.0])) # Results error = np.linalg.norm(estimated_pos - true_pos) @@ -146,11 +144,15 @@ def example_toa_with_noise(): print(f"\nOver {trials} noise draws, against Eq. (4.107):") print(f" HDOP for this geometry : {hdop:.3f}") print(f" predicted HDOP x sigma : {predicted:.4f} m") - print(f" measured RMS error : {measured:.4f} m " - f"({measured / predicted:.2f}x predicted)") - print(f" a single draw lands anywhere in " - f"[{np.percentile(errors, 10):.3f}, " - f"{np.percentile(errors, 90):.3f}] m (10th-90th percentile)") + print( + f" measured RMS error : {measured:.4f} m " + f"({measured / predicted:.2f}x predicted)" + ) + print( + f" a single draw lands anywhere in " + f"[{np.percentile(errors, 10):.3f}, " + f"{np.percentile(errors, 90):.3f}] m (10th-90th percentile)" + ) return anchors, true_pos, estimated_pos @@ -179,15 +181,19 @@ def example_toa_with_clock_bias(): true_clock_bias_m = true_clock_bias_s * SPEED_OF_LIGHT # ~3.0 meters print(f"\nTrue position: {true_pos}") - print(f"True clock bias: {true_clock_bias_s*1e9:.2f} ns = {true_clock_bias_m:.3f} m") + print( + f"True clock bias: {true_clock_bias_s*1e9:.2f} ns = {true_clock_bias_m:.3f} m" + ) print(f" (1 ns = {SPEED_OF_LIGHT*1e-9:.3f} m, 1 m = {1e9/SPEED_OF_LIGHT:.3f} ns)") # Compute ranges WITH clock bias using measurement model # toa_range() takes clock_bias_s in SECONDS - ranges_biased = np.array([ - toa_range(anchor, true_pos, clock_bias_s=true_clock_bias_s) - for anchor in anchors - ]) + ranges_biased = np.array( + [ + toa_range(anchor, true_pos, clock_bias_s=true_clock_bias_s) + for anchor in anchors + ] + ) # Also compute true geometric ranges (no bias) true_ranges = np.array([toa_range(anchor, true_pos) for anchor in anchors]) @@ -199,9 +205,7 @@ def example_toa_with_clock_bias(): # Solve with clock bias estimation # The solver estimates bias in METERS (book convention) initial_guess = np.array([6.0, 6.0, 0.0]) # [x, y, bias_m] - pos, bias_m, info = toa_solve_with_clock_bias( - anchors, ranges_biased, initial_guess - ) + pos, bias_m, info = toa_solve_with_clock_bias(anchors, ranges_biased, initial_guess) # Convert estimated bias from meters to seconds for interpretation bias_s = bias_m / SPEED_OF_LIGHT @@ -265,10 +269,7 @@ def example_rss_positioning(): print(f"RSS measurements: {rss_measurements}") ranges_from_rss = np.array( - [ - rss_to_distance(rss, p_ref_dbm, path_loss_exp) - for rss in rss_measurements - ] + [rss_to_distance(rss, p_ref_dbm, path_loss_exp) for rss in rss_measurements] ) print(f"Estimated ranges: {ranges_from_rss}") @@ -396,7 +397,9 @@ def example_rtt_measurement(): # Without correction: overestimate distance range_wrong = rtt_to_range(rtt_with_proc) - print(f"\n Range without correction: {range_wrong:.2f} m (ERROR: +{range_wrong - distance:.2f} m)") + print( + f"\n Range without correction: {range_wrong:.2f} m (ERROR: +{range_wrong - distance:.2f} m)" + ) # With correction: correct distance range_correct = rtt_to_range(rtt_with_proc, processing_time=processing_time) @@ -427,10 +430,11 @@ def example_rtt_measurement(): # Single measurement rtt, info = simulate_rtt_measurement( - anchor, agent, + anchor, + agent, processing_time=50e-9, processing_time_std=5e-9, # 5 ns std - clock_drift_std=2e-9, # 2 ns std + clock_drift_std=2e-9, # 2 ns std ) print(f"\n True range: {info['true_range']:.2f} m") @@ -445,12 +449,13 @@ def example_rtt_measurement(): errors = [] for _ in range(100): _, info = simulate_rtt_measurement( - anchor, agent, + anchor, + agent, processing_time=50e-9, processing_time_std=5e-9, clock_drift_std=2e-9, ) - errors.append(info['range_estimate'] - 15.0) + errors.append(info["range_estimate"] - 15.0) errors = np.array(errors) print(f" Mean error: {np.mean(errors):.4f} m") @@ -460,12 +465,15 @@ def example_rtt_measurement(): print("\n--- RTT-Based Positioning Example ---") # Multiple anchors - anchors = np.array([ - [0, 0, 0], - [20, 0, 0], - [20, 20, 0], - [0, 20, 0], - ], dtype=float) + anchors = np.array( + [ + [0, 0, 0], + [20, 0, 0], + [20, 20, 0], + [0, 20, 0], + ], + dtype=float, + ) true_pos = np.array([8.0, 12.0, 0.0]) print(f"\n True position: {true_pos[:2]}") @@ -474,18 +482,21 @@ def example_rtt_measurement(): ranges_from_rtt = [] for i, anchor in enumerate(anchors): rtt, info = simulate_rtt_measurement( - anchor, true_pos, + anchor, + true_pos, processing_time=50e-9, processing_time_std=3e-9, ) - ranges_from_rtt.append(info['range_estimate']) - print(f" Anchor {i+1}: RTT={rtt*1e9:.1f}ns -> Range={info['range_estimate']:.3f}m " - f"(true: {info['true_range']:.2f}m)") + ranges_from_rtt.append(info["range_estimate"]) + print( + f" Anchor {i+1}: RTT={rtt*1e9:.1f}ns -> Range={info['range_estimate']:.3f}m " + f"(true: {info['true_range']:.2f}m)" + ) ranges_from_rtt = np.array(ranges_from_rtt) # Position using TOA solver - positioner = TOAPositioner(anchors[:, :2], method='iterative_ls') + positioner = TOAPositioner(anchors[:, :2], method="iterative_ls") est_pos, info = positioner.solve( ranges_from_rtt, initial_guess=np.array([10.0, 10.0]) ) @@ -512,7 +523,8 @@ def example_wls_vs_ls(): # Deliberately asymmetric layout: three close anchors on the left, # one distant anchor on the right. anchors = np.array( - [[0, 0], [0, 8], [2, 4], [15, 5]], dtype=float, + [[0, 0], [0, 8], [2, 4], [15, 5]], + dtype=float, ) true_pos = np.array([6.0, 4.0]) @@ -528,19 +540,20 @@ def example_wls_vs_ls(): errors_wls = [] for _ in range(n_trials): - true_ranges = np.array( - [toa_range(a, true_pos) for a in anchors] - ) + true_ranges = np.array([toa_range(a, true_pos) for a in anchors]) noisy_ranges = true_ranges + np.random.randn(len(anchors)) * sigma_per_anchor init = np.array([5.0, 5.0]) pos_ls, info_ls = TOAPositioner(anchors, method="iterative_ls").solve( - noisy_ranges, initial_guess=init, + noisy_ranges, + initial_guess=init, ) - cov = np.diag(sigma_per_anchor ** 2) + cov = np.diag(sigma_per_anchor**2) pos_wls, info_wls = TOAPositioner(anchors, method="iterative_wls").solve( - noisy_ranges, initial_guess=init, covariance=cov, + noisy_ranges, + initial_guess=init, + covariance=cov, ) if info_ls["converged"]: @@ -551,8 +564,8 @@ def example_wls_vs_ls(): errors_ls = np.array(errors_ls) errors_wls = np.array(errors_wls) - rmse_ls = np.sqrt(np.mean(errors_ls ** 2)) - rmse_wls = np.sqrt(np.mean(errors_wls ** 2)) + rmse_ls = np.sqrt(np.mean(errors_ls**2)) + rmse_wls = np.sqrt(np.mean(errors_wls**2)) print(f"\nMonte-Carlo results ({n_trials} trials):") print(f" LS RMSE: {rmse_ls:.3f} m (converged {len(errors_ls)}/{n_trials})") @@ -598,8 +611,7 @@ def main(): print("=" * 70) fig = plot_toa_positioning(anchors1, true_pos1, est_pos1, info1["history"]) - paths = save_figure(fig, Path(__file__).parent / "figs", - "toa_positioning_example") + paths = save_figure(fig, Path(__file__).parent / "figs", "toa_positioning_example") print(f"\nFigure saved: {paths[0]}") show_figures_if_requested() @@ -611,6 +623,3 @@ def main(): if __name__ == "__main__": main() - - - diff --git a/ch5_fingerprinting/__init__.py b/ch5_fingerprinting/__init__.py index 633a24b..c358e3a 100644 --- a/ch5_fingerprinting/__init__.py +++ b/ch5_fingerprinting/__init__.py @@ -15,5 +15,3 @@ __version__ = "0.1.0" __all__ = [] - - diff --git a/ch5_fingerprinting/example_classification.py b/ch5_fingerprinting/example_classification.py index 9ae13a1..65c30bc 100644 --- a/ch5_fingerprinting/example_classification.py +++ b/ch5_fingerprinting/example_classification.py @@ -37,6 +37,7 @@ # committed figures and reported accuracies can be regenerated exactly. DEFAULT_SEED = 42 + def load_multifloor_database() -> FingerprintDatabase: """Load the shipped three-floor Wi-Fi database, as the other ch5 examples do. @@ -88,19 +89,12 @@ def evaluate_classification_accuracy(db: FingerprintDatabase, rng=None): print("\n--- Training classifiers ---") print(" 1. Random Forest (n_estimators=100)") rf_classifier = fit_classifier( - db, - classifier_type="random_forest", - zone_type="rp", - n_estimators=100 + db, classifier_type="random_forest", zone_type="rp", n_estimators=100 ) print(" 2. SVM (RBF kernel)") svm_classifier = fit_classifier( - db, - classifier_type="svm", - zone_type="rp", - kernel="rbf", - C=1.0 + db, classifier_type="svm", zone_type="rp", kernel="rbf", C=1.0 ) # Two measurements, because only the second one is an accuracy. @@ -141,8 +135,12 @@ def evaluate_classification_accuracy(db: FingerprintDatabase, rng=None): for _ in range(n_queries): idx = rng.integers(0, db.n_reference_points) query = features[idx] + rng.standard_normal(db.n_features) * noise_std - rf_hit += np.allclose(rf_classifier.predict(query)[0], db.locations[idx], atol=0.1) - svm_hit += np.allclose(svm_classifier.predict(query)[0], db.locations[idx], atol=0.1) + rf_hit += np.allclose( + rf_classifier.predict(query)[0], db.locations[idx], atol=0.1 + ) + svm_hit += np.allclose( + svm_classifier.predict(query)[0], db.locations[idx], atol=0.1 + ) rf_accuracy = 100 * rf_hit / n_queries svm_accuracy = 100 * svm_hit / n_queries @@ -153,8 +151,9 @@ def evaluate_classification_accuracy(db: FingerprintDatabase, rng=None): return rf_classifier, svm_classifier -def evaluate_noisy_queries(db: FingerprintDatabase, rf_classifier, svm_classifier, - rng=None): +def evaluate_noisy_queries( + db: FingerprintDatabase, rf_classifier, svm_classifier, rng=None +): """Test classification with noisy queries. Args: @@ -188,7 +187,10 @@ def evaluate_noisy_queries(db: FingerprintDatabase, rf_classifier, svm_classifie # Random RP rp_idx = rng.integers(0, db.n_reference_points) true_loc = db.locations[rp_idx] - query = db.get_mean_features()[rp_idx] + rng.standard_normal(db.n_features) * noise_std + query = ( + db.get_mean_features()[rp_idx] + + rng.standard_normal(db.n_features) * noise_std + ) # RF classification pred_rf, _ = rf_classifier.predict(query) @@ -219,19 +221,20 @@ def evaluate_noisy_queries(db: FingerprintDatabase, rf_classifier, svm_classifie # Plot fig, ax = plt.subplots(figsize=(10, 6)) - ax.plot(noise_levels, rf_errors, 'o-', label='Random Forest', linewidth=2) - ax.plot(noise_levels, svm_errors, 's-', label='SVM', linewidth=2) - ax.plot(noise_levels, knn_errors, '^-', label='k-NN (k=5)', linewidth=2) - ax.set_xlabel('Noise Standard Deviation (dBm)', fontsize=12) - ax.set_ylabel('Positioning Error RMSE (m)', fontsize=12) - ax.set_title('Classification vs k-NN: Robustness to Noise', fontsize=14) + ax.plot(noise_levels, rf_errors, "o-", label="Random Forest", linewidth=2) + ax.plot(noise_levels, svm_errors, "s-", label="SVM", linewidth=2) + ax.plot(noise_levels, knn_errors, "^-", label="k-NN (k=5)", linewidth=2) + ax.set_xlabel("Noise Standard Deviation (dBm)", fontsize=12) + ax.set_ylabel("Positioning Error RMSE (m)", fontsize=12) + ax.set_title("Classification vs k-NN: Robustness to Noise", fontsize=14) ax.legend(fontsize=11) ax.grid(True, alpha=0.3) # Save figure (svg + pdf + png via the shared layer) plt.tight_layout() - paths = save_figure(fig, Path(__file__).parent / "figs", - "classification_noise_robustness") + paths = save_figure( + fig, Path(__file__).parent / "figs", "classification_noise_robustness" + ) print(f"\n [OK] Saved figure: {paths[0]}") plt.close() @@ -258,7 +261,9 @@ def evaluate_hierarchical_localization(db: FingerprintDatabase, rng=None): for _ in range(n_queries): rp_idx = rng.integers(0, db.n_reference_points) - query = db.get_mean_features()[rp_idx] + rng.standard_normal(db.n_features) * 3.0 + query = ( + db.get_mean_features()[rp_idx] + rng.standard_normal(db.n_features) * 3.0 + ) queries.append(query) true_locs.append(db.locations[rp_idx]) true_floors.append(db.floor_ids[rp_idx]) @@ -282,11 +287,7 @@ def evaluate_hierarchical_localization(db: FingerprintDatabase, rng=None): floor_hit = [] for i, query in enumerate(queries): pred, info = hierarchical_localize( - query, - db, - coarse_method="floor", - fine_method="knn", - k=5 + query, db, coarse_method="floor", fine_method="knn", k=5 ) hier_errors.append(np.linalg.norm(pred - true_locs[i])) floor_hit.append(info["coarse_floor"] == true_floors[i]) @@ -295,8 +296,10 @@ def evaluate_hierarchical_localization(db: FingerprintDatabase, rng=None): floor_correct = int(floor_hit.sum()) hier_rmse = np.sqrt(np.mean(np.array(hier_errors) ** 2)) floor_accuracy = 100 * floor_correct / n_queries - print(f" Floor classification accuracy: {floor_accuracy:.1f}% " - f"({floor_correct}/{n_queries}, chance = {100 / db.n_floors:.1f}%)") + print( + f" Floor classification accuracy: {floor_accuracy:.1f}% " + f"({floor_correct}/{n_queries}, chance = {100 / db.n_floors:.1f}%)" + ) # Both numbers, because the second one is conditional and the difference # matters. The single line here used to read "RMSE (given correct floor)" @@ -307,9 +310,11 @@ def evaluate_hierarchical_localization(db: FingerprintDatabase, rng=None): print(f" RMSE, all queries: {hier_rmse:.2f} m") if floor_correct: subset = np.array(hier_errors)[floor_hit] - subset_rmse = float(np.sqrt(np.mean(subset ** 2))) - print(f" RMSE, correct-floor subset: {subset_rmse:.2f} m " - f"(n = {floor_correct})") + subset_rmse = float(np.sqrt(np.mean(subset**2))) + print( + f" RMSE, correct-floor subset: {subset_rmse:.2f} m " + f"(n = {floor_correct})" + ) if floor_correct < n_queries: print(" The subset figure is the accuracy of the queries the coarse") print(" step got right, so it flatters the method whenever that step") @@ -320,10 +325,7 @@ def evaluate_hierarchical_localization(db: FingerprintDatabase, rng=None): hier_rf_errors = [] for i, query in enumerate(queries): pred, info = hierarchical_localize( - query, - db, - coarse_method="random_forest", - fine_method="map" + query, db, coarse_method="random_forest", fine_method="map" ) hier_rf_errors.append(np.linalg.norm(pred - true_locs[i])) hier_rf_rmse = np.sqrt(np.mean(np.array(hier_rf_errors) ** 2)) @@ -334,11 +336,7 @@ def evaluate_hierarchical_localization(db: FingerprintDatabase, rng=None): hier_pm_errors = [] for i, query in enumerate(queries): pred, info = hierarchical_localize( - query, - db, - coarse_method="floor", - fine_method="posterior_mean", - top_k=10 + query, db, coarse_method="floor", fine_method="posterior_mean", top_k=10 ) hier_pm_errors.append(np.linalg.norm(pred - true_locs[i])) hier_pm_rmse = np.sqrt(np.mean(np.array(hier_pm_errors) ** 2)) @@ -363,11 +361,16 @@ def evaluate_hierarchical_localization(db: FingerprintDatabase, rng=None): print(f" {'Method':<24}{'RMSE':>8}{'median':>9}{'exact hit':>12}") for name, errs in runs.items(): e = np.asarray(errs) - print(f" {name:<24}{np.sqrt(np.mean(e ** 2)):>7.2f}m{np.median(e):>8.2f}m" - f"{100 * np.mean(e < 1e-9):>11.1f}%") + print( + f" {name:<24}{np.sqrt(np.mean(e ** 2)):>7.2f}m{np.median(e):>8.2f}m" + f"{100 * np.mean(e < 1e-9):>11.1f}%" + ) - candidates = {k: float(np.sqrt(np.mean(np.asarray(v) ** 2))) - for k, v in runs.items() if k != "Direct k-NN (baseline)"} + candidates = { + k: float(np.sqrt(np.mean(np.asarray(v) ** 2))) + for k, v in runs.items() + if k != "Direct k-NN (baseline)" + } beat = {k: v for k, v in candidates.items() if v < direct_rmse - 1e-9} tied = {k: v for k, v in candidates.items() if abs(v - direct_rmse) <= 1e-9} @@ -375,8 +378,10 @@ def evaluate_hierarchical_localization(db: FingerprintDatabase, rng=None): if beat: best = min(beat, key=beat.get) gain = 100 * (direct_rmse - beat[best]) / direct_rmse - print(f" Best: hierarchical ({best}) at {beat[best]:.2f} m, " - f"{gain:.1f}% below the baseline.") + print( + f" Best: hierarchical ({best}) at {beat[best]:.2f} m, " + f"{gain:.1f}% below the baseline." + ) exact = 100 * np.mean(np.asarray(runs[best]) < 1e-9) if exact > 20: print(f" Read that with the exact-hit column: {exact:.0f}% of its") @@ -406,7 +411,7 @@ def evaluate_hierarchical_localization(db: FingerprintDatabase, rng=None): "Hierarchical (RF -> MAP)": hier_rf_errors, "Hierarchical (Floor -> PM)": hier_pm_errors, }, - title='Error Distribution (CDF)', + title="Error Distribution (CDF)", ax=ax, title_fontweight="normal", ) @@ -416,59 +421,67 @@ def evaluate_hierarchical_localization(db: FingerprintDatabase, rng=None): ax = axes[0, 1] ax.boxplot( [direct_errors, hier_errors, hier_rf_errors, hier_pm_errors], - tick_labels=['Direct\nk-NN', 'Hier\nFloor->kNN', 'Hier\nRF->MAP', 'Hier\nFloor->PM'], - showfliers=False + tick_labels=[ + "Direct\nk-NN", + "Hier\nFloor->kNN", + "Hier\nRF->MAP", + "Hier\nFloor->PM", + ], + showfliers=False, ) - ax.set_ylabel('Positioning Error (m)', fontsize=11) - ax.set_title('Error Distribution (Box Plot)', fontsize=12) - ax.grid(True, alpha=0.3, axis='y') + ax.set_ylabel("Positioning Error (m)", fontsize=11) + ax.set_title("Error Distribution (Box Plot)", fontsize=12) + ax.grid(True, alpha=0.3, axis="y") # RMSE comparison ax = axes[1, 0] - methods = ['Direct\nk-NN', 'Hier\nFloor->kNN', 'Hier\nRF->MAP', 'Hier\nFloor->PM'] + methods = ["Direct\nk-NN", "Hier\nFloor->kNN", "Hier\nRF->MAP", "Hier\nFloor->PM"] rmses = [direct_rmse, hier_rmse, hier_rf_rmse, hier_pm_rmse] - bars = ax.bar(methods, rmses, color=['C0', 'C1', 'C2', 'C3'], alpha=0.7) - ax.set_ylabel('RMSE (m)', fontsize=11) - ax.set_title('RMSE Comparison', fontsize=12) - ax.grid(True, alpha=0.3, axis='y') + bars = ax.bar(methods, rmses, color=["C0", "C1", "C2", "C3"], alpha=0.7) + ax.set_ylabel("RMSE (m)", fontsize=11) + ax.set_title("RMSE Comparison", fontsize=12) + ax.grid(True, alpha=0.3, axis="y") # Add values on bars for bar, rmse in zip(bars, rmses): height = bar.get_height() ax.text( bar.get_x() + bar.get_width() / 2, height, - f'{rmse:.2f}m', - ha='center', - va='bottom', - fontsize=10 + f"{rmse:.2f}m", + ha="center", + va="bottom", + fontsize=10, ) # Floor classification confusion matrix (for hierarchical methods) ax = axes[1, 1] ax.text( - 0.5, 0.6, + 0.5, + 0.6, f"Floor Classification\nAccuracy: {floor_accuracy:.1f}%", - ha='center', - va='center', + ha="center", + va="center", fontsize=14, - transform=ax.transAxes + transform=ax.transAxes, ) ax.text( - 0.5, 0.4, + 0.5, + 0.4, f"Correct: {floor_correct}/{n_queries}", - ha='center', - va='center', + ha="center", + va="center", fontsize=12, - transform=ax.transAxes + transform=ax.transAxes, ) - ax.axis('off') + ax.axis("off") - plt.suptitle('Hierarchical Localization Performance', fontsize=14, y=0.995) + plt.suptitle("Hierarchical Localization Performance", fontsize=14, y=0.995) plt.tight_layout() # Save figure (svg + pdf + png via the shared layer) - paths = save_figure(fig, Path(__file__).parent / "figs", - "hierarchical_localization") + paths = save_figure( + fig, Path(__file__).parent / "figs", "hierarchical_localization" + ) print(f"\n [OK] Saved figure: {paths[0]}") plt.close() @@ -516,4 +529,3 @@ def main(): if __name__ == "__main__": main() - diff --git a/ch5_fingerprinting/example_comparison.py b/ch5_fingerprinting/example_comparison.py index 56f7975..b8093a6 100644 --- a/ch5_fingerprinting/example_comparison.py +++ b/ch5_fingerprinting/example_comparison.py @@ -97,17 +97,30 @@ def generate_test_queries( if ap_positions is not None and pl_cfg: return _generate_queries_pathloss( - db, np.asarray(ap_positions), pl_cfg, - n_queries=n_queries, floor_id=floor_id, noise_std=noise_std, + db, + np.asarray(ap_positions), + pl_cfg, + n_queries=n_queries, + floor_id=floor_id, + noise_std=noise_std, ) return _generate_queries_holdout( - db, n_queries=n_queries, floor_id=floor_id, noise_std=noise_std, + db, + n_queries=n_queries, + floor_id=floor_id, + noise_std=noise_std, ) def _generate_queries_pathloss( - db, ap_positions, pl_cfg, *, n_queries, floor_id, noise_std, + db, + ap_positions, + pl_cfg, + *, + n_queries, + floor_id, + noise_std, ): """Physics-based query generation using the log-distance path-loss model.""" p0 = pl_cfg.get("P0_dBm", -30.0) @@ -127,10 +140,12 @@ def _generate_queries_pathloss( min_x, max_x = rp_locs[:, 0].min(), rp_locs[:, 0].max() min_y, max_y = rp_locs[:, 1].min(), rp_locs[:, 1].max() - true_locs = np.column_stack([ - np.random.uniform(min_x, max_x, n_queries), - np.random.uniform(min_y, max_y, n_queries), - ]) + true_locs = np.column_stack( + [ + np.random.uniform(min_x, max_x, n_queries), + np.random.uniform(min_y, max_y, n_queries), + ] + ) n_aps = len(ap_positions) query_fingerprints = np.empty((n_queries, n_aps)) @@ -281,17 +296,19 @@ def evaluate_scenario(scenario_name, db, queries, true_locs, floor_id=None): errors.append(np.linalg.norm(est_loc - true_loc)) times.append((t_end - t_start) * 1000) - results.append({ - "method": method_name, - "category": "Deterministic", - "errors": np.array(errors), - "times": np.array(times), - "rmse": np.sqrt(np.mean(np.array(errors)**2)), - "median": np.median(errors), - "p90": np.percentile(errors, 90), - "mean_time_ms": np.mean(times), - "ops_per_query": ops_per_query[method_name], - }) + results.append( + { + "method": method_name, + "category": "Deterministic", + "errors": np.array(errors), + "times": np.array(times), + "rmse": np.sqrt(np.mean(np.array(errors) ** 2)), + "median": np.median(errors), + "p90": np.percentile(errors, 90), + "mean_time_ms": np.mean(times), + "ops_per_query": ops_per_query[method_name], + } + ) print(f"RMSE={results[-1]['rmse']:.2f}m") # k-NN @@ -300,23 +317,31 @@ def evaluate_scenario(scenario_name, db, queries, true_locs, floor_id=None): errors, times = [], [] for query, true_loc in zip(queries, true_locs): t_start = time.perf_counter() - est_loc = knn_localize(query, db, k=3, metric="euclidean", - weighting="inverse_distance", floor_id=floor_id) + est_loc = knn_localize( + query, + db, + k=3, + metric="euclidean", + weighting="inverse_distance", + floor_id=floor_id, + ) t_end = time.perf_counter() errors.append(np.linalg.norm(est_loc - true_loc)) times.append((t_end - t_start) * 1000) - results.append({ - "method": method_name, - "category": "Deterministic", - "errors": np.array(errors), - "times": np.array(times), - "rmse": np.sqrt(np.mean(np.array(errors)**2)), - "median": np.median(errors), - "p90": np.percentile(errors, 90), - "mean_time_ms": np.mean(times), - "ops_per_query": ops_per_query[method_name], - }) + results.append( + { + "method": method_name, + "category": "Deterministic", + "errors": np.array(errors), + "times": np.array(times), + "rmse": np.sqrt(np.mean(np.array(errors) ** 2)), + "median": np.median(errors), + "p90": np.percentile(errors, 90), + "mean_time_ms": np.mean(times), + "ops_per_query": ops_per_query[method_name], + } + ) print(f"RMSE={results[-1]['rmse']:.2f}m") # Probabilistic methods @@ -338,17 +363,19 @@ def evaluate_scenario(scenario_name, db, queries, true_locs, floor_id=None): errors.append(np.linalg.norm(est_loc - true_loc)) times.append((t_end - t_start) * 1000) - results.append({ - "method": method_name, - "category": "Probabilistic", - "errors": np.array(errors), - "times": np.array(times), - "rmse": np.sqrt(np.mean(np.array(errors)**2)), - "median": np.median(errors), - "p90": np.percentile(errors, 90), - "mean_time_ms": np.mean(times), - "ops_per_query": ops_per_query[method_name], - }) + results.append( + { + "method": method_name, + "category": "Probabilistic", + "errors": np.array(errors), + "times": np.array(times), + "rmse": np.sqrt(np.mean(np.array(errors) ** 2)), + "median": np.median(errors), + "p90": np.percentile(errors, 90), + "mean_time_ms": np.mean(times), + "ops_per_query": ops_per_query[method_name], + } + ) print(f"RMSE={results[-1]['rmse']:.2f}m") # Posterior Mean (Full) @@ -362,17 +389,19 @@ def evaluate_scenario(scenario_name, db, queries, true_locs, floor_id=None): errors.append(np.linalg.norm(est_loc - true_loc)) times.append((t_end - t_start) * 1000) - results.append({ - "method": method_name, - "category": "Probabilistic", - "errors": np.array(errors), - "times": np.array(times), - "rmse": np.sqrt(np.mean(np.array(errors)**2)), - "median": np.median(errors), - "p90": np.percentile(errors, 90), - "mean_time_ms": np.mean(times), - "ops_per_query": ops_per_query[method_name], - }) + results.append( + { + "method": method_name, + "category": "Probabilistic", + "errors": np.array(errors), + "times": np.array(times), + "rmse": np.sqrt(np.mean(np.array(errors) ** 2)), + "median": np.median(errors), + "p90": np.percentile(errors, 90), + "mean_time_ms": np.mean(times), + "ops_per_query": ops_per_query[method_name], + } + ) print(f"RMSE={results[-1]['rmse']:.2f}m") # Posterior Mean (Top-k) - Book guidance: typically sufficient @@ -381,22 +410,26 @@ def evaluate_scenario(scenario_name, db, queries, true_locs, floor_id=None): errors, times = [], [] for query, true_loc in zip(queries, true_locs): t_start = time.perf_counter() - est_loc = posterior_mean_localize(query, model_bayes, floor_id=floor_id, top_k=10) + est_loc = posterior_mean_localize( + query, model_bayes, floor_id=floor_id, top_k=10 + ) t_end = time.perf_counter() errors.append(np.linalg.norm(est_loc - true_loc)) times.append((t_end - t_start) * 1000) - results.append({ - "method": method_name, - "category": "Probabilistic", - "errors": np.array(errors), - "times": np.array(times), - "rmse": np.sqrt(np.mean(np.array(errors)**2)), - "median": np.median(errors), - "p90": np.percentile(errors, 90), - "mean_time_ms": np.mean(times), - "ops_per_query": ops_per_query[method_name], - }) + results.append( + { + "method": method_name, + "category": "Probabilistic", + "errors": np.array(errors), + "times": np.array(times), + "rmse": np.sqrt(np.mean(np.array(errors) ** 2)), + "median": np.median(errors), + "p90": np.percentile(errors, 90), + "mean_time_ms": np.mean(times), + "ops_per_query": ops_per_query[method_name], + } + ) print(f"RMSE={results[-1]['rmse']:.2f}m") # Pattern Recognition @@ -417,17 +450,19 @@ def evaluate_scenario(scenario_name, db, queries, true_locs, floor_id=None): errors.append(np.linalg.norm(est_loc - true_loc)) times.append((t_end - t_start) * 1000) - results.append({ - "method": method_name, - "category": "Pattern Recognition", - "errors": np.array(errors), - "times": np.array(times), - "rmse": np.sqrt(np.mean(np.array(errors)**2)), - "median": np.median(errors), - "p90": np.percentile(errors, 90), - "mean_time_ms": np.mean(times), - "ops_per_query": ops_per_query[method_name], - }) + results.append( + { + "method": method_name, + "category": "Pattern Recognition", + "errors": np.array(errors), + "times": np.array(times), + "rmse": np.sqrt(np.mean(np.array(errors) ** 2)), + "median": np.median(errors), + "p90": np.percentile(errors, 90), + "mean_time_ms": np.mean(times), + "ops_per_query": ops_per_query[method_name], + } + ) print(f"RMSE={results[-1]['rmse']:.2f}m") return results @@ -442,9 +477,9 @@ def main(): formatter_class=argparse.RawDescriptionHelpFormatter, ).parse_args() - print("="*70) + print("=" * 70) print("Chapter 5: Fingerprinting Methods Comparison") - print("="*70) + print("=" * 70) # Load database print("\nLoading fingerprint database...") @@ -455,83 +490,99 @@ def main(): all_results = {} # Scenario 1: Baseline (low noise, single floor) - print("\n" + "="*70) + print("\n" + "=" * 70) print("SCENARIO 1: Baseline (low noise, single floor)") - print("="*70) + print("=" * 70) queries1, true_locs1, _ = generate_test_queries( db, n_queries=200, floor_id=0, noise_std=1.0, seed=42 ) all_results["Baseline"] = evaluate_scenario( - "Baseline (extra sigma=1dBm on top of 4dBm shadowing, Floor 0)", db, queries1, true_locs1, floor_id=0 + "Baseline (extra sigma=1dBm on top of 4dBm shadowing, Floor 0)", + db, + queries1, + true_locs1, + floor_id=0, ) # Scenario 2: Moderate noise - print("\n" + "="*70) + print("\n" + "=" * 70) print("SCENARIO 2: Moderate Noise") - print("="*70) + print("=" * 70) queries2, true_locs2, _ = generate_test_queries( db, n_queries=200, floor_id=0, noise_std=2.0, seed=43 ) all_results["Moderate Noise"] = evaluate_scenario( - "Moderate (extra sigma=2dBm on top of 4dBm shadowing, Floor 0)", db, queries2, true_locs2, floor_id=0 + "Moderate (extra sigma=2dBm on top of 4dBm shadowing, Floor 0)", + db, + queries2, + true_locs2, + floor_id=0, ) # Scenario 3: High noise - print("\n" + "="*70) + print("\n" + "=" * 70) print("SCENARIO 3: High Noise") - print("="*70) + print("=" * 70) queries3, true_locs3, _ = generate_test_queries( db, n_queries=200, floor_id=0, noise_std=5.0, seed=44 ) all_results["High Noise"] = evaluate_scenario( - "High (extra sigma=5dBm on top of 4dBm shadowing, Floor 0)", db, queries3, true_locs3, floor_id=0 + "High (extra sigma=5dBm on top of 4dBm shadowing, Floor 0)", + db, + queries3, + true_locs3, + floor_id=0, ) # Print summary table - print("\n" + "="*70) + print("\n" + "=" * 70) print("COMPREHENSIVE RESULTS SUMMARY") - print("="*70) + print("=" * 70) for scenario_name, results in all_results.items(): print(f"\n{scenario_name}:") - print(f"{'Method':<20} {'Category':<20} {'RMSE (m)':<12} {'Median (m)':<12} {'P90 (m)':<12} {'Time (ms)':<12}") - print("-"*90) + print( + f"{'Method':<20} {'Category':<20} {'RMSE (m)':<12} {'Median (m)':<12} {'P90 (m)':<12} {'Time (ms)':<12}" + ) + print("-" * 90) for r in results: - print(f"{r['method']:<20} {r['category']:<20} {r['rmse']:<12.2f} " - f"{r['median']:<12.2f} {r['p90']:<12.2f} {r['mean_time_ms']:<12.3f}") + print( + f"{r['method']:<20} {r['category']:<20} {r['rmse']:<12.2f} " + f"{r['median']:<12.2f} {r['p90']:<12.2f} {r['mean_time_ms']:<12.3f}" + ) # Visualizations - print("\n" + "="*70) + print("\n" + "=" * 70) print("Generating comparison visualizations...") - print("="*70) + print("=" * 70) fig = plt.figure(figsize=(18, 12)) # Plot 1: RMSE comparison across scenarios ax1 = plt.subplot(3, 3, 1) - methods = [r['method'] for r in all_results["Baseline"]] + methods = [r["method"] for r in all_results["Baseline"]] x = np.arange(len(methods)) width = 0.25 for i, (scenario_name, results) in enumerate(all_results.items()): - rmses = [r['rmse'] for r in results] - ax1.bar(x + i*width, rmses, width, label=scenario_name, alpha=0.8) + rmses = [r["rmse"] for r in results] + ax1.bar(x + i * width, rmses, width, label=scenario_name, alpha=0.8) - ax1.set_ylabel('RMSE (m)') - ax1.set_title('RMSE Comparison Across Scenarios') + ax1.set_ylabel("RMSE (m)") + ax1.set_title("RMSE Comparison Across Scenarios") ax1.set_xticks(x + width) - ax1.set_xticklabels(methods, rotation=45, ha='right', fontsize=8) + ax1.set_xticklabels(methods, rotation=45, ha="right", fontsize=8) ax1.legend(fontsize=8) - ax1.grid(True, alpha=0.3, axis='y') + ax1.grid(True, alpha=0.3, axis="y") # Plot 2: Error CDF (Baseline scenario) ax2 = plt.subplot(3, 3, 2) plot_error_cdf( - {r['method']: r['errors'] for r in all_results["Baseline"]}, - title='Error CDF (Baseline)', + {r["method"]: r["errors"] for r in all_results["Baseline"]}, + title="Error CDF (Baseline)", ax=ax2, title_fontweight="normal", ) @@ -540,26 +591,26 @@ def main(): # Plot 3: Computation time comparison ax3 = plt.subplot(3, 3, 3) - methods = [r['method'] for r in all_results["Baseline"]] - ops = [r['ops_per_query'] for r in all_results["Baseline"]] - colors = ['blue', 'cyan', 'red', 'orange', 'green', 'purple'] - ax3.barh(methods, ops, color=colors[:len(methods)], alpha=0.7) - ax3.set_xscale('log') - ax3.set_xlabel('Operations per query') - ax3.set_title('Per-Query Cost (Baseline)') - ax3.grid(True, alpha=0.3, axis='x') + methods = [r["method"] for r in all_results["Baseline"]] + ops = [r["ops_per_query"] for r in all_results["Baseline"]] + colors = ["blue", "cyan", "red", "orange", "green", "purple"] + ax3.barh(methods, ops, color=colors[: len(methods)], alpha=0.7) + ax3.set_xscale("log") + ax3.set_xlabel("Operations per query") + ax3.set_title("Per-Query Cost (Baseline)") + ax3.grid(True, alpha=0.3, axis="x") # Plot 4: Box plot comparison (Baseline) ax4 = plt.subplot(3, 3, 4) - error_data = [r['errors'] for r in all_results["Baseline"]] + error_data = [r["errors"] for r in all_results["Baseline"]] bp = ax4.boxplot(error_data, tick_labels=methods, patch_artist=True) - colors_box = ['lightblue', 'lightcyan', 'lightcoral', 'lightsalmon', 'lightgreen'] - for patch, color in zip(bp['boxes'], colors_box): + colors_box = ["lightblue", "lightcyan", "lightcoral", "lightsalmon", "lightgreen"] + for patch, color in zip(bp["boxes"], colors_box): patch.set_facecolor(color) - ax4.set_ylabel('Positioning Error (m)') - ax4.set_title('Error Distribution (Baseline)') - plt.setp(ax4.xaxis.get_majorticklabels(), rotation=45, ha='right', fontsize=8) - ax4.grid(True, alpha=0.3, axis='y') + ax4.set_ylabel("Positioning Error (m)") + ax4.set_title("Error Distribution (Baseline)") + plt.setp(ax4.xaxis.get_majorticklabels(), rotation=45, ha="right", fontsize=8) + ax4.grid(True, alpha=0.3, axis="y") # Plot 5: Robustness to noise (RMSE vs noise std) ax5 = plt.subplot(3, 3, 5) @@ -569,13 +620,15 @@ def main(): for i, method in enumerate(methods): rmses = [] for scenario_name in scenario_names: - method_result = [r for r in all_results[scenario_name] if r['method'] == method][0] - rmses.append(method_result['rmse']) - ax5.plot(noise_levels, rmses, 'o-', label=method, linewidth=2, markersize=6) - - ax5.set_xlabel('RSS Noise Std (dBm)') - ax5.set_ylabel('RMSE (m)') - ax5.set_title('Robustness to Measurement Noise') + method_result = [ + r for r in all_results[scenario_name] if r["method"] == method + ][0] + rmses.append(method_result["rmse"]) + ax5.plot(noise_levels, rmses, "o-", label=method, linewidth=2, markersize=6) + + ax5.set_xlabel("RSS Noise Std (dBm)") + ax5.set_ylabel("RMSE (m)") + ax5.set_title("Robustness to Measurement Noise") ax5.legend(fontsize=7) ax5.grid(True, alpha=0.3) @@ -589,13 +642,19 @@ def main(): # match the per-query cost panel above, so a method reads the same in both. ax6 = plt.subplot(3, 3, 6) for r, color in zip(all_results["Baseline"], colors): - ax6.scatter(r['ops_per_query'], r['rmse'], s=150, alpha=0.7, - color=color, label=r['method']) + ax6.scatter( + r["ops_per_query"], + r["rmse"], + s=150, + alpha=0.7, + color=color, + label=r["method"], + ) ax6.legend(fontsize=7) - ax6.set_xscale('log') - ax6.set_xlabel('Operations per query') - ax6.set_ylabel('RMSE (m)') - ax6.set_title('Cost vs Accuracy Trade-off') + ax6.set_xscale("log") + ax6.set_xlabel("Operations per query") + ax6.set_ylabel("RMSE (m)") + ax6.set_title("Cost vs Accuracy Trade-off") ax6.grid(True, alpha=0.3) # Plot 7: Category comparison @@ -603,74 +662,77 @@ def main(): categories = ["Deterministic", "Probabilistic", "Pattern Recognition"] cat_rmses = {} for cat in categories: - cat_methods = [r for r in all_results["Baseline"] if r['category'] == cat] - cat_rmses[cat] = [r['rmse'] for r in cat_methods] + cat_methods = [r for r in all_results["Baseline"] if r["category"] == cat] + cat_rmses[cat] = [r["rmse"] for r in cat_methods] positions = [1, 2, 3] - bp = ax7.boxplot(cat_rmses.values(), positions=positions, tick_labels=cat_rmses.keys(), - patch_artist=True) - for patch in bp['boxes']: - patch.set_facecolor('lightyellow') - ax7.set_ylabel('RMSE (m)') - ax7.set_title('Performance by Category') - plt.setp(ax7.xaxis.get_majorticklabels(), rotation=15, ha='right') - ax7.grid(True, alpha=0.3, axis='y') + bp = ax7.boxplot( + cat_rmses.values(), + positions=positions, + tick_labels=cat_rmses.keys(), + patch_artist=True, + ) + for patch in bp["boxes"]: + patch.set_facecolor("lightyellow") + ax7.set_ylabel("RMSE (m)") + ax7.set_title("Performance by Category") + plt.setp(ax7.xaxis.get_majorticklabels(), rotation=15, ha="right") + ax7.grid(True, alpha=0.3, axis="y") # Plot 8: Percentile comparison ax8 = plt.subplot(3, 3, 8) x = np.arange(len(methods)) - p50 = [r['median'] for r in all_results["Baseline"]] - p90 = [r['p90'] for r in all_results["Baseline"]] + p50 = [r["median"] for r in all_results["Baseline"]] + p90 = [r["p90"] for r in all_results["Baseline"]] width = 0.35 - ax8.bar(x - width/2, p50, width, label='Median (P50)', alpha=0.8) - ax8.bar(x + width/2, p90, width, label='P90', alpha=0.8) - ax8.set_ylabel('Error (m)') - ax8.set_title('Median vs P90 Errors') + ax8.bar(x - width / 2, p50, width, label="Median (P50)", alpha=0.8) + ax8.bar(x + width / 2, p90, width, label="P90", alpha=0.8) + ax8.set_ylabel("Error (m)") + ax8.set_title("Median vs P90 Errors") ax8.set_xticks(x) - ax8.set_xticklabels(methods, rotation=45, ha='right', fontsize=8) + ax8.set_xticklabels(methods, rotation=45, ha="right", fontsize=8) ax8.legend() - ax8.grid(True, alpha=0.3, axis='y') + ax8.grid(True, alpha=0.3, axis="y") # Plot 9: Summary radar chart - ax9 = plt.subplot(3, 3, 9, projection='polar') + ax9 = plt.subplot(3, 3, 9, projection="polar") # Normalize metrics for radar chart baseline_results = all_results["Baseline"] - metrics = ['RMSE', 'Median', 'P90'] + metrics = ["RMSE", "Median", "P90"] # Select 3 representative methods selected_methods = ["NN (Euclidean)", "MAP", "Linear Regression"] - angles = np.linspace(0, 2*np.pi, len(metrics), endpoint=False).tolist() + angles = np.linspace(0, 2 * np.pi, len(metrics), endpoint=False).tolist() angles += angles[:1] for method_name in selected_methods: - method_result = [r for r in baseline_results if r['method'] == method_name][0] + method_result = [r for r in baseline_results if r["method"] == method_name][0] values = [ - method_result['rmse'] / 10, # Normalize - method_result['median'] / 10, - method_result['p90'] / 15, + method_result["rmse"] / 10, # Normalize + method_result["median"] / 10, + method_result["p90"] / 15, ] values += values[:1] - ax9.plot(angles, values, 'o-', linewidth=2, label=method_name) + ax9.plot(angles, values, "o-", linewidth=2, label=method_name) ax9.fill(angles, values, alpha=0.15) ax9.set_xticks(angles[:-1]) ax9.set_xticklabels(metrics) ax9.set_ylim(0, 1) - ax9.set_title('Performance Profile\n(Normalized)', pad=20) - ax9.legend(loc='upper right', bbox_to_anchor=(1.3, 1.0), fontsize=8) + ax9.set_title("Performance Profile\n(Normalized)", pad=20) + ax9.legend(loc="upper right", bbox_to_anchor=(1.3, 1.0), fontsize=8) ax9.grid(True) plt.tight_layout() # Save (svg + pdf + png via the shared layer) - paths = save_figure(fig, Path(__file__).parent / "figs", - "comparison_all_methods") + paths = save_figure(fig, Path(__file__).parent / "figs", "comparison_all_methods") print(f"Saved: {paths[0]}") - print("\n" + "="*70) + print("\n" + "=" * 70) print("COMPARISON COMPLETE!") - print("="*70) + print("=" * 70) print("\nKey Insights:") print(" 1. Speed: Linear Regression >> NN > k-NN ~= MAP ~= Posterior Mean") print(" 2. Accuracy (low noise): Probabilistic ~= k-NN > NN > Linear Reg") @@ -710,4 +772,3 @@ def main(): if __name__ == "__main__": main() - diff --git a/ch5_fingerprinting/example_deterministic.py b/ch5_fingerprinting/example_deterministic.py index 0468b37..867bf5f 100644 --- a/ch5_fingerprinting/example_deterministic.py +++ b/ch5_fingerprinting/example_deterministic.py @@ -8,7 +8,7 @@ - NN positioning (Eq. 5.1): i* = argmin_i D(z, f_i) - k-NN positioning (Eq. 5.2): x̂ = Σ w_i x_i / Σ w_i -Author: Li-Ta Hsu +Author: Li-Ta Hsu Date: December 2024 """ @@ -38,14 +38,14 @@ def generate_test_queries(db, n_queries=100, floor_id=None, noise_std=0.0, seed=42): """ Generate test query fingerprints. - + Args: db: FingerprintDatabase. n_queries: Number of test queries. floor_id: Floor to generate queries on (None = random floors). noise_std: RSS measurement noise std (dBm). seed: Random seed. - + Returns: Tuple of (query_fingerprints, true_locations, floor_ids). """ @@ -67,10 +67,12 @@ def generate_test_queries(db, n_queries=100, floor_id=None, noise_std=0.0, seed= min_x, max_x = rp_locs[:, 0].min(), rp_locs[:, 0].max() min_y, max_y = rp_locs[:, 1].min(), rp_locs[:, 1].max() - true_locs = np.column_stack([ - np.random.uniform(min_x, max_x, n_queries), - np.random.uniform(min_y, max_y, n_queries), - ]) + true_locs = np.column_stack( + [ + np.random.uniform(min_x, max_x, n_queries), + np.random.uniform(min_y, max_y, n_queries), + ] + ) # Generate fingerprints by interpolating from nearby RPs query_fingerprints = [] @@ -153,14 +155,14 @@ def per_query_operations(db, floor_id=None, k=None, **_unused): def evaluate_positioning_method(method_name, method_fn, queries, true_locs, **kwargs): """ Evaluate a positioning method. - + Args: method_name: Name of method. method_fn: Positioning function. queries: Query fingerprints, shape (N, n_features). true_locs: True locations, shape (N, 2). **kwargs: Additional arguments for method_fn. - + Returns: Dictionary with errors, computation time, etc. """ @@ -212,9 +214,9 @@ def main(): formatter_class=argparse.RawDescriptionHelpFormatter, ).parse_args() - print("="*70) + print("=" * 70) print("Chapter 5: Deterministic Fingerprinting (NN and k-NN)") - print("="*70) + print("=" * 70) # Load database print("\n1. Loading fingerprint database...") @@ -222,8 +224,10 @@ def main(): db = load_fingerprint_database(db_path) print(f" Database: {db}") - print(f" Location range: x=[{db.locations[:, 0].min():.1f}, {db.locations[:, 0].max():.1f}]m, " - f"y=[{db.locations[:, 1].min():.1f}, {db.locations[:, 1].max():.1f}]m") + print( + f" Location range: x=[{db.locations[:, 0].min():.1f}, {db.locations[:, 0].max():.1f}]m, " + f"y=[{db.locations[:, 1].min():.1f}, {db.locations[:, 1].max():.1f}]m" + ) # Generate test queries print("\n2. Generating test queries...") @@ -245,48 +249,76 @@ def main(): results = [] # NN - Euclidean - results.append(evaluate_positioning_method( - "NN (Euclidean)", - nn_localize, - queries, true_locs, - db=db, metric="euclidean", floor_id=floor_id - )) + results.append( + evaluate_positioning_method( + "NN (Euclidean)", + nn_localize, + queries, + true_locs, + db=db, + metric="euclidean", + floor_id=floor_id, + ) + ) # NN - Manhattan - results.append(evaluate_positioning_method( - "NN (Manhattan)", - nn_localize, - queries, true_locs, - db=db, metric="manhattan", floor_id=floor_id - )) + results.append( + evaluate_positioning_method( + "NN (Manhattan)", + nn_localize, + queries, + true_locs, + db=db, + metric="manhattan", + floor_id=floor_id, + ) + ) # k-NN with varying k for k in [3, 5, 7]: - results.append(evaluate_positioning_method( - f"k-NN (k={k}, inv-dist)", - knn_localize, - queries, true_locs, - db=db, k=k, metric="euclidean", weighting="inverse_distance", floor_id=floor_id - )) + results.append( + evaluate_positioning_method( + f"k-NN (k={k}, inv-dist)", + knn_localize, + queries, + true_locs, + db=db, + k=k, + metric="euclidean", + weighting="inverse_distance", + floor_id=floor_id, + ) + ) # k-NN uniform weights - results.append(evaluate_positioning_method( - "k-NN (k=5, uniform)", - knn_localize, - queries, true_locs, - db=db, k=5, metric="euclidean", weighting="uniform", floor_id=floor_id - )) + results.append( + evaluate_positioning_method( + "k-NN (k=5, uniform)", + knn_localize, + queries, + true_locs, + db=db, + k=5, + metric="euclidean", + weighting="uniform", + floor_id=floor_id, + ) + ) # Print summary table - print("\n" + "="*70) + print("\n" + "=" * 70) print("RESULTS SUMMARY") - print("="*70) - print(f"{'Method':<25} {'RMSE (m)':<12} {'Median (m)':<12} {'90th % (m)':<12} {'Time (ms)':<12}") - print("-"*70) + print("=" * 70) + print( + f"{'Method':<25} {'RMSE (m)':<12} {'Median (m)':<12} {'90th % (m)':<12} {'Time (ms)':<12}" + ) + print("-" * 70) for r in results: - print(f"{r['method']:<25} {r['rmse']:<12.2f} {r['median_error']:<12.2f} " - f"{r['p90']:<12.2f} {r['mean_time_ms']:<12.3f}") + print( + f"{r['method']:<25} {r['rmse']:<12.2f} {r['median_error']:<12.2f} " + f"{r['p90']:<12.2f} {r['mean_time_ms']:<12.3f}" + ) # Visualize results print("\n4. Generating visualizations...") @@ -296,61 +328,77 @@ def main(): # Plot 1: Reference points and test queries ax1 = plt.subplot(2, 3, 1) floor_mask = db.get_floor_mask(floor_id) - ax1.scatter(db.locations[floor_mask, 0], db.locations[floor_mask, 1], - c='blue', marker='s', s=50, alpha=0.6, label='Reference Points') - ax1.scatter(true_locs[:50, 0], true_locs[:50, 1], - c='red', marker='x', s=30, alpha=0.8, label='Test Queries (sample)') - ax1.set_xlabel('X (m)') - ax1.set_ylabel('Y (m)') - ax1.set_title('Reference Points & Test Queries') + ax1.scatter( + db.locations[floor_mask, 0], + db.locations[floor_mask, 1], + c="blue", + marker="s", + s=50, + alpha=0.6, + label="Reference Points", + ) + ax1.scatter( + true_locs[:50, 0], + true_locs[:50, 1], + c="red", + marker="x", + s=30, + alpha=0.8, + label="Test Queries (sample)", + ) + ax1.set_xlabel("X (m)") + ax1.set_ylabel("Y (m)") + ax1.set_title("Reference Points & Test Queries") ax1.legend() ax1.grid(True, alpha=0.3) - ax1.axis('equal') + ax1.axis("equal") # Plot 2: Error CDF ax2 = plt.subplot(2, 3, 2) plot_error_cdf( - {r['method']: r['errors'] for r in results}, - title='Cumulative Distribution of Errors', + {r["method"]: r["errors"] for r in results}, + title="Cumulative Distribution of Errors", ax=ax2, title_fontweight="normal", ) ax2.legend(fontsize=8) - worst = max(np.max(r['errors']) for r in results) + worst = max(np.max(r["errors"]) for r in results) ax2.set_xlim(0, min(20, worst)) # Plot 3: Error histogram ax3 = plt.subplot(2, 3, 3) for i, r in enumerate(results[:3]): # Show first 3 methods - ax3.hist(r['errors'], bins=30, alpha=0.5, label=r['method']) - ax3.set_xlabel('Positioning Error (m)') - ax3.set_ylabel('Count') - ax3.set_title('Error Distribution (First 3 Methods)') + ax3.hist(r["errors"], bins=30, alpha=0.5, label=r["method"]) + ax3.set_xlabel("Positioning Error (m)") + ax3.set_ylabel("Count") + ax3.set_title("Error Distribution (First 3 Methods)") ax3.legend(fontsize=8) - ax3.grid(True, alpha=0.3, axis='y') + ax3.grid(True, alpha=0.3, axis="y") # Plot 4: Box plot comparison ax4 = plt.subplot(2, 3, 4) - error_data = [r['errors'] for r in results] - method_names = [r['method'] for r in results] + error_data = [r["errors"] for r in results] + method_names = [r["method"] for r in results] bp = ax4.boxplot(error_data, tick_labels=method_names, patch_artist=True) - for patch in bp['boxes']: - patch.set_facecolor('lightblue') - ax4.set_ylabel('Positioning Error (m)') - ax4.set_title('Error Distribution by Method') - ax4.tick_params(axis='x', rotation=45) - ax4.grid(True, alpha=0.3, axis='y') - plt.setp(ax4.xaxis.get_majorticklabels(), rotation=45, ha='right', fontsize=8) + for patch in bp["boxes"]: + patch.set_facecolor("lightblue") + ax4.set_ylabel("Positioning Error (m)") + ax4.set_title("Error Distribution by Method") + ax4.tick_params(axis="x", rotation=45) + ax4.grid(True, alpha=0.3, axis="y") + plt.setp(ax4.xaxis.get_majorticklabels(), rotation=45, ha="right", fontsize=8) # Plot 5: RMSE vs k for k-NN ax5 = plt.subplot(2, 3, 5) - knn_results = [r for r in results if 'k-NN' in r['method'] and 'inv-dist' in r['method']] - k_values = [int(r['method'].split('k=')[1].split(',')[0]) for r in knn_results] - rmse_values = [r['rmse'] for r in knn_results] - ax5.plot(k_values, rmse_values, 'o-', linewidth=2, markersize=8) - ax5.set_xlabel('k (Number of Neighbors)') - ax5.set_ylabel('RMSE (m)') - ax5.set_title('Effect of k on k-NN Performance') + knn_results = [ + r for r in results if "k-NN" in r["method"] and "inv-dist" in r["method"] + ] + k_values = [int(r["method"].split("k=")[1].split(",")[0]) for r in knn_results] + rmse_values = [r["rmse"] for r in knn_results] + ax5.plot(k_values, rmse_values, "o-", linewidth=2, markersize=8) + ax5.set_xlabel("k (Number of Neighbors)") + ax5.set_ylabel("RMSE (m)") + ax5.set_title("Effect of k on k-NN Performance") ax5.grid(True, alpha=0.3) ax5.set_xticks(k_values) @@ -364,26 +412,32 @@ def main(): # anything. Only a bigger database moves the cost. ax6 = plt.subplot(2, 3, 6) for r in results: - ax6.scatter(r['ops_per_query'], r['rmse'], s=100, alpha=0.7) - ax6.annotate(r['method'], (r['ops_per_query'], r['rmse']), - xytext=(5, 5), textcoords='offset points', fontsize=7) - ax6.set_xlabel('Operations per query') - ax6.set_ylabel('RMSE (m)') - ax6.set_title('Accuracy is Free: Cost is the Database Scan') + ax6.scatter(r["ops_per_query"], r["rmse"], s=100, alpha=0.7) + ax6.annotate( + r["method"], + (r["ops_per_query"], r["rmse"]), + xytext=(5, 5), + textcoords="offset points", + fontsize=7, + ) + ax6.set_xlabel("Operations per query") + ax6.set_ylabel("RMSE (m)") + ax6.set_title("Accuracy is Free: Cost is the Database Scan") ax6.grid(True, alpha=0.3) plt.tight_layout() # Save figure (svg + pdf + png via the shared layer) - paths = save_figure(fig, Path(__file__).parent / "figs", - "deterministic_positioning") + paths = save_figure( + fig, Path(__file__).parent / "figs", "deterministic_positioning" + ) print(f" Saved: {paths[0]}") show_figures_if_requested() - print("\n" + "="*70) + print("\n" + "=" * 70) print("Example complete!") - print("="*70) + print("=" * 70) # Findings computed from the run above, so they cannot disagree with the # table the reader just saw. The line that used to sit at the bottom of # this list -- "Manhattan distance can be faster than Euclidean in some @@ -400,15 +454,21 @@ def main(): uni = by_name.get("k-NN (k=5, uniform)") if idw and uni: better = "better" if idw["rmse"] < uni["rmse"] else "worse" - print(f" - At k=5, inverse-distance weighting is {better} than uniform: " - f"{idw['rmse']:.2f} m vs {uni['rmse']:.2f} m RMSE") + print( + f" - At k=5, inverse-distance weighting is {better} than uniform: " + f"{idw['rmse']:.2f} m vs {uni['rmse']:.2f} m RMSE" + ) ks = {r["method"]: r for r in results if "inv-dist" in r["method"]} if ks: best = min(ks.values(), key=lambda r: r["rmse"]) - spread = max(r["rmse"] for r in ks.values()) - min(r["rmse"] for r in ks.values()) - print(f" - Best k here is {best['method'].split('k=')[1][0]} at " - f"{best['rmse']:.2f} m, but only {spread:.2f} m separates k=3/5/7;") + spread = max(r["rmse"] for r in ks.values()) - min( + r["rmse"] for r in ks.values() + ) + print( + f" - Best k here is {best['method'].split('k=')[1][0]} at " + f"{best['rmse']:.2f} m, but only {spread:.2f} m separates k=3/5/7;" + ) print(" the optimal k depends on RP density and noise level") euclid, manhattan = by_name.get("NN (Euclidean)"), by_name.get("NN (Manhattan)") @@ -419,8 +479,10 @@ def main(): print(f" - Both NN metrics cost the same {e_ops:,} operations per query") else: print(f" - NN costs {e_ops:,} operations Euclidean, {m_ops:,} Manhattan") - print(f" and across every variant the count spans {max(ops) - min(ops)} " - f"operations ({100 * (max(ops) - min(ops)) / min(ops):.1f}%), all of") + print( + f" and across every variant the count spans {max(ops) - min(ops)} " + f"operations ({100 * (max(ops) - min(ops)) / min(ops):.1f}%), all of" + ) print(" it the k multiply-adds in the weighted average. The database") print(" scan is the cost: metric and weighting are free, k nearly so.") print(" - Do not read an ordering into the Time column. Timing this") @@ -436,4 +498,3 @@ def main(): if __name__ == "__main__": main() - diff --git a/ch5_fingerprinting/example_pattern_recognition.py b/ch5_fingerprinting/example_pattern_recognition.py index 76626fa..403b6f9 100644 --- a/ch5_fingerprinting/example_pattern_recognition.py +++ b/ch5_fingerprinting/example_pattern_recognition.py @@ -37,13 +37,13 @@ def split_train_test(db, test_ratio=0.3, floor_id=None, seed=42): """ Split database into train and test sets. - + Args: db: FingerprintDatabase. test_ratio: Fraction of data for testing. floor_id: Floor to use (None = all floors). seed: Random seed. - + Returns: Tuple of (train_db, test_db). """ @@ -84,12 +84,12 @@ def split_train_test(db, test_ratio=0.3, floor_id=None, seed=42): def evaluate_model(model, test_db, floor_id=None): """ Evaluate trained model on test set. - + Args: model: Trained LinearRegressionLocalizer. test_db: Test FingerprintDatabase. floor_id: Floor to evaluate on. - + Returns: Dictionary with errors and metrics. """ @@ -133,9 +133,9 @@ def main(): formatter_class=argparse.RawDescriptionHelpFormatter, ).parse_args() - print("="*70) + print("=" * 70) print("Chapter 5: Pattern Recognition (Linear Regression)") - print("="*70) + print("=" * 70) # Load database print("\n1. Loading fingerprint database...") @@ -148,8 +148,10 @@ def main(): floor_id = 0 train_db, test_db = split_train_test(db, test_ratio=0.3, floor_id=floor_id) - print(f" Floor {floor_id} - Train: {train_db.n_reference_points} RPs, " - f"Test: {test_db.n_reference_points} RPs") + print( + f" Floor {floor_id} - Train: {train_db.n_reference_points} RPs, " + f"Test: {test_db.n_reference_points} RPs" + ) # Train models with different regularization print("\n3. Training Linear Regression models...") @@ -187,23 +189,29 @@ def main(): model = models[reg_val] test_result = evaluate_model(model, test_db, floor_id=floor_id) test_results[reg_val] = test_result - print(f"\n lambda={reg_val}: Test RMSE={test_result['rmse']:.2f}m, " - f"R^2={test_result['r2']:.3f}, " - f"Time={test_result['time_per_query_ms']:.3f}ms") + print( + f"\n lambda={reg_val}: Test RMSE={test_result['rmse']:.2f}m, " + f"R^2={test_result['r2']:.3f}, " + f"Time={test_result['time_per_query_ms']:.3f}ms" + ) # Print summary - print("\n" + "="*70) + print("\n" + "=" * 70) print("RESULTS SUMMARY") - print("="*70) - print(f"{'lambda':<10} {'Train RMSE':<15} {'Test RMSE':<15} " - f"{'Test R^2':<12} {'Time (ms)':<12}") - print("-"*70) + print("=" * 70) + print( + f"{'lambda':<10} {'Train RMSE':<15} {'Test RMSE':<15} " + f"{'Test R^2':<12} {'Time (ms)':<12}" + ) + print("-" * 70) for reg_val in reg_values: tr = train_results[reg_val] te = test_results[reg_val] - print(f"{reg_val:<10.1f} {tr['rmse']:<15.2f} {te['rmse']:<15.2f} " - f"{te['r2']:<12.3f} {te['time_per_query_ms']:<12.3f}") + print( + f"{reg_val:<10.1f} {tr['rmse']:<15.2f} {te['rmse']:<15.2f} " + f"{te['r2']:<12.3f} {te['time_per_query_ms']:<12.3f}" + ) # Visualizations print("\n5. Generating visualizations...") @@ -213,13 +221,13 @@ def main(): # Plot 1: Weight matrix visualization ax1 = plt.subplot(2, 4, 1) model = models[1.0] # Use λ=1.0 model - im = ax1.imshow(model.weights, cmap='RdBu_r', aspect='auto') - ax1.set_xlabel('AP Index') - ax1.set_ylabel('Coordinate (x, y)') - ax1.set_title('Learned Weight Matrix W') + im = ax1.imshow(model.weights, cmap="RdBu_r", aspect="auto") + ax1.set_xlabel("AP Index") + ax1.set_ylabel("Coordinate (x, y)") + ax1.set_title("Learned Weight Matrix W") ax1.set_yticks([0, 1]) - ax1.set_yticklabels(['x', 'y']) - plt.colorbar(im, ax=ax1, label='Weight') + ax1.set_yticklabels(["x", "y"]) + plt.colorbar(im, ax=ax1, label="Weight") # Plot 2: Prediction vs Ground Truth ax2 = plt.subplot(2, 4, 2) @@ -228,35 +236,36 @@ def main(): test_locs = test_db.locations[mask] pred_locs = model.predict_batch(test_features) - ax2.scatter(test_locs[:, 0], pred_locs[:, 0], alpha=0.5, s=30, label='x') - ax2.scatter(test_locs[:, 1], pred_locs[:, 1], alpha=0.5, s=30, label='y') + ax2.scatter(test_locs[:, 0], pred_locs[:, 0], alpha=0.5, s=30, label="x") + ax2.scatter(test_locs[:, 1], pred_locs[:, 1], alpha=0.5, s=30, label="y") lim_min = min(test_locs.min(), pred_locs.min()) lim_max = max(test_locs.max(), pred_locs.max()) - ax2.plot([lim_min, lim_max], [lim_min, lim_max], 'k--', alpha=0.5) - ax2.set_xlabel('True (m)') - ax2.set_ylabel('Predicted (m)') - ax2.set_title('Prediction vs Ground Truth') + ax2.plot([lim_min, lim_max], [lim_min, lim_max], "k--", alpha=0.5) + ax2.set_xlabel("True (m)") + ax2.set_ylabel("Predicted (m)") + ax2.set_title("Prediction vs Ground Truth") ax2.legend() ax2.grid(True, alpha=0.3) - ax2.axis('equal') + ax2.axis("equal") # Plot 3: Spatial error distribution ax3 = plt.subplot(2, 4, 3) errors_2d = np.linalg.norm(pred_locs - test_locs, axis=1) - scatter = ax3.scatter(test_locs[:, 0], test_locs[:, 1], - c=errors_2d, s=100, cmap='YlOrRd', alpha=0.8) - ax3.set_xlabel('X (m)') - ax3.set_ylabel('Y (m)') - ax3.set_title('Spatial Error Distribution') - plt.colorbar(scatter, ax=ax3, label='Error (m)') + scatter = ax3.scatter( + test_locs[:, 0], test_locs[:, 1], c=errors_2d, s=100, cmap="YlOrRd", alpha=0.8 + ) + ax3.set_xlabel("X (m)") + ax3.set_ylabel("Y (m)") + ax3.set_title("Spatial Error Distribution") + plt.colorbar(scatter, ax=ax3, label="Error (m)") ax3.grid(True, alpha=0.3) - ax3.axis('equal') + ax3.axis("equal") # Plot 4: Error CDF for different λ ax4 = plt.subplot(2, 4, 4) plot_error_cdf( - {f'λ={reg_val}': test_results[reg_val]['errors'] for reg_val in reg_values}, - title='Error CDF for Different λ', + {f"λ={reg_val}": test_results[reg_val]["errors"] for reg_val in reg_values}, + title="Error CDF for Different λ", ax=ax4, title_fontweight="normal", ) @@ -264,67 +273,69 @@ def main(): # Plot 5: Train vs Test RMSE ax5 = plt.subplot(2, 4, 5) - train_rmse = [train_results[r]['rmse'] for r in reg_values] - test_rmse = [test_results[r]['rmse'] for r in reg_values] + train_rmse = [train_results[r]["rmse"] for r in reg_values] + test_rmse = [test_results[r]["rmse"] for r in reg_values] x = np.arange(len(reg_values)) width = 0.35 - ax5.bar(x - width/2, train_rmse, width, label='Train', alpha=0.8) - ax5.bar(x + width/2, test_rmse, width, label='Test', alpha=0.8) - ax5.set_xlabel('Regularization λ') - ax5.set_ylabel('RMSE (m)') - ax5.set_title('Train vs Test RMSE') + ax5.bar(x - width / 2, train_rmse, width, label="Train", alpha=0.8) + ax5.bar(x + width / 2, test_rmse, width, label="Test", alpha=0.8) + ax5.set_xlabel("Regularization λ") + ax5.set_ylabel("RMSE (m)") + ax5.set_title("Train vs Test RMSE") ax5.set_xticks(x) - ax5.set_xticklabels([f'{r}' for r in reg_values]) + ax5.set_xticklabels([f"{r}" for r in reg_values]) ax5.legend() - ax5.grid(True, alpha=0.3, axis='y') + ax5.grid(True, alpha=0.3, axis="y") # Plot 6: R² vs λ ax6 = plt.subplot(2, 4, 6) - test_r2 = [test_results[r]['r2'] for r in reg_values] - ax6.plot(reg_values, test_r2, 'o-', linewidth=2, markersize=8) - ax6.set_xlabel('Regularization λ') - ax6.set_ylabel('R² Score') - ax6.set_title('Test R² vs Regularization') - ax6.set_xscale('log') + test_r2 = [test_results[r]["r2"] for r in reg_values] + ax6.plot(reg_values, test_r2, "o-", linewidth=2, markersize=8) + ax6.set_xlabel("Regularization λ") + ax6.set_ylabel("R² Score") + ax6.set_title("Test R² vs Regularization") + ax6.set_xscale("log") ax6.grid(True, alpha=0.3) - ax6.axhline(y=1.0, color='k', linestyle='--', alpha=0.3, label='Perfect') - ax6.axhline(y=0.0, color='k', linestyle='--', alpha=0.3) + ax6.axhline(y=1.0, color="k", linestyle="--", alpha=0.3, label="Perfect") + ax6.axhline(y=0.0, color="k", linestyle="--", alpha=0.3) ax6.legend() # Plot 7: Overfitting analysis ax7 = plt.subplot(2, 4, 7) - train_rmse = np.array([train_results[r]['rmse'] for r in reg_values]) - test_rmse = np.array([test_results[r]['rmse'] for r in reg_values]) + train_rmse = np.array([train_results[r]["rmse"] for r in reg_values]) + test_rmse = np.array([test_results[r]["rmse"] for r in reg_values]) overfit_gap = test_rmse - train_rmse - ax7.plot(reg_values, overfit_gap, 'o-', linewidth=2, markersize=8, color='red') - ax7.set_xlabel('Regularization λ') - ax7.set_ylabel('Overfitting Gap (m)') - ax7.set_title('Test RMSE - Train RMSE') - ax7.set_xscale('log') + ax7.plot(reg_values, overfit_gap, "o-", linewidth=2, markersize=8, color="red") + ax7.set_xlabel("Regularization λ") + ax7.set_ylabel("Overfitting Gap (m)") + ax7.set_title("Test RMSE - Train RMSE") + ax7.set_xscale("log") ax7.grid(True, alpha=0.3) - ax7.axhline(y=0, color='k', linestyle='--', alpha=0.5) + ax7.axhline(y=0, color="k", linestyle="--", alpha=0.5) # Plot 8: Box plot of errors ax8 = plt.subplot(2, 4, 8) - error_data = [test_results[r]['errors'] for r in reg_values] - bp = ax8.boxplot(error_data, tick_labels=[f'λ={r}' for r in reg_values], - patch_artist=True) - for patch in bp['boxes']: - patch.set_facecolor('lightgreen') - ax8.set_ylabel('Positioning Error (m)') - ax8.set_title('Error Distribution by λ') - ax8.grid(True, alpha=0.3, axis='y') + error_data = [test_results[r]["errors"] for r in reg_values] + bp = ax8.boxplot( + error_data, tick_labels=[f"λ={r}" for r in reg_values], patch_artist=True + ) + for patch in bp["boxes"]: + patch.set_facecolor("lightgreen") + ax8.set_ylabel("Positioning Error (m)") + ax8.set_title("Error Distribution by λ") + ax8.grid(True, alpha=0.3, axis="y") plt.tight_layout() # Save (svg + pdf + png via the shared layer) - paths = save_figure(fig, Path(__file__).parent / "figs", - "pattern_recognition_positioning") + paths = save_figure( + fig, Path(__file__).parent / "figs", "pattern_recognition_positioning" + ) print(f" Saved: {paths[0]}") - print("\n" + "="*70) + print("\n" + "=" * 70) print("Example complete!") - print("="*70) + print("=" * 70) print("\nKey Findings:") print(" - Linear regression learns direct RSS->location mapping") print(" - Very fast prediction (single matrix multiplication)") @@ -342,4 +353,3 @@ def main(): if __name__ == "__main__": main() - diff --git a/ch5_fingerprinting/example_probabilistic.py b/ch5_fingerprinting/example_probabilistic.py index 6518b65..0feec44 100644 --- a/ch5_fingerprinting/example_probabilistic.py +++ b/ch5_fingerprinting/example_probabilistic.py @@ -56,10 +56,12 @@ def generate_test_queries(db, n_queries=100, floor_id=None, noise_std=0.0, seed= min_x, max_x = rp_locs[:, 0].min(), rp_locs[:, 0].max() min_y, max_y = rp_locs[:, 1].min(), rp_locs[:, 1].max() - true_locs = np.column_stack([ - np.random.uniform(min_x, max_x, n_queries), - np.random.uniform(min_y, max_y, n_queries), - ]) + true_locs = np.column_stack( + [ + np.random.uniform(min_x, max_x, n_queries), + np.random.uniform(min_y, max_y, n_queries), + ] + ) query_fingerprints = [] @@ -140,29 +142,60 @@ def visualize_posterior(model, query, true_loc, floor_id, ax, title): # Create grid for visualization # Scatter plot with posterior as color - scatter = ax.scatter(rp_locs[:, 0], rp_locs[:, 1], - c=posteriors, s=100, cmap='hot', alpha=0.8, - vmin=0, vmax=posteriors.max()) + scatter = ax.scatter( + rp_locs[:, 0], + rp_locs[:, 1], + c=posteriors, + s=100, + cmap="hot", + alpha=0.8, + vmin=0, + vmax=posteriors.max(), + ) # Mark estimates x_map = map_localize(query, model, floor_id=floor_id) x_post_mean = posterior_mean_localize(query, model, floor_id=floor_id) - ax.scatter(*x_map, marker='s', s=200, c='blue', edgecolors='white', - linewidth=2, label='MAP', zorder=10) - ax.scatter(*x_post_mean, marker='^', s=200, c='green', edgecolors='white', - linewidth=2, label='Post. Mean', zorder=10) - ax.scatter(*true_loc, marker='*', s=300, c='yellow', edgecolors='black', - linewidth=2, label='True', zorder=10) + ax.scatter( + *x_map, + marker="s", + s=200, + c="blue", + edgecolors="white", + linewidth=2, + label="MAP", + zorder=10, + ) + ax.scatter( + *x_post_mean, + marker="^", + s=200, + c="green", + edgecolors="white", + linewidth=2, + label="Post. Mean", + zorder=10, + ) + ax.scatter( + *true_loc, + marker="*", + s=300, + c="yellow", + edgecolors="black", + linewidth=2, + label="True", + zorder=10, + ) - ax.set_xlabel('X (m)') - ax.set_ylabel('Y (m)') + ax.set_xlabel("X (m)") + ax.set_ylabel("Y (m)") ax.set_title(title) ax.legend(fontsize=8) ax.grid(True, alpha=0.3) - ax.axis('equal') + ax.axis("equal") - plt.colorbar(scatter, ax=ax, label='p(x_i|z)') + plt.colorbar(scatter, ax=ax, label="p(x_i|z)") def main(): @@ -174,9 +207,9 @@ def main(): formatter_class=argparse.RawDescriptionHelpFormatter, ).parse_args() - print("="*70) + print("=" * 70) print("Chapter 5: Probabilistic Fingerprinting (Bayesian Methods)") - print("="*70) + print("=" * 70) # Load database print("\n1. Loading fingerprint database...") @@ -222,31 +255,43 @@ def main(): model = models[std_val] # MAP - results.append(evaluate_method( - f"MAP (std={std_val}dBm)", - map_localize, - queries, true_locs, - model=model, floor_id=floor_id - )) + results.append( + evaluate_method( + f"MAP (std={std_val}dBm)", + map_localize, + queries, + true_locs, + model=model, + floor_id=floor_id, + ) + ) # Posterior Mean - results.append(evaluate_method( - f"Post.Mean (std={std_val}dBm)", - posterior_mean_localize, - queries, true_locs, - model=model, floor_id=floor_id - )) + results.append( + evaluate_method( + f"Post.Mean (std={std_val}dBm)", + posterior_mean_localize, + queries, + true_locs, + model=model, + floor_id=floor_id, + ) + ) # Print summary - print("\n" + "="*70) + print("\n" + "=" * 70) print("RESULTS SUMMARY") - print("="*70) - print(f"{'Method':<28} {'RMSE (m)':<12} {'Median (m)':<12} {'P90 (m)':<12} {'Time (ms)':<12}") - print("-"*70) + print("=" * 70) + print( + f"{'Method':<28} {'RMSE (m)':<12} {'Median (m)':<12} {'P90 (m)':<12} {'Time (ms)':<12}" + ) + print("-" * 70) for r in results: - print(f"{r['method']:<28} {r['rmse']:<12.2f} {r['median']:<12.2f} " - f"{r['p90']:<12.2f} {r['mean_time_ms']:<12.3f}") + print( + f"{r['method']:<28} {r['rmse']:<12.2f} {r['median']:<12.2f} " + f"{r['p90']:<12.2f} {r['mean_time_ms']:<12.3f}" + ) # Visualizations print("\n5. Generating visualizations...") @@ -256,14 +301,20 @@ def main(): # Plot 1-3: Posterior probability maps for different std for idx, std_val in enumerate(std_values): ax = plt.subplot(3, 3, idx + 1) - visualize_posterior(models[std_val], queries[0], true_locs[0], - floor_id, ax, f'Posterior Map (std={std_val}dBm)') + visualize_posterior( + models[std_val], + queries[0], + true_locs[0], + floor_id, + ax, + f"Posterior Map (std={std_val}dBm)", + ) # Plot 4: Error CDF comparison ax4 = plt.subplot(3, 3, 4) plot_error_cdf( - {r['method']: r['errors'] for r in results}, - title='Cumulative Distribution of Errors', + {r["method"]: r["errors"] for r in results}, + title="Cumulative Distribution of Errors", ax=ax4, title_fontweight="normal", ) @@ -272,47 +323,70 @@ def main(): # Plot 5: Box plot comparison ax5 = plt.subplot(3, 3, 5) - error_data = [r['errors'] for r in results] - method_names = [r['method'].replace(' (std=', '\n(').replace('dBm)', ')') for r in results] + error_data = [r["errors"] for r in results] + method_names = [ + r["method"].replace(" (std=", "\n(").replace("dBm)", ")") for r in results + ] bp = ax5.boxplot(error_data, tick_labels=method_names, patch_artist=True) - for patch in bp['boxes']: - patch.set_facecolor('lightcoral') - ax5.set_ylabel('Positioning Error (m)') - ax5.set_title('Error Distribution by Method') - plt.setp(ax5.xaxis.get_majorticklabels(), rotation=45, ha='right', fontsize=7) - ax5.grid(True, alpha=0.3, axis='y') + for patch in bp["boxes"]: + patch.set_facecolor("lightcoral") + ax5.set_ylabel("Positioning Error (m)") + ax5.set_title("Error Distribution by Method") + plt.setp(ax5.xaxis.get_majorticklabels(), rotation=45, ha="right", fontsize=7) + ax5.grid(True, alpha=0.3, axis="y") # Plot 6: RMSE vs std ax6 = plt.subplot(3, 3, 6) - map_results = [r for r in results if 'MAP' in r['method']] - pm_results = [r for r in results if 'Post.Mean' in r['method']] - - ax6.plot(std_values, [r['rmse'] for r in map_results], 'o-', - linewidth=2, markersize=8, label='MAP') - ax6.plot(std_values, [r['rmse'] for r in pm_results], 's-', - linewidth=2, markersize=8, label='Posterior Mean') - ax6.set_xlabel('Model Std (dBm)') - ax6.set_ylabel('RMSE (m)') - ax6.set_title('Effect of Model Uncertainty (std)') + map_results = [r for r in results if "MAP" in r["method"]] + pm_results = [r for r in results if "Post.Mean" in r["method"]] + + ax6.plot( + std_values, + [r["rmse"] for r in map_results], + "o-", + linewidth=2, + markersize=8, + label="MAP", + ) + ax6.plot( + std_values, + [r["rmse"] for r in pm_results], + "s-", + linewidth=2, + markersize=8, + label="Posterior Mean", + ) + ax6.set_xlabel("Model Std (dBm)") + ax6.set_ylabel("RMSE (m)") + ax6.set_title("Effect of Model Uncertainty (std)") ax6.legend() ax6.grid(True, alpha=0.3) # Plot 7: MAP vs Posterior Mean scatter ax7 = plt.subplot(3, 3, 7) - map_rmse = [r['rmse'] for r in map_results] - pm_rmse = [r['rmse'] for r in pm_results] + map_rmse = [r["rmse"] for r in map_results] + pm_rmse = [r["rmse"] for r in pm_results] ax7.scatter(map_rmse, pm_rmse, s=150, alpha=0.7) for i, std in enumerate(std_values): - ax7.annotate(f'std={std}', (map_rmse[i], pm_rmse[i]), - xytext=(5, 5), textcoords='offset points') - ax7.plot([min(map_rmse), max(map_rmse)], [min(map_rmse), max(map_rmse)], - 'k--', alpha=0.5, label='x=y') - ax7.set_xlabel('MAP RMSE (m)') - ax7.set_ylabel('Posterior Mean RMSE (m)') - ax7.set_title('MAP vs Posterior Mean Accuracy') + ax7.annotate( + f"std={std}", + (map_rmse[i], pm_rmse[i]), + xytext=(5, 5), + textcoords="offset points", + ) + ax7.plot( + [min(map_rmse), max(map_rmse)], + [min(map_rmse), max(map_rmse)], + "k--", + alpha=0.5, + label="x=y", + ) + ax7.set_xlabel("MAP RMSE (m)") + ax7.set_ylabel("Posterior Mean RMSE (m)") + ax7.set_title("MAP vs Posterior Mean Accuracy") ax7.legend() ax7.grid(True, alpha=0.3) - ax7.axis('equal') + ax7.axis("equal") # Plot 8: Per-query cost, counted rather than timed # @@ -336,16 +410,27 @@ def main(): # MAP then takes an argmax over the RPs (Eq. 5.4); the posterior mean # exponentiates and forms a weighted sum over them instead (Eq. 5.5). estimator_ops = [n_rps, n_rps + n_rps * n_dims] - labels = ['MAP', 'Posterior Mean'] - - ax8.bar(labels, [likelihood_ops] * 2, label='Gaussian log-densities', - alpha=0.85, color='steelblue') - ax8.bar(labels, estimator_ops, bottom=[likelihood_ops] * 2, - label='Estimator step', alpha=0.85, color='indianred') - ax8.set_ylabel('Operations per query') - ax8.set_title('Per-Query Cost (independent of std)') + labels = ["MAP", "Posterior Mean"] + + ax8.bar( + labels, + [likelihood_ops] * 2, + label="Gaussian log-densities", + alpha=0.85, + color="steelblue", + ) + ax8.bar( + labels, + estimator_ops, + bottom=[likelihood_ops] * 2, + label="Estimator step", + alpha=0.85, + color="indianred", + ) + ax8.set_ylabel("Operations per query") + ax8.set_title("Per-Query Cost (independent of std)") ax8.legend(fontsize=7) - ax8.grid(True, alpha=0.3, axis='y') + ax8.grid(True, alpha=0.3, axis="y") # Plot 9: Example posterior distribution ax9 = plt.subplot(3, 3, 9) @@ -358,21 +443,22 @@ def main(): # Sort and plot top 20 RPs sorted_idx = np.argsort(posteriors)[::-1][:20] ax9.bar(range(len(sorted_idx)), posteriors[sorted_idx]) - ax9.set_xlabel('RP Index (sorted by posterior)') - ax9.set_ylabel('Posterior Probability') - ax9.set_title('Posterior Distribution (Top 20 RPs)') - ax9.grid(True, alpha=0.3, axis='y') + ax9.set_xlabel("RP Index (sorted by posterior)") + ax9.set_ylabel("Posterior Probability") + ax9.set_title("Posterior Distribution (Top 20 RPs)") + ax9.grid(True, alpha=0.3, axis="y") plt.tight_layout() # Save (svg + pdf + png via the shared layer) - paths = save_figure(fig, Path(__file__).parent / "figs", - "probabilistic_positioning") + paths = save_figure( + fig, Path(__file__).parent / "figs", "probabilistic_positioning" + ) print(f" Saved: {paths[0]}") - print("\n" + "="*70) + print("\n" + "=" * 70) print("Example complete!") - print("="*70) + print("=" * 70) print("\nKey Findings:") print(" - MAP provides discrete estimates (selects one RP)") print(" - Posterior Mean provides smooth estimates (weighted average)") @@ -389,4 +475,3 @@ def main(): if __name__ == "__main__": main() - diff --git a/ch5_fingerprinting/example_walk_posterior.py b/ch5_fingerprinting/example_walk_posterior.py index 42e4dea..e9db776 100644 --- a/ch5_fingerprinting/example_walk_posterior.py +++ b/ch5_fingerprinting/example_walk_posterior.py @@ -84,10 +84,7 @@ def _build_l_walk(locations): Returns: List of indices into ``locations`` tracing the walk. """ - key = { - (round(float(x)), round(float(y))): i - for i, (x, y) in enumerate(locations) - } + key = {(round(float(x)), round(float(y))): i for i, (x, y) in enumerate(locations)} x_max = int(round(locations[:, 0].max())) y_max = int(round(locations[:, 1].max())) step = int(round(np.min(np.diff(np.unique(locations[:, 0]))))) @@ -164,40 +161,84 @@ def _draw_frame(axes, walk, index): # near 0.9, the rest near 1e-4 -- so the gamma-compressed norm is what # lifts the runner-up cells enough to see where the mass sits. scatter = axes[0].scatter( - locations[:, 0], locations[:, 1], c=posterior, s=160, - cmap=POSTERIOR_CMAP, norm=POSTERIOR_NORM, marker="s", + locations[:, 0], + locations[:, 1], + c=posterior, + s=160, + cmap=POSTERIOR_CMAP, + norm=POSTERIOR_NORM, + marker="s", + ) + axes[0].plot( + walk["true_xy"][: index + 1, 0], + walk["true_xy"][: index + 1, 1], + "-", + color="white", + linewidth=1.2, + alpha=0.6, + ) + axes[0].scatter( + *truth, + marker="*", + s=280, + c="red", + edgecolors="white", + linewidths=1.5, + zorder=6, + label="true position", ) - axes[0].plot(walk["true_xy"][: index + 1, 0], - walk["true_xy"][: index + 1, 1], - "-", color="white", linewidth=1.2, alpha=0.6) - axes[0].scatter(*truth, marker="*", s=280, c="red", - edgecolors="white", linewidths=1.5, zorder=6, - label="true position") # Hollow ring, so the bright peak cell it sits on stays visible -- a solid # marker would hide the very cell it reports and make the runner-up look # like the answer. - axes[0].scatter(*estimate, marker="o", s=260, facecolors="none", - edgecolors="white", linewidths=2.5, zorder=6, - label="MAP estimate (peak cell)") + axes[0].scatter( + *estimate, + marker="o", + s=260, + facecolors="none", + edgecolors="white", + linewidths=2.5, + zorder=6, + label="MAP estimate (peak cell)", + ) if error > ALIASING_THRESHOLD: - axes[0].plot([truth[0], estimate[0]], [truth[1], estimate[1]], - "--", color="red", linewidth=1.8, zorder=5) + axes[0].plot( + [truth[0], estimate[0]], + [truth[1], estimate[1]], + "--", + color="red", + linewidth=1.8, + zorder=5, + ) axes[0].set_xlabel("x [m]") axes[0].set_ylabel("y [m]") axes[0].set_aspect("equal") axes[0].legend(fontsize=8, loc="upper left", framealpha=0.9) - verdict = "ALIASED: MAP jumped to a radio-similar spot" if ( - error > ALIASING_THRESHOLD) else "MAP on target" + verdict = ( + "ALIASED: MAP jumped to a radio-similar spot" + if (error > ALIASING_THRESHOLD) + else "MAP on target" + ) axes[0].set_title( f"step {index + 1}/{len(walk['errors'])} - {verdict}", fontsize=10 ) # --- error trace, with the aliasing threshold marked steps = np.arange(1, index + 2) - axes[1].plot(steps, walk["errors"][: index + 1], "-o", color="tab:blue", - markersize=4, linewidth=1.5) - axes[1].axhline(ALIASING_THRESHOLD, color="red", linestyle="--", - linewidth=1.3, label=f"aliasing (> {ALIASING_THRESHOLD:.0f} m)") + axes[1].plot( + steps, + walk["errors"][: index + 1], + "-o", + color="tab:blue", + markersize=4, + linewidth=1.5, + ) + axes[1].axhline( + ALIASING_THRESHOLD, + color="red", + linestyle="--", + linewidth=1.3, + label=f"aliasing (> {ALIASING_THRESHOLD:.0f} m)", + ) axes[1].set_xlim(0.5, len(walk["errors"]) + 0.5) axes[1].set_ylim(-1, max(walk["errors"].max() * 1.1, 12)) axes[1].grid(alpha=0.3) @@ -216,8 +257,9 @@ def _draw_frame(axes, walk, index): def _add_posterior_colorbar(fig, axes): """Attach the single shared p(x|z) colorbar to the heat-map axes.""" mappable = plt.cm.ScalarMappable(norm=POSTERIOR_NORM, cmap=POSTERIOR_CMAP) - fig.colorbar(mappable, ax=axes[0], fraction=0.046, pad=0.04, - label="p(x | z), Eq. (5.3)") + fig.colorbar( + mappable, ax=axes[0], fraction=0.046, pad=0.04, label="p(x | z), Eq. (5.3)" + ) def animate_walk(walk): @@ -250,9 +292,7 @@ def update(frame: int): def plot_walk_summary(walk) -> plt.Figure: """Static counterpart: an aliased step beside the error trace.""" aliased = np.where(walk["errors"] > ALIASING_THRESHOLD)[0] - highlight = int(aliased[0]) if len(aliased) else int( - np.argmax(walk["errors"]) - ) + highlight = int(aliased[0]) if len(aliased) else int(np.argmax(walk["errors"])) fig, axes = plt.subplots(1, 2, figsize=(13, 5.2)) _draw_frame(axes, walk, highlight) @@ -261,13 +301,18 @@ def plot_walk_summary(walk) -> plt.Figure: # Redraw the error panel over the whole walk, not just up to the step. steps = np.arange(1, len(walk["errors"]) + 1) axes[1].clear() - axes[1].plot(steps, walk["errors"], "-o", color="tab:blue", markersize=4, - linewidth=1.5) - axes[1].axhline(ALIASING_THRESHOLD, color="red", linestyle="--", - linewidth=1.3, label=f"aliasing (> {ALIASING_THRESHOLD:.0f} m)") + axes[1].plot( + steps, walk["errors"], "-o", color="tab:blue", markersize=4, linewidth=1.5 + ) + axes[1].axhline( + ALIASING_THRESHOLD, + color="red", + linestyle="--", + linewidth=1.3, + label=f"aliasing (> {ALIASING_THRESHOLD:.0f} m)", + ) for step in aliased: - axes[1].scatter(step + 1, walk["errors"][step], s=90, c="red", - zorder=5) + axes[1].scatter(step + 1, walk["errors"][step], s=90, c="red", zorder=5) axes[1].set_xlim(0.5, len(walk["errors"]) + 0.5) axes[1].set_ylim(-1, max(walk["errors"].max() * 1.1, 12)) axes[1].grid(alpha=0.3) @@ -295,14 +340,21 @@ def main() -> None: parser = argparse.ArgumentParser( description="Fingerprint posterior along a walk (Chapter 5)" ) - parser.add_argument("--data", default=DEFAULT_DATA, - help="Fingerprint database directory") - parser.add_argument("--out-dir", default=str(FIGS_DIR), - help="Output directory for figures") - parser.add_argument("--noise", type=float, default=NOISE_STD, - help="Measurement noise std in dB") - parser.add_argument("--animate", action="store_true", default=False, - help="Also render the walking-posterior GIF (slower)") + parser.add_argument( + "--data", default=DEFAULT_DATA, help="Fingerprint database directory" + ) + parser.add_argument( + "--out-dir", default=str(FIGS_DIR), help="Output directory for figures" + ) + parser.add_argument( + "--noise", type=float, default=NOISE_STD, help="Measurement noise std in dB" + ) + parser.add_argument( + "--animate", + action="store_true", + default=False, + help="Also render the walking-posterior GIF (slower)", + ) args = parser.parse_args() print("=" * 70) @@ -313,25 +365,32 @@ def main() -> None: errors = walk["errors"] aliased = int(np.sum(errors > ALIASING_THRESHOLD)) - print(f" L-walk of {len(errors)} reference points, " - f"noise {args.noise:.0f} dB") - print(f" posterior peaks on a median of " - f"{int(np.median(walk['hot_counts']))} cell(s) -- it stays sharp") - print(f" MAP error: median {np.median(errors):.1f} m " - f"mean {errors.mean():.1f} m max {errors.max():.1f} m") - print(f" aliasing jumps (> {ALIASING_THRESHOLD:.0f} m): " - f"{aliased} of {len(errors)} steps") + print(f" L-walk of {len(errors)} reference points, " f"noise {args.noise:.0f} dB") + print( + f" posterior peaks on a median of " + f"{int(np.median(walk['hot_counts']))} cell(s) -- it stays sharp" + ) + print( + f" MAP error: median {np.median(errors):.1f} m " + f"mean {errors.mean():.1f} m max {errors.max():.1f} m" + ) + print( + f" aliasing jumps (> {ALIASING_THRESHOLD:.0f} m): " + f"{aliased} of {len(errors)} steps" + ) print(" -> the median says 'perfect', the mean says otherwise\n") - paths = save_figure(plot_walk_summary(walk), args.out_dir, - "ch5_walk_posterior") - print(f" saved ch5_walk_posterior: " - f"{', '.join(p.suffix.lstrip('.') for p in paths)}") + paths = save_figure(plot_walk_summary(walk), args.out_dir, "ch5_walk_posterior") + print( + f" saved ch5_walk_posterior: " + f"{', '.join(p.suffix.lstrip('.') for p in paths)}" + ) if args.animate: fig, update, n_frames = animate_walk(walk) - path = save_animation(fig, update, n_frames, args.out_dir, - "ch5_walk_posterior", fps=3) + path = save_animation( + fig, update, n_frames, args.out_dir, "ch5_walk_posterior", fps=3 + ) plt.close(fig) size_mb = path.stat().st_size / (1024 * 1024) print(f" saved {path.name}: {n_frames} frames, {size_mb:.2f} MB") diff --git a/ch6_dead_reckoning/__init__.py b/ch6_dead_reckoning/__init__.py index 9433b20..6f09b2c 100644 --- a/ch6_dead_reckoning/__init__.py +++ b/ch6_dead_reckoning/__init__.py @@ -27,5 +27,3 @@ __version__ = "1.0.0" __all__ = [] - - diff --git a/ch6_dead_reckoning/example_allan_variance.py b/ch6_dead_reckoning/example_allan_variance.py index 482b4ec..11d1386 100644 --- a/ch6_dead_reckoning/example_allan_variance.py +++ b/ch6_dead_reckoning/example_allan_variance.py @@ -59,11 +59,11 @@ #: and its absence is why three unit errors survived here: nothing ever put the #: answer next to the question. IMU_SPECS = { - 'consumer': { - 'gyro_arw': np.deg2rad(0.5), # deg/sqrt(hr) → rad/sqrt(s) - 'gyro_bias_instability': np.deg2rad(10.0) / 3600.0, # deg/hr → rad/s - 'gyro_rrw': np.deg2rad(0.01), # deg/s/sqrt(hr) - 'accel_vrw': 0.01, # m/s/sqrt(s) + "consumer": { + "gyro_arw": np.deg2rad(0.5), # deg/sqrt(hr) → rad/sqrt(s) + "gyro_bias_instability": np.deg2rad(10.0) / 3600.0, # deg/hr → rad/s + "gyro_rrw": np.deg2rad(0.01), # deg/s/sqrt(hr) + "accel_vrw": 0.01, # m/s/sqrt(s) # The "/ 3600.0" that used to be here was spurious -- a bias # instability in m/s^2 is already a rate, so there is nothing to # convert from per-hour. It made the consumer accelerometer @@ -71,14 +71,14 @@ # and small enough that the Allan curve had no flat region at all: the # estimator returned the white-noise floor, 5.4e-4, and the example # printed that as a bias instability. The tactical spec had it right. - 'accel_bias_instability': 0.0001, # m/s² + "accel_bias_instability": 0.0001, # m/s² }, - 'tactical': { - 'gyro_arw': np.deg2rad(0.05), - 'gyro_bias_instability': np.deg2rad(1.0) / 3600.0, # deg/hr → rad/s - 'gyro_rrw': np.deg2rad(0.001), - 'accel_vrw': 0.001, - 'accel_bias_instability': 0.00001, # m/s² + "tactical": { + "gyro_arw": np.deg2rad(0.05), + "gyro_bias_instability": np.deg2rad(1.0) / 3600.0, # deg/hr → rad/s + "gyro_rrw": np.deg2rad(0.001), + "accel_vrw": 0.001, + "accel_bias_instability": 0.00001, # m/s² }, } @@ -99,23 +99,26 @@ def injected_si(grade): Dict with the same keys as the spec, in rad/sqrt(s), rad/s, rad/s^(3/2), m/s^(3/2) and m/s^2. """ - spec = IMU_SPECS.get(grade, IMU_SPECS['consumer']) + spec = IMU_SPECS.get(grade, IMU_SPECS["consumer"]) return { - 'gyro_arw': spec['gyro_arw'] / np.sqrt(3600), - 'gyro_bias_instability': spec['gyro_bias_instability'], - 'gyro_rrw': spec['gyro_rrw'] / np.sqrt(3600), - 'accel_vrw': spec['accel_vrw'], - 'accel_bias_instability': spec['accel_bias_instability'], + "gyro_arw": spec["gyro_arw"] / np.sqrt(3600), + "gyro_bias_instability": spec["gyro_bias_instability"], + "gyro_rrw": spec["gyro_rrw"] / np.sqrt(3600), + "accel_vrw": spec["accel_vrw"], + "accel_bias_instability": spec["accel_bias_instability"], } def generate_imu_stationary_data( - duration=3600.0, fs=100.0, imu_grade='consumer', return_components=False, + duration=3600.0, + fs=100.0, + imu_grade="consumer", + return_components=False, seed=DEFAULT_SEED, ): """ Generate synthetic stationary IMU data with realistic noise. - + Args: duration: Duration [s] (recommend 1-24 hours). fs: Sampling frequency [Hz]. @@ -144,8 +147,8 @@ def generate_imu_stationary_data( # right. One definition, consumed everywhere. spec = injected_si(imu_grade) - gyro_noise_density = spec['gyro_arw'] # rad/sqrt(s) - accel_noise_density = spec['accel_vrw'] # m/s/sqrt(s) + gyro_noise_density = spec["gyro_arw"] # rad/sqrt(s) + accel_noise_density = spec["accel_vrw"] # m/s/sqrt(s) # Create RNG for reproducibility rng = np.random.default_rng(seed) @@ -173,7 +176,7 @@ def generate_imu_stationary_data( # Scale to match target BI (using Allan deviation convention) bi_noise = scale_to_bias_instability( pink_unit=pink_unit, - target_bi_rad_s=spec['gyro_bias_instability'], + target_bi_rad_s=spec["gyro_bias_instability"], allan_sigma_func=allan_variance, tau_grid_s=tau_grid, fs=fs, @@ -182,7 +185,7 @@ def generate_imu_stationary_data( # 3) Rate Random Walk (diffusion of bias, slope +1/2) # Single random walk term (NOT double cumsum) - rrw_coeff = spec['gyro_rrw'] # rad/s/sqrt(s), already converted + rrw_coeff = spec["gyro_rrw"] # rad/s/sqrt(s), already converted rrw_bias = np.cumsum(rng.standard_normal(N)) * rrw_coeff * np.sqrt(dt) # Combine all three components @@ -190,9 +193,9 @@ def generate_imu_stationary_data( # Store components for first axis (debug mode) if axis == 0 and return_components: - gyro_components['arw'] = arw_noise - gyro_components['bi'] = bi_noise - gyro_components['rrw'] = rrw_bias + gyro_components["arw"] = arw_noise + gyro_components["bi"] = bi_noise + gyro_components["rrw"] = rrw_bias # === ACCEL: VRW + BI === @@ -203,7 +206,7 @@ def generate_imu_stationary_data( pink_unit_accel = pink_noise_1f_fft(N, fs, rng=rng) accel_bi_noise = scale_to_bias_instability( pink_unit=pink_unit_accel, - target_bi_rad_s=spec['accel_bias_instability'], + target_bi_rad_s=spec["accel_bias_instability"], allan_sigma_func=allan_variance, tau_grid_s=tau_grid, fs=fs, @@ -215,8 +218,8 @@ def generate_imu_stationary_data( # Store components for first axis (debug mode) if axis == 0 and return_components: - accel_components['vrw'] = vrw_noise - accel_components['bi'] = accel_bi_noise + accel_components["vrw"] = vrw_noise + accel_components["bi"] = accel_bi_noise if return_components: return t, gyro_data, accel_data, gyro_components, accel_components @@ -224,12 +227,10 @@ def generate_imu_stationary_data( return t, gyro_data, accel_data -def plot_allan_deviation_components( - fs, components, sensor_type, grade, figs_dir -): +def plot_allan_deviation_components(fs, components, sensor_type, grade, figs_dir): """ Plot Allan deviation for individual noise components (debug mode). - + This helps verify that each component produces the expected slope: - ARW (white noise): slope -1/2 - BI (pink noise): slope ~0 (flat region) @@ -237,14 +238,14 @@ def plot_allan_deviation_components( """ fig, ax = plt.subplots(figsize=(14, 9)) - colors = {'arw': 'blue', 'bi': 'green', 'rrw': 'red', 'vrw': 'blue'} + colors = {"arw": "blue", "bi": "green", "rrw": "red", "vrw": "blue"} labels = { - 'arw': 'ARW (Angle Random Walk)', - 'bi': 'BI (Bias Instability)', - 'rrw': 'RRW (Rate Random Walk)', - 'vrw': 'VRW (Velocity Random Walk)', + "arw": "ARW (Angle Random Walk)", + "bi": "BI (Bias Instability)", + "rrw": "RRW (Rate Random Walk)", + "vrw": "VRW (Velocity Random Walk)", } - expected_slopes = {'arw': -0.5, 'bi': 0.0, 'rrw': 0.5, 'vrw': -0.5} + expected_slopes = {"arw": -0.5, "bi": 0.0, "rrw": 0.5, "vrw": -0.5} tau_grid = np.logspace(0, 3, 50) # 1s to 1000s @@ -253,11 +254,9 @@ def plot_allan_deviation_components( taus, sigma = allan_variance(component_data, fs, tau_grid) # Plot - color = colors.get(key, 'black') + color = colors.get(key, "black") label = labels.get(key, key.upper()) - ax.loglog( - taus, sigma, '-', color=color, linewidth=2, label=label, alpha=0.8 - ) + ax.loglog(taus, sigma, "-", color=color, linewidth=2, label=label, alpha=0.8) # Add expected slope indicator slope = expected_slopes.get(key, 0.0) @@ -268,35 +267,33 @@ def plot_allan_deviation_components( tau_ref = np.array([tau_mid / 3, tau_mid * 3]) sigma_ref = sigma_mid * (tau_ref / tau_mid) ** slope - ax.loglog(tau_ref, sigma_ref, '--', color=color, alpha=0.4, linewidth=1) + ax.loglog(tau_ref, sigma_ref, "--", color=color, alpha=0.4, linewidth=1) # Add slope annotation - slope_text = f'slope = {slope:+.1f}' + slope_text = f"slope = {slope:+.1f}" ax.text( tau_mid * 1.5, sigma_mid * 1.2, slope_text, fontsize=9, color=color, - style='italic', + style="italic", ) - ax.set_xlabel('Averaging Time τ [s]', fontsize=13, fontweight='bold') - ax.set_ylabel( - 'Allan Deviation [rad/s] or [m/s²]', fontsize=13, fontweight='bold' - ) + ax.set_xlabel("Averaging Time τ [s]", fontsize=13, fontweight="bold") + ax.set_ylabel("Allan Deviation [rad/s] or [m/s²]", fontsize=13, fontweight="bold") ax.set_title( - f'Allan Variance Component Analysis: {grade.capitalize()} {sensor_type}\n' - 'Debug Mode: Individual Noise Components', + f"Allan Variance Component Analysis: {grade.capitalize()} {sensor_type}\n" + "Debug Mode: Individual Noise Components", fontsize=14, - fontweight='bold', + fontweight="bold", ) - ax.legend(fontsize=11, loc='best', framealpha=0.9) - ax.grid(True, which='both', alpha=0.3, linestyle=':') + ax.legend(fontsize=11, loc="best", framealpha=0.9) + ax.grid(True, which="both", alpha=0.3, linestyle=":") ax.set_xlim([taus[0], taus[-1]]) plt.tight_layout() - filename = f'allan_{sensor_type.lower()}_{grade}_debug_components' + filename = f"allan_{sensor_type.lower()}_{grade}_debug_components" paths = save_figure(fig, figs_dir, filename) print(f" [DEBUG] Saved: {paths[0]}") @@ -309,7 +306,7 @@ def plot_allan_deviation(taus, adev, noise_params, sensor_type, grade, figs_dir) fig, ax = plt.subplots(figsize=(12, 8)) # Plot Allan deviation - ax.loglog(taus, adev, 'b-', linewidth=2, label=f'{sensor_type} Allan Deviation') + ax.loglog(taus, adev, "b-", linewidth=2, label=f"{sensor_type} Allan Deviation") # This function is called for both sensors, and used to label both with the # gyroscope's units: the accelerometer figure reported its bias instability @@ -317,37 +314,47 @@ def plot_allan_deviation(taus, adev, noise_params, sensor_type, grade, figs_dir) # no white-noise marker on the accelerometer at all, because it looked only # for 'angle_random_walk' -- so the one parameter an hour of data recovers # correctly was the one the figure omitted. - is_accel = sensor_type.lower().startswith('accel') + is_accel = sensor_type.lower().startswith("accel") if is_accel: - white_key, white_name = 'velocity_random_walk', 'VRW' - fmt_white = lambda v: f'{v:.5f} m/s/√s' - fmt_bi = lambda v: f'{v:.2e} m/s²' - fmt_rrw = lambda v: f'{v:.2e} m/s²/√s' + white_key, white_name = "velocity_random_walk", "VRW" + fmt_white = lambda v: f"{v:.5f} m/s/√s" + fmt_bi = lambda v: f"{v:.2e} m/s²" + fmt_rrw = lambda v: f"{v:.2e} m/s²/√s" else: - white_key, white_name = 'angle_random_walk', 'ARW' + white_key, white_name = "angle_random_walk", "ARW" # rad/sqrt(s) -> deg/sqrt(hr) is rad2deg then x60, because # sqrt(3600) = 60. The x60 was missing here as well as in the console # output, so the legend and the printed table were wrong in the same # way -- and agreeing with each other is what made it look right. - fmt_white = lambda v: f'{np.rad2deg(v)*60:.3f} °/√hr' - fmt_bi = lambda v: f'{np.rad2deg(v)*3600:.2f} °/hr' - fmt_rrw = lambda v: f'{np.rad2deg(v)*60:.4f} °/s/√hr' + fmt_white = lambda v: f"{np.rad2deg(v)*60:.3f} °/√hr" + fmt_bi = lambda v: f"{np.rad2deg(v)*3600:.2f} °/hr" + fmt_rrw = lambda v: f"{np.rad2deg(v)*60:.4f} °/s/√hr" # Mark identified parameters if white_key in noise_params: # White noise: read at tau=1s on the -1/2 slope. white_value = noise_params[white_key] - ax.loglog(1.0, white_value, 'ro', markersize=10, - label=f'{white_name} = {fmt_white(white_value)}') + ax.loglog( + 1.0, + white_value, + "ro", + markersize=10, + label=f"{white_name} = {fmt_white(white_value)}", + ) # Draw reference line tau_ref = np.array([0.1, 10]) - arw_line = white_value * (tau_ref / 1.0)**(-0.5) - ax.loglog(tau_ref, arw_line, 'r--', alpha=0.5, linewidth=1) - ax.text(0.15, white_value*1.5, f'Slope = -1/2\n({white_name})', - fontsize=9, color='red') + arw_line = white_value * (tau_ref / 1.0) ** (-0.5) + ax.loglog(tau_ref, arw_line, "r--", alpha=0.5, linewidth=1) + ax.text( + 0.15, + white_value * 1.5, + f"Slope = -1/2\n({white_name})", + fontsize=9, + color="red", + ) - if 'bias_instability' in noise_params: + if "bias_instability" in noise_params: # Bias instability is read off the minimum of the curve, so put the # marker there. It used to be drawn at (100 s, B): the tau came from # `noise_params.get('bi_tau', 100.0)` and characterize_imu_noise @@ -356,9 +363,14 @@ def plot_allan_deviation(taus, adev, noise_params, sensor_type, grade, figs_dir) # therefore sat off the line in both axes -- at a tau that was not the # minimum and a height the curve never reaches. bi_index = int(np.argmin(adev)) - bi_value = noise_params['bias_instability'] - ax.loglog(taus[bi_index], adev[bi_index], 'gs', markersize=10, - label=f'BI = {fmt_bi(bi_value)}') + bi_value = noise_params["bias_instability"] + ax.loglog( + taus[bi_index], + adev[bi_index], + "gs", + markersize=10, + label=f"BI = {fmt_bi(bi_value)}", + ) # The minimum can fall on the last tau -- with few clusters left the # tail is noisy and often dips, and on a curve that never flattens it # is simply the last point -- so label up and to the left when it does, @@ -372,41 +384,48 @@ def plot_allan_deviation(taus, adev, noise_params, sensor_type, grade, figs_dir) tail = taus >= taus[-1] / 10.0 tail_slope = np.polyfit(np.log10(taus[tail]), np.log10(adev[tail]), 1)[0] if abs(tail_slope) < 0.2: - bi_text = 'Slope = 0\n(Bias Instability)' + bi_text = "Slope = 0\n(Bias Instability)" else: - bi_text = f'curve minimum\n(slope {tail_slope:+.2f}, no plateau)' + bi_text = f"curve minimum\n(slope {tail_slope:+.2f}, no plateau)" ax.annotate( bi_text, xy=(taus[bi_index], adev[bi_index]), xytext=(-10, 18) if near_right_edge else (8, 0), - textcoords='offset points', - ha='right' if near_right_edge else 'left', - va='center', fontsize=9, color='green', + textcoords="offset points", + ha="right" if near_right_edge else "left", + va="center", + fontsize=9, + color="green", ) - if 'rate_random_walk' in noise_params: + if "rate_random_walk" in noise_params: # RRW: slope +1/2 at long tau. The Allan deviation of a rate random # walk is sigma(tau) = K*sqrt(tau/3) -- the 3 comes from the Allan # variance of a Wiener process, and is not a unit conversion. Dividing # by 3600 instead put the marker 35x below the curve it annotates. - rrw_value = noise_params['rate_random_walk'] + rrw_value = noise_params["rate_random_walk"] rrw_tau = taus[-10] if len(taus) > 10 else taus[-1] rrw_adev = rrw_value * np.sqrt(rrw_tau / 3.0) - ax.loglog(rrw_tau, rrw_adev, 'md', markersize=10, - label=f'RRW = {fmt_rrw(rrw_value)}') + ax.loglog( + rrw_tau, rrw_adev, "md", markersize=10, label=f"RRW = {fmt_rrw(rrw_value)}" + ) - ax.set_xlabel('Averaging Time τ [s]', fontsize=12) + ax.set_xlabel("Averaging Time τ [s]", fontsize=12) # One sensor per figure, so name its unit rather than offering both. - unit = '[m/s²]' if sensor_type.lower().startswith('accel') else '[rad/s]' - ax.set_ylabel(f'Allan Deviation {unit}', fontsize=12) - ax.set_title(f'Allan Variance: {grade.capitalize()} {sensor_type}', fontsize=14, fontweight='bold') - ax.legend(fontsize=10, loc='best') - ax.grid(True, which='both', alpha=0.3) + unit = "[m/s²]" if sensor_type.lower().startswith("accel") else "[rad/s]" + ax.set_ylabel(f"Allan Deviation {unit}", fontsize=12) + ax.set_title( + f"Allan Variance: {grade.capitalize()} {sensor_type}", + fontsize=14, + fontweight="bold", + ) + ax.legend(fontsize=10, loc="best") + ax.grid(True, which="both", alpha=0.3) ax.set_xlim([taus[0], taus[-1]]) plt.tight_layout() - filename = f'allan_{sensor_type.lower()}_{grade}' + filename = f"allan_{sensor_type.lower()}_{grade}" paths = save_figure(fig, figs_dir, filename) print(f" [OK] Saved: {paths[0]}") @@ -431,9 +450,9 @@ def main(): # Check for debug mode debug_mode = args.debug - print("\n" + "="*70) + print("\n" + "=" * 70) print("Chapter 6: Allan Variance for IMU Noise Characterization") - print("="*70) + print("=" * 70) print("\nDemonstrates IMU noise identification using Allan variance.") print("Key equations: 6.56-6.58 (Allan variance and deviation)") if debug_mode: @@ -443,7 +462,7 @@ def main(): # Configuration duration = 3600.0 # 1 hour (recommend 1-24 hours for real data) fs = 100.0 # Hz - grade = 'consumer' + grade = "consumer" print("Configuration:") print(f" Duration: {duration/3600:.1f} hours") @@ -479,31 +498,33 @@ def main(): print(f" Time: {time.time()-start:.2f} s") # Create output directory - figs_dir = Path(__file__).parent / 'figs' + figs_dir = Path(__file__).parent / "figs" figs_dir.mkdir(exist_ok=True) # Plot gyro print("\nGenerating plots...") - plot_allan_deviation(taus, adev, noise_char['gyro'], 'Gyroscope', grade, figs_dir) + plot_allan_deviation(taus, adev, noise_char["gyro"], "Gyroscope", grade, figs_dir) # Plot accel taus_a, adev_a = allan_variance(accel_data[:, 0], fs, taus=None) - plot_allan_deviation(taus_a, adev_a, noise_char['accel'], 'Accelerometer', grade, figs_dir) + plot_allan_deviation( + taus_a, adev_a, noise_char["accel"], "Accelerometer", grade, figs_dir + ) # Debug mode: plot individual components if debug_mode: print("\n[DEBUG MODE] Plotting individual noise components...") plot_allan_deviation_components( - fs, gyro_components, 'Gyroscope', grade, figs_dir + fs, gyro_components, "Gyroscope", grade, figs_dir ) plot_allan_deviation_components( - fs, accel_components, 'Accelerometer', grade, figs_dir + fs, accel_components, "Accelerometer", grade, figs_dir ) # Print results - print("\n" + "="*70) + print("\n" + "=" * 70) print("RESULTS - IMU Noise Characterization") - print("="*70) + print("=" * 70) # characterize_imu_noise returns SI: ARW in rad/sqrt(s), RRW in # rad/s^(3/2). Both convert to a "per sqrt(hour)" unit with rad2deg then # x60, because sqrt(3600) = 60 -- the factor its own docstring uses. @@ -513,34 +534,46 @@ def main(): # better than navigation grade. RRW was multiplied by 3600 instead of 60, # sixty times too large in the other direction. print(f"\nGyroscope ({grade}):") - print(f" Angle Random Walk (ARW): {np.rad2deg(noise_char['gyro']['angle_random_walk'])*60:.4f} deg/sqrt(hr)") - print(f" Bias Instability (BI): {np.rad2deg(noise_char['gyro']['bias_instability'])*3600:.2f} deg/hr") - print(f" Rate Random Walk (RRW): {np.rad2deg(noise_char['gyro']['rate_random_walk'])*60:.5f} deg/s/sqrt(hr)") + print( + f" Angle Random Walk (ARW): {np.rad2deg(noise_char['gyro']['angle_random_walk'])*60:.4f} deg/sqrt(hr)" + ) + print( + f" Bias Instability (BI): {np.rad2deg(noise_char['gyro']['bias_instability'])*3600:.2f} deg/hr" + ) + print( + f" Rate Random Walk (RRW): {np.rad2deg(noise_char['gyro']['rate_random_walk'])*60:.5f} deg/s/sqrt(hr)" + ) print(f"\nAccelerometer ({grade}):") - print(f" Velocity Random Walk (VRW): {noise_char['accel']['velocity_random_walk']:.5f} m/s/sqrt(s)") - print(f" Bias Instability: {noise_char['accel']['bias_instability']:.6f} m/s^2") + print( + f" Velocity Random Walk (VRW): {noise_char['accel']['velocity_random_walk']:.5f} m/s/sqrt(s)" + ) + print( + f" Bias Instability: {noise_char['accel']['bias_instability']:.6f} m/s^2" + ) # The check this example never made. It runs on synthetic data, so the # right answer is known exactly -- and printing the recovered value next to # it is the only thing that would have caught the three unit errors above, # each of which looked entirely plausible on its own. injected = injected_si(grade) - g, a = noise_char['gyro'], noise_char['accel'] - print("\n" + "-"*70) + g, a = noise_char["gyro"], noise_char["accel"] + print("\n" + "-" * 70) print("Recovered vs injected (this is synthetic data: the answer is known)") - print("-"*70) + print("-" * 70) print(f" {'quantity':<28} {'injected':>11} {'recovered':>11} {'ratio':>7}") for label, key_in, value_out in ( - ("gyro ARW [rad/sqrt(s)]", 'gyro_arw', g['angle_random_walk']), - ("gyro BI [rad/s]", 'gyro_bias_instability', g['bias_instability']), - ("gyro RRW [rad/s^1.5]", 'gyro_rrw', g['rate_random_walk']), - ("accel VRW [m/s^1.5]", 'accel_vrw', a['velocity_random_walk']), - ("accel BI [m/s^2]", 'accel_bias_instability', a['bias_instability']), + ("gyro ARW [rad/sqrt(s)]", "gyro_arw", g["angle_random_walk"]), + ("gyro BI [rad/s]", "gyro_bias_instability", g["bias_instability"]), + ("gyro RRW [rad/s^1.5]", "gyro_rrw", g["rate_random_walk"]), + ("accel VRW [m/s^1.5]", "accel_vrw", a["velocity_random_walk"]), + ("accel BI [m/s^2]", "accel_bias_instability", a["bias_instability"]), ): value_in = injected[key_in] - print(f" {label:<28} {value_in:11.3e} {value_out:11.3e} " - f"{value_out / value_in:6.1f}x") + print( + f" {label:<28} {value_in:11.3e} {value_out:11.3e} " + f"{value_out / value_in:6.1f}x" + ) # Two of those ratios are not estimator error -- they are parameters this # record cannot identify, and saying which is more useful than the numbers. @@ -550,18 +583,18 @@ def main(): # evidence in the output means the reader does not have to catch a warning. print() for sensor, key, wanted, what in ( - ('gyro', 'gyro', +0.5, 'rate random walk'), - ('accel', 'accel', 0.0, 'bias instability'), + ("gyro", "gyro", +0.5, "rate random walk"), + ("accel", "accel", 0.0, "bias instability"), ): - taus_s = np.asarray(noise_char[key]['taus']) - adev_s = np.asarray(noise_char[key]['adev']) + taus_s = np.asarray(noise_char[key]["taus"]) + adev_s = np.asarray(noise_char[key]["adev"]) long_tau = taus_s >= taus_s[-1] / 10.0 - slope = np.polyfit( - np.log10(taus_s[long_tau]), np.log10(adev_s[long_tau]), 1 - )[0] + slope = np.polyfit(np.log10(taus_s[long_tau]), np.log10(adev_s[long_tau]), 1)[0] verdict = "as expected" if abs(slope - wanted) < 0.2 else "NOT REACHED" - print(f" {sensor:<5} long-tau slope {slope:+.2f} " - f"(needs {wanted:+.2f} for {what}) -- {verdict}") + print( + f" {sensor:<5} long-tau slope {slope:+.2f} " + f"(needs {wanted:+.2f} for {what}) -- {verdict}" + ) print() print(" The gyro curve is still on its bias-instability shoulder and the") @@ -574,9 +607,9 @@ def main(): print(" real lesson of the 1-24 hour guidance above, and it is why ARW and") print(" VRW -- both read at short tau -- come back within 10%.") - print("\n" + "-"*70) + print("\n" + "-" * 70) print("Reference IMU Grades:") - print("-"*70) + print("-" * 70) print(" Grade | ARW [deg/sqrt(hr)] | BI [deg/hr] | Cost") print(" -----------|--------------------|--------------|--------") print(" Consumer | 0.1 - 1.0 | 10 - 100 | $1-10") @@ -590,7 +623,7 @@ def main(): print(" BI (pink): ~0 slope (flat region at mid tau)") print(" RRW (random walk): +1/2 slope (long tau)") print() - print("="*70) + print("=" * 70) print("KEY INSIGHT: Allan variance reveals ALL noise sources!") print(" - Slope -1/2: Angle/Velocity Random Walk") print(" - Slope 0: Bias Instability (minimum)") @@ -600,11 +633,10 @@ def main(): print("\nTo run without debug mode: python example_allan_variance.py") else: print("\nTo see component breakdown: python example_allan_variance.py --debug") - print("="*70) + print("=" * 70) print() show_figures_if_requested() if __name__ == "__main__": main() - diff --git a/ch6_dead_reckoning/example_comparison.py b/ch6_dead_reckoning/example_comparison.py index f8086f3..9bef446 100644 --- a/ch6_dead_reckoning/example_comparison.py +++ b/ch6_dead_reckoning/example_comparison.py @@ -76,9 +76,7 @@ ) -def _speed_envelope( - tau: np.ndarray, duration: float, ramp: float -) -> np.ndarray: +def _speed_envelope(tau: np.ndarray, duration: float, ramp: float) -> np.ndarray: """Raised-cosine trapezoid rising 0->1, holding, then falling 1->0. A walker does not reach 1.2 m/s in one 10 ms sample. Stepping the speed @@ -103,9 +101,7 @@ def _speed_envelope( rising = tau < ramp env[rising] = 0.5 * (1.0 - np.cos(np.pi * tau[rising] / ramp)) falling = tau > duration - ramp - env[falling] = 0.5 * ( - 1.0 - np.cos(np.pi * (duration - tau[falling]) / ramp) - ) + env[falling] = 0.5 * (1.0 - np.cos(np.pi * (duration - tau[falling]) / ramp)) return np.clip(env, 0.0, 1.0) @@ -250,9 +246,7 @@ def generate_mixed_trajectory( end = min(n_samples, k + walk_samples) if end > k: tau = np.arange(end - k) * dt - speed[k:end] = v_walk * _speed_envelope( - tau, walk_samples * dt, ramp_time - ) + speed[k:end] = v_walk * _speed_envelope(tau, walk_samples * dt, ramp_time) heading_true[k:end] = current_heading k = end if k >= n_samples: @@ -263,9 +257,7 @@ def generate_mixed_trajectory( end = min(n_samples, k + stop_samples) heading_true[k:end] = current_heading if seg + 1 < len(segment_headings): - delta_psi = _wrap_to_pi( - segment_headings[seg + 1] - segment_headings[seg] - ) + delta_psi = _wrap_to_pi(segment_headings[seg + 1] - segment_headings[seg]) turn_start = k + int(round((stop_duration - turn_time) / 2.0 / dt)) turn_end = min(n_samples, turn_start + int(round(turn_time / dt))) if turn_end > turn_start: @@ -293,11 +285,7 @@ def generate_mixed_trajectory( # Vertical gait. The envelope is the normalised speed, so the bob fades in # and out with the walk instead of switching on at a stance boundary. bob_vel_amplitude = bob_accel_mps2 / (2.0 * np.pi * step_freq) - vel_up = ( - (speed / v_walk) - * bob_vel_amplitude - * np.cos(2.0 * np.pi * step_freq * t) - ) + vel_up = (speed / v_walk) * bob_vel_amplitude * np.cos(2.0 * np.pi * step_freq * t) vel_true = np.column_stack( [speed * np.cos(heading_true), speed * np.sin(heading_true), vel_up] @@ -342,9 +330,7 @@ def generate_mixed_trajectory( np.zeros(n_samples), ] ) - vel_wheel_body = np.column_stack( - [speed, np.zeros(n_samples), np.zeros(n_samples)] - ) + vel_wheel_body = np.column_stack([speed, np.zeros(n_samples), np.zeros(n_samples)]) vel_wheel_body += omega_cross_lever wheel_speed_true = vel_wheel_body @ C_SPEED_TO_BODY @@ -471,9 +457,7 @@ def run_imu_only( pos[0] = p for k in range(1, n_samples): - q, v, p = strapdown_update( - q, v, p, gyro[k - 1], accel[k - 1], dt, frame=frame - ) + q, v, p = strapdown_update(q, v, p, gyro[k - 1], accel[k - 1], dt, frame=frame) pos[k] = p return pos @@ -522,9 +506,7 @@ def run_imu_zupt( sigma_g = imu_params.gyro_arw_rad_sqrt_s * np.sqrt(1 / dt) for k in range(1, n_samples): - q, v, p = strapdown_update( - q, v, p, gyro[k - 1], accel[k - 1], dt, frame=frame - ) + q, v, p = strapdown_update(q, v, p, gyro[k - 1], accel[k - 1], dt, frame=frame) # Windowed ZUPT detection (OFFLINE/POST-PROCESSING) # Uses centered window (includes future samples) - appropriate for batch @@ -536,9 +518,7 @@ def run_imu_zupt( gyro_window = gyro[window_start:window_end] if len(accel_window) >= window_size // 2: - if detect_zupt_windowed( - accel_window, gyro_window, sigma_a, sigma_g, gamma - ): + if detect_zupt_windowed(accel_window, gyro_window, sigma_a, sigma_g, gamma): v = np.zeros(3) zupt_detected[k] = True @@ -573,17 +553,36 @@ def run_wheel_odom( for k in range(1, n_samples): p = wheel_odom_update( - p, q, wheel[k - 1], gyro[k - 1], lever_arm, dt, + p, + q, + wheel[k - 1], + gyro[k - 1], + lever_arm, + dt, C_S_A=C_SPEED_TO_BODY, ) # Update quaternion from gyro q_new = q.copy() - dq = 0.5 * dt * np.array([ - -q[1]*gyro[k-1,0] - q[2]*gyro[k-1,1] - q[3]*gyro[k-1,2], - q[0]*gyro[k-1,0] + q[2]*gyro[k-1,2] - q[3]*gyro[k-1,1], - q[0]*gyro[k-1,1] - q[1]*gyro[k-1,2] + q[3]*gyro[k-1,0], - q[0]*gyro[k-1,2] + q[1]*gyro[k-1,1] - q[2]*gyro[k-1,0] - ]) + dq = ( + 0.5 + * dt + * np.array( + [ + -q[1] * gyro[k - 1, 0] + - q[2] * gyro[k - 1, 1] + - q[3] * gyro[k - 1, 2], + q[0] * gyro[k - 1, 0] + + q[2] * gyro[k - 1, 2] + - q[3] * gyro[k - 1, 1], + q[0] * gyro[k - 1, 1] + - q[1] * gyro[k - 1, 2] + + q[3] * gyro[k - 1, 0], + q[0] * gyro[k - 1, 2] + + q[1] * gyro[k - 1, 1] + - q[2] * gyro[k - 1, 0], + ] + ) + ) q = q_new + dq q = q / np.linalg.norm(q) pos[k] = p @@ -697,11 +696,11 @@ def plot_comparison( fig1 = plot_trajectory_2d( pos_true[:, :2], {name: pos[:, :2] for name, pos in results.items()}, - title='IMU alone drifts to 54 m RMSE; ZUPT cuts it to 8.8, odometry and PDR to under a metre', - axis_labels=('East [m]', 'North [m]'), + title="IMU alone drifts to 54 m RMSE; ZUPT cuts it to 8.8, odometry and PDR to under a metre", + axis_labels=("East [m]", "North [m]"), zoom_to_truth=True, ) - paths = save_figure(fig1, figs_dir, 'comparison_trajectories') + paths = save_figure(fig1, figs_dir, "comparison_trajectories") print(f" [OK] Saved: {paths[0]}") # Figure 2: error magnitude over time. Log scale because unaided IMU drift @@ -710,37 +709,33 @@ def plot_comparison( fig2 = plot_error_magnitude_time( errors, t=t, - title='Chapter 6 Comparison: Position Error vs Time', + title="Chapter 6 Comparison: Position Error vs Time", log_scale=True, ) - paths = save_figure(fig2, figs_dir, 'comparison_error_time') + paths = save_figure(fig2, figs_dir, "comparison_error_time") print(f" [OK] Saved: {paths[0]}") # Figure 3: error CDF. - fig3 = plot_error_cdf( - errors, title='Chapter 6 Comparison: Error CDF' - ) - paths = save_figure(fig3, figs_dir, 'comparison_error_cdf') + fig3 = plot_error_cdf(errors, title="Chapter 6 Comparison: Error CDF") + paths = save_figure(fig3, figs_dir, "comparison_error_cdf") print(f" [OK] Saved: {paths[0]}") - plt.close('all') + plt.close("all") # Compute metrics metrics = {} for name, pos in results.items(): error = np.linalg.norm(errors[name], axis=1) metrics[name] = { - 'rmse': float(np.sqrt(np.mean(error**2))), - 'final': float(error[-1]), - 'median': float(np.median(error)), - 'p90': float(np.percentile(error, 90)), + "rmse": float(np.sqrt(np.mean(error**2))), + "final": float(error[-1]), + "median": float(np.median(error)), + "p90": float(np.percentile(error, 90)), # Horizontal path length. A method that has silently stopped # tracking still scores well on error alone, because this ground # truth returns to its own start point -- so report the distance # actually traced next to it. - 'path': float( - np.sum(np.linalg.norm(np.diff(pos[:, :2], axis=0), axis=1)) - ), + "path": float(np.sum(np.linalg.norm(np.diff(pos[:, :2], axis=0), axis=1))), } return metrics @@ -755,9 +750,9 @@ def main() -> None: formatter_class=argparse.RawDescriptionHelpFormatter, ).parse_args() - print("\n" + "="*75) + print("\n" + "=" * 75) print("Chapter 6: COMPREHENSIVE COMPARISON of Dead Reckoning Methods") - print("="*75) + print("=" * 75) print("\nCompares all major DR approaches on a common trajectory.") print("Demonstrates trade-offs and the critical need for drift correction.\n") @@ -779,9 +774,17 @@ def main() -> None: print() print("Generating trajectory with correct IMU forward model...") - (t, pos_true, vel_true, accel_body, gyro_body, heading_true, mag_body, - stance, wheel_true) = generate_mixed_trajectory( - duration, dt, frame, lever_arm_a=LEVER_ARM_A) + ( + t, + pos_true, + vel_true, + accel_body, + gyro_body, + heading_true, + mag_body, + stance, + wheel_true, + ) = generate_mixed_trajectory(duration, dt, frame, lever_arm_a=LEVER_ARM_A) total_dist = np.sum(np.linalg.norm(np.diff(pos_true[:, :2], axis=0), axis=1)) print(f" Total distance: {total_dist:.1f} m (horizontal)") @@ -789,8 +792,8 @@ def main() -> None: print("\nAdding sensor noise...") accel_meas, gyro_meas, mag_meas, wheel_meas = add_sensor_noise( - accel_body, gyro_body, mag_body, wheel_true, dt, imu_params, - seed=DEFAULT_SEED) + accel_body, gyro_body, mag_body, wheel_true, dt, imu_params, seed=DEFAULT_SEED + ) initial = NavStateQPVP(q=np.array([1, 0, 0, 0]), v=vel_true[0], p=pos_true[0]) @@ -800,77 +803,99 @@ def main() -> None: print(" 1. IMU only (pure strapdown)...") start = time.time() - methods['IMU Only'] = run_imu_only(t, accel_meas, gyro_meas, initial, frame) + methods["IMU Only"] = run_imu_only(t, accel_meas, gyro_meas, initial, frame) print(f" Time: {time.time()-start:.3f} s") print(" 2. IMU + ZUPT (windowed, Eq. 6.44)...") start = time.time() - methods['IMU + ZUPT'], zupt_detected = run_imu_zupt( - t, accel_meas, gyro_meas, initial, frame, imu_params) + methods["IMU + ZUPT"], zupt_detected = run_imu_zupt( + t, accel_meas, gyro_meas, initial, frame, imu_params + ) print(f" Time: {time.time()-start:.3f} s") - print(f" ZUPT fired on {100 * zupt_detected.mean():.1f}% of samples " - f"({100 * stance.mean():.1f}% truly stationary)") + print( + f" ZUPT fired on {100 * zupt_detected.mean():.1f}% of samples " + f"({100 * stance.mean():.1f}% truly stationary)" + ) print(" 3. Wheel Odometry...") start = time.time() - methods['Wheel Odom'] = run_wheel_odom( - t, wheel_meas, gyro_meas, initial, LEVER_ARM_A) + methods["Wheel Odom"] = run_wheel_odom( + t, wheel_meas, gyro_meas, initial, LEVER_ARM_A + ) print(f" Time: {time.time()-start:.3f} s") print(" 4. PDR (step-and-heading)...") start = time.time() - methods['PDR (Mag)'], step_count = run_pdr(t, accel_meas, mag_meas, height) + methods["PDR (Mag)"], step_count = run_pdr(t, accel_meas, mag_meas, height) print(f" Time: {time.time()-start:.3f} s") print(f" Steps detected: {step_count}") - figs_dir = Path(__file__).parent / 'figs' + figs_dir = Path(__file__).parent / "figs" figs_dir.mkdir(exist_ok=True) print("\nGenerating comparison plots...") metrics = plot_comparison(t, pos_true, methods, figs_dir) # Print comparison table - print("\n" + "="*75) + print("\n" + "=" * 75) print("RESULTS - Performance Comparison (horizontal error)") - print("="*75) - print(f"\n{'Method':<20} {'RMSE [m]':>10} {'Final [m]':>10} {'Median [m]':>10} " - f"{'90% [m]':>10} {'Path [m]':>10}") - print("-"*75) - print(f"{'(ground truth)':<20} {'-':>10} {'-':>10} {'-':>10} {'-':>10} " - f"{total_dist:>10.1f}") - - for name in ['IMU Only', 'IMU + ZUPT', 'Wheel Odom', 'PDR (Mag)']: + print("=" * 75) + print( + f"\n{'Method':<20} {'RMSE [m]':>10} {'Final [m]':>10} {'Median [m]':>10} " + f"{'90% [m]':>10} {'Path [m]':>10}" + ) + print("-" * 75) + print( + f"{'(ground truth)':<20} {'-':>10} {'-':>10} {'-':>10} {'-':>10} " + f"{total_dist:>10.1f}" + ) + + for name in ["IMU Only", "IMU + ZUPT", "Wheel Odom", "PDR (Mag)"]: m = metrics[name] - print(f"{name:<20} {m['rmse']:>10.2f} {m['final']:>10.2f} " - f"{m['median']:>10.2f} {m['p90']:>10.2f} {m['path']:>10.1f}") + print( + f"{name:<20} {m['rmse']:>10.2f} {m['final']:>10.2f} " + f"{m['median']:>10.2f} {m['p90']:>10.2f} {m['path']:>10.1f}" + ) print(f"\nFigures saved to: {resolve_figs_dir(figs_dir)}/") print() - print("="*75) + print("=" * 75) print("KEY INSIGHTS:") zupt_reduction = 100 * ( - 1 - metrics['IMU + ZUPT']['rmse'] / metrics['IMU Only']['rmse'] + 1 - metrics["IMU + ZUPT"]["rmse"] / metrics["IMU Only"]["rmse"] + ) + pdr_overrun = 100 * (metrics["PDR (Mag)"]["path"] / total_dist - 1) + print( + f" 1. IMU-only: UNBOUNDED. {metrics['IMU Only']['final']:.0f} m off " + f"after {duration:.0f} s, tracing {metrics['IMU Only']['path']:.0f} m " + f"for a {total_dist:.0f} m walk." + ) + print(" Unusable without corrections.") + print( + f" 2. IMU+ZUPT: {zupt_reduction:.0f}% RMSE reduction " + f"({metrics['IMU Only']['rmse']:.0f} m -> " + f"{metrics['IMU + ZUPT']['rmse']:.1f} m), detector active on " + f"{100 * zupt_detected.mean():.0f}% of samples." + ) + print( + " Velocity is reset while standing but attitude is never " + "corrected, so error still grows -- far more slowly." + ) + print( + f" 3. Wheel Odom: BOUNDED. Error follows distance, not time: RMSE " + f"{metrics['Wheel Odom']['rmse']:.2f} m over {total_dist:.0f} m, set " + f"by the 2% encoder scale error." + ) + print( + " 'Final' is near zero only because the loop closes on its own " + "start point; read 'Path' instead." + ) + print( + f" 4. PDR: BOUNDED, heading-limited. {step_count} detected steps " + f"cover {metrics['PDR (Mag)']['path']:.1f} m against " + f"{total_dist:.1f} m ({pdr_overrun:+.1f}%), RMSE " + f"{metrics['PDR (Mag)']['rmse']:.2f} m." ) - pdr_overrun = 100 * (metrics['PDR (Mag)']['path'] / total_dist - 1) - print(f" 1. IMU-only: UNBOUNDED. {metrics['IMU Only']['final']:.0f} m off " - f"after {duration:.0f} s, tracing {metrics['IMU Only']['path']:.0f} m " - f"for a {total_dist:.0f} m walk.") - print( " Unusable without corrections.") - print(f" 2. IMU+ZUPT: {zupt_reduction:.0f}% RMSE reduction " - f"({metrics['IMU Only']['rmse']:.0f} m -> " - f"{metrics['IMU + ZUPT']['rmse']:.1f} m), detector active on " - f"{100 * zupt_detected.mean():.0f}% of samples.") - print( " Velocity is reset while standing but attitude is never " - "corrected, so error still grows -- far more slowly.") - print(f" 3. Wheel Odom: BOUNDED. Error follows distance, not time: RMSE " - f"{metrics['Wheel Odom']['rmse']:.2f} m over {total_dist:.0f} m, set " - f"by the 2% encoder scale error.") - print( " 'Final' is near zero only because the loop closes on its own " - "start point; read 'Path' instead.") - print(f" 4. PDR: BOUNDED, heading-limited. {step_count} detected steps " - f"cover {metrics['PDR (Mag)']['path']:.1f} m against " - f"{total_dist:.1f} m ({pdr_overrun:+.1f}%), RMSE " - f"{metrics['PDR (Mag)']['rmse']:.2f} m.") print() print(" The 'Path' column is the check that makes the rest meaningful: a") print(" method frozen at the origin scores well on error alone, because this") @@ -881,7 +906,7 @@ def main() -> None: print(" - Use wheel encoders for vehicles") print(" - Use magnetometer for heading reference") print(" - Best: Multi-sensor fusion (Chapter 8)") - print("="*75) + print("=" * 75) print() show_figures_if_requested() diff --git a/ch6_dead_reckoning/example_environment.py b/ch6_dead_reckoning/example_environment.py index a9495fa..3b88a19 100644 --- a/ch6_dead_reckoning/example_environment.py +++ b/ch6_dead_reckoning/example_environment.py @@ -45,6 +45,7 @@ # committed figures can be regenerated exactly. DEFAULT_SEED = 42 + def generate_building_walk(duration=180.0, dt=0.1, rng=None): """ Generate multi-floor building walk with floor changes. @@ -76,14 +77,14 @@ def generate_building_walk(duration=180.0, dt=0.1, rng=None): # Walk pattern: ground floor → 2nd floor → 3rd floor → ground floor_schedule = [ - (0, 60, 0), # Ground floor for 60s - (60, 70, 1), # Climb to floor 1 + (0, 60, 0), # Ground floor for 60s + (60, 70, 1), # Climb to floor 1 (70, 110, 1), # Floor 1 for 40s - (110, 120, 2), # Climb to floor 2 - (120, 150, 2), # Floor 2 for 30s - (150, 160, 1), # Descend to floor 1 - (160, 170, 0), # Descend to ground - (170, 180, 0), # Ground floor + (110, 120, 2), # Climb to floor 2 + (120, 150, 2), # Floor 2 for 30s + (150, 160, 1), # Descend to floor 1 + (160, 170, 0), # Descend to ground + (170, 180, 0), # Ground floor ] # Heading: continuously rotating while walking @@ -104,13 +105,13 @@ def generate_building_walk(duration=180.0, dt=0.1, rng=None): # Position (x,y random walk, z from floor) if k > 0: - pos_true[k, 0] = pos_true[k-1, 0] + 0.1*rng.standard_normal() - pos_true[k, 1] = pos_true[k-1, 1] + 0.1*rng.standard_normal() + pos_true[k, 0] = pos_true[k - 1, 0] + 0.1 * rng.standard_normal() + pos_true[k, 1] = pos_true[k - 1, 1] + 0.1 * rng.standard_normal() pos_true[k, 2] = current_floor * floor_height # Attitude (device orientation changes) - att_true[k, 0] = 0.1 * np.sin(2*np.pi*t[k]/10) # Roll oscillation - att_true[k, 1] = 0.05 * np.sin(2*np.pi*t[k]/15) # Pitch oscillation + att_true[k, 0] = 0.1 * np.sin(2 * np.pi * t[k] / 10) # Roll oscillation + att_true[k, 1] = 0.05 * np.sin(2 * np.pi * t[k] / 15) # Pitch oscillation att_true[k, 2] = heading_rate * t[k] # Continuous rotation # Magnetometer, in the convention core.sensors.mag_heading inverts: @@ -141,7 +142,7 @@ def generate_building_walk(duration=180.0, dt=0.1, rng=None): g = 9.81 M = 0.029 # Molar mass of air [kg/mol] R_gas = 8.314 # Gas constant [J/(mol·K)] - pressure_true[k] = p0 * (1 - L*h/T)**(g*M/(R_gas*L)) + pressure_true[k] = p0 * (1 - L * h / T) ** (g * M / (R_gas * L)) return t, pos_true, att_true, mag_true, pressure_true, floor_true @@ -187,7 +188,7 @@ def add_env_sensor_noise(mag_true, pressure_true, t, dt, rng=None): pressure_noise = rng.standard_normal(N) * 10.0 # Pa (typical noise) # Slow weather drift (pressure changes over time) - weather_drift = 50.0 * np.sin(2*np.pi*t/(duration)) # ±50 Pa drift + weather_drift = 50.0 * np.sin(2 * np.pi * t / (duration)) # ±50 Pa drift pressure_meas = pressure_true + pressure_noise + weather_drift @@ -212,8 +213,17 @@ def run_baro_altitude(pressure_meas, p0=101325.0, T=288.15): return alt_est -def plot_results(t, att_true, mag_meas, heading_est, pressure_meas, alt_est, - floor_true, floor_detected, figs_dir): +def plot_results( + t, + att_true, + mag_meas, + heading_est, + pressure_meas, + alt_est, + floor_true, + floor_detected, + figs_dir, +): """Generate plots.""" heading_true = att_true[:, 2] # yaw @@ -227,58 +237,102 @@ def plot_results(t, att_true, mag_meas, heading_est, pressure_meas, alt_est, # panel invites impossible -- they never overlap even when they agree. # The error below still uses the unwrapped truth via wrap_angle_diff. heading_true_wrapped = np.arctan2(np.sin(heading_true), np.cos(heading_true)) - ax1.plot(t, np.rad2deg(heading_true_wrapped), 'k-', linewidth=2, - label='True Heading (wrapped)') - ax1.plot(t, np.rad2deg(heading_est), 'b-', linewidth=2, alpha=0.7, label='Mag Heading') - ax1.set_ylabel('Heading [deg]', fontsize=12) - ax1.set_title('Magnetometer Example: Heading with Disturbances', fontsize=14, fontweight='bold') + ax1.plot( + t, + np.rad2deg(heading_true_wrapped), + "k-", + linewidth=2, + label="True Heading (wrapped)", + ) + ax1.plot( + t, np.rad2deg(heading_est), "b-", linewidth=2, alpha=0.7, label="Mag Heading" + ) + ax1.set_ylabel("Heading [deg]", fontsize=12) + ax1.set_title( + "Magnetometer Example: Heading with Disturbances", + fontsize=14, + fontweight="bold", + ) ax1.legend(fontsize=11) ax1.grid(True, alpha=0.3) # Compute heading error with proper angle wrapping - heading_error_rad = np.array([wrap_angle_diff(heading_est[i], heading_true[i]) - for i in range(len(heading_est))]) + heading_error_rad = np.array( + [ + wrap_angle_diff(heading_est[i], heading_true[i]) + for i in range(len(heading_est)) + ] + ) heading_error = np.abs(np.rad2deg(heading_error_rad)) # Absolute error in degrees # Note: By using wrap_angle_diff, heading_error is guaranteed to be <= 180° - ax2.plot(t, heading_error, 'r-', linewidth=2) - ax2.axhline(10, color='orange', linestyle='--', label='10° threshold') - ax2.fill_between(t, 0, 100, where=(t>=30) & (t<50), alpha=0.2, color='red', label='Disturbance zone') - ax2.fill_between(t, 0, 100, where=(t>=100) & (t<120), alpha=0.2, color='red') - ax2.set_xlabel('Time [s]', fontsize=12) - ax2.set_ylabel('Heading Error [deg]', fontsize=12) + ax2.plot(t, heading_error, "r-", linewidth=2) + ax2.axhline(10, color="orange", linestyle="--", label="10° threshold") + ax2.fill_between( + t, + 0, + 100, + where=(t >= 30) & (t < 50), + alpha=0.2, + color="red", + label="Disturbance zone", + ) + ax2.fill_between(t, 0, 100, where=(t >= 100) & (t < 120), alpha=0.2, color="red") + ax2.set_xlabel("Time [s]", fontsize=12) + ax2.set_ylabel("Heading Error [deg]", fontsize=12) ax2.legend(fontsize=10) ax2.grid(True, alpha=0.3) ax2.set_xlim([0, t[-1]]) plt.tight_layout() - paths = save_figure(fig1, figs_dir, 'environment_mag_heading') + paths = save_figure(fig1, figs_dir, "environment_mag_heading") print(f" [OK] Saved: {paths[0]}") # Figure 2: Barometric altitude and floor detection fig2, (ax1, ax2) = plt.subplots(2, 1, figsize=(12, 8), sharex=True) floor_heights = floor_true * 3.5 - ax1.plot(t, floor_heights, 'k-', linewidth=3, label='True Altitude', drawstyle='steps-post') - ax1.plot(t, alt_est, 'b-', linewidth=1.5, alpha=0.7, label='Baro Altitude') - ax1.set_ylabel('Altitude [m]', fontsize=12) - ax1.set_title('Barometer Example: Altitude and Floor Detection', fontsize=14, fontweight='bold') + ax1.plot( + t, + floor_heights, + "k-", + linewidth=3, + label="True Altitude", + drawstyle="steps-post", + ) + ax1.plot(t, alt_est, "b-", linewidth=1.5, alpha=0.7, label="Baro Altitude") + ax1.set_ylabel("Altitude [m]", fontsize=12) + ax1.set_title( + "Barometer Example: Altitude and Floor Detection", + fontsize=14, + fontweight="bold", + ) ax1.legend(fontsize=11) ax1.grid(True, alpha=0.3) - ax2.plot(t, floor_true, 'k-', linewidth=3, label='True Floor', drawstyle='steps-post') - ax2.plot(t, floor_detected, 'b--', linewidth=2, marker='o', markersize=3, label='Detected Floor') - ax2.set_xlabel('Time [s]', fontsize=12) - ax2.set_ylabel('Floor Number', fontsize=12) + ax2.plot( + t, floor_true, "k-", linewidth=3, label="True Floor", drawstyle="steps-post" + ) + ax2.plot( + t, + floor_detected, + "b--", + linewidth=2, + marker="o", + markersize=3, + label="Detected Floor", + ) + ax2.set_xlabel("Time [s]", fontsize=12) + ax2.set_ylabel("Floor Number", fontsize=12) ax2.set_yticks([0, 1, 2]) ax2.legend(fontsize=11) ax2.grid(True, alpha=0.3) ax2.set_xlim([0, t[-1]]) plt.tight_layout() - paths = save_figure(fig2, figs_dir, 'environment_baro_altitude') + paths = save_figure(fig2, figs_dir, "environment_baro_altitude") print(f" [OK] Saved: {paths[0]}") - plt.close('all') + plt.close("all") return heading_error @@ -292,9 +346,9 @@ def main(): formatter_class=argparse.RawDescriptionHelpFormatter, ).parse_args() - print("\n" + "="*70) + print("\n" + "=" * 70) print("Chapter 6: Environmental Sensors (Magnetometer + Barometer)") - print("="*70) + print("=" * 70) print("\nDemonstrates absolute references with indoor challenges.") print("Key equations: 6.51-6.54 (mag heading, tilt comp, baro altitude)\n") @@ -311,11 +365,15 @@ def main(): # are consecutive draws from a single seeded stream rather than two copies # of the same one. rng = np.random.default_rng(DEFAULT_SEED) - t, pos_true, att_true, mag_true, pressure_true, floor_true = generate_building_walk(duration, dt, rng=rng) + t, pos_true, att_true, mag_true, pressure_true, floor_true = generate_building_walk( + duration, dt, rng=rng + ) print(f" Floors visited: {np.unique(floor_true)}") print("\nAdding sensor noise + disturbances...") - mag_meas, pressure_meas = add_env_sensor_noise(mag_true, pressure_true, t, dt, rng=rng) + mag_meas, pressure_meas = add_env_sensor_noise( + mag_true, pressure_true, t, dt, rng=rng + ) print("\nComputing magnetometer heading...") start = time.time() @@ -330,36 +388,47 @@ def main(): alt_smooth = np.zeros_like(alt_est) alt_smooth[0] = alt_est[0] for k in range(1, len(alt_est)): - alt_smooth[k] = smooth_measurement_simple(alt_smooth[k-1], alt_est[k], alpha=0.1) + alt_smooth[k] = smooth_measurement_simple( + alt_smooth[k - 1], alt_est[k], alpha=0.1 + ) # Detect floor changes floor_detected = np.zeros_like(floor_true) current_floor = 0 for k in range(1, len(t)): - delta_floor = detect_floor_change(alt_smooth[k-1], alt_smooth[k], floor_height=3.5, threshold=1.5) + delta_floor = detect_floor_change( + alt_smooth[k - 1], alt_smooth[k], floor_height=3.5, threshold=1.5 + ) current_floor += delta_floor floor_detected[k] = max(0, min(2, current_floor)) # Clamp to [0, 2] print(f" Time: {time.time()-start:.3f} s") - figs_dir = Path(__file__).parent / 'figs' + figs_dir = Path(__file__).parent / "figs" figs_dir.mkdir(exist_ok=True) print("\nGenerating plots...") heading_error = plot_results( - t, att_true, mag_meas, heading_est, pressure_meas, alt_smooth, - floor_true, floor_detected, figs_dir + t, + att_true, + mag_meas, + heading_est, + pressure_meas, + alt_smooth, + floor_true, + floor_detected, + figs_dir, ) # Metrics heading_rmse = np.sqrt(np.mean(heading_error**2)) - alt_error = np.abs(alt_smooth - floor_true*3.5) + alt_error = np.abs(alt_smooth - floor_true * 3.5) alt_rmse = np.sqrt(np.mean(alt_error**2)) floor_accuracy = np.sum(floor_detected == floor_true) / len(floor_true) * 100 - print("\n" + "="*70) + print("\n" + "=" * 70) print("RESULTS") - print("="*70) + print("=" * 70) print("Magnetometer Heading:") print(f" RMSE: {heading_rmse:.1f}°") print(f" Max error: {np.max(heading_error):.1f}°") @@ -371,16 +440,15 @@ def main(): print() print(f"Figures saved to: {resolve_figs_dir(figs_dir)}/") print() - print("="*70) + print("=" * 70) print("KEY INSIGHT: Environmental sensors provide absolute references!") print(" Magnetometer: bounds heading drift (when clean).") print(" Barometer: provides floor-level positioning.") print(" BUT sensitive to indoor disturbances (steel, weather).") - print("="*70) + print("=" * 70) print() show_figures_if_requested() if __name__ == "__main__": main() - diff --git a/ch6_dead_reckoning/example_imu_strapdown.py b/ch6_dead_reckoning/example_imu_strapdown.py index 9ddc3c3..5f6bd15 100644 --- a/ch6_dead_reckoning/example_imu_strapdown.py +++ b/ch6_dead_reckoning/example_imu_strapdown.py @@ -48,16 +48,17 @@ # figures can be regenerated exactly; see the noise function below. DEFAULT_SEED = 42 + def generate_figure8_trajectory(duration=100.0, dt=0.01, frame=None, lat_deg=45.0): """ Generate a figure-8 trajectory with correct IMU forward model. - + Args: duration: Total duration [s]. dt: Time step [s]. frame: Frame convention (default: ENU). lat_deg: Latitude in degrees for gravity model (default: 45.0°). - + Returns: Tuple of (t, pos_true, vel_true, quat_true, accel_body, gyro_body). """ @@ -94,12 +95,9 @@ def generate_figure8_trajectory(duration=100.0, dt=0.01, frame=None, lat_deg=45. yaw = np.arctan2(vy, vx) # Heading = direction of velocity vector # Convert to quaternions (scalar-first, body-to-map) - quat_true = np.column_stack([ - np.cos(yaw / 2), - np.zeros_like(t), - np.zeros_like(t), - np.sin(yaw / 2) - ]) + quat_true = np.column_stack( + [np.cos(yaw / 2), np.zeros_like(t), np.zeros_like(t), np.sin(yaw / 2)] + ) # Generate IMU measurements using correct forward model with Eq. (6.8) accel_body, gyro_body = generate_imu_from_trajectory( @@ -109,14 +107,15 @@ def generate_figure8_trajectory(duration=100.0, dt=0.01, frame=None, lat_deg=45. dt=dt, frame=frame, g=9.81, - lat_rad=lat_rad + lat_rad=lat_rad, ) return t, pos_true, vel_true, quat_true, accel_body, gyro_body -def add_imu_noise(accel_true, gyro_true, dt, imu_params: IMUNoiseParams, - seed: int = DEFAULT_SEED): +def add_imu_noise( + accel_true, gyro_true, dt, imu_params: IMUNoiseParams, seed: int = DEFAULT_SEED +): """ Add realistic IMU noise and biases using explicit unit conversions. @@ -156,8 +155,9 @@ def add_imu_noise(accel_true, gyro_true, dt, imu_params: IMUNoiseParams, return accel_meas, gyro_meas, accel_bias, gyro_bias -def run_imu_strapdown(t, accel_meas, gyro_meas, initial_state, frame, - lat_rad=np.deg2rad(45.0)): +def run_imu_strapdown( + t, accel_meas, gyro_meas, initial_state, frame, lat_rad=np.deg2rad(45.0) +): """ Run pure IMU strapdown integration (no corrections). @@ -194,19 +194,12 @@ def run_imu_strapdown(t, accel_meas, gyro_meas, initial_state, frame, # Propagate for k in range(1, N): # Get IMU measurements - omega_b = gyro_meas[k-1] - f_b = accel_meas[k-1] + omega_b = gyro_meas[k - 1] + f_b = accel_meas[k - 1] # Strapdown update (Eqs. 6.2-6.10) with Eq. (6.8) gravity q, v, p = strapdown_update( - q=q, - v=v, - p=p, - omega_b=omega_b, - f_b=f_b, - dt=dt, - frame=frame, - lat_rad=lat_rad + q=q, v=v, p=p, omega_b=omega_b, f_b=f_b, dt=dt, frame=frame, lat_rad=lat_rad ) # Store @@ -220,10 +213,10 @@ def run_imu_strapdown(t, accel_meas, gyro_meas, initial_state, frame, def quat_to_euler(q): """ Convert quaternion to Euler angles (roll, pitch, yaw). - + Args: q: Quaternion [q0, q1, q2, q3], shape (4,) or (N, 4). - + Returns: Euler angles [roll, pitch, yaw] in radians, shape (3,) or (N, 3). """ @@ -236,13 +229,13 @@ def quat_to_euler(q): q0, q1, q2, q3 = q[:, 0], q[:, 1], q[:, 2], q[:, 3] # Roll (x-axis rotation) - roll = np.arctan2(2*(q0*q1 + q2*q3), 1 - 2*(q1**2 + q2**2)) + roll = np.arctan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1**2 + q2**2)) # Pitch (y-axis rotation) - pitch = np.arcsin(np.clip(2*(q0*q2 - q3*q1), -1.0, 1.0)) + pitch = np.arcsin(np.clip(2 * (q0 * q2 - q3 * q1), -1.0, 1.0)) # Yaw (z-axis rotation) - yaw = np.arctan2(2*(q0*q3 + q1*q2), 1 - 2*(q2**2 + q3**2)) + yaw = np.arctan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2**2 + q3**2)) euler = np.column_stack([roll, pitch, yaw]) @@ -251,10 +244,12 @@ def quat_to_euler(q): return euler -def plot_results(t, pos_true, pos_est, vel_true, vel_est, quat_true, quat_est, figs_dir): +def plot_results( + t, pos_true, pos_est, vel_true, vel_est, quat_true, quat_est, figs_dir +): """ Generate publication-quality plots. - + Args: t: Time array [s]. pos_true: True position [m], shape (N, 3). @@ -277,61 +272,95 @@ def plot_results(t, pos_true, pos_est, vel_true, vel_est, quat_true, quat_est, f # Figure 1: Trajectory (2D) fig1, ax1 = plt.subplots(figsize=(10, 8)) - ax1.plot(pos_true[:, 0], pos_true[:, 1], 'k-', linewidth=2, label='True Trajectory') - ax1.plot(pos_est[:, 0], pos_est[:, 1], 'r--', linewidth=2, label='IMU Estimated (no corrections)') - ax1.scatter(pos_true[0, 0], pos_true[0, 1], c='g', s=100, marker='o', label='Start', zorder=5) - ax1.scatter(pos_true[-1, 0], pos_true[-1, 1], c='b', s=100, marker='s', label='End (true)', zorder=5) - ax1.scatter(pos_est[-1, 0], pos_est[-1, 1], c='r', s=100, marker='x', label='End (estimated)', zorder=5) - ax1.set_xlabel('East [m]', fontsize=12) - ax1.set_ylabel('North [m]', fontsize=12) - ax1.set_title('IMU Strapdown: Trajectory (Pure Integration, No Corrections)', fontsize=14) + ax1.plot(pos_true[:, 0], pos_true[:, 1], "k-", linewidth=2, label="True Trajectory") + ax1.plot( + pos_est[:, 0], + pos_est[:, 1], + "r--", + linewidth=2, + label="IMU Estimated (no corrections)", + ) + ax1.scatter( + pos_true[0, 0], + pos_true[0, 1], + c="g", + s=100, + marker="o", + label="Start", + zorder=5, + ) + ax1.scatter( + pos_true[-1, 0], + pos_true[-1, 1], + c="b", + s=100, + marker="s", + label="End (true)", + zorder=5, + ) + ax1.scatter( + pos_est[-1, 0], + pos_est[-1, 1], + c="r", + s=100, + marker="x", + label="End (estimated)", + zorder=5, + ) + ax1.set_xlabel("East [m]", fontsize=12) + ax1.set_ylabel("North [m]", fontsize=12) + ax1.set_title( + "IMU Strapdown: Trajectory (Pure Integration, No Corrections)", fontsize=14 + ) ax1.legend(fontsize=10) ax1.grid(True, alpha=0.3) - ax1.axis('equal') + ax1.axis("equal") plt.tight_layout() - paths = save_figure(fig1, figs_dir, 'imu_strapdown_trajectory') + paths = save_figure(fig1, figs_dir, "imu_strapdown_trajectory") print(f" [OK] Saved: {paths[0]}") # Figure 2: Position Error vs Time fig2, ax2 = plt.subplots(figsize=(12, 6)) - ax2.plot(t, pos_error, 'r-', linewidth=2) - ax2.set_xlabel('Time [s]', fontsize=12) - ax2.set_ylabel('Position Error [m]', fontsize=12) - ax2.set_title('IMU Strapdown: Position Error vs Time (Unbounded Drift)', fontsize=14) + ax2.plot(t, pos_error, "r-", linewidth=2) + ax2.set_xlabel("Time [s]", fontsize=12) + ax2.set_ylabel("Position Error [m]", fontsize=12) + ax2.set_title( + "IMU Strapdown: Position Error vs Time (Unbounded Drift)", fontsize=14 + ) ax2.grid(True, alpha=0.3) ax2.set_xlim([0, t[-1]]) plt.tight_layout() - paths = save_figure(fig2, figs_dir, 'imu_strapdown_error_time') + paths = save_figure(fig2, figs_dir, "imu_strapdown_error_time") print(f" [OK] Saved: {paths[0]}") # Figure 3: Attitude Evolution fig3, axes = plt.subplots(3, 1, figsize=(12, 10), sharex=True) - axes[0].plot(t, np.rad2deg(att_true[:, 0]), 'k-', linewidth=2, label='True') - axes[0].plot(t, np.rad2deg(att_est[:, 0]), 'r--', linewidth=2, label='Estimated') - axes[0].set_ylabel('Roll [deg]', fontsize=12) + axes[0].plot(t, np.rad2deg(att_true[:, 0]), "k-", linewidth=2, label="True") + axes[0].plot(t, np.rad2deg(att_est[:, 0]), "r--", linewidth=2, label="Estimated") + axes[0].set_ylabel("Roll [deg]", fontsize=12) axes[0].legend(fontsize=10) axes[0].grid(True, alpha=0.3) - axes[0].set_title('IMU Strapdown: Attitude Evolution', fontsize=14) + axes[0].set_title("IMU Strapdown: Attitude Evolution", fontsize=14) - axes[1].plot(t, np.rad2deg(att_true[:, 1]), 'k-', linewidth=2, label='True') - axes[1].plot(t, np.rad2deg(att_est[:, 1]), 'r--', linewidth=2, label='Estimated') - axes[1].set_ylabel('Pitch [deg]', fontsize=12) + axes[1].plot(t, np.rad2deg(att_true[:, 1]), "k-", linewidth=2, label="True") + axes[1].plot(t, np.rad2deg(att_est[:, 1]), "r--", linewidth=2, label="Estimated") + axes[1].set_ylabel("Pitch [deg]", fontsize=12) axes[1].legend(fontsize=10) axes[1].grid(True, alpha=0.3) - axes[2].plot(t, np.rad2deg(att_true[:, 2]), 'k-', linewidth=2, label='True') - axes[2].plot(t, np.rad2deg(att_est[:, 2]), 'r--', linewidth=2, label='Estimated') - axes[2].set_ylabel('Yaw [deg]', fontsize=12) - axes[2].set_xlabel('Time [s]', fontsize=12) + axes[2].plot(t, np.rad2deg(att_true[:, 2]), "k-", linewidth=2, label="True") + axes[2].plot(t, np.rad2deg(att_est[:, 2]), "r--", linewidth=2, label="Estimated") + axes[2].set_ylabel("Yaw [deg]", fontsize=12) + axes[2].set_xlabel("Time [s]", fontsize=12) axes[2].legend(fontsize=10) axes[2].grid(True, alpha=0.3) plt.tight_layout() - paths = save_figure(fig3, figs_dir, 'imu_strapdown_attitude') + paths = save_figure(fig3, figs_dir, "imu_strapdown_attitude") print(f" [OK] Saved: {paths[0]}") - plt.close('all') + plt.close("all") return pos_error, vel_error, att_error @@ -345,9 +374,9 @@ def main(): formatter_class=argparse.RawDescriptionHelpFormatter, ).parse_args() - print("\n" + "="*60) + print("\n" + "=" * 60) print("Chapter 6: IMU Strapdown Integration (Pure, No Corrections)") - print("="*60) + print("=" * 60) print("\nThis example demonstrates UNBOUNDED DRIFT in pure IMU integration.") print("Key equations: 6.2-6.4 (quaternion), 6.7 (velocity), 6.10 (position)\n") @@ -376,8 +405,10 @@ def main(): # Generate true trajectory with correct IMU forward model print("Generating trajectory...") - t, pos_true, vel_true, quat_true, accel_body, gyro_body = generate_figure8_trajectory( - duration=duration, dt=dt, frame=frame, lat_deg=lat_deg + t, pos_true, vel_true, quat_true, accel_body, gyro_body = ( + generate_figure8_trajectory( + duration=duration, dt=dt, frame=frame, lat_deg=lat_deg + ) ) total_distance = np.sum(np.linalg.norm(np.diff(pos_true, axis=0), axis=1)) @@ -389,8 +420,12 @@ def main(): accel_body, gyro_body, dt, imu_params ) # Print realized bias values (random samples from the bias distribution) - print(f" Gyro bias (realized): {units.format_gyro_bias(np.linalg.norm(gyro_bias))}") - print(f" Accel bias (realized): {units.format_accel_bias(np.linalg.norm(accel_bias))}") + print( + f" Gyro bias (realized): {units.format_gyro_bias(np.linalg.norm(gyro_bias))}" + ) + print( + f" Accel bias (realized): {units.format_accel_bias(np.linalg.norm(accel_bias))}" + ) # Initial state (perfect knowledge) initial_state = NavStateQPVP( @@ -409,7 +444,7 @@ def main(): print(f" Computation time: {elapsed:.3f} s ({len(t)/elapsed:.0f}x real-time)") # Create output directory - figs_dir = Path(__file__).parent / 'figs' + figs_dir = Path(__file__).parent / "figs" figs_dir.mkdir(exist_ok=True) # Generate plots @@ -426,10 +461,12 @@ def main(): drift_percent = (final_pos_error / total_distance) * 100 # Print results - print("\n" + "="*60) + print("\n" + "=" * 60) print("RESULTS (IMU-only, no corrections)") - print("="*60) - print(f" Final Position Error: {final_pos_error:.1f} m ({drift_percent:.1f}% of distance)") + print("=" * 60) + print( + f" Final Position Error: {final_pos_error:.1f} m ({drift_percent:.1f}% of distance)" + ) print(f" Max Velocity Error: {max_vel_error:.2f} m/s") print(" Max Attitude Error:") print(f" Roll: {max_att_error[0]:.1f}°") @@ -439,16 +476,15 @@ def main(): print() print(f"Figures saved to: {resolve_figs_dir(figs_dir)}/") print() - print("="*60) + print("=" * 60) print("KEY INSIGHT: IMU drift is UNBOUNDED without corrections!") print(" Velocity errors integrate to position errors.") print(" Errors grow without bound over time.") print(" Solutions: ZUPT, wheel fusion, GPS, etc.") - print("="*60) + print("=" * 60) print() show_figures_if_requested() if __name__ == "__main__": main() - diff --git a/ch6_dead_reckoning/example_pdr.py b/ch6_dead_reckoning/example_pdr.py index 768406d..7c5f895 100644 --- a/ch6_dead_reckoning/example_pdr.py +++ b/ch6_dead_reckoning/example_pdr.py @@ -70,6 +70,7 @@ #: unphysical step the rounded corners removed, just at the end of the record. BRAKE_DISTANCE_M = 1.0 + def compute_step_length( height: float, f_step: float, @@ -79,14 +80,14 @@ def compute_step_length( ) -> float: """ Compute step length using selected model. - + Args: height: User height in meters f_step: Step frequency in Hz model: Model selection: 'book' (Eq. 6.49), 'weinberg' (actual Weinberg), or 'power_law' (old) G_w: Weinberg gain parameter (required if model='weinberg') f_step_window: Per-step accel window (required if model='weinberg') - + Returns: Step length in meters """ @@ -102,63 +103,67 @@ def compute_step_length( # Legacy power-law (deprecated but kept for compatibility) return step_length(height, f_step) else: - raise ValueError(f"Unknown model: {model}. Choose 'book', 'weinberg', or 'power_law'") + raise ValueError( + f"Unknown model: {model}. Choose 'book', 'weinberg', or 'power_law'" + ) def load_pdr_dataset(data_dir: str) -> Dict: """Load PDR dataset from directory. - + Args: data_dir: Path to dataset directory (e.g., 'data/sim/ch6_pdr_corridor_walk') - + Returns: Dictionary with time, ground truth, and sensor measurements """ path = Path(data_dir) data = { - 't': np.loadtxt(path / 'time.txt'), - 'pos_true': np.loadtxt(path / 'ground_truth_position.txt'), - 'heading_true': np.loadtxt(path / 'ground_truth_heading.txt'), - 'accel_meas': np.loadtxt(path / 'accel.txt'), - 'gyro_meas': np.loadtxt(path / 'gyro.txt'), - 'mag_meas': np.loadtxt(path / 'magnetometer.txt'), - 'step_times': np.loadtxt(path / 'step_times.txt'), + "t": np.loadtxt(path / "time.txt"), + "pos_true": np.loadtxt(path / "ground_truth_position.txt"), + "heading_true": np.loadtxt(path / "ground_truth_heading.txt"), + "accel_meas": np.loadtxt(path / "accel.txt"), + "gyro_meas": np.loadtxt(path / "gyro.txt"), + "mag_meas": np.loadtxt(path / "magnetometer.txt"), + "step_times": np.loadtxt(path / "step_times.txt"), } # Load config if available - config_path = path / 'config.json' + config_path = path / "config.json" if config_path.exists(): with open(config_path) as f: - data['config'] = json.load(f) + data["config"] = json.load(f) return data -def run_pdr_from_dataset(data: Dict, height: float = 1.75, step_model: str = "book") -> Dict: +def run_pdr_from_dataset( + data: Dict, height: float = 1.75, step_model: str = "book" +) -> Dict: """Run PDR algorithm on loaded dataset. - + Uses the book's peak detection method (Eqs. 6.46-6.47) for step detection: 1. Compute total acceleration magnitude (6.46) 2. Subtract gravity (6.47) 3. Filter the signal 4. Detect peaks - + Args: data: Dataset dictionary from load_pdr_dataset height: Pedestrian height in meters step_model: Step-length model: 'book' (Eq. 6.49, default), 'power_law' (old) - + Returns: Dictionary with estimated positions and headings - + Note: Weinberg model not supported here (requires per-step windows, needs refactoring) """ - t = data['t'] - accel_meas = data['accel_meas'] - gyro_meas = data['gyro_meas'] - mag_meas = data['mag_meas'] + t = data["t"] + accel_meas = data["accel_meas"] + gyro_meas = data["gyro_meas"] + mag_meas = data["mag_meas"] N = len(t) dt = t[1] - t[0] if len(t) > 1 else 0.01 @@ -176,7 +181,7 @@ def run_pdr_from_dataset(data: Dict, height: float = 1.75, step_model: str = "bo g=9.81, min_peak_height=1.0, # m/s² above gravity min_peak_distance=0.3, # seconds between steps - lowpass_cutoff=5.0 # Hz low-pass filter + lowpass_cutoff=5.0, # Hz low-pass filter ) print(f" Detected {len(step_indices)} steps") @@ -193,12 +198,16 @@ def run_pdr_from_dataset(data: Dict, height: float = 1.75, step_model: str = "bo # absolute reference (magnetometer, GPS, or user input) because gyros measure # only CHANGES in heading, not absolute direction! heading_gyro[0] = 0.0 # Start at 0° (East), will drift due to gyro bias - heading_mag[0] = mag_heading(mag_meas[0], roll=0.0, pitch=0.0, declination=0.0) # Absolute reference + heading_mag[0] = mag_heading( + mag_meas[0], roll=0.0, pitch=0.0, declination=0.0 + ) # Absolute reference # Run PDR with gyro heading for k in range(1, N): # Integrate gyro heading - heading_gyro[k] = integrate_gyro_heading(heading_gyro[k-1], gyro_meas[k, 2], dt) + heading_gyro[k] = integrate_gyro_heading( + heading_gyro[k - 1], gyro_meas[k, 2], dt + ) heading_gyro[k] = wrap_heading(heading_gyro[k]) # Update position on step events @@ -216,9 +225,9 @@ def run_pdr_from_dataset(data: Dict, height: float = 1.75, step_model: str = "bo L = compute_step_length(height, f_step, model=step_model) # Update position (Eq. 6.50) - pos_gyro[k] = pdr_step_update(pos_gyro[k-1], L, heading_gyro[k-1]) + pos_gyro[k] = pdr_step_update(pos_gyro[k - 1], L, heading_gyro[k - 1]) else: - pos_gyro[k] = pos_gyro[k-1] + pos_gyro[k] = pos_gyro[k - 1] # Run PDR with magnetometer heading for k in range(1, N): @@ -240,44 +249,46 @@ def run_pdr_from_dataset(data: Dict, height: float = 1.75, step_model: str = "bo L = compute_step_length(height, f_step, model=step_model) # Update position (Eq. 6.50) - pos_mag[k] = pdr_step_update(pos_mag[k-1], L, heading_mag[k-1]) + pos_mag[k] = pdr_step_update(pos_mag[k - 1], L, heading_mag[k - 1]) else: - pos_mag[k] = pos_mag[k-1] + pos_mag[k] = pos_mag[k - 1] return { - 't': t, - 'pos_gyro': pos_gyro, - 'pos_mag': pos_mag, - 'heading_gyro': heading_gyro, - 'heading_mag': heading_mag, - 'step_count_gyro': len(step_indices), - 'step_count_mag': len(step_indices), - 'step_indices': step_indices, + "t": t, + "pos_gyro": pos_gyro, + "pos_mag": pos_mag, + "heading_gyro": heading_gyro, + "heading_mag": heading_mag, + "step_count_gyro": len(step_indices), + "step_count_mag": len(step_indices), + "step_indices": step_indices, } -def run_with_dataset(data_dir: str, height: float = 1.75, lat_deg: float = 45.0, step_model: str = "book") -> None: +def run_with_dataset( + data_dir: str, height: float = 1.75, lat_deg: float = 45.0, step_model: str = "book" +) -> None: """Run PDR example using pre-generated dataset. - + Args: data_dir: Path to dataset directory height: Pedestrian height in meters lat_deg: Latitude in degrees step_model: Step-length model selection """ - print("\n" + "="*70) + print("\n" + "=" * 70) print("Chapter 6: Pedestrian Dead Reckoning (PDR)") print(f"Using dataset: {data_dir}") - print("="*70) + print("=" * 70) # Load dataset print("\nLoading dataset...") data = load_pdr_dataset(data_dir) - t = data['t'] - pos_true = data['pos_true'] - heading_true = data['heading_true'] - step_times = data['step_times'] + t = data["t"] + pos_true = data["pos_true"] + heading_true = data["heading_true"] + step_times = data["step_times"] total_dist = np.sum(np.linalg.norm(np.diff(pos_true, axis=0), axis=1)) @@ -298,97 +309,128 @@ def run_with_dataset(data_dir: str, height: float = 1.75, lat_deg: float = 45.0, print(f" Steps detected (mag): {results['step_count_mag']}") # Compute errors - error_gyro = np.linalg.norm(results['pos_gyro'] - pos_true, axis=1) - error_mag = np.linalg.norm(results['pos_mag'] - pos_true, axis=1) + error_gyro = np.linalg.norm(results["pos_gyro"] - pos_true, axis=1) + error_mag = np.linalg.norm(results["pos_mag"] - pos_true, axis=1) rmse_gyro = np.sqrt(np.mean(error_gyro**2)) rmse_mag = np.sqrt(np.mean(error_mag**2)) - print("\n" + "="*70) + print("\n" + "=" * 70) print("RESULTS") - print("="*70) + print("=" * 70) print("PDR (Gyro Heading - drifts unbounded):") - print(f" Final error: {error_gyro[-1]:.1f} m ({error_gyro[-1]/total_dist*100:.1f}% of distance)") + print( + f" Final error: {error_gyro[-1]:.1f} m ({error_gyro[-1]/total_dist*100:.1f}% of distance)" + ) print(f" RMSE: {rmse_gyro:.1f} m") print() print("PDR (Magnetometer Heading - absolute but noisy):") - print(f" Final error: {error_mag[-1]:.1f} m ({error_mag[-1]/total_dist*100:.1f}% of distance)") + print( + f" Final error: {error_mag[-1]:.1f} m ({error_mag[-1]/total_dist*100:.1f}% of distance)" + ) print(f" RMSE: {rmse_mag:.1f} m") # Plot results - figs_dir = Path(__file__).parent / 'figs' + figs_dir = Path(__file__).parent / "figs" figs_dir.mkdir(exist_ok=True) print("\nGenerating plots...") fig, axes = plt.subplots(2, 2, figsize=(14, 10)) - fig.suptitle('PDR: Dataset Analysis', fontsize=14, fontweight='bold') + fig.suptitle("PDR: Dataset Analysis", fontsize=14, fontweight="bold") # Trajectory ax = axes[0, 0] - ax.plot(pos_true[:, 0], pos_true[:, 1], 'k-', linewidth=3, label='True Path') - ax.plot(results['pos_gyro'][:, 0], results['pos_gyro'][:, 1], 'r--', linewidth=2, alpha=0.7, label='PDR (Gyro)') - ax.plot(results['pos_mag'][:, 0], results['pos_mag'][:, 1], 'b-', linewidth=2, label='PDR (Mag)') - ax.scatter(0, 0, c='g', s=150, marker='o', label='Start', zorder=5) - ax.set_xlabel('East [m]') - ax.set_ylabel('North [m]') - ax.set_title('PDR Trajectory Comparison') + ax.plot(pos_true[:, 0], pos_true[:, 1], "k-", linewidth=3, label="True Path") + ax.plot( + results["pos_gyro"][:, 0], + results["pos_gyro"][:, 1], + "r--", + linewidth=2, + alpha=0.7, + label="PDR (Gyro)", + ) + ax.plot( + results["pos_mag"][:, 0], + results["pos_mag"][:, 1], + "b-", + linewidth=2, + label="PDR (Mag)", + ) + ax.scatter(0, 0, c="g", s=150, marker="o", label="Start", zorder=5) + ax.set_xlabel("East [m]") + ax.set_ylabel("North [m]") + ax.set_title("PDR Trajectory Comparison") ax.legend() ax.grid(True, alpha=0.3) - ax.axis('equal') + ax.axis("equal") # Position error ax = axes[0, 1] - ax.plot(t, error_gyro, 'r-', linewidth=2, label='Gyro Heading') - ax.plot(t, error_mag, 'b-', linewidth=2, label='Mag Heading') - ax.set_xlabel('Time [s]') - ax.set_ylabel('Position Error [m]') - ax.set_title('Position Error vs Time') + ax.plot(t, error_gyro, "r-", linewidth=2, label="Gyro Heading") + ax.plot(t, error_mag, "b-", linewidth=2, label="Mag Heading") + ax.set_xlabel("Time [s]") + ax.set_ylabel("Position Error [m]") + ax.set_title("Position Error vs Time") ax.legend() ax.grid(True, alpha=0.3) # Heading comparison ax = axes[1, 0] - ax.plot(t, np.rad2deg(heading_true), 'k-', linewidth=2, label='True') - ax.plot(t, np.rad2deg(results['heading_gyro']), 'r--', linewidth=2, alpha=0.7, label='Gyro') - ax.plot(t, np.rad2deg(results['heading_mag']), 'b-', linewidth=1.5, alpha=0.7, label='Mag') - ax.set_xlabel('Time [s]') - ax.set_ylabel('Heading [deg]') - ax.set_title('Heading Comparison') + ax.plot(t, np.rad2deg(heading_true), "k-", linewidth=2, label="True") + ax.plot( + t, + np.rad2deg(results["heading_gyro"]), + "r--", + linewidth=2, + alpha=0.7, + label="Gyro", + ) + ax.plot( + t, + np.rad2deg(results["heading_mag"]), + "b-", + linewidth=1.5, + alpha=0.7, + label="Mag", + ) + ax.set_xlabel("Time [s]") + ax.set_ylabel("Heading [deg]") + ax.set_title("Heading Comparison") ax.legend() ax.grid(True, alpha=0.3) # Heading error ax = axes[1, 1] - heading_error_gyro = np.abs(wrap_heading(results['heading_gyro'] - heading_true)) - heading_error_mag = np.abs(wrap_heading(results['heading_mag'] - heading_true)) - ax.plot(t, np.rad2deg(heading_error_gyro), 'r-', linewidth=2, label='Gyro Error') - ax.plot(t, np.rad2deg(heading_error_mag), 'b-', linewidth=2, label='Mag Error') - ax.set_xlabel('Time [s]') - ax.set_ylabel('Heading Error [deg]') - ax.set_title('Heading Error') + heading_error_gyro = np.abs(wrap_heading(results["heading_gyro"] - heading_true)) + heading_error_mag = np.abs(wrap_heading(results["heading_mag"] - heading_true)) + ax.plot(t, np.rad2deg(heading_error_gyro), "r-", linewidth=2, label="Gyro Error") + ax.plot(t, np.rad2deg(heading_error_mag), "b-", linewidth=2, label="Mag Error") + ax.set_xlabel("Time [s]") + ax.set_ylabel("Heading Error [deg]") + ax.set_title("Heading Error") ax.legend() ax.grid(True, alpha=0.3) plt.tight_layout() - paths = save_figure(fig, figs_dir, 'pdr_dataset_results') + paths = save_figure(fig, figs_dir, "pdr_dataset_results") print(f" [OK] Saved: {paths[0]}") show_figures_if_requested() - print("\n" + "="*70) + print("\n" + "=" * 70) print("KEY INSIGHT: Heading errors DOMINATE PDR accuracy!") print(" Gyro drifts unbounded -> unusable alone.") print(" Magnetometer provides absolute reference (with noise).") - print("="*70) + print("=" * 70) def generate_corridor_walk(duration=120.0, dt=0.01, step_freq=2.0, frame=None): """ Generate rectangular corridor walk with turns and synthetic walking dynamics. Uses correct IMU forward model with added vertical oscillations for step detection. - + Returns: t, pos_true, heading_true, accel_body, gyro_body, mag_body, expected_steps """ if frame is None: @@ -444,7 +486,7 @@ def generate_corridor_walk(duration=120.0, dt=0.01, step_freq=2.0, frame=None): # costs v^2 / (2 d) = 0.98 m/s^2, the same as the corners themselves. cruise_length = total_length - BRAKE_DISTANCE_M t_cruise = cruise_length / v_walk - decel = v_walk ** 2 / (2.0 * BRAKE_DISTANCE_M) + decel = v_walk**2 / (2.0 * BRAKE_DISTANCE_M) t_brake = v_walk / decel def distance_at(time_s): @@ -452,7 +494,7 @@ def distance_at(time_s): if time_s <= t_cruise: return v_walk * time_s tau = min(time_s - t_cruise, t_brake) - return cruise_length + v_walk * tau - 0.5 * decel * tau ** 2 + return cruise_length + v_walk * tau - 0.5 * decel * tau**2 starts = np.cumsum([0.0] + [seg[1] for seg in segments]) for k in range(N): @@ -478,12 +520,10 @@ def distance_at(time_s): # Speed follows the profile, so velocity stays the derivative of # position through the braking phase too. - speed = v_walk if t[k] <= t_cruise else max( - v_walk - decel * (t[k] - t_cruise), 0.0 + speed = ( + v_walk if t[k] <= t_cruise else max(v_walk - decel * (t[k] - t_cruise), 0.0) ) - vel_2d[k] = speed * np.array([ - np.cos(heading_true[k]), np.sin(heading_true[k]) - ]) + vel_2d[k] = speed * np.array([np.cos(heading_true[k]), np.sin(heading_true[k])]) heading_true = np.unwrap(heading_true) @@ -492,12 +532,9 @@ def distance_at(time_s): vel_map = np.column_stack([vel_2d, np.zeros(N)]) # Create quaternion trajectory (yaw only, roll/pitch = 0) - quat_b_to_m = np.column_stack([ - np.cos(heading_true / 2), - np.zeros(N), - np.zeros(N), - np.sin(heading_true / 2) - ]) + quat_b_to_m = np.column_stack( + [np.cos(heading_true / 2), np.zeros(N), np.zeros(N), np.sin(heading_true / 2)] + ) # Add synthetic walking accelerations (vertical oscillations for step detection) # Walking creates periodic vertical accelerations at step frequency @@ -534,22 +571,22 @@ def distance_at(time_s): quat_b_to_m=quat_b_to_m, dt=dt, frame=frame, - g=9.81 + g=9.81, ) # Generate magnetometer measurements (points to magnetic north in body frame) mag_body = np.zeros((N, 3)) - mag_north_map = np.array([1.0, 0.0, 0.0]) # North = x-axis in ENU map frame (conventionally) + mag_north_map = np.array( + [1.0, 0.0, 0.0] + ) # North = x-axis in ENU map frame (conventionally) for k in range(N): # Rotate north vector from map to body frame # C_M^B = (C_B^M)^T yaw = heading_true[k] - C_yaw = np.array([ - [np.cos(yaw), np.sin(yaw), 0], - [-np.sin(yaw), np.cos(yaw), 0], - [0, 0, 1] - ]) + C_yaw = np.array( + [[np.cos(yaw), np.sin(yaw), 0], [-np.sin(yaw), np.cos(yaw), 0], [0, 0, 1]] + ) mag_body[k] = C_yaw.T @ mag_north_map # Steps actually taken: the walker is only moving until the lap closes at @@ -560,8 +597,14 @@ def distance_at(time_s): return t, pos_2d, heading_true, accel_body, gyro_body, mag_body, expected_steps -def add_sensor_noise(accel_body, gyro_body, mag_body, dt, - imu_params: IMUNoiseParams, seed: int = DEFAULT_SEED): +def add_sensor_noise( + accel_body, + gyro_body, + mag_body, + dt, + imu_params: IMUNoiseParams, + seed: int = DEFAULT_SEED, +): """Add realistic sensor noise with explicit units. Args: @@ -595,7 +638,7 @@ def add_sensor_noise(accel_body, gyro_body, mag_body, dt, # Add disturbances at specific times (simulating steel structures) disturb_intervals = [(20, 30), (70, 80)] # seconds for start, end in disturb_intervals: - mask = (np.arange(N)*dt >= start) & (np.arange(N)*dt < end) + mask = (np.arange(N) * dt >= start) & (np.arange(N) * dt < end) mag_disturbance[mask] = rng.standard_normal((np.sum(mask), 3)) * 0.3 gyro_meas = gyro_body + gyro_bias + gyro_noise @@ -608,9 +651,9 @@ def add_sensor_noise(accel_body, gyro_body, mag_body, dt, def run_pdr_gyro_heading(t, accel_meas, gyro_meas, height=1.75, step_model="book"): """ Run PDR with gyro-integrated heading (drifts). - + Uses proper peak detection (Eqs. 6.46-6.47) instead of threshold crossing. - + Args: t: Time array accel_meas: Accelerometer measurements @@ -632,7 +675,7 @@ def run_pdr_gyro_heading(t, accel_meas, gyro_meas, height=1.75, step_model="book g=9.81, min_peak_height=1.0, # 1 m/s² above gravity min_peak_distance=0.3, # 0.3s between steps (max ~3.3 steps/s) - lowpass_cutoff=5.0 # 5 Hz low-pass filter + lowpass_cutoff=5.0, # 5 Hz low-pass filter ) step_count = len(step_indices) @@ -646,7 +689,7 @@ def run_pdr_gyro_heading(t, accel_meas, gyro_meas, height=1.75, step_model="book # Process time series for k in range(1, N): # Integrate gyro heading - heading_est[k] = integrate_gyro_heading(heading_est[k-1], gyro_meas[k, 2], dt) + heading_est[k] = integrate_gyro_heading(heading_est[k - 1], gyro_meas[k, 2], dt) heading_est[k] = wrap_heading(heading_est[k]) # Update position on step events @@ -664,19 +707,21 @@ def run_pdr_gyro_heading(t, accel_meas, gyro_meas, height=1.75, step_model="book L = compute_step_length(height, f_step, model=step_model) # Update position (Eq. 6.50) - pos_est[k] = pdr_step_update(pos_est[k-1], L, heading_est[k-1]) + pos_est[k] = pdr_step_update(pos_est[k - 1], L, heading_est[k - 1]) else: - pos_est[k] = pos_est[k-1] + pos_est[k] = pos_est[k - 1] return pos_est, heading_est, step_count -def run_pdr_mag_heading(t, accel_meas, gyro_meas, mag_meas, height=1.75, step_model="book"): +def run_pdr_mag_heading( + t, accel_meas, gyro_meas, mag_meas, height=1.75, step_model="book" +): """ Run PDR with magnetometer heading (absolute but noisy). - + Uses proper peak detection (Eqs. 6.46-6.47) instead of threshold crossing. - + Args: t: Time array accel_meas: Accelerometer measurements @@ -699,7 +744,7 @@ def run_pdr_mag_heading(t, accel_meas, gyro_meas, mag_meas, height=1.75, step_mo g=9.81, min_peak_height=1.0, # 1 m/s² above gravity min_peak_distance=0.3, # 0.3s between steps - lowpass_cutoff=5.0 # 5 Hz low-pass filter + lowpass_cutoff=5.0, # 5 Hz low-pass filter ) step_count = len(step_indices) @@ -708,7 +753,9 @@ def run_pdr_mag_heading(t, accel_meas, gyro_meas, mag_meas, height=1.75, step_mo # Initialize heading from magnetometer (absolute reference) # NOTE FOR STUDENTS: Unlike gyro heading which can start anywhere, magnetometer # provides an absolute heading reference. This is the proper way to initialize! - heading_est[0] = mag_heading(mag_meas[0], roll=0.0, pitch=0.0, declination=0.0) # Absolute heading + heading_est[0] = mag_heading( + mag_meas[0], roll=0.0, pitch=0.0, declination=0.0 + ) # Absolute heading # Process time series for k in range(1, N): @@ -731,14 +778,16 @@ def run_pdr_mag_heading(t, accel_meas, gyro_meas, mag_meas, height=1.75, step_mo L = compute_step_length(height, f_step, model=step_model) # Update position (Eq. 6.50) - pos_est[k] = pdr_step_update(pos_est[k-1], L, heading_est[k-1]) + pos_est[k] = pdr_step_update(pos_est[k - 1], L, heading_est[k - 1]) else: - pos_est[k] = pos_est[k-1] + pos_est[k] = pos_est[k - 1] return pos_est, heading_est, step_count -def plot_results(t, pos_true, pos_gyro, pos_mag, heading_true, heading_gyro, heading_mag, figs_dir): +def plot_results( + t, pos_true, pos_gyro, pos_mag, heading_true, heading_gyro, heading_mag, figs_dir +): """Generate publication-quality plots.""" error_gyro = np.linalg.norm(pos_gyro - pos_true, axis=1) @@ -746,75 +795,93 @@ def plot_results(t, pos_true, pos_gyro, pos_mag, heading_true, heading_gyro, hea # Figure 1: Trajectory fig1, ax = plt.subplots(figsize=(12, 8)) - ax.plot(pos_true[:, 0], pos_true[:, 1], 'k-', linewidth=3, label='True Path') - ax.plot(pos_gyro[:, 0], pos_gyro[:, 1], 'r--', linewidth=2, alpha=0.7, label='PDR (Gyro Heading)') - ax.plot(pos_mag[:, 0], pos_mag[:, 1], 'b-', linewidth=2, label='PDR (Mag Heading)') - ax.scatter(0, 0, c='g', s=150, marker='o', label='Start', zorder=5) - ax.set_xlabel('East [m]', fontsize=12) - ax.set_ylabel('North [m]', fontsize=12) - ax.set_title('PDR Example: Corridor Walk (Rectangular Path)', fontsize=14, fontweight='bold') + ax.plot(pos_true[:, 0], pos_true[:, 1], "k-", linewidth=3, label="True Path") + ax.plot( + pos_gyro[:, 0], + pos_gyro[:, 1], + "r--", + linewidth=2, + alpha=0.7, + label="PDR (Gyro Heading)", + ) + ax.plot(pos_mag[:, 0], pos_mag[:, 1], "b-", linewidth=2, label="PDR (Mag Heading)") + ax.scatter(0, 0, c="g", s=150, marker="o", label="Start", zorder=5) + ax.set_xlabel("East [m]", fontsize=12) + ax.set_ylabel("North [m]", fontsize=12) + ax.set_title( + "PDR Example: Corridor Walk (Rectangular Path)", fontsize=14, fontweight="bold" + ) ax.legend(fontsize=11) ax.grid(True, alpha=0.3) - ax.axis('equal') + ax.axis("equal") plt.tight_layout() - paths = save_figure(fig1, figs_dir, 'pdr_trajectory') + paths = save_figure(fig1, figs_dir, "pdr_trajectory") print(f" [OK] Saved: {paths[0]}") # Figure 2: Heading comparison fig2, (ax1, ax2) = plt.subplots(2, 1, figsize=(12, 8), sharex=True) - ax1.plot(t, np.rad2deg(heading_true), 'k-', linewidth=2, label='True Heading') - ax1.plot(t, np.rad2deg(heading_gyro), 'r--', linewidth=2, alpha=0.7, label='Gyro Integrated') - ax1.plot(t, np.rad2deg(heading_mag), 'b-', linewidth=1.5, alpha=0.7, label='Magnetometer') - ax1.set_ylabel('Heading [deg]', fontsize=12) - ax1.set_title('PDR Example: Heading Comparison', fontsize=14, fontweight='bold') + ax1.plot(t, np.rad2deg(heading_true), "k-", linewidth=2, label="True Heading") + ax1.plot( + t, + np.rad2deg(heading_gyro), + "r--", + linewidth=2, + alpha=0.7, + label="Gyro Integrated", + ) + ax1.plot( + t, np.rad2deg(heading_mag), "b-", linewidth=1.5, alpha=0.7, label="Magnetometer" + ) + ax1.set_ylabel("Heading [deg]", fontsize=12) + ax1.set_title("PDR Example: Heading Comparison", fontsize=14, fontweight="bold") ax1.legend(fontsize=10) ax1.grid(True, alpha=0.3) heading_error_gyro = np.abs(wrap_heading(heading_gyro - heading_true)) heading_error_mag = np.abs(wrap_heading(heading_mag - heading_true)) - ax2.plot(t, np.rad2deg(heading_error_gyro), 'r-', linewidth=2, label='Gyro Error') - ax2.plot(t, np.rad2deg(heading_error_mag), 'b-', linewidth=2, label='Mag Error') - ax2.set_xlabel('Time [s]', fontsize=12) - ax2.set_ylabel('Heading Error [deg]', fontsize=12) + ax2.plot(t, np.rad2deg(heading_error_gyro), "r-", linewidth=2, label="Gyro Error") + ax2.plot(t, np.rad2deg(heading_error_mag), "b-", linewidth=2, label="Mag Error") + ax2.set_xlabel("Time [s]", fontsize=12) + ax2.set_ylabel("Heading Error [deg]", fontsize=12) ax2.legend(fontsize=10) ax2.grid(True, alpha=0.3) ax2.set_xlim([0, t[-1]]) plt.tight_layout() - paths = save_figure(fig2, figs_dir, 'pdr_heading') + paths = save_figure(fig2, figs_dir, "pdr_heading") print(f" [OK] Saved: {paths[0]}") # Figure 3: Position error fig3, ax = plt.subplots(figsize=(12, 6)) - ax.plot(t, error_gyro, 'r-', linewidth=2, label='PDR (Gyro Heading)') - ax.plot(t, error_mag, 'b-', linewidth=2, label='PDR (Mag Heading)') - ax.set_xlabel('Time [s]', fontsize=12) - ax.set_ylabel('Position Error [m]', fontsize=12) - ax.set_title('PDR Example: Position Error vs Time', fontsize=14, fontweight='bold') + ax.plot(t, error_gyro, "r-", linewidth=2, label="PDR (Gyro Heading)") + ax.plot(t, error_mag, "b-", linewidth=2, label="PDR (Mag Heading)") + ax.set_xlabel("Time [s]", fontsize=12) + ax.set_ylabel("Position Error [m]", fontsize=12) + ax.set_title("PDR Example: Position Error vs Time", fontsize=14, fontweight="bold") ax.legend(fontsize=11) ax.grid(True, alpha=0.3) ax.set_xlim([0, t[-1]]) plt.tight_layout() - paths = save_figure(fig3, figs_dir, 'pdr_error') + paths = save_figure(fig3, figs_dir, "pdr_error") print(f" [OK] Saved: {paths[0]}") - plt.close('all') + plt.close("all") return error_gyro, error_mag def run_with_inline_data(lat_deg: float = 45.0, step_model: str = "book"): """Run with inline generated data (original behavior). - + Args: lat_deg: Latitude in degrees step_model: Step-length model selection """ - print("\n" + "="*70) + print("\n" + "=" * 70) print("Chapter 6: Pedestrian Dead Reckoning (PDR) - Step-and-Heading") print("(Using inline generated data)") - print("="*70) + print("=" * 70) print("\nDemonstrates the critical importance of heading accuracy in PDR.") print("Key equations: 6.46-6.50 (step detection, length, position update)\n") @@ -829,7 +896,7 @@ def run_with_inline_data(lat_deg: float = 45.0, step_model: str = "book"): gyro_rrw_rad_s_sqrt_s=0.0, accel_bias_mps2=units.mg_to_mps2(10.0), accel_vrw_mps_sqrt_s=units.mps_per_sqrt_hour_to_mps_per_sqrt_sec(0.01), - grade='consumer (high gyro drift)' + grade="consumer (high gyro drift)", ) print("Configuration:") @@ -843,8 +910,8 @@ def run_with_inline_data(lat_deg: float = 45.0, step_model: str = "book"): print() print("Generating trajectory with correct IMU forward model...") - t, pos_true, heading_true, accel_body, gyro_body, mag_body, expected_steps = generate_corridor_walk( - duration, dt, step_freq=2.0, frame=frame + t, pos_true, heading_true, accel_body, gyro_body, mag_body, expected_steps = ( + generate_corridor_walk(duration, dt, step_freq=2.0, frame=frame) ) total_dist = np.sum(np.linalg.norm(np.diff(pos_true, axis=0), axis=1)) @@ -852,39 +919,56 @@ def run_with_inline_data(lat_deg: float = 45.0, step_model: str = "book"): print(f" Expected steps: {expected_steps} (at 2.0 Hz step frequency)") print("\nAdding sensor noise...") - accel_meas, gyro_meas, mag_meas = add_sensor_noise(accel_body, gyro_body, mag_body, dt, imu_params) + accel_meas, gyro_meas, mag_meas = add_sensor_noise( + accel_body, gyro_body, mag_body, dt, imu_params + ) print(f"\nRunning PDR with gyro heading (step model: {step_model})...") start = time.time() - pos_gyro, heading_gyro, steps_gyro = run_pdr_gyro_heading(t, accel_meas, gyro_meas, height, step_model=step_model) + pos_gyro, heading_gyro, steps_gyro = run_pdr_gyro_heading( + t, accel_meas, gyro_meas, height, step_model=step_model + ) print(f" Time: {time.time()-start:.3f} s, Steps detected: {steps_gyro}") print(f"\nRunning PDR with magnetometer heading (step model: {step_model})...") start = time.time() - pos_mag, heading_mag, steps_mag = run_pdr_mag_heading(t, accel_meas, gyro_meas, mag_meas, height, step_model=step_model) + pos_mag, heading_mag, steps_mag = run_pdr_mag_heading( + t, accel_meas, gyro_meas, mag_meas, height, step_model=step_model + ) print(f" Time: {time.time()-start:.3f} s, Steps detected: {steps_mag}") - figs_dir = Path(__file__).parent / 'figs' + figs_dir = Path(__file__).parent / "figs" figs_dir.mkdir(exist_ok=True) print("\nGenerating plots...") error_gyro, error_mag = plot_results( - t, pos_true, pos_gyro, pos_mag, heading_true, heading_gyro, heading_mag, figs_dir + t, + pos_true, + pos_gyro, + pos_mag, + heading_true, + heading_gyro, + heading_mag, + figs_dir, ) # Metrics rmse_gyro = np.sqrt(np.mean(error_gyro**2)) rmse_mag = np.sqrt(np.mean(error_mag**2)) - print("\n" + "="*70) + print("\n" + "=" * 70) print("RESULTS") - print("="*70) + print("=" * 70) print("PDR (Gyro Heading - drifts unbounded):") - print(f" Final error: {error_gyro[-1]:.1f} m ({error_gyro[-1]/total_dist*100:.1f}% of distance)") + print( + f" Final error: {error_gyro[-1]:.1f} m ({error_gyro[-1]/total_dist*100:.1f}% of distance)" + ) print(f" RMSE: {rmse_gyro:.1f} m") print() print("PDR (Magnetometer Heading - absolute but noisy):") - print(f" Final error: {error_mag[-1]:.1f} m ({error_mag[-1]/total_dist*100:.1f}% of distance)") + print( + f" Final error: {error_mag[-1]:.1f} m ({error_mag[-1]/total_dist*100:.1f}% of distance)" + ) print(f" RMSE: {rmse_mag:.1f} m") print() # Decompose the error before attributing it. Most of what these two runs @@ -910,7 +994,9 @@ def run_with_inline_data(lat_deg: float = 45.0, step_model: str = "book"): # Measured from the trajectory rather than assumed. An earlier version of # this budget divided the distance by an assumed 0.5 m gait to get a # "true" step count, which inverted the attribution entirely. - walk_speed = np.linalg.norm(np.diff(pos_true[:, :2], axis=0), axis=1) / (t[1] - t[0]) + walk_speed = np.linalg.norm(np.diff(pos_true[:, :2], axis=0), axis=1) / ( + t[1] - t[0] + ) moving = walk_speed > 0.05 walking_time = float(np.sum(moving)) * (t[1] - t[0]) true_step_len = float(np.mean(walk_speed[moving])) / 2.0 @@ -919,35 +1005,55 @@ def run_with_inline_data(lat_deg: float = 45.0, step_model: str = "book"): print(" Where the error comes from, now that the trajectory is one a") print(" pedestrian could actually walk:") - print(f" 1. Step length, and that is essentially all of it. PDR " - f"believes it walked {stepped:.1f} m against a true {walked:.1f} m, " - f"{100 * (stepped / walked - 1):+.0f}%.") - print(f" Detection is sound -- {steps_gyro} steps found against " - f"{true_steps} taken, within {abs(steps_gyro - true_steps)} -- so " - f"the gap is the model: Eq. (6.49) returns " - f"{stepped / max(steps_gyro, 1):.3f} m") - print(f" per step for a {height:.2f} m walker at this cadence while " - f"the simulated gait is {true_step_len:.3f} m. Step length is the " - f"parameter PDR is") - print(" most sensitive to, and it is the one a real deployment has " - "to calibrate per user.") - print(f" 2. Heading. The gyro ends {heading_drift_deg:.1f} deg from " - f"truth, which is its realised bias integrated over {t[-1]:.0f} s " - f"and nothing else.") - print(" That is what drift at this grade actually looks like. It " - "used to read 163 deg and none of it was drift: this generator " - "turned each") - print(" corner 90 deg inside one 0.01 s sample -- 9000 deg/s -- " - "which the gyro forward model cannot represent, so the *true* gyro") - print(" integrated to 162 deg over a lap whose heading comes round " - "to 360. The estimator was faithfully reporting a rotation the " - "data never") - print(f" contained. Chapter 8 had the identical defect at the " - f"identical 9000 deg/s. The corners are rounded now " - f"({CORNER_RADIUS_M:.0f} m, {np.degrees(1.4 / CORNER_RADIUS_M):.0f} deg/s),") - print(f" and the gait oscillation no longer runs through the " - f"{t[-1] - walking_time:.0f} s of standing still that was worth 73 " - f"phantom steps. Final error: 80.7 m -> {final_gyro:.1f} m.") + print( + f" 1. Step length, and that is essentially all of it. PDR " + f"believes it walked {stepped:.1f} m against a true {walked:.1f} m, " + f"{100 * (stepped / walked - 1):+.0f}%." + ) + print( + f" Detection is sound -- {steps_gyro} steps found against " + f"{true_steps} taken, within {abs(steps_gyro - true_steps)} -- so " + f"the gap is the model: Eq. (6.49) returns " + f"{stepped / max(steps_gyro, 1):.3f} m" + ) + print( + f" per step for a {height:.2f} m walker at this cadence while " + f"the simulated gait is {true_step_len:.3f} m. Step length is the " + f"parameter PDR is" + ) + print( + " most sensitive to, and it is the one a real deployment has " + "to calibrate per user." + ) + print( + f" 2. Heading. The gyro ends {heading_drift_deg:.1f} deg from " + f"truth, which is its realised bias integrated over {t[-1]:.0f} s " + f"and nothing else." + ) + print( + " That is what drift at this grade actually looks like. It " + "used to read 163 deg and none of it was drift: this generator " + "turned each" + ) + print( + " corner 90 deg inside one 0.01 s sample -- 9000 deg/s -- " + "which the gyro forward model cannot represent, so the *true* gyro" + ) + print( + " integrated to 162 deg over a lap whose heading comes round " + "to 360. The estimator was faithfully reporting a rotation the " + "data never" + ) + print( + f" contained. Chapter 8 had the identical defect at the " + f"identical 9000 deg/s. The corners are rounded now " + f"({CORNER_RADIUS_M:.0f} m, {np.degrees(1.4 / CORNER_RADIUS_M):.0f} deg/s)," + ) + print( + f" and the gait oscillation no longer runs through the " + f"{t[-1] - walking_time:.0f} s of standing still that was worth 73 " + f"phantom steps. Final error: 80.7 m -> {final_gyro:.1f} m." + ) print() # Report where the figures actually went. save_figure resolves this # internally through IPIN_FIGS_DIR, so printing the requested path made @@ -955,7 +1061,7 @@ def run_with_inline_data(lat_deg: float = 45.0, step_model: str = "book"): # whenever the variable was set -- which is every test run. print(f"Figures saved to: {resolve_figs_dir(figs_dir)}/") print() - print("="*70) + print("=" * 70) print("KEY INSIGHT: Check that a simulated truth is achievable before") print(" reading an estimator's error as the estimator's. This") print(" example reported 80.7 m and blamed unbounded gyro") @@ -972,7 +1078,7 @@ def run_with_inline_data(lat_deg: float = 45.0, step_model: str = "book"): print(" residual. Step length is what a real deployment must") print(" calibrate per user; heading matters too, which is why") print(" best practice is still a complementary filter.") - print("="*70) + print("=" * 70) print("\nTip: Run with --data ch6_pdr_corridor_walk to use pre-generated dataset") @@ -991,24 +1097,32 @@ def main(): # Specify pedestrian height python example_pdr.py --data ch6_pdr_corridor_walk --height 1.80 - """ + """, ) parser.add_argument( - "--data", type=str, default=None, - help="Dataset name or path (e.g., 'ch6_pdr_corridor_walk' or full path)" + "--data", + type=str, + default=None, + help="Dataset name or path (e.g., 'ch6_pdr_corridor_walk' or full path)", ) parser.add_argument( - "--height", type=float, default=1.75, - help="Pedestrian height in meters (default: 1.75)" + "--height", + type=float, + default=1.75, + help="Pedestrian height in meters (default: 1.75)", ) parser.add_argument( - "--latitude", type=float, default=45.0, - help="Latitude in degrees for gravity model (Eq. 6.8, default: 45.0)" + "--latitude", + type=float, + default=45.0, + help="Latitude in degrees for gravity model (Eq. 6.8, default: 45.0)", ) parser.add_argument( - "--step-model", type=str, default="book", + "--step-model", + type=str, + default="book", choices=["book", "power_law"], - help="Step-length model: 'book' (Eq. 6.49, default) or 'power_law' (old)" + help="Step-length model: 'book' (Eq. 6.49, default) or 'power_law' (old)", ) args = parser.parse_args() @@ -1019,7 +1133,9 @@ def main(): if not data_path.exists(): data_path = resolve_data_path(Path("data/sim") / args.data) if not data_path.exists(): - print(f"Error: Dataset not found at '{args.data}' or 'data/sim/{args.data}'") + print( + f"Error: Dataset not found at '{args.data}' or 'data/sim/{args.data}'" + ) print("\nAvailable datasets:") sim_dir = resolve_data_path(Path("data/sim")) if sim_dir.exists(): @@ -1028,11 +1144,15 @@ def main(): print(f" - {d.name}") return - run_with_dataset(str(data_path), height=args.height, lat_deg=args.latitude, step_model=args.step_model) + run_with_dataset( + str(data_path), + height=args.height, + lat_deg=args.latitude, + step_model=args.step_model, + ) else: run_with_inline_data(lat_deg=args.latitude, step_model=args.step_model) if __name__ == "__main__": main() - diff --git a/ch6_dead_reckoning/example_wheel_odometry.py b/ch6_dead_reckoning/example_wheel_odometry.py index ea00b2e..17410c8 100644 --- a/ch6_dead_reckoning/example_wheel_odometry.py +++ b/ch6_dead_reckoning/example_wheel_odometry.py @@ -57,10 +57,10 @@ ) -def generate_vehicle_trajectory(shape='square', duration=80.0, dt=0.01): +def generate_vehicle_trajectory(shape="square", duration=80.0, dt=0.01): """ Generate vehicle trajectory (square or circle). - + Returns: t, pos_true, vel_true, quat_true, wheel_speed_true, gyro_true """ t = np.arange(0, duration, dt) @@ -69,10 +69,12 @@ def generate_vehicle_trajectory(shape='square', duration=80.0, dt=0.01): pos_true = np.zeros((N, 3)) vel_true = np.zeros((N, 3)) quat_true = np.zeros((N, 4)) - wheel_speed_true = np.zeros((N, 3)) # Speed frame: [0, v_forward, 0] (book convention) + wheel_speed_true = np.zeros( + (N, 3) + ) # Speed frame: [0, v_forward, 0] (book convention) gyro_true = np.zeros((N, 3)) - if shape == 'square': + if shape == "square": # Square: 20m sides, 5 m/s speed, 90° turns side_length = 20.0 v_drive = 5.0 # m/s @@ -99,38 +101,45 @@ def generate_vehicle_trajectory(shape='square', duration=80.0, dt=0.01): gyro_true[k, 2] = 0 else: # Turning wheel_speed_true[k] = np.array([0, 0, 0]) # Stop to turn - gyro_true[k, 2] = np.pi/2 / turn_time # 90°/2s + gyro_true[k, 2] = np.pi / 2 / turn_time # 90°/2s # Update state if k > 0: current_heading += gyro_true[k, 2] * dt - v_map = wheel_speed_true[k, 1] * np.array([np.cos(current_heading), np.sin(current_heading), 0]) + v_map = wheel_speed_true[k, 1] * np.array( + [np.cos(current_heading), np.sin(current_heading), 0] + ) current_pos += v_map * dt pos_true[k] = current_pos - vel_true[k, :2] = wheel_speed_true[k, 1] * np.array([np.cos(current_heading), np.sin(current_heading)]) + vel_true[k, :2] = wheel_speed_true[k, 1] * np.array( + [np.cos(current_heading), np.sin(current_heading)] + ) # Quaternion (yaw only) - quat_true[k] = np.array([np.cos(current_heading/2), 0, 0, np.sin(current_heading/2)]) + quat_true[k] = np.array( + [np.cos(current_heading / 2), 0, 0, np.sin(current_heading / 2)] + ) else: # circle - omega = 2*np.pi / duration # One full circle + omega = 2 * np.pi / duration # One full circle radius = 15.0 v_drive = radius * omega for k in range(N): angle = omega * t[k] - pos_true[k] = np.array([radius*np.cos(angle), radius*np.sin(angle), 0]) + pos_true[k] = np.array([radius * np.cos(angle), radius * np.sin(angle), 0]) vel_true[k] = v_drive * np.array([-np.sin(angle), np.cos(angle), 0]) wheel_speed_true[k] = np.array([0, v_drive, 0]) # Book: y=forward gyro_true[k, 2] = omega - quat_true[k] = np.array([np.cos(angle/2), 0, 0, np.sin(angle/2)]) + quat_true[k] = np.array([np.cos(angle / 2), 0, 0, np.sin(angle / 2)]) return t, pos_true, vel_true, quat_true, wheel_speed_true, gyro_true -def add_wheel_noise(wheel_speed_true, gyro_true, add_slip=False, - slip_intervals=None, seed=DEFAULT_SEED): +def add_wheel_noise( + wheel_speed_true, gyro_true, add_slip=False, slip_intervals=None, seed=DEFAULT_SEED +): """Add wheel encoder noise and optional slip. Args: @@ -163,7 +172,7 @@ def add_wheel_noise(wheel_speed_true, gyro_true, add_slip=False, # Add wheel slip during turns if add_slip and slip_intervals: for start, end in slip_intervals: - mask = (np.arange(N)*0.01 >= start) & (np.arange(N)*0.01 < end) + mask = (np.arange(N) * 0.01 >= start) & (np.arange(N) * 0.01 < end) # During slip, wheel speed overestimates actual motion wheel_meas[mask, 1] *= 1.3 # 30% overestimate (y-component) @@ -186,8 +195,8 @@ def run_wheel_odometry(t, wheel_speed, gyro, initial_state, lever_arm): p = wheel_odom_update( p=p, q=q, - v_s=wheel_speed[k-1], - omega_a=gyro[k-1], + v_s=wheel_speed[k - 1], + omega_a=gyro[k - 1], lever_arm_a=lever_arm, dt=dt, C_S_A=C_SPEED_TO_BODY, @@ -195,12 +204,26 @@ def run_wheel_odometry(t, wheel_speed, gyro, initial_state, lever_arm): # Update quaternion q_new = q.copy() - dq = 0.5 * dt * np.array([ - -q[1]*gyro[k-1,0] - q[2]*gyro[k-1,1] - q[3]*gyro[k-1,2], - q[0]*gyro[k-1,0] + q[2]*gyro[k-1,2] - q[3]*gyro[k-1,1], - q[0]*gyro[k-1,1] - q[1]*gyro[k-1,2] + q[3]*gyro[k-1,0], - q[0]*gyro[k-1,2] + q[1]*gyro[k-1,1] - q[2]*gyro[k-1,0] - ]) + dq = ( + 0.5 + * dt + * np.array( + [ + -q[1] * gyro[k - 1, 0] + - q[2] * gyro[k - 1, 1] + - q[3] * gyro[k - 1, 2], + q[0] * gyro[k - 1, 0] + + q[2] * gyro[k - 1, 2] + - q[3] * gyro[k - 1, 1], + q[0] * gyro[k - 1, 1] + - q[1] * gyro[k - 1, 2] + + q[3] * gyro[k - 1, 0], + q[0] * gyro[k - 1, 2] + + q[1] * gyro[k - 1, 1] + - q[2] * gyro[k - 1, 0], + ] + ) + ) q = q_new + dq q = q / np.linalg.norm(q) @@ -217,35 +240,48 @@ def plot_results(t, pos_true, pos_odom, pos_odom_slip, figs_dir): # Figure 1: Trajectory fig1, ax = plt.subplots(figsize=(10, 10)) - ax.plot(pos_true[:, 0], pos_true[:, 1], 'k-', linewidth=3, label='True Trajectory') - ax.plot(pos_odom[:, 0], pos_odom[:, 1], 'b--', linewidth=2, label='Wheel Odom (no slip)') - ax.plot(pos_odom_slip[:, 0], pos_odom_slip[:, 1], 'r--', linewidth=2, alpha=0.7, label='Wheel Odom (with slip)') - ax.scatter(0, 0, c='g', s=150, marker='o', label='Start', zorder=5) - ax.set_xlabel('East [m]', fontsize=12) - ax.set_ylabel('North [m]', fontsize=12) - ax.set_title('Wheel Odometry Example: Square Path', fontsize=14, fontweight='bold') + ax.plot(pos_true[:, 0], pos_true[:, 1], "k-", linewidth=3, label="True Trajectory") + ax.plot( + pos_odom[:, 0], pos_odom[:, 1], "b--", linewidth=2, label="Wheel Odom (no slip)" + ) + ax.plot( + pos_odom_slip[:, 0], + pos_odom_slip[:, 1], + "r--", + linewidth=2, + alpha=0.7, + label="Wheel Odom (with slip)", + ) + ax.scatter(0, 0, c="g", s=150, marker="o", label="Start", zorder=5) + ax.set_xlabel("East [m]", fontsize=12) + ax.set_ylabel("North [m]", fontsize=12) + ax.set_title("Wheel Odometry Example: Square Path", fontsize=14, fontweight="bold") ax.legend(fontsize=11) ax.grid(True, alpha=0.3) - ax.axis('equal') + ax.axis("equal") plt.tight_layout() - paths = save_figure(fig1, figs_dir, 'wheel_odom_trajectory') + paths = save_figure(fig1, figs_dir, "wheel_odom_trajectory") print(f" [OK] Saved: {paths[0]}") # Figure 2: Error fig2, ax = plt.subplots(figsize=(12, 6)) - ax.plot(t, error_odom, 'b-', linewidth=2, label='No Slip') - ax.plot(t, error_slip, 'r-', linewidth=2, label='With Slip') - ax.set_xlabel('Time [s]', fontsize=12) - ax.set_ylabel('Position Error [m]', fontsize=12) - ax.set_title('Wheel Odometry Example: Position Error (Bounded Drift)', fontsize=14, fontweight='bold') + ax.plot(t, error_odom, "b-", linewidth=2, label="No Slip") + ax.plot(t, error_slip, "r-", linewidth=2, label="With Slip") + ax.set_xlabel("Time [s]", fontsize=12) + ax.set_ylabel("Position Error [m]", fontsize=12) + ax.set_title( + "Wheel Odometry Example: Position Error (Bounded Drift)", + fontsize=14, + fontweight="bold", + ) ax.legend(fontsize=11) ax.grid(True, alpha=0.3) ax.set_xlim([0, t[-1]]) plt.tight_layout() - paths = save_figure(fig2, figs_dir, 'wheel_odom_error') + paths = save_figure(fig2, figs_dir, "wheel_odom_error") print(f" [OK] Saved: {paths[0]}") - plt.close('all') + plt.close("all") return error_odom, error_slip @@ -261,9 +297,9 @@ def main(): formatter_class=argparse.RawDescriptionHelpFormatter, ).parse_args() - print("\n" + "="*70) + print("\n" + "=" * 70) print("Chapter 6: Wheel Odometry Dead Reckoning for Vehicles") - print("="*70) + print("=" * 70) print("\nDemonstrates bounded drift and sensitivity to wheel slip.") print("Key equations: 6.11-6.15 (lever arm, frame transform, position)\n") @@ -276,7 +312,9 @@ def main(): print(" Lever Arm: [1.5, 0, -0.3] m\n") print("Generating trajectory...") - t, pos_true, vel_true, quat_true, wheel_true, gyro_true = generate_vehicle_trajectory('square', duration, dt) + t, pos_true, vel_true, quat_true, wheel_true, gyro_true = ( + generate_vehicle_trajectory("square", duration, dt) + ) total_dist = np.sum(np.linalg.norm(np.diff(pos_true, axis=0), axis=1)) print(f" Total distance: {total_dist:.1f} m") @@ -295,7 +333,9 @@ def main(): # identical errors to three significant figures while the module docstring # advertised "sensitivity to wheel slip". slip_intervals = [(2, 4), (8, 10), (14, 16), (20, 22)] # On the straights - wheel_slip, gyro_slip = add_wheel_noise(wheel_true, gyro_true, add_slip=True, slip_intervals=slip_intervals) + wheel_slip, gyro_slip = add_wheel_noise( + wheel_true, gyro_true, add_slip=True, slip_intervals=slip_intervals + ) # Initial state initial = NavStateQPVP(q=quat_true[0], v=vel_true[0], p=pos_true[0]) @@ -311,24 +351,30 @@ def main(): pos_odom_slip = run_wheel_odometry(t, wheel_slip, gyro_slip, initial, lever_arm) print(f" Time: {time.time()-start:.3f} s") - figs_dir = Path(__file__).parent / 'figs' + figs_dir = Path(__file__).parent / "figs" figs_dir.mkdir(exist_ok=True) print("\nGenerating plots...") - error_odom, error_slip = plot_results(t, pos_true, pos_odom, pos_odom_slip, figs_dir) + error_odom, error_slip = plot_results( + t, pos_true, pos_odom, pos_odom_slip, figs_dir + ) rmse_odom = np.sqrt(np.mean(error_odom**2)) rmse_slip = np.sqrt(np.mean(error_slip**2)) - print("\n" + "="*70) + print("\n" + "=" * 70) print("RESULTS") - print("="*70) + print("=" * 70) print("Wheel Odometry (no slip):") - print(f" Final error: {error_odom[-1]:.2f} m ({error_odom[-1]/total_dist*100:.1f}% of distance)") + print( + f" Final error: {error_odom[-1]:.2f} m ({error_odom[-1]/total_dist*100:.1f}% of distance)" + ) print(f" RMSE: {rmse_odom:.2f} m") print() print("Wheel Odometry (with 30% slip on four 2 s straights):") - print(f" Final error: {error_slip[-1]:.2f} m ({error_slip[-1]/total_dist*100:.1f}% of distance)") + print( + f" Final error: {error_slip[-1]:.2f} m ({error_slip[-1]/total_dist*100:.1f}% of distance)" + ) print(f" RMSE: {rmse_slip:.2f} m") print() # Read the RMSE, not the final error. This route is a closed square, so @@ -339,20 +385,21 @@ def main(): print(" Slip cost, read properly:") print(f" RMSE {rmse_odom:.2f} m -> {rmse_slip:.2f} m") print(f" peak track separation {np.abs(error_slip - error_odom).max():.2f} m") - print(f" (4 windows x 2 s x {SLIP_SPEED_HINT:.1f} m/s x 30% = " - f"{4 * 2 * SLIP_SPEED_HINT * 0.3:.1f} m of phantom travel, as injected)") + print( + f" (4 windows x 2 s x {SLIP_SPEED_HINT:.1f} m/s x 30% = " + f"{4 * 2 * SLIP_SPEED_HINT * 0.3:.1f} m of phantom travel, as injected)" + ) print() print(f"Figures saved to: {resolve_figs_dir(figs_dir)}/") print() - print("="*70) + print("=" * 70) print("KEY INSIGHT: Wheel odometry drift is BOUNDED!") print(" Errors ~1-5% of distance (vs unbounded for IMU).") print(" BUT very sensitive to wheel slip (turns, ice, etc).") - print("="*70) + print("=" * 70) print() show_figures_if_requested() if __name__ == "__main__": main() - diff --git a/ch6_dead_reckoning/example_zupt.py b/ch6_dead_reckoning/example_zupt.py index a171d55..93f0bed 100644 --- a/ch6_dead_reckoning/example_zupt.py +++ b/ch6_dead_reckoning/example_zupt.py @@ -54,14 +54,14 @@ def generate_walking_trajectory( """ Generate walking trajectory with periodic stops (stance phases). Uses correct IMU forward model. - + Args: duration: Total duration [s]. dt: Time step [s]. step_freq: Steps per second when walking. step_length: Length per step [m]. frame: Frame convention (default: ENU). - + Returns: Tuple of (t, pos_true, vel_true, quat_true, accel_body, gyro_body, stance_mask). """ @@ -128,12 +128,9 @@ def generate_walking_trajectory( yaw[k] = yaw[k - 1] # Maintain previous heading during stance # Convert to quaternions (scalar-first, body-to-map) - quat_true = np.column_stack([ - np.cos(yaw / 2), - np.zeros(N), - np.zeros(N), - np.sin(yaw / 2) - ]) + quat_true = np.column_stack( + [np.cos(yaw / 2), np.zeros(N), np.zeros(N), np.sin(yaw / 2)] + ) # Generate IMU measurements using correct forward model accel_body, gyro_body = generate_imu_from_trajectory( @@ -142,7 +139,7 @@ def generate_walking_trajectory( quat_b_to_m=quat_true, dt=dt, frame=frame, - g=9.81 + g=9.81, ) return t, pos_true, vel_true, quat_true, accel_body, gyro_body, stance_mask @@ -182,19 +179,27 @@ def run_imu_only(t, accel_meas, gyro_meas, initial_state, frame): pos_est[0], vel_est[0] = p, v for k in range(1, N): - q, v, p = strapdown_update(q, v, p, gyro_meas[k-1], accel_meas[k-1], dt, frame=frame) + q, v, p = strapdown_update( + q, v, p, gyro_meas[k - 1], accel_meas[k - 1], dt, frame=frame + ) pos_est[k], vel_est[k] = p, v return pos_est, vel_est def run_imu_with_zupt( - t, accel_meas, gyro_meas, initial_state, frame, imu_params, - window_size=10, gamma=1e6 + t, + accel_meas, + gyro_meas, + initial_state, + frame, + imu_params, + window_size=10, + gamma=1e6, ): """ Run IMU with ZUPT corrections using windowed detector (Eq. 6.44). - + Args: t: Time array [s]. accel_meas: Measured acceleration [m/s²], shape (N, 3). @@ -204,7 +209,7 @@ def run_imu_with_zupt( imu_params: IMUNoiseParams with noise specifications. window_size: ZUPT detector window size (samples). Default: 10. gamma: ZUPT detection threshold. Default: 1e6. - + Returns: Tuple of (pos_est, vel_est, zupt_detections). """ @@ -226,7 +231,9 @@ def run_imu_with_zupt( for k in range(1, N): # Propagate - q, v, p = strapdown_update(q, v, p, gyro_meas[k-1], accel_meas[k-1], dt, frame=frame) + q, v, p = strapdown_update( + q, v, p, gyro_meas[k - 1], accel_meas[k - 1], dt, frame=frame + ) # ZUPT detection using windowed test statistic (Eq. 6.44) # Build window centered at current sample @@ -250,11 +257,12 @@ def run_imu_with_zupt( # Detect ZUPT if window has enough samples if len(accel_window) >= window_size // 2: is_stationary = detect_zupt_windowed( - accel_window, gyro_window, + accel_window, + gyro_window, sigma_a=sigma_a, sigma_g=sigma_g, gamma=gamma, - g=9.81 + g=9.81, ) else: is_stationary = False @@ -271,15 +279,21 @@ def run_imu_with_zupt( def run_imu_with_zupt_ekf( - t, accel_meas, gyro_meas, initial_state, frame, imu_params, - window_size=10, gamma=10.0 + t, + accel_meas, + gyro_meas, + initial_state, + frame, + imu_params, + window_size=10, + gamma=10.0, ): """ Run IMU with ZUPT corrections using EKF (Eqs. 6.40-6.43 + 6.45). - + This is the proper implementation that uses Kalman filter measurement update instead of hard-coding v=0. - + Args: t: Time array [s]. accel_meas: Measured acceleration [m/s²], shape (N, 3). @@ -289,7 +303,7 @@ def run_imu_with_zupt_ekf( imu_params: IMUNoiseParams with noise specifications. window_size: ZUPT detector window size (samples). Default: 10. gamma: ZUPT detection threshold. Default: 10.0. - + Returns: Tuple of (pos_est, vel_est, zupt_detections). """ @@ -299,9 +313,7 @@ def run_imu_with_zupt_ekf( # Initialize EKF (sigma_zupt = 0.001 makes ZUPT measurements highly trusted) ekf = ZUPT_EKF(frame=frame, imu_params=imu_params, sigma_zupt=0.001) state = ekf.initialize( - p0=initial_state.p.copy(), - v0=initial_state.v.copy(), - q0=initial_state.q.copy() + p0=initial_state.p.copy(), v0=initial_state.v.copy(), q0=initial_state.q.copy() ) pos_est = np.zeros((N, 3)) @@ -316,7 +328,7 @@ def run_imu_with_zupt_ekf( for k in range(1, N): # EKF Prediction Step - state = ekf.predict(state, gyro_meas[k-1], accel_meas[k-1], dt) + state = ekf.predict(state, gyro_meas[k - 1], accel_meas[k - 1], dt) # ZUPT detection using raw measurements (bias-agnostic detector) window_start = max(0, k - window_size // 2) @@ -328,11 +340,12 @@ def run_imu_with_zupt_ekf( # Detect ZUPT if window has enough samples if len(accel_window) >= window_size // 2: is_stationary = detect_zupt_windowed( - accel_window, gyro_window, + accel_window, + gyro_window, sigma_a=sigma_a, sigma_g=sigma_g, gamma=gamma, - g=9.81 + g=9.81, ) else: is_stationary = False @@ -348,8 +361,17 @@ def run_imu_with_zupt_ekf( return pos_est, vel_est, zupt_detections -def plot_results(t, pos_true, pos_imu, pos_zupt, vel_imu, vel_zupt, - zupt_detections, stance_mask, figs_dir): +def plot_results( + t, + pos_true, + pos_imu, + pos_zupt, + vel_imu, + vel_zupt, + zupt_detections, + stance_mask, + figs_dir, +): """Generate publication-quality plots.""" # Compute errors @@ -358,35 +380,81 @@ def plot_results(t, pos_true, pos_imu, pos_zupt, vel_imu, vel_zupt, # Figure 1: Trajectory comparison fig1, ax1 = plt.subplots(figsize=(12, 8)) - ax1.plot(pos_true[:, 0], pos_true[:, 1], 'k-', linewidth=3, label='True Trajectory', zorder=1) - ax1.plot(pos_imu[:, 0], pos_imu[:, 1], 'r--', linewidth=2, alpha=0.7, label='IMU only (no ZUPT)', zorder=2) - ax1.plot(pos_zupt[:, 0], pos_zupt[:, 1], 'g-', linewidth=2, label='IMU + ZUPT', zorder=3) - ax1.scatter(pos_true[0, 0], pos_true[0, 1], c='blue', s=150, marker='o', label='Start', zorder=5) - ax1.scatter(pos_true[-1, 0], pos_true[-1, 1], c='red', s=150, marker='s', label='End', zorder=5) - ax1.set_xlabel('East [m]', fontsize=12) - ax1.set_ylabel('North [m]', fontsize=12) - ax1.set_title('ZUPT Example: Trajectory Comparison (Walking with Stops)', fontsize=14, fontweight='bold') - ax1.legend(fontsize=11, loc='best') + ax1.plot( + pos_true[:, 0], + pos_true[:, 1], + "k-", + linewidth=3, + label="True Trajectory", + zorder=1, + ) + ax1.plot( + pos_imu[:, 0], + pos_imu[:, 1], + "r--", + linewidth=2, + alpha=0.7, + label="IMU only (no ZUPT)", + zorder=2, + ) + ax1.plot( + pos_zupt[:, 0], pos_zupt[:, 1], "g-", linewidth=2, label="IMU + ZUPT", zorder=3 + ) + ax1.scatter( + pos_true[0, 0], + pos_true[0, 1], + c="blue", + s=150, + marker="o", + label="Start", + zorder=5, + ) + ax1.scatter( + pos_true[-1, 0], + pos_true[-1, 1], + c="red", + s=150, + marker="s", + label="End", + zorder=5, + ) + ax1.set_xlabel("East [m]", fontsize=12) + ax1.set_ylabel("North [m]", fontsize=12) + ax1.set_title( + "ZUPT Example: Trajectory Comparison (Walking with Stops)", + fontsize=14, + fontweight="bold", + ) + ax1.legend(fontsize=11, loc="best") ax1.grid(True, alpha=0.3) - ax1.axis('equal') + ax1.axis("equal") plt.tight_layout() - paths = save_figure(fig1, figs_dir, 'zupt_trajectory') + paths = save_figure(fig1, figs_dir, "zupt_trajectory") print(f" [OK] Saved: {paths[0]}") # Figure 2: Position error comparison fig2, ax2 = plt.subplots(figsize=(12, 6)) - ax2.plot(t, error_imu, 'r-', linewidth=2, label='IMU only (no ZUPT)', alpha=0.8) - ax2.plot(t, error_zupt, 'g-', linewidth=2, label='IMU + ZUPT') - ax2.fill_between(t, 0, np.max(error_imu)*1.1, where=stance_mask, - alpha=0.2, color='gray', label='True stance phases') - ax2.set_xlabel('Time [s]', fontsize=12) - ax2.set_ylabel('Position Error [m]', fontsize=12) - ax2.set_title('ZUPT Example: Position Error vs Time', fontsize=14, fontweight='bold') + ax2.plot(t, error_imu, "r-", linewidth=2, label="IMU only (no ZUPT)", alpha=0.8) + ax2.plot(t, error_zupt, "g-", linewidth=2, label="IMU + ZUPT") + ax2.fill_between( + t, + 0, + np.max(error_imu) * 1.1, + where=stance_mask, + alpha=0.2, + color="gray", + label="True stance phases", + ) + ax2.set_xlabel("Time [s]", fontsize=12) + ax2.set_ylabel("Position Error [m]", fontsize=12) + ax2.set_title( + "ZUPT Example: Position Error vs Time", fontsize=14, fontweight="bold" + ) ax2.legend(fontsize=11) ax2.grid(True, alpha=0.3) ax2.set_xlim([0, t[-1]]) plt.tight_layout() - paths = save_figure(fig2, figs_dir, 'zupt_error_time') + paths = save_figure(fig2, figs_dir, "zupt_error_time") print(f" [OK] Saved: {paths[0]}") # Figure 3: ZUPT detector performance @@ -395,38 +463,52 @@ def plot_results(t, pos_true, pos_imu, pos_zupt, vel_imu, vel_zupt, # Velocity magnitude vel_mag_imu = np.linalg.norm(vel_imu, axis=1) vel_mag_zupt = np.linalg.norm(vel_zupt, axis=1) - ax3a.plot(t, vel_mag_imu, 'r-', linewidth=1.5, label='IMU only', alpha=0.7) - ax3a.plot(t, vel_mag_zupt, 'g-', linewidth=2, label='IMU + ZUPT') - ax3a.fill_between(t, 0, np.max(vel_mag_imu)*1.1, where=stance_mask, - alpha=0.2, color='gray', label='True stance') - ax3a.set_ylabel('Velocity [m/s]', fontsize=12) - ax3a.set_title('ZUPT Example: Velocity and Detector Timeline', fontsize=14, fontweight='bold') + ax3a.plot(t, vel_mag_imu, "r-", linewidth=1.5, label="IMU only", alpha=0.7) + ax3a.plot(t, vel_mag_zupt, "g-", linewidth=2, label="IMU + ZUPT") + ax3a.fill_between( + t, + 0, + np.max(vel_mag_imu) * 1.1, + where=stance_mask, + alpha=0.2, + color="gray", + label="True stance", + ) + ax3a.set_ylabel("Velocity [m/s]", fontsize=12) + ax3a.set_title( + "ZUPT Example: Velocity and Detector Timeline", fontsize=14, fontweight="bold" + ) ax3a.legend(fontsize=10) ax3a.grid(True, alpha=0.3) # ZUPT detections - ax3b.fill_between(t, 0, 1, where=stance_mask, alpha=0.3, color='gray', label='True stance') - ax3b.fill_between(t, 0, 1, where=zupt_detections, alpha=0.5, color='green', label='ZUPT detected') - ax3b.set_xlabel('Time [s]', fontsize=12) - ax3b.set_ylabel('Detection', fontsize=12) + ax3b.fill_between( + t, 0, 1, where=stance_mask, alpha=0.3, color="gray", label="True stance" + ) + ax3b.fill_between( + t, 0, 1, where=zupt_detections, alpha=0.5, color="green", label="ZUPT detected" + ) + ax3b.set_xlabel("Time [s]", fontsize=12) + ax3b.set_ylabel("Detection", fontsize=12) ax3b.set_ylim([-0.1, 1.1]) ax3b.set_yticks([0, 1]) - ax3b.set_yticklabels(['Moving', 'Stationary']) - ax3b.legend(fontsize=10, loc='upper right') - ax3b.grid(True, alpha=0.3, axis='x') + ax3b.set_yticklabels(["Moving", "Stationary"]) + ax3b.legend(fontsize=10, loc="upper right") + ax3b.grid(True, alpha=0.3, axis="x") ax3b.set_xlim([0, t[-1]]) plt.tight_layout() - paths = save_figure(fig3, figs_dir, 'zupt_detector_timeline') + paths = save_figure(fig3, figs_dir, "zupt_detector_timeline") print(f" [OK] Saved: {paths[0]}") - plt.close('all') + plt.close("all") return error_imu, error_zupt -def animate_zupt_drift(t, pos_true, pos_imu, pos_zupt, stance_mask, - zupt_detections, n_frames: int = 40): +def animate_zupt_drift( + t, pos_true, pos_imu, pos_zupt, stance_mask, zupt_detections, n_frames: int = 40 +): """Build the ZUPT drift animation, Section 6.5. Dead reckoning fails *over time* -- that is the whole point of the chapter, @@ -477,16 +559,45 @@ def update(frame: int): for ax in axes: ax.clear() - axes[0].plot(pos_true[:end, 0], pos_true[:end, 1], "k-", - linewidth=2.0, label="ground truth") - axes[0].plot(pos_imu[:end, 0], pos_imu[:end, 1], "-", - color="#d62728", linewidth=1.6, label="IMU only") - axes[0].plot(pos_zupt[:end, 0], pos_zupt[:end, 1], "-", - color="#1f77b4", linewidth=1.6, label="IMU + ZUPT") - axes[0].plot(pos_imu[end - 1, 0], pos_imu[end - 1, 1], "o", - color="#d62728", markersize=7, markeredgecolor="k") - axes[0].plot(pos_zupt[end - 1, 0], pos_zupt[end - 1, 1], "o", - color="#1f77b4", markersize=7, markeredgecolor="k") + axes[0].plot( + pos_true[:end, 0], + pos_true[:end, 1], + "k-", + linewidth=2.0, + label="ground truth", + ) + axes[0].plot( + pos_imu[:end, 0], + pos_imu[:end, 1], + "-", + color="#d62728", + linewidth=1.6, + label="IMU only", + ) + axes[0].plot( + pos_zupt[:end, 0], + pos_zupt[:end, 1], + "-", + color="#1f77b4", + linewidth=1.6, + label="IMU + ZUPT", + ) + axes[0].plot( + pos_imu[end - 1, 0], + pos_imu[end - 1, 1], + "o", + color="#d62728", + markersize=7, + markeredgecolor="k", + ) + axes[0].plot( + pos_zupt[end - 1, 0], + pos_zupt[end - 1, 1], + "o", + color="#1f77b4", + markersize=7, + markeredgecolor="k", + ) axes[0].set_xlim(*xlim) axes[0].set_ylim(*ylim) axes[0].set_aspect("equal") @@ -501,12 +612,29 @@ def update(frame: int): ) # Zoomed to the walk, where the ZUPT track is actually legible. - axes[1].plot(pos_true[:end, 0], pos_true[:end, 1], "k-", - linewidth=2.0, label="ground truth") - axes[1].plot(pos_zupt[:end, 0], pos_zupt[:end, 1], "-", - color="#1f77b4", linewidth=1.6, label="IMU + ZUPT") - axes[1].plot(pos_zupt[end - 1, 0], pos_zupt[end - 1, 1], "o", - color="#1f77b4", markersize=7, markeredgecolor="k") + axes[1].plot( + pos_true[:end, 0], + pos_true[:end, 1], + "k-", + linewidth=2.0, + label="ground truth", + ) + axes[1].plot( + pos_zupt[:end, 0], + pos_zupt[:end, 1], + "-", + color="#1f77b4", + linewidth=1.6, + label="IMU + ZUPT", + ) + axes[1].plot( + pos_zupt[end - 1, 0], + pos_zupt[end - 1, 1], + "o", + color="#1f77b4", + markersize=7, + markeredgecolor="k", + ) axes[1].set_xlim(*zoom_xlim) axes[1].set_ylim(*zoom_ylim) axes[1].set_aspect("equal") @@ -518,14 +646,32 @@ def update(frame: int): "zoom: ZUPT vs truth (IMU-only is off this scale)", fontsize=10 ) - axes[2].plot(t[:end], error_imu[:end], "-", color="#d62728", - linewidth=1.6, label="IMU only") - axes[2].plot(t[:end], error_zupt[:end], "-", color="#1f77b4", - linewidth=1.6, label="IMU + ZUPT") + axes[2].plot( + t[:end], + error_imu[:end], + "-", + color="#d62728", + linewidth=1.6, + label="IMU only", + ) + axes[2].plot( + t[:end], + error_zupt[:end], + "-", + color="#1f77b4", + linewidth=1.6, + label="IMU + ZUPT", + ) # Shade the stance phases: this is where the correction happens. - axes[2].fill_between(t[:end], 0, max_error, - where=stance_mask[:end], color="0.85", - step="mid", zorder=0) + axes[2].fill_between( + t[:end], + 0, + max_error, + where=stance_mask[:end], + color="0.85", + step="mid", + zorder=0, + ) axes[2].set_xlim(t[0], t[-1]) axes[2].set_ylim(0, max_error) axes[2].grid(alpha=0.3) @@ -558,9 +704,9 @@ def main(animate: bool = False): # Set random seed for reproducibility np.random.seed(42) - print("\n" + "="*70) + print("\n" + "=" * 70) print("Chapter 6: Zero-Velocity Update (ZUPT) for Foot-Mounted IMU") - print("="*70) + print("=" * 70) print("\nDemonstrates drift elimination using ZUPT during stance phases.") print("Key equations: 6.44 (ZUPT detection), 6.45 (ZUPT correction)\n") @@ -586,8 +732,8 @@ def main(animate: bool = False): # Generate trajectory with correct IMU forward model print("Generating walking trajectory with stance phases...") - t, pos_true, vel_true, quat_true, accel_body, gyro_body, stance_mask = generate_walking_trajectory( - duration, dt, step_freq, step_length, frame + t, pos_true, vel_true, quat_true, accel_body, gyro_body, stance_mask = ( + generate_walking_trajectory(duration, dt, step_freq, step_length, frame) ) total_distance = np.sum(np.linalg.norm(np.diff(pos_true, axis=0), axis=1)) @@ -617,8 +763,14 @@ def main(animate: bool = False): print("\nRunning IMU + ZUPT-EKF (Kalman filter update)...") start = time.time() pos_zupt, vel_zupt, zupt_detections = run_imu_with_zupt_ekf( - t, accel_meas, gyro_meas, initial_state, frame, imu_params, - window_size=10, gamma=1000.0 # Much higher threshold for noisy consumer IMU + t, + accel_meas, + gyro_meas, + initial_state, + frame, + imu_params, + window_size=10, + gamma=1000.0, # Much higher threshold for noisy consumer IMU ) elapsed_zupt = time.time() - start detection_rate = np.sum(zupt_detections) / len(zupt_detections) * 100 @@ -627,14 +779,21 @@ def main(animate: bool = False): print(" Method: EKF measurement update (not hard-coded v=0)") # Create output directory - figs_dir = Path(__file__).parent / 'figs' + figs_dir = Path(__file__).parent / "figs" figs_dir.mkdir(exist_ok=True) # Generate plots print("\nGenerating plots...") error_imu, error_zupt = plot_results( - t, pos_true, pos_imu, pos_zupt, vel_imu, vel_zupt, - zupt_detections, stance_mask, figs_dir + t, + pos_true, + pos_imu, + pos_zupt, + vel_imu, + vel_zupt, + zupt_detections, + stance_mask, + figs_dir, ) # Compute metrics @@ -642,18 +801,22 @@ def main(animate: bool = False): final_error_zupt = error_zupt[-1] rmse_imu = np.sqrt(np.mean(error_imu**2)) rmse_zupt = np.sqrt(np.mean(error_zupt**2)) - improvement = (1 - rmse_zupt/rmse_imu) * 100 + improvement = (1 - rmse_zupt / rmse_imu) * 100 # Print results - print("\n" + "="*70) + print("\n" + "=" * 70) print("RESULTS") - print("="*70) + print("=" * 70) print("IMU-only (no ZUPT):") - print(f" Final error: {final_error_imu:.2f} m ({final_error_imu/total_distance*100:.1f}% of distance)") + print( + f" Final error: {final_error_imu:.2f} m ({final_error_imu/total_distance*100:.1f}% of distance)" + ) print(f" RMSE: {rmse_imu:.2f} m") print() print("IMU + ZUPT:") - print(f" Final error: {final_error_zupt:.2f} m ({final_error_zupt/total_distance*100:.1f}% of distance)") + print( + f" Final error: {final_error_zupt:.2f} m ({final_error_zupt/total_distance*100:.1f}% of distance)" + ) print(f" RMSE: {rmse_zupt:.2f} m") print() print(f"Improvement: {improvement:.1f}% reduction in RMSE") @@ -663,20 +826,19 @@ def main(animate: bool = False): fig, update, n_frames = animate_zupt_drift( t, pos_true, pos_imu, pos_zupt, stance_mask, zupt_detections ) - path = save_animation(fig, update, n_frames, figs_dir, - "ch6_zupt_drift", fps=5) + path = save_animation(fig, update, n_frames, figs_dir, "ch6_zupt_drift", fps=5) plt.close(fig) size_mb = path.stat().st_size / (1024 * 1024) print(f" [OK] Saved: {path} ({n_frames} frames, {size_mb:.2f} MB)") print(f"Figures saved to: {resolve_figs_dir(figs_dir)}/") print() - print("="*70) + print("=" * 70) print("KEY INSIGHT: ZUPT-EKF corrects velocity drift using Kalman updates!") print(" Eqs. 6.40-6.43 (Kalman filter) + Eq. 6.45 (ZUPT measurement)") print(" Essential for foot-mounted IMU navigation.") print(" Typical improvement: >90% error reduction.") - print("="*70) + print("=" * 70) print() show_figures_if_requested() @@ -686,9 +848,9 @@ def main(animate: bool = False): description="ZUPT for foot-mounted IMU (Chapter 6)" ) parser.add_argument( - "--animate", action="store_true", default=False, - help="Also render the drift animation GIF (slower)" + "--animate", + action="store_true", + default=False, + help="Also render the drift animation GIF (slower)", ) main(animate=parser.parse_args().animate) - - diff --git a/ch7_slam/__init__.py b/ch7_slam/__init__.py index 843418b..bffcec2 100644 --- a/ch7_slam/__init__.py +++ b/ch7_slam/__init__.py @@ -27,5 +27,3 @@ __version__ = "0.1.0" __all__ = [] - - diff --git a/ch7_slam/example_bundle_adjustment.py b/ch7_slam/example_bundle_adjustment.py index 094069a..cad11fa 100644 --- a/ch7_slam/example_bundle_adjustment.py +++ b/ch7_slam/example_bundle_adjustment.py @@ -137,11 +137,7 @@ def generate_landmarks( Returns: Landmarks array, shape (n_landmarks, 3) in [x, y, z] format. """ - landmarks = np.random.uniform( - -area_size / 2, - area_size / 2, - (n_landmarks, 3) - ) + landmarks = np.random.uniform(-area_size / 2, area_size / 2, (n_landmarks, 3)) # Ensure landmarks are at reasonable height landmarks[:, 2] = np.abs(landmarks[:, 2]) # Z should be positive @@ -241,10 +237,7 @@ def add_noise_to_estimates( Returns: Tuple of (noisy_poses, noisy_landmarks). """ - noisy_poses = [ - pose + np.random.normal(0, pose_noise, pose.shape) - for pose in poses - ] + noisy_poses = [pose + np.random.normal(0, pose_noise, pose.shape) for pose in poses] noisy_landmarks = landmarks + np.random.normal(0, landmark_noise, landmarks.shape) @@ -263,11 +256,11 @@ def create_ba_animation( fps: int = 3, ) -> None: """Create animated GIF showing bundle adjustment optimization. - + Shows two panels: 1. Top-down view: Camera poses, landmarks, and observation constraints 2. Error vs iteration: Convergence curve with current marker - + Args: poses_true: Ground truth camera poses landmarks_true: Ground truth 3D landmarks @@ -297,34 +290,43 @@ def create_ba_animation( # Panel 1: Top-down view ax1.set_xlim(xlim) ax1.set_ylim(ylim) - ax1.set_xlabel('X (m)') - ax1.set_ylabel('Y (m)') - ax1.set_aspect('equal') + ax1.set_xlabel("X (m)") + ax1.set_ylabel("Y (m)") + ax1.set_aspect("equal") ax1.grid(True, alpha=0.3) # Static elements: ground truth true_xy = np.array([[p[0], p[1]] for p in poses_true]) - ax1.plot(true_xy[:, 0], true_xy[:, 1], 'g-', linewidth=2, alpha=0.5, label='Truth') - ax1.scatter(landmarks_true[:, 0], landmarks_true[:, 1], - c='gray', marker='x', s=50, alpha=0.5, label='Landmarks (true)') + ax1.plot(true_xy[:, 0], true_xy[:, 1], "g-", linewidth=2, alpha=0.5, label="Truth") + ax1.scatter( + landmarks_true[:, 0], + landmarks_true[:, 1], + c="gray", + marker="x", + s=50, + alpha=0.5, + label="Landmarks (true)", + ) # Dynamic elements - pose_scatter = ax1.scatter([], [], c='blue', marker='^', s=100, label='Cameras') - landmark_scatter = ax1.scatter([], [], c='red', marker='o', s=40, alpha=0.7, label='Landmarks') + pose_scatter = ax1.scatter([], [], c="blue", marker="^", s=100, label="Cameras") + landmark_scatter = ax1.scatter( + [], [], c="red", marker="o", s=40, alpha=0.7, label="Landmarks" + ) constraint_lines = [] - ax1.legend(loc='upper right', fontsize=8) + ax1.legend(loc="upper right", fontsize=8) # Panel 2: Error vs iteration ax2.set_xlim(-0.5, n_frames - 0.5) ax2.set_ylim(0, max(error_history) * 1.2) - ax2.set_xlabel('Iteration') - ax2.set_ylabel('Total Error') - ax2.set_title('Convergence') + ax2.set_xlabel("Iteration") + ax2.set_ylabel("Total Error") + ax2.set_title("Convergence") ax2.grid(True, alpha=0.3) - error_line, = ax2.plot([], [], 'b-', linewidth=2) - error_marker, = ax2.plot([], [], 'ro', markersize=10) + (error_line,) = ax2.plot([], [], "b-", linewidth=2) + (error_marker,) = ax2.plot([], [], "ro", markersize=10) # Pre-compute constraint subset for visualization (limit for clarity) constraint_pairs = [] @@ -358,23 +360,26 @@ def update(frame): # Draw constraint lines (camera to landmark) for pose_id, landmark_id in constraint_pairs: px, py = poses_current[pose_id][0], poses_current[pose_id][1] - lx, ly = landmarks_current[landmark_id][0], landmarks_current[landmark_id][1] - line, = ax1.plot([px, lx], [py, ly], 'purple', linewidth=0.5, alpha=0.3) + lx, ly = ( + landmarks_current[landmark_id][0], + landmarks_current[landmark_id][1], + ) + (line,) = ax1.plot([px, lx], [py, ly], "purple", linewidth=0.5, alpha=0.3) constraint_lines.append(line) # Update error plot - error_line.set_data(range(frame + 1), error_history[:frame + 1]) + error_line.set_data(range(frame + 1), error_history[: frame + 1]) error_marker.set_data([frame], [error_history[frame]]) # Update titles - ax1.set_title(f'Top View (iteration {frame})') + ax1.set_title(f"Top View (iteration {frame})") if frame == 0: - ax2.set_title('Convergence (initial)') + ax2.set_title("Convergence (initial)") elif frame == n_frames - 1: - ax2.set_title(f'Convergence (final error: {error_history[-1]:.4f})') + ax2.set_title(f"Convergence (final error: {error_history[-1]:.4f})") else: - ax2.set_title(f'Convergence (error: {error_history[frame]:.4f})') + ax2.set_title(f"Convergence (error: {error_history[frame]:.4f})") return artists @@ -386,8 +391,13 @@ def update(frame): print(f" Creating animation with {n_frames} frames...") output_path = Path(output_path) written = save_animation( - fig, update, n_frames, output_path.parent, output_path.stem, - fps=fps, init=init, + fig, + update, + n_frames, + output_path.parent, + output_path.stem, + fps=fps, + init=init, ) plt.close(fig) @@ -427,51 +437,94 @@ def plot_bundle_adjustment_results( # Plot landmarks ax1.scatter( - landmarks_true[:, 0], landmarks_true[:, 1], - c='gray', marker='x', s=50, alpha=0.5, label='Landmarks (true)' + landmarks_true[:, 0], + landmarks_true[:, 1], + c="gray", + marker="x", + s=50, + alpha=0.5, + label="Landmarks (true)", ) ax1.scatter( - landmarks_init[:, 0], landmarks_init[:, 1], - c='red', marker='o', s=30, alpha=0.3, label='Landmarks (init)' + landmarks_init[:, 0], + landmarks_init[:, 1], + c="red", + marker="o", + s=30, + alpha=0.3, + label="Landmarks (init)", ) ax1.scatter( - landmarks_opt[:, 0], landmarks_opt[:, 1], - c='blue', marker='o', s=30, alpha=0.5, label='Landmarks (opt)' + landmarks_opt[:, 0], + landmarks_opt[:, 1], + c="blue", + marker="o", + s=30, + alpha=0.5, + label="Landmarks (opt)", ) # Plot trajectories - ax1.plot(true_xy[:, 0], true_xy[:, 1], 'g-', linewidth=2, label='Poses (true)', alpha=0.7) - ax1.plot(init_xy[:, 0], init_xy[:, 1], 'r--', linewidth=2, label='Poses (init)', alpha=0.7) - ax1.plot(opt_xy[:, 0], opt_xy[:, 1], 'b-', linewidth=2, label='Poses (opt)', alpha=0.8) + ax1.plot( + true_xy[:, 0], true_xy[:, 1], "g-", linewidth=2, label="Poses (true)", alpha=0.7 + ) + ax1.plot( + init_xy[:, 0], + init_xy[:, 1], + "r--", + linewidth=2, + label="Poses (init)", + alpha=0.7, + ) + ax1.plot( + opt_xy[:, 0], opt_xy[:, 1], "b-", linewidth=2, label="Poses (opt)", alpha=0.8 + ) - ax1.set_xlabel('X [m]', fontsize=11) - ax1.set_ylabel('Y [m]', fontsize=11) - ax1.set_title('Bundle Adjustment: Top View', fontsize=12, fontweight='bold') + ax1.set_xlabel("X [m]", fontsize=11) + ax1.set_ylabel("Y [m]", fontsize=11) + ax1.set_title("Bundle Adjustment: Top View", fontsize=12, fontweight="bold") ax1.legend(fontsize=9) ax1.grid(True, alpha=0.3) - ax1.axis('equal') + ax1.axis("equal") # --- Plot 2: Position Errors --- ax2 = fig.add_subplot(132) - pose_errors_init = np.array([ - np.linalg.norm(poses_init[i][:2] - poses_true[i][:2]) - for i in range(len(poses_true)) - ]) - pose_errors_opt = np.array([ - np.linalg.norm(poses_opt[i][:2] - poses_true[i][:2]) - for i in range(len(poses_true)) - ]) - + pose_errors_init = np.array( + [ + np.linalg.norm(poses_init[i][:2] - poses_true[i][:2]) + for i in range(len(poses_true)) + ] + ) + pose_errors_opt = np.array( + [ + np.linalg.norm(poses_opt[i][:2] - poses_true[i][:2]) + for i in range(len(poses_true)) + ] + ) pose_indices = np.arange(len(poses_true)) - ax2.plot(pose_indices, pose_errors_init, 'r--', linewidth=2, label='Pose Error (init)', alpha=0.7) - ax2.plot(pose_indices, pose_errors_opt, 'b-', linewidth=2, label='Pose Error (opt)', alpha=0.8) + ax2.plot( + pose_indices, + pose_errors_init, + "r--", + linewidth=2, + label="Pose Error (init)", + alpha=0.7, + ) + ax2.plot( + pose_indices, + pose_errors_opt, + "b-", + linewidth=2, + label="Pose Error (opt)", + alpha=0.8, + ) - ax2.set_xlabel('Pose Index', fontsize=11) - ax2.set_ylabel('Position Error [m]', fontsize=11) - ax2.set_title('Camera Pose Errors', fontsize=12, fontweight='bold') + ax2.set_xlabel("Pose Index", fontsize=11) + ax2.set_ylabel("Position Error [m]", fontsize=11) + ax2.set_title("Camera Pose Errors", fontsize=12, fontweight="bold") ax2.legend(fontsize=9) ax2.grid(True, alpha=0.3) @@ -479,11 +532,11 @@ def plot_bundle_adjustment_results( ax3 = fig.add_subplot(133) iterations = np.arange(len(error_history)) - ax3.semilogy(iterations, error_history, 'b-', linewidth=2, marker='o', markersize=4) + ax3.semilogy(iterations, error_history, "b-", linewidth=2, marker="o", markersize=4) - ax3.set_xlabel('Iteration', fontsize=11) - ax3.set_ylabel('Total Error (log scale)', fontsize=11) - ax3.set_title('Bundle Adjustment Convergence', fontsize=12, fontweight='bold') + ax3.set_xlabel("Iteration", fontsize=11) + ax3.set_ylabel("Total Error (log scale)", fontsize=11) + ax3.set_title("Bundle Adjustment Convergence", fontsize=12, fontweight="bold") ax3.grid(True, alpha=0.3) plt.tight_layout() @@ -499,7 +552,7 @@ def plot_bundle_adjustment_results( def main(animate: bool = False): """Run complete bundle adjustment example. - + Args: animate: If True, generate animated GIF showing optimization. """ @@ -520,10 +573,10 @@ def main(animate: bool = False): fy=500.0, cx=320.0, cy=240.0, - k1=-0.05, # Slight barrel distortion - k2=0.01, # Secondary radial - p1=0.001, # Tangential - p2=0.001, # Tangential + k1=-0.05, # Slight barrel distortion + k2=0.01, # Secondary radial + p1=0.001, # Tangential + p2=0.001, # Tangential ) print(f" Camera: fx={intrinsics.fx}, fy={intrinsics.fy}") print(f" Distortion: k1={intrinsics.k1}, k2={intrinsics.k2}") @@ -568,17 +621,21 @@ def main(animate: bool = False): poses_init, landmarks_init = add_noise_to_estimates( poses_true, landmarks_true, - pose_noise=0.3, # 30cm position, ~17deg heading (larger noise for better visualization) - landmark_noise=0.5, # 50cm landmark position (increased from 10cm) + pose_noise=0.3, # 30cm position, ~17deg heading (larger noise for better visualization) + landmark_noise=0.5, # 50cm landmark position (increased from 10cm) ) - pose_init_rmse = np.sqrt(np.mean([ - np.linalg.norm(poses_init[i][:2] - poses_true[i][:2])**2 - for i in range(n_poses) - ])) - landmark_init_rmse = np.sqrt(np.mean( - np.linalg.norm(landmarks_init - landmarks_true, axis=1)**2 - )) + pose_init_rmse = np.sqrt( + np.mean( + [ + np.linalg.norm(poses_init[i][:2] - poses_true[i][:2]) ** 2 + for i in range(n_poses) + ] + ) + ) + landmark_init_rmse = np.sqrt( + np.mean(np.linalg.norm(landmarks_init - landmarks_true, axis=1) ** 2) + ) print(f" Initial pose RMSE: {pose_init_rmse:.4f} m") print(f" Initial landmark RMSE: {landmark_init_rmse:.4f} m") @@ -604,7 +661,7 @@ def main(animate: bool = False): n_factors = 0 # Inverse covariance for the pixel measurements. Weighting each residual by # 1/sigma^2 = 4 is why graph.compute_error() is not in pixels. - pixel_info = np.eye(2) / (PIXEL_NOISE_STD ** 2) + pixel_info = np.eye(2) / (PIXEL_NOISE_STD**2) for pose_id, obs_list in observations.items(): for landmark_id, observed_pixel in obs_list: @@ -620,6 +677,7 @@ def main(animate: bool = False): # Add weak prior on first pose to prevent gauge freedom from core.slam.factors import create_prior_factor + prior_info = np.diag([10.0, 10.0, 10.0]) # Weak prior prior_factor = create_prior_factor(0, poses_true[0], information=prior_info) graph.add_factor(prior_factor) @@ -637,8 +695,10 @@ def main(animate: bool = False): initial_error = graph.compute_error() initial_px = reprojection_residuals_px(graph, n_factors - 1) print(f" Initial cost (weighted sum of squares): {initial_error:.3f}") - print(f" Initial reprojection error: {rms(initial_px):.1f} px RMS, " - f"worst {initial_px.max():.1f} px") + print( + f" Initial reprojection error: {rms(initial_px):.1f} px RMS, " + f"worst {initial_px.max():.1f} px" + ) # Track per-iteration states for animation poses_history = [] @@ -647,14 +707,16 @@ def main(animate: bool = False): # Store initial state poses_history.append([graph.variables[i].copy() for i in range(n_poses)]) - landmarks_history.append(np.array([graph.variables[n_poses + i].copy() for i in range(n_landmarks)])) + landmarks_history.append( + np.array([graph.variables[n_poses + i].copy() for i in range(n_landmarks)]) + ) # Custom Levenberg-Marquardt loop to capture per-iteration states # LM is more stable for BA than pure Gauss-Newton max_iterations = 25 tol = 1e-6 # Tighter tolerance for more iterations mu = 10.0 # Higher initial damping for slower, smoother convergence - nu = 1.5 # Smaller damping increase factor + nu = 1.5 # Smaller damping increase factor for iteration in range(max_iterations): # Build linearized system: H δx = b where b = -J^T Λ r @@ -673,7 +735,9 @@ def main(animate: bool = False): saved_vars = {k: v.copy() for k, v in graph.variables.items()} # Apply update - graph._update_variables(delta_x * 0.3) # Apply only 30% of step for slower convergence + graph._update_variables( + delta_x * 0.3 + ) # Apply only 30% of step for slower convergence # Compute new error new_error = graph.compute_error() @@ -703,7 +767,9 @@ def main(animate: bool = False): # Store state after this iteration poses_history.append([graph.variables[i].copy() for i in range(n_poses)]) - landmarks_history.append(np.array([graph.variables[n_poses + i].copy() for i in range(n_landmarks)])) + landmarks_history.append( + np.array([graph.variables[n_poses + i].copy() for i in range(n_landmarks)]) + ) # Check convergence if abs(current_error - new_error) < tol: @@ -720,8 +786,10 @@ def main(animate: bool = False): final_error = error_history[-1] final_px = reprojection_residuals_px(graph, n_factors - 1) print(f" Final cost (weighted sum of squares): {final_error:.3f}") - print(f" Final reprojection error: {rms(final_px):.2f} px RMS, " - f"worst {final_px.max():.2f} px") + print( + f" Final reprojection error: {rms(final_px):.2f} px RMS, " + f"worst {final_px.max():.2f} px" + ) print(f" Iterations: {len(error_history) - 1}") # Report the cost drop as a factor, not a percentage. @@ -738,10 +806,14 @@ def main(animate: bool = False): # The interpretable statements are the pixel RMS above -- which lands at # the 0.5 px noise the observations were generated with, i.e. the optimum # -- and the pose and landmark accuracy below. - print(f" Cost reduced {initial_error / final_error:,.0f}x " - f"({100 * (1 - final_error / initial_error):.4f}%)") - print(f" Final RMS is at the {PIXEL_NOISE_STD:.1f} px measurement noise " - f"floor, so the solve is converged, not merely improved.") + print( + f" Cost reduced {initial_error / final_error:,.0f}x " + f"({100 * (1 - final_error / initial_error):.4f}%)" + ) + print( + f" Final RMS is at the {PIXEL_NOISE_STD:.1f} px measurement noise " + f"floor, so the solve is converged, not merely improved." + ) # Extract optimized poses and landmarks poses_opt = [optimized_vars[i] for i in range(n_poses)] @@ -753,14 +825,12 @@ def main(animate: bool = False): print("\n7. Evaluating bundle adjustment results...") # Pose errors - pose_errors_init = np.array([ - np.linalg.norm(poses_init[i][:2] - poses_true[i][:2]) - for i in range(n_poses) - ]) - pose_errors_opt = np.array([ - np.linalg.norm(poses_opt[i][:2] - poses_true[i][:2]) - for i in range(n_poses) - ]) + pose_errors_init = np.array( + [np.linalg.norm(poses_init[i][:2] - poses_true[i][:2]) for i in range(n_poses)] + ) + pose_errors_opt = np.array( + [np.linalg.norm(poses_opt[i][:2] - poses_true[i][:2]) for i in range(n_poses)] + ) pose_rmse_init = np.sqrt(np.mean(pose_errors_init**2)) pose_rmse_opt = np.sqrt(np.mean(pose_errors_opt**2)) @@ -778,25 +848,35 @@ def main(animate: bool = False): print(f" Landmark RMSE (initial): {landmark_rmse_init:.4f} m") print(f" Landmark RMSE (optimized): {landmark_rmse_opt:.4f} m") - print(f" Landmark improvement: {(1 - landmark_rmse_opt / landmark_rmse_init) * 100:.2f}%") + print( + f" Landmark improvement: {(1 - landmark_rmse_opt / landmark_rmse_init) * 100:.2f}%" + ) # ------------------------------------------------------------------------ # 8. Visualize Results # ------------------------------------------------------------------------ print("\n8. Visualizing results...") plot_bundle_adjustment_results( - poses_true, poses_init, poses_opt, - landmarks_true, landmarks_init, landmarks_opt, - error_history + poses_true, + poses_init, + poses_opt, + landmarks_true, + landmarks_init, + landmarks_opt, + error_history, ) # Generate animation if requested if animate: print("\n9. Generating bundle adjustment animation...") create_ba_animation( - poses_true, landmarks_true, - poses_history, landmarks_history, error_history, - observations, n_poses, + poses_true, + landmarks_true, + poses_history, + landmarks_history, + error_history, + observations, + n_poses, output_path="ch7_slam/figs/bundle_adjustment.gif", fps=3, ) @@ -812,8 +892,10 @@ def main(animate: bool = False): print(f" - Total observations: {total_observations}") # In pixels, not as a percentage of a weighted sum of squares. This line # printed "Reprojection error reduction: 100.0%" for a 99.998938% drop. - print(f" - Reprojection error: {rms(initial_px):.1f} -> {rms(final_px):.2f} px RMS " - f"(noise floor {PIXEL_NOISE_STD:.1f} px)") + print( + f" - Reprojection error: {rms(initial_px):.1f} -> {rms(final_px):.2f} px RMS " + f"(noise floor {PIXEL_NOISE_STD:.1f} px)" + ) print(f" - Pose accuracy: {pose_rmse_opt:.4f} m RMSE") print(f" - Landmark accuracy: {landmark_rmse_opt:.4f} m RMSE") print() @@ -830,10 +912,11 @@ def main(animate: bool = False): description="Chapter 7: Visual Bundle Adjustment Example" ) parser.add_argument( - "--animate", action="store_true", default=False, - help="Generate animated GIF showing optimization process" + "--animate", + action="store_true", + default=False, + help="Generate animated GIF showing optimization process", ) args = parser.parse_args() main(animate=args.animate) - diff --git a/ch7_slam/example_pose_graph_slam.py b/ch7_slam/example_pose_graph_slam.py index 193f581..ba46381 100644 --- a/ch7_slam/example_pose_graph_slam.py +++ b/ch7_slam/example_pose_graph_slam.py @@ -79,36 +79,36 @@ def load_slam_dataset(data_dir: str) -> Dict: """Load SLAM dataset from directory. - + Args: data_dir: Path to dataset directory (e.g., 'data/sim/ch7_slam_2d_square') - + Returns: Dictionary with poses, landmarks, loop closures, and scans """ path = resolve_data_path(data_dir) data = { - 'true_poses': np.loadtxt(path / 'ground_truth_poses.txt'), - 'odom_poses': np.loadtxt(path / 'odometry_poses.txt'), - 'landmarks': np.loadtxt(path / 'landmarks.txt'), - 'loop_closures': np.loadtxt(path / 'loop_closures.txt'), + "true_poses": np.loadtxt(path / "ground_truth_poses.txt"), + "odom_poses": np.loadtxt(path / "odometry_poses.txt"), + "landmarks": np.loadtxt(path / "landmarks.txt"), + "loop_closures": np.loadtxt(path / "loop_closures.txt"), } # Load scans from npz - scans_npz = np.load(path / 'scans.npz') - data['scans'] = [scans_npz[f'scan_{i}'] for i in range(len(data['true_poses']))] + scans_npz = np.load(path / "scans.npz") + data["scans"] = [scans_npz[f"scan_{i}"] for i in range(len(data["true_poses"]))] # Load config - with open(path / 'config.json') as f: - data['config'] = json.load(f) + with open(path / "config.json") as f: + data["config"] = json.load(f) return data def run_with_dataset(data_dir: str, use_loop_oracle: bool = False) -> None: """Run pose graph SLAM using pre-generated dataset. - + Args: data_dir: Path to dataset directory use_loop_oracle: If True, use distance-based oracle instead of observation-based @@ -121,13 +121,13 @@ def run_with_dataset(data_dir: str, use_loop_oracle: bool = False) -> None: # Load dataset data = load_slam_dataset(data_dir) - config = data['config'] + config = data["config"] - true_poses = [data['true_poses'][i] for i in range(len(data['true_poses']))] - odom_poses = [data['odom_poses'][i] for i in range(len(data['odom_poses']))] - landmarks = data['landmarks'] - scans = data['scans'] - loop_closure_data = data['loop_closures'] + true_poses = [data["true_poses"][i] for i in range(len(data["true_poses"]))] + odom_poses = [data["odom_poses"][i] for i in range(len(data["odom_poses"]))] + landmarks = data["landmarks"] + scans = data["scans"] + loop_closure_data = data["loop_closures"] n_poses = len(true_poses) @@ -177,19 +177,25 @@ def run_with_dataset(data_dir: str, use_loop_oracle: bool = False) -> None: result = frontend.step(i, odom_delta, scans[i]) # Store results - frontend_poses.append(result['pose_est']) - pred_poses.append(result['pose_pred']) - match_qualities.append(result['match_quality']) + frontend_poses.append(result["pose_est"]) + pred_poses.append(result["pose_pred"]) + match_qualities.append(result["match_quality"]) # Compute correction magnitude (if not first step) if i > 0: - correction = np.linalg.norm(result['pose_est'][:2] - result['pose_pred'][:2]) + correction = np.linalg.norm( + result["pose_est"][:2] - result["pose_pred"][:2] + ) corrections.append(correction) # Compute front-end statistics n_converged = sum(1 for mq in match_qualities if mq.converged) converged_qualities = [mq for mq in match_qualities if mq.converged] - avg_residual = np.mean([mq.residual for mq in converged_qualities]) if converged_qualities else 0.0 + avg_residual = ( + np.mean([mq.residual for mq in converged_qualities]) + if converged_qualities + else 0.0 + ) avg_correction = np.mean(corrections) if corrections else 0.0 print(f"\n Processed {n_poses} steps") @@ -229,7 +235,7 @@ def run_with_dataset(data_dir: str, use_loop_oracle: bool = False) -> None: scans=scans, use_observation_based=not use_loop_oracle, # Default: observation-based distance_threshold=None, # No distance gating (observation-based is primary) - min_time_separation=20 # Lower to find more candidates + min_time_separation=20, # Lower to find more candidates ) mode_str = "oracle" if use_loop_oracle else "observation-based" @@ -239,7 +245,9 @@ def run_with_dataset(data_dir: str, use_loop_oracle: bool = False) -> None: # Also show what the dataset provided (for reference) if loop_closure_data.ndim == 1: loop_closure_data = loop_closure_data.reshape(1, -1) - print(f" [Reference: Dataset provided {len(loop_closure_data)} ground truth loop closure indices]") + print( + f" [Reference: Dataset provided {len(loop_closure_data)} ground truth loop closure indices]" + ) # Build pose graph print("\n" + "-" * 70) @@ -286,8 +294,12 @@ def run_with_dataset(data_dir: str, use_loop_oracle: bool = False) -> None: loop_information=loop_info, ) - print(f" Pose graph: {len(graph.variables)} variables, {len(graph.factors)} factors") - print(f" Factors: 1 prior + {len(odometry_measurements)} odometry + {len(loop_measurements)} loop closures") + print( + f" Pose graph: {len(graph.variables)} variables, {len(graph.factors)} factors" + ) + print( + f" Factors: 1 prior + {len(odometry_measurements)} odometry + {len(loop_measurements)} loop closures" + ) # Optimize print("\n" + "-" * 70) @@ -314,9 +326,21 @@ def run_with_dataset(data_dir: str, use_loop_oracle: bool = False) -> None: print("\n" + "-" * 70) print("Results:") - odom_errors = np.array([np.linalg.norm(odom_poses[i][:2] - true_poses[i][:2]) for i in range(n_poses)]) - frontend_errors = np.array([np.linalg.norm(frontend_poses[i][:2] - true_poses[i][:2]) for i in range(n_poses)]) - opt_errors = np.array([np.linalg.norm(optimized_poses[i][:2] - true_poses[i][:2]) for i in range(n_poses)]) + odom_errors = np.array( + [np.linalg.norm(odom_poses[i][:2] - true_poses[i][:2]) for i in range(n_poses)] + ) + frontend_errors = np.array( + [ + np.linalg.norm(frontend_poses[i][:2] - true_poses[i][:2]) + for i in range(n_poses) + ] + ) + opt_errors = np.array( + [ + np.linalg.norm(optimized_poses[i][:2] - true_poses[i][:2]) + for i in range(n_poses) + ] + ) odom_rmse = np.sqrt(np.mean(odom_errors**2)) frontend_rmse = np.sqrt(np.mean(frontend_errors**2)) @@ -324,7 +348,9 @@ def run_with_dataset(data_dir: str, use_loop_oracle: bool = False) -> None: print(f" Odometry RMSE: {odom_rmse:.4f} m (baseline)") print(f" Frontend RMSE: {frontend_rmse:.4f} m (scan-to-map corrected)") - print(f" Optimized RMSE: {opt_rmse:.4f} m (backend with {len(loop_closures)} loop closures)") + print( + f" Optimized RMSE: {opt_rmse:.4f} m (backend with {len(loop_closures)} loop closures)" + ) if odom_rmse > 0: frontend_improvement = (1 - frontend_rmse / odom_rmse) * 100 @@ -339,8 +365,13 @@ def run_with_dataset(data_dir: str, use_loop_oracle: bool = False) -> None: print("\n" + "-" * 70) print("Generating plots...") plot_slam_results( - true_poses, odom_poses, frontend_poses, optimized_poses, - landmarks, loop_closures, scans + true_poses, + odom_poses, + frontend_poses, + optimized_poses, + landmarks, + loop_closures, + scans, ) print("\n" + "=" * 70) @@ -348,7 +379,9 @@ def run_with_dataset(data_dir: str, use_loop_oracle: bool = False) -> None: print("=" * 70) print("\nSummary:") print(f" - Trajectory: {n_poses} poses") - print(f" - Front-end: {n_converged}/{n_poses} converged ({100*n_converged/n_poses:.1f}%)") + print( + f" - Front-end: {n_converged}/{n_poses} converged ({100*n_converged/n_poses:.1f}%)" + ) print(f" - Loop closures: {len(loop_closures)} (observation-based detection)") print(f" - Odometry drift: {final_drift:.3f} m") print(f" - Odometry RMSE: {odom_rmse:.4f} m (baseline)") @@ -363,6 +396,7 @@ def run_with_dataset(data_dir: str, use_loop_oracle: bool = False) -> None: # Machine-readable summary for automated testing import json + summary = { "mode": "dataset", # "used" means step() was called, which is not the same as it having @@ -378,7 +412,7 @@ def run_with_dataset(data_dir: str, use_loop_oracle: bool = False) -> None: "odom": round(odom_rmse, 4), "frontend": round(frontend_rmse, 4), "optimized": round(opt_rmse, 4), - } + }, } print(f"\n[SLAM_SUMMARY] {json.dumps(summary)}") @@ -448,15 +482,15 @@ def generate_corridor_loop_trajectory( ) -> List[np.ndarray]: """ Generate a trajectory that goes down a corridor, turns, and returns. - + The robot returns on the SAME path (same Y, same heading) to ensure scans are directly comparable for ICP loop closure detection. - + Args: corridor_length: How far to travel before turning (meters). n_poses_out: Number of poses on outbound leg. n_poses_back: Number of poses on return leg. - + Returns: List of poses [x, y, yaw] representing the trajectory. """ @@ -492,14 +526,14 @@ def generate_smooth_square_trajectory( ) -> List[np.ndarray]: """ Generate a smooth square trajectory with many poses for SLAM front-end. - + Uses more poses per side and smooth heading transitions for better scan-to-map ICP matching. - + Args: side_length: Length of each side in meters. n_poses_per_side: Number of poses per side (more = smoother). - + Returns: List of poses [x, y, yaw] representing the trajectory. """ @@ -558,15 +592,15 @@ def generate_square_loop_trajectory( ) -> List[np.ndarray]: """ Generate a square loop trajectory with multiple laps for SLAM. - + This creates a closed-loop trajectory where the robot revisits the same locations, enabling loop closure detection. - + Args: side_length: Length of each side in meters. n_poses_per_side: Number of poses per side (more = smoother). n_laps: Number of complete laps around the square. - + Returns: List of poses [x, y, yaw] representing the trajectory. Start and end poses are identical (closed loop). @@ -611,16 +645,16 @@ def create_room_walls( ) -> Tuple[List[Tuple[np.ndarray, np.ndarray]], np.ndarray]: """ Create wall segments forming a room around a square trajectory. - + The walls are placed slightly outside the trajectory to ensure LiDAR scans hit the walls at reasonable ranges. Internal features (pillars, partial walls) break symmetry for reliable ICP and meaningful loop closure descriptors. - + Args: side_length: Size of the square trajectory. wall_offset: Distance from trajectory to walls. - + Returns: Tuple of: - List of (start_point, end_point) tuples defining walls. @@ -644,12 +678,14 @@ def create_room_walls( walls.append((np.array([max_coord, min_coord]), np.array([max_coord, max_coord]))) # Room corners as landmarks - landmarks_list.extend([ - [min_coord, min_coord], # SW - [max_coord, min_coord], # SE - [max_coord, max_coord], # NE - [min_coord, max_coord], # NW - ]) + landmarks_list.extend( + [ + [min_coord, min_coord], # SW + [max_coord, min_coord], # SE + [max_coord, max_coord], # NE + [min_coord, max_coord], # NW + ] + ) # ========================================================================= # Internal features to break symmetry (CRITICAL for loop closure!) @@ -707,18 +743,18 @@ def create_corridor_walls( ) -> List[Tuple[np.ndarray, np.ndarray]]: """ Create wall segments forming a closed rectangular corridor. - + The corridor runs along the X-axis with: - Side walls at y=+width/2 and y=-width/2 - End walls at x=0 and x=length - + The end walls break translational symmetry, making scans at different X positions distinguishable. This enables reliable ICP matching. - + Args: length: Corridor length in meters (X direction). width: Corridor width in meters (Y direction). - + Returns: List of (start_point, end_point) tuples defining walls. """ @@ -836,7 +872,7 @@ def detect_loop_closures( When the robot returns to a previously visited location, loop closures enforce the close-loop constraint from Eq. (7.22): residual = ln((ΔT_ij')^{-1} T_i^{-1} T_j)^∨ - + where ΔT_ij' is the scan-matched transform from ICP, and T_i^{-1} T_j is the transform implied by the odometry chain. @@ -865,8 +901,12 @@ def detect_loop_closures( # Observation-based detection using scan descriptors # Use strict ICP residual threshold to reject bad alignments # For corridor environment, residuals above 0.1 often indicate wrong local minima - print(" Loop closure: candidate from descriptor similarity (observation-based)") - print(" Using frontend_poses for initial guess (scan-to-map corrected trajectory)") + print( + " Loop closure: candidate from descriptor similarity (observation-based)" + ) + print( + " Using frontend_poses for initial guess (scan-to-map corrected trajectory)" + ) print() detector = LoopClosureDetector2D( @@ -898,9 +938,11 @@ def detect_loop_closures( loop_closures = [] for lc in loop_closures_obj: loop_closures.append((lc.j, lc.i, lc.rel_pose, lc.covariance)) - print(f" Verified: {lc.j} <-> {lc.i}, " - f"desc_sim={lc.descriptor_similarity:.3f}, " - f"icp_residual={lc.icp_residual:.4f}, iters={lc.icp_iterations}") + print( + f" Verified: {lc.j} <-> {lc.i}, " + f"desc_sim={lc.descriptor_similarity:.3f}, " + f"icp_residual={lc.icp_residual:.4f}, iters={lc.icp_iterations}" + ) return loop_closures @@ -953,7 +995,9 @@ def detect_loop_closures( # Return (from_id=i, to_id=j, rel_pose=i_to_j) loop_closures.append((i, j, rel_pose, cov)) - print(f" Loop closure: {i} <-> {j}, residual={residual:.4f}, iters={iters}") + print( + f" Loop closure: {i} <-> {j}, residual={residual:.4f}, iters={iters}" + ) except Exception: # ICP failed, skip this pair @@ -969,12 +1013,12 @@ def build_map_from_poses( ) -> np.ndarray: """ Build a map point cloud by transforming all scans using given poses. - + Args: poses: List of SE(2) poses [x, y, theta]. scans: List of scan point clouds (Nx2 arrays). downsample_voxel: Voxel size for downsampling (0 = no downsampling). - + Returns: Map point cloud as Nx2 array. """ @@ -1019,14 +1063,14 @@ def create_slam_animation( fps: int = 5, ) -> None: """Create animated GIF showing SLAM pipeline evolution. - + Shows three panels: 1. Map + Trajectory: map growing, trajectories evolving, current pose 2. Constraint Graph: odometry edges (gray) + loop closure edges (magenta) 3. Error: Position error vs time with loop closure markers - + After front-end completes, shows optimization correction animation. - + Args: true_poses: Ground truth trajectory odom_poses: Noisy odometry trajectory @@ -1043,12 +1087,17 @@ def create_slam_animation( n_poses = len(true_poses) # Compute errors at each timestep - odom_errors = [np.linalg.norm(odom_poses[i][:2] - true_poses[i][:2]) - for i in range(n_poses)] - frontend_errors = [np.linalg.norm(frontend_poses[i][:2] - true_poses[i][:2]) - for i in range(n_poses)] - optimized_errors = [np.linalg.norm(optimized_poses[i][:2] - true_poses[i][:2]) - for i in range(n_poses)] + odom_errors = [ + np.linalg.norm(odom_poses[i][:2] - true_poses[i][:2]) for i in range(n_poses) + ] + frontend_errors = [ + np.linalg.norm(frontend_poses[i][:2] - true_poses[i][:2]) + for i in range(n_poses) + ] + optimized_errors = [ + np.linalg.norm(optimized_poses[i][:2] - true_poses[i][:2]) + for i in range(n_poses) + ] # Find when each loop closure is detected (use the larger index) loop_closure_times = {max(lc[0], lc[1]): (lc[0], lc[1]) for lc in loop_closures} @@ -1071,55 +1120,72 @@ def create_slam_animation( # Initialize plot elements # Panel 1: Map + Trajectory - map_scatter = ax1.scatter([], [], s=1, c='blue', alpha=0.3, label='Map') - current_scan_scatter = ax1.scatter([], [], s=10, c='red', alpha=0.8, label='Current scan') - true_line, = ax1.plot([], [], 'g-', linewidth=1, alpha=0.5, label='Ground truth') - odom_line, = ax1.plot([], [], 'r--', linewidth=1, alpha=0.7, label='Odometry') - frontend_line, = ax1.plot([], [], 'b-', linewidth=2, label='Frontend') - current_pose_marker, = ax1.plot([], [], 'ko', markersize=8, markerfacecolor='yellow') - ax1.annotate('', xy=(0, 0), xytext=(0, 0), - arrowprops=dict(arrowstyle='->', color='purple', lw=2), - visible=False) + map_scatter = ax1.scatter([], [], s=1, c="blue", alpha=0.3, label="Map") + current_scan_scatter = ax1.scatter( + [], [], s=10, c="red", alpha=0.8, label="Current scan" + ) + (true_line,) = ax1.plot([], [], "g-", linewidth=1, alpha=0.5, label="Ground truth") + (odom_line,) = ax1.plot([], [], "r--", linewidth=1, alpha=0.7, label="Odometry") + (frontend_line,) = ax1.plot([], [], "b-", linewidth=2, label="Frontend") + (current_pose_marker,) = ax1.plot( + [], [], "ko", markersize=8, markerfacecolor="yellow" + ) + ax1.annotate( + "", + xy=(0, 0), + xytext=(0, 0), + arrowprops=dict(arrowstyle="->", color="purple", lw=2), + visible=False, + ) ax1.set_xlim(xlim) ax1.set_ylim(ylim) - ax1.set_xlabel('X (m)') - ax1.set_ylabel('Y (m)') - ax1.set_title('Map + Trajectory') - ax1.legend(loc='upper right', fontsize=8) - ax1.set_aspect('equal') + ax1.set_xlabel("X (m)") + ax1.set_ylabel("Y (m)") + ax1.set_title("Map + Trajectory") + ax1.legend(loc="upper right", fontsize=8) + ax1.set_aspect("equal") ax1.grid(True, alpha=0.3) # Panel 2: Constraint graph ax2.set_xlim(xlim) ax2.set_ylim(ylim) - ax2.set_xlabel('X (m)') - ax2.set_ylabel('Y (m)') - ax2.set_title('Pose Graph Constraints') - ax2.set_aspect('equal') + ax2.set_xlabel("X (m)") + ax2.set_ylabel("Y (m)") + ax2.set_title("Pose Graph Constraints") + ax2.set_aspect("equal") ax2.grid(True, alpha=0.3) # Initialize trajectory line for Panel 2 (will show poses during optimization) - graph_traj_line, = ax2.plot([], [], 'b-', linewidth=1.5, alpha=0.6, label='Trajectory') + (graph_traj_line,) = ax2.plot( + [], [], "b-", linewidth=1.5, alpha=0.6, label="Trajectory" + ) # Add legend entries for constraint types # These will be shown when constraints appear - ax2.plot([], [], 'gray', linewidth=1, alpha=0.5, label='Odometry edges') - ax2.plot([], [], color='magenta', linestyle='--', linewidth=2.5, alpha=0.8, - label='Loop closure constraints') - ax2.legend(loc='upper right', fontsize=8) + ax2.plot([], [], "gray", linewidth=1, alpha=0.5, label="Odometry edges") + ax2.plot( + [], + [], + color="magenta", + linestyle="--", + linewidth=2.5, + alpha=0.8, + label="Loop closure constraints", + ) + ax2.legend(loc="upper right", fontsize=8) # Panel 3: Error plot ax3.set_xlim(0, n_poses) ax3.set_ylim(0, max(odom_errors) * 1.2) - ax3.set_xlabel('Pose Index') - ax3.set_ylabel('Position Error (m)') - ax3.set_title('Position Error vs Time') + ax3.set_xlabel("Pose Index") + ax3.set_ylabel("Position Error (m)") + ax3.set_title("Position Error vs Time") ax3.grid(True, alpha=0.3) - odom_error_line, = ax3.plot([], [], 'r-', label='Odometry', alpha=0.7) - frontend_error_line, = ax3.plot([], [], 'b-', label='Frontend', linewidth=2) - ax3.legend(loc='upper left', fontsize=8) + (odom_error_line,) = ax3.plot([], [], "r-", label="Odometry", alpha=0.7) + (frontend_error_line,) = ax3.plot([], [], "b-", label="Frontend", linewidth=2) + ax3.legend(loc="upper left", fontsize=8) # Storage for accumulated map points accumulated_map = [] @@ -1160,9 +1226,9 @@ def update(frame): current_scan_scatter.set_offsets(scan_global) # Update trajectories (up to current pose) - true_xy = np.array([[p[0], p[1]] for p in true_poses[:i+1]]) - odom_xy = np.array([[p[0], p[1]] for p in odom_poses[:i+1]]) - frontend_xy = np.array([[p[0], p[1]] for p in frontend_poses[:i+1]]) + true_xy = np.array([[p[0], p[1]] for p in true_poses[: i + 1]]) + odom_xy = np.array([[p[0], p[1]] for p in odom_poses[: i + 1]]) + frontend_xy = np.array([[p[0], p[1]] for p in frontend_poses[: i + 1]]) if len(true_xy) > 0: true_line.set_data(true_xy[:, 0], true_xy[:, 1]) @@ -1175,36 +1241,51 @@ def update(frame): current_pose_marker.set_data([frontend_poses[i][0]], [frontend_poses[i][1]]) # Update error plot - odom_error_line.set_data(range(i+1), odom_errors[:i+1]) - frontend_error_line.set_data(range(i+1), frontend_errors[:i+1]) + odom_error_line.set_data(range(i + 1), odom_errors[: i + 1]) + frontend_error_line.set_data(range(i + 1), frontend_errors[: i + 1]) # Draw odometry edge (constraint) if i > 0: - ax2.plot([frontend_poses[i-1][0], frontend_poses[i][0]], - [frontend_poses[i-1][1], frontend_poses[i][1]], - 'gray', linewidth=1, alpha=0.5) + ax2.plot( + [frontend_poses[i - 1][0], frontend_poses[i][0]], + [frontend_poses[i - 1][1], frontend_poses[i][1]], + "gray", + linewidth=1, + alpha=0.5, + ) # Check for loop closure at this timestep if i in loop_closure_times: lc_i, lc_j = loop_closure_times[i] # Draw loop closure ONLY in constraint graph (Panel 2) # Draw edge (thick magenta dashed line) - line = ax2.plot([frontend_poses[lc_i][0], frontend_poses[lc_j][0]], - [frontend_poses[lc_i][1], frontend_poses[lc_j][1]], - color='magenta', linestyle='--', linewidth=2.5, alpha=0.8)[0] + line = ax2.plot( + [frontend_poses[lc_i][0], frontend_poses[lc_j][0]], + [frontend_poses[lc_i][1], frontend_poses[lc_j][1]], + color="magenta", + linestyle="--", + linewidth=2.5, + alpha=0.8, + )[0] loop_edges.append(line) # Draw dots at loop closure poses - ax2.scatter([frontend_poses[lc_i][0], frontend_poses[lc_j][0]], - [frontend_poses[lc_i][1], frontend_poses[lc_j][1]], - c='magenta', s=100, edgecolors='white', linewidths=2, - zorder=10, alpha=0.9) + ax2.scatter( + [frontend_poses[lc_i][0], frontend_poses[lc_j][0]], + [frontend_poses[lc_i][1], frontend_poses[lc_j][1]], + c="magenta", + s=100, + edgecolors="white", + linewidths=2, + zorder=10, + alpha=0.9, + ) # Mark on error plot (Panel 3) - ax3.axvline(x=i, color='magenta', linestyle='--', alpha=0.5) + ax3.axvline(x=i, color="magenta", linestyle="--", alpha=0.5) # Update title with progress - ax1.set_title(f'Map + Trajectory (pose {i+1}/{n_poses})') + ax1.set_title(f"Map + Trajectory (pose {i+1}/{n_poses})") else: # Phase 2: Optimization animation @@ -1214,7 +1295,9 @@ def update(frame): # Interpolate from frontend to optimized interp_poses = [] for j in range(n_poses): - interp_pose = (1 - alpha) * frontend_poses[j] + alpha * optimized_poses[j] + interp_pose = (1 - alpha) * frontend_poses[j] + alpha * optimized_poses[ + j + ] interp_poses.append(interp_pose) # Rebuild map with interpolated poses @@ -1243,26 +1326,34 @@ def update(frame): current_pose_marker.set_data([], []) # Update error (interpolate) - interp_errors = [(1 - alpha) * frontend_errors[j] + alpha * optimized_errors[j] - for j in range(n_poses)] + interp_errors = [ + (1 - alpha) * frontend_errors[j] + alpha * optimized_errors[j] + for j in range(n_poses) + ] frontend_error_line.set_data(range(n_poses), interp_errors) # Update title if opt_frame == 0: - ax1.set_title('Optimization: Initial') - ax2.set_title('Pose Graph (optimizing...)') + ax1.set_title("Optimization: Initial") + ax2.set_title("Pose Graph (optimizing...)") elif opt_frame == n_opt_frames - 1: - ax1.set_title('Optimization: Complete!') - ax2.set_title(f'Pose Graph ({len(loop_closures)} loop closures)') + ax1.set_title("Optimization: Complete!") + ax2.set_title(f"Pose Graph ({len(loop_closures)} loop closures)") # Add optimized trajectory as dashed line opt_xy = np.array([[p[0], p[1]] for p in optimized_poses]) - ax1.plot(opt_xy[:, 0], opt_xy[:, 1], 'c--', linewidth=2, - label='Optimized', alpha=0.8) - ax1.legend(loc='upper right', fontsize=8) + ax1.plot( + opt_xy[:, 0], + opt_xy[:, 1], + "c--", + linewidth=2, + label="Optimized", + alpha=0.8, + ) + ax1.legend(loc="upper right", fontsize=8) else: progress = int(alpha * 100) - ax1.set_title(f'Optimization: {progress}%') + ax1.set_title(f"Optimization: {progress}%") return artists @@ -1274,8 +1365,13 @@ def update(frame): print(f" Creating animation with {total_frames} frames...") output_path = Path(output_path) written = save_animation( - fig, update, total_frames, output_path.parent, output_path.stem, - fps=fps, init=init, + fig, + update, + total_frames, + output_path.parent, + output_path.stem, + fps=fps, + init=init, ) plt.close(fig) @@ -1309,8 +1405,8 @@ def plot_slam_results( gs = fig.add_gridspec(2, 3, hspace=0.25, wspace=0.25) ax_traj = fig.add_subplot(gs[:, 0]) # Trajectories (full height, left) ax_map_before = fig.add_subplot(gs[0, 1]) # Map before (top middle) - ax_map_after = fig.add_subplot(gs[1, 1]) # Map after (bottom middle) - ax_error = fig.add_subplot(gs[:, 2]) # Errors (full height, right) + ax_map_after = fig.add_subplot(gs[1, 1]) # Map after (bottom middle) + ax_error = fig.add_subplot(gs[:, 2]) # Errors (full height, right) axes = [ax_traj, ax_map_before, ax_map_after, ax_error] else: fig, axes = plt.subplots(1, 2, figsize=(16, 7)) @@ -1339,7 +1435,9 @@ def plot_slam_results( label="Ground Truth", alpha=0.7, ) - ax_traj.scatter(true_xy[0, 0], true_xy[0, 1], c="green", marker="o", s=100, zorder=5) + ax_traj.scatter( + true_xy[0, 0], true_xy[0, 1], c="green", marker="o", s=100, zorder=5 + ) # Plot odometry (with drift) odom_xy = np.array([[p[0], p[1]] for p in odom_poses]) @@ -1380,8 +1478,11 @@ def plot_slam_results( ax_traj.set_xlabel("X [m]", fontsize=12) ax_traj.set_ylabel("Y [m]", fontsize=12) - ax_traj.set_title("4 Trajectories: Truth / Odom / Front-end / Optimized", - fontsize=13, fontweight="bold") + ax_traj.set_title( + "4 Trajectories: Truth / Odom / Front-end / Optimized", + fontsize=13, + fontweight="bold", + ) ax_traj.legend(fontsize=10) ax_traj.grid(True, alpha=0.3) ax_traj.axis("equal") @@ -1418,13 +1519,22 @@ def plot_slam_results( alpha=0.3, label="Map Points (Front-end)", ) - ax_map_before.plot(frontend_xy[:, 0], frontend_xy[:, 1], "orange", - linestyle="-.", linewidth=1.5, alpha=0.6) - ax_map_before.scatter(frontend_xy[0, 0], frontend_xy[0, 1], c="orange", - marker="o", s=80, zorder=5) + ax_map_before.plot( + frontend_xy[:, 0], + frontend_xy[:, 1], + "orange", + linestyle="-.", + linewidth=1.5, + alpha=0.6, + ) + ax_map_before.scatter( + frontend_xy[0, 0], frontend_xy[0, 1], c="orange", marker="o", s=80, zorder=5 + ) ax_map_before.set_xlabel("X [m]", fontsize=12) ax_map_before.set_ylabel("Y [m]", fontsize=12) - ax_map_before.set_title("Map Before Backend (Front-end)", fontsize=13, fontweight="bold") + ax_map_before.set_title( + "Map Before Backend (Front-end)", fontsize=13, fontweight="bold" + ) ax_map_before.legend(fontsize=9, loc="upper right") ax_map_before.grid(True, alpha=0.3) ax_map_before.axis("equal") @@ -1449,13 +1559,17 @@ def plot_slam_results( label="Map Points (Optimized)", ) ax_map_after.plot(opt_xy[:, 0], opt_xy[:, 1], "b-", linewidth=1.5, alpha=0.6) - ax_map_after.scatter(opt_xy[0, 0], opt_xy[0, 1], c="blue", marker="o", s=80, zorder=5) + ax_map_after.scatter( + opt_xy[0, 0], opt_xy[0, 1], c="blue", marker="o", s=80, zorder=5 + ) # Loop closures NOT shown in static map (only in animation) ax_map_after.set_xlabel("X [m]", fontsize=12) ax_map_after.set_ylabel("Y [m]", fontsize=12) - ax_map_after.set_title("Map After Backend (Optimized)", fontsize=13, fontweight="bold") + ax_map_after.set_title( + "Map After Backend (Optimized)", fontsize=13, fontweight="bold" + ) ax_map_after.legend(fontsize=9, loc="upper right") ax_map_after.grid(True, alpha=0.3) ax_map_after.axis("equal") @@ -1464,10 +1578,16 @@ def plot_slam_results( # Compute position errors odom_errors = np.array( - [np.linalg.norm(odom_poses[i][:2] - true_poses[i][:2]) for i in range(len(true_poses))] + [ + np.linalg.norm(odom_poses[i][:2] - true_poses[i][:2]) + for i in range(len(true_poses)) + ] ) frontend_errors = np.array( - [np.linalg.norm(frontend_poses[i][:2] - true_poses[i][:2]) for i in range(len(true_poses))] + [ + np.linalg.norm(frontend_poses[i][:2] - true_poses[i][:2]) + for i in range(len(true_poses)) + ] ) opt_errors = np.array( [ @@ -1482,8 +1602,13 @@ def plot_slam_results( timesteps, odom_errors, "r--", linewidth=2, label="Odometry Error", alpha=0.7 ) ax_error.plot( - timesteps, frontend_errors, "orange", linestyle="-.", linewidth=1.5, - label="Front-end Error", alpha=0.7 + timesteps, + frontend_errors, + "orange", + linestyle="-.", + linewidth=1.5, + label="Front-end Error", + alpha=0.7, ) ax_error.plot( timesteps, opt_errors, "b-", linewidth=2, label="Optimized Error", alpha=0.8 @@ -1497,11 +1622,21 @@ def plot_slam_results( closure_at = [j for _, j, _, _ in loop_closures] rug = blended_transform_factory(ax_error.transData, ax_error.transAxes) ax_error.vlines( - closure_at, 0.0, 0.04, transform=rug, - color="magenta", alpha=0.7, linewidth=1, + closure_at, + 0.0, + 0.04, + transform=rug, + color="magenta", + alpha=0.7, + linewidth=1, + ) + ax_error.plot( + [], + [], + color="magenta", + linewidth=1, + label=f"Loop closure ({len(closure_at)})", ) - ax_error.plot([], [], color="magenta", linewidth=1, - label=f"Loop closure ({len(closure_at)})") ax_error.set_xlabel("Pose Index", fontsize=12) ax_error.set_ylabel("Position Error [m]", fontsize=12) @@ -1513,7 +1648,9 @@ def plot_slam_results( fig.suptitle( "Complete SLAM Pipeline: Odometry → Front-end (Scan-to-Map ICP) → " "Loop Closure → Backend Optimization", - fontsize=14, fontweight="bold", y=0.98 + fontsize=14, + fontweight="bold", + y=0.98, ) # Save to figs directory with deterministic filename. Resolved from this @@ -1526,6 +1663,7 @@ def plot_slam_results( # redirects a test run away from the committed figure -- and it also emits # svg and pdf, which the raw savefig did not. from pathlib import Path + figs_dir = Path(__file__).resolve().parent / "figs" paths = save_figure(fig, figs_dir, "slam_with_maps") print(f"\n[OK] Saved figure: {paths[0]}") @@ -1540,19 +1678,19 @@ def run_with_inline_data( animate: bool = False, ): """Run complete pose graph SLAM example with inline data. - + Args: use_loop_oracle: If True, use distance-based oracle instead of observation-based. Default is False (observation-based). trajectory_type: "square" (default) or "corridor". n_laps: Number of laps for square trajectory (default: 2). animate: If True, generate animated GIF showing SLAM pipeline. - + This mode generates: - A square loop trajectory with multiple laps (for loop closure) - Dense wall scans (suitable for scan-to-map matching) - Moderate odometry drift (correctable by SLAM) - + Demonstrates the full SLAM pipeline: - Front-end: Prediction -> Scan-to-Map -> Map Update - Loop closure: Observation-based detection @@ -1591,7 +1729,9 @@ def run_with_inline_data( # a multiple of 2pi. Unwrapped, a perfect closure reports 360 deg. closure_yaw = abs(wrap_angle(end_pose[2] - start_pose[2])) print(f" Generated {n_poses} poses (square, {n_laps} laps)") - print(f" Loop closure check: dist={closure_dist:.3f}m, yaw={np.degrees(closure_yaw):.1f}deg") + print( + f" Loop closure check: dist={closure_dist:.3f}m, yaw={np.degrees(closure_yaw):.1f}deg" + ) # Compute trajectory bounds all_x = [p[0] for p in true_poses] @@ -1628,9 +1768,14 @@ def run_with_inline_data( print(f" Created {len(walls)} wall segments (parallel walls)") # Landmarks at corners for visualization - landmarks = np.array([ - [-1.0, -2.0], [20.0, -2.0], [20.0, 2.0], [-1.0, 2.0], - ]) + landmarks = np.array( + [ + [-1.0, -2.0], + [20.0, -2.0], + [20.0, 2.0], + [-1.0, 2.0], + ] + ) # ------------------------------------------------------------------------ # 3. Simulate Odometry with Moderate Drift @@ -1658,11 +1803,12 @@ def run_with_inline_data( scans = [] for i, pose in enumerate(true_poses): scan = generate_scan_with_occlusion( - pose, walls, - num_rays=360, # 360-degree coverage (1° resolution) - max_range=20.0, # Reasonable range for indoor LiDAR - noise_std=0.02, # 2cm measurement noise - min_range=0.1, # 10cm minimum range (sensor blind zone) + pose, + walls, + num_rays=360, # 360-degree coverage (1° resolution) + max_range=20.0, # Reasonable range for indoor LiDAR + noise_std=0.02, # 2cm measurement noise + min_range=0.1, # 10cm minimum range (sensor blind zone) ) scans.append(scan) avg_points = np.mean([len(s) for s in scans]) @@ -1680,8 +1826,8 @@ def run_with_inline_data( # Initialize front-end with submap for scan-to-map alignment # Use first odometry pose as initial pose (trajectory starts at origin for square) frontend = SlamFrontend2D( - submap_voxel_size=0.2, # Voxel size for map downsampling - min_map_points=5, # Minimum points needed for ICP + submap_voxel_size=0.2, # Voxel size for map downsampling + min_map_points=5, # Minimum points needed for ICP # RMS alignment error per correspondence, in metres. Matches here land # at 0.06-0.12 m, roughly the 0.2 m voxel quantisation floor, so this # rejects genuinely bad alignments without cutting into good ones. @@ -1711,21 +1857,25 @@ def run_with_inline_data( result = frontend.step(i, odom_delta, scans[i]) # Store results - frontend_poses.append(result['pose_est']) - pred_poses.append(result['pose_pred']) - match_qualities.append(result['match_quality']) + frontend_poses.append(result["pose_est"]) + pred_poses.append(result["pose_pred"]) + match_qualities.append(result["match_quality"]) # Compute correction magnitude (difference between prediction and estimate) if i > 0: correction_xy = np.linalg.norm( - result['pose_est'][:2] - result['pose_pred'][:2] + result["pose_est"][:2] - result["pose_pred"][:2] ) corrections.append(correction_xy) # Compute front-end statistics n_converged = sum(1 for mq in match_qualities if mq.converged) converged_qualities = [mq for mq in match_qualities if mq.converged] - avg_residual = np.mean([mq.residual for mq in converged_qualities]) if converged_qualities else 0.0 + avg_residual = ( + np.mean([mq.residual for mq in converged_qualities]) + if converged_qualities + else 0.0 + ) avg_correction = np.mean(corrections) if corrections else 0.0 print(f"\n Processed {n_poses} steps") @@ -1775,7 +1925,7 @@ def run_with_inline_data( scans, use_observation_based=not use_loop_oracle, # Default: observation-based distance_threshold=None, # No distance gating by default - min_time_separation=10 + min_time_separation=10, ) print(f" Detected {len(loop_closures)} loop closures") @@ -1814,8 +1964,12 @@ def run_with_inline_data( loop_information=loop_info, ) - print(f" Pose graph: {len(graph.variables)} variables, {len(graph.factors)} factors") - print(f" Factors: 1 prior + {len(odometry_measurements)} odometry + {len(loop_measurements)} loop closures") + print( + f" Pose graph: {len(graph.variables)} variables, {len(graph.factors)} factors" + ) + print( + f" Factors: 1 prior + {len(odometry_measurements)} odometry + {len(loop_measurements)} loop closures" + ) # ------------------------------------------------------------------------ # 9. Optimize Pose Graph (Back-End) @@ -1849,10 +2003,16 @@ def run_with_inline_data( [np.linalg.norm(odom_poses[i][:2] - true_poses[i][:2]) for i in range(n_poses)] ) frontend_errors = np.array( - [np.linalg.norm(frontend_poses[i][:2] - true_poses[i][:2]) for i in range(n_poses)] + [ + np.linalg.norm(frontend_poses[i][:2] - true_poses[i][:2]) + for i in range(n_poses) + ] ) opt_errors = np.array( - [np.linalg.norm(optimized_poses[i][:2] - true_poses[i][:2]) for i in range(n_poses)] + [ + np.linalg.norm(optimized_poses[i][:2] - true_poses[i][:2]) + for i in range(n_poses) + ] ) odom_rmse = np.sqrt(np.mean(odom_errors**2)) @@ -1861,7 +2021,9 @@ def run_with_inline_data( print(f" Odometry RMSE: {odom_rmse:.4f} m (baseline)") print(f" Frontend RMSE: {frontend_rmse:.4f} m (scan-to-map corrected)") - print(f" Optimized RMSE: {opt_rmse:.4f} m (backend with {len(loop_closures)} loop closures)") + print( + f" Optimized RMSE: {opt_rmse:.4f} m (backend with {len(loop_closures)} loop closures)" + ) if odom_rmse > 0: frontend_improvement = (1 - frontend_rmse / odom_rmse) * 100 @@ -1878,8 +2040,13 @@ def run_with_inline_data( # ------------------------------------------------------------------------ print("\n11. Visualizing results...") plot_slam_results( - true_poses, odom_poses, frontend_poses, optimized_poses, - landmarks, loop_closures, scans + true_poses, + odom_poses, + frontend_poses, + optimized_poses, + landmarks, + loop_closures, + scans, ) # Generate animation if requested @@ -1887,8 +2054,15 @@ def run_with_inline_data( print("\n12. Generating SLAM animation...") gif_path = f"ch7_slam/figs/slam_pipeline_{trajectory_type}.gif" create_slam_animation( - true_poses, odom_poses, frontend_poses, optimized_poses, - scans, loop_closures, trajectory_type, gif_path, fps=5 + true_poses, + odom_poses, + frontend_poses, + optimized_poses, + scans, + loop_closures, + trajectory_type, + gif_path, + fps=5, ) print() @@ -1897,7 +2071,9 @@ def run_with_inline_data( print("=" * 70) print() print("Summary:") - print(f" - Trajectory: {trajectory_type}, {n_poses} poses, {n_laps if trajectory_type == 'square' else 1} lap(s)") + print( + f" - Trajectory: {trajectory_type}, {n_poses} poses, {n_laps if trajectory_type == 'square' else 1} lap(s)" + ) print(f" - Loop closures: {len(loop_closures)} (observation-based detection)") print(f" - Odometry drift: {final_drift:.3f} m") print(f" - Odometry RMSE: {odom_rmse:.4f} m (baseline)") @@ -1957,7 +2133,7 @@ def run_with_inline_data( "odom": round(odom_rmse, 4), "frontend": round(frontend_rmse, 4), "optimized": round(opt_rmse, 4), - } + }, } print(f"\n[SLAM_SUMMARY] {json.dumps(summary)}") @@ -1980,29 +2156,39 @@ def main(): # Run with pre-generated dataset python example_pose_graph_slam.py --data ch7_slam_2d_square - """ + """, ) parser.add_argument( - "--data", type=str, default=None, - help="Dataset name or path (e.g., 'ch7_slam_2d_square' or full path)" + "--data", + type=str, + default=None, + help="Dataset name or path (e.g., 'ch7_slam_2d_square' or full path)", ) parser.add_argument( - "--trajectory", type=str, default="square", + "--trajectory", + type=str, + default="square", choices=["square", "corridor"], - help="Trajectory type for inline mode (default: square)" + help="Trajectory type for inline mode (default: square)", ) parser.add_argument( - "--laps", type=int, default=3, - help="Number of laps for square trajectory (default: 3)" + "--laps", + type=int, + default=3, + help="Number of laps for square trajectory (default: 3)", ) parser.add_argument( - "--loop_oracle", action="store_true", default=False, + "--loop_oracle", + action="store_true", + default=False, help="[DEPRECATED] Use distance-based oracle for loop closure instead of " - "observation-based detection. For comparison/debugging only." + "observation-based detection. For comparison/debugging only.", ) parser.add_argument( - "--animate", action="store_true", default=False, - help="Generate animated GIF showing SLAM pipeline evolution" + "--animate", + action="store_true", + default=False, + help="Generate animated GIF showing SLAM pipeline evolution", ) args = parser.parse_args() @@ -2013,7 +2199,9 @@ def main(): if not data_path.exists(): data_path = resolve_data_path(Path("data/sim") / args.data) if not data_path.exists(): - print(f"Error: Dataset not found at '{args.data}' or 'data/sim/{args.data}'") + print( + f"Error: Dataset not found at '{args.data}' or 'data/sim/{args.data}'" + ) print("\nAvailable datasets:") sim_dir = resolve_data_path(Path("data/sim")) if sim_dir.exists(): @@ -2035,4 +2223,3 @@ def main(): if __name__ == "__main__": main() - diff --git a/ch7_slam/example_scan_matching_visualization.py b/ch7_slam/example_scan_matching_visualization.py index 3501ee7..b98ae92 100644 --- a/ch7_slam/example_scan_matching_visualization.py +++ b/ch7_slam/example_scan_matching_visualization.py @@ -177,15 +177,32 @@ def plot_icp_correspondences(max_pairs: int = 45) -> plt.Figure: matched_source, matched_target, _ = find_correspondences( moved, target, max_distance=1.0 ) - ax.plot(target[:, 0], target[:, 1], ".", color="#1f77b4", - markersize=3, label="target scan") - ax.plot(moved[:, 0], moved[:, 1], ".", color="#d62728", - markersize=3, label="source scan") + ax.plot( + target[:, 0], + target[:, 1], + ".", + color="#1f77b4", + markersize=3, + label="target scan", + ) + ax.plot( + moved[:, 0], + moved[:, 1], + ".", + color="#d62728", + markersize=3, + label="source scan", + ) stride = max(1, len(matched_source) // max_pairs) - for src_pt, tgt_pt in zip(matched_source[::stride], - matched_target[::stride]): - ax.plot([src_pt[0], tgt_pt[0]], [src_pt[1], tgt_pt[1]], - "-", color="0.4", linewidth=0.7, alpha=0.8) + for src_pt, tgt_pt in zip(matched_source[::stride], matched_target[::stride]): + ax.plot( + [src_pt[0], tgt_pt[0]], + [src_pt[1], tgt_pt[1]], + "-", + color="0.4", + linewidth=0.7, + alpha=0.8, + ) ax.set_title( f"{title}\n{len(matched_source)} correspondences, Eq. (7.11)", fontsize=10, @@ -196,8 +213,9 @@ def plot_icp_correspondences(max_pairs: int = 45) -> plt.Figure: ax.legend(fontsize=8, loc="upper right") axes[0].set_ylabel("y [m]") - axes[2].semilogy(np.arange(1, len(residuals) + 1), residuals, - "o-", color="#2ca02c", markersize=4) + axes[2].semilogy( + np.arange(1, len(residuals) + 1), residuals, "o-", color="#2ca02c", markersize=4 + ) axes[2].set_title("Eq. (7.10) objective per iteration", fontsize=10) axes[2].set_xlabel("iteration") axes[2].set_ylabel("sum of squared distances") @@ -225,8 +243,15 @@ def plot_ndt_voxels(voxel_size: float = 1.0) -> plt.Figure: ndt_map = build_ndt_map(target, voxel_size=voxel_size) fig, ax = plt.subplots(figsize=(7.5, 6.5)) - ax.plot(target[:, 0], target[:, 1], ".", color="0.55", markersize=3, - label="target scan", zorder=1) + ax.plot( + target[:, 0], + target[:, 1], + ".", + color="0.55", + markersize=3, + label="target scan", + zorder=1, + ) for cell in ndt_map.values(): mean = np.asarray(cell["mean"], dtype=float) @@ -267,9 +292,9 @@ def plot_ndt_voxels(voxel_size: float = 1.0) -> plt.Figure: return fig -def plot_ndt_score_surface(voxel_size: float = 1.0, - half_width: float = 1.6, - resolution: int = 90) -> plt.Figure: +def plot_ndt_score_surface( + voxel_size: float = 1.0, half_width: float = 1.6, resolution: int = 90 +) -> plt.Figure: """Figure 3: the (7.16) objective surface and the descent path. Args: @@ -312,31 +337,59 @@ def plot_ndt_score_surface(voxel_size: float = 1.0, # Runs that differ only in step size now agree, so report the spread rather # than plotting each one on top of the others. by_step = { - step: ndt_align(source, target, voxel_size=voxel_size, step_size=step, - max_iterations=200)[0] + step: ndt_align( + source, target, voxel_size=voxel_size, step_size=step, max_iterations=200 + )[0] for step in (0.05, 0.1, 0.3, 0.5, 1.0) } spread = max( float(np.linalg.norm(a[:2] - b[:2])) - for a in by_step.values() for b in by_step.values() + for a in by_step.values() + for b in by_step.values() ) fig, axes = plt.subplots(1, 2, figsize=(13.5, 5.6)) - mesh = axes[0].pcolormesh( - offsets, offsets, scores, shading="auto", cmap="viridis" - ) + mesh = axes[0].pcolormesh(offsets, offsets, scores, shading="auto", cmap="viridis") fig.colorbar(mesh, ax=axes[0], label="NDT score (lower is better)") - axes[0].plot(path[:, 0], path[:, 1], "-o", color="#ff7f0e", markersize=4, - linewidth=1.4, markeredgecolor="k", markeredgewidth=0.4, - label=f"Gauss-Newton path ({iterations} iters)") - axes[0].plot(TRUE_MOTION[0], TRUE_MOTION[1], "*", color="white", - markersize=16, markeredgecolor="k", - label="true alignment") - axes[0].plot(aligned_pose[0], aligned_pose[1], "o", color="#d62728", - markersize=8, markeredgecolor="k", label="ndt_align result") - axes[0].plot(0.0, 0.0, "s", color="#ff7f0e", markersize=8, - markeredgecolor="k", label="initial guess") + axes[0].plot( + path[:, 0], + path[:, 1], + "-o", + color="#ff7f0e", + markersize=4, + linewidth=1.4, + markeredgecolor="k", + markeredgewidth=0.4, + label=f"Gauss-Newton path ({iterations} iters)", + ) + axes[0].plot( + TRUE_MOTION[0], + TRUE_MOTION[1], + "*", + color="white", + markersize=16, + markeredgecolor="k", + label="true alignment", + ) + axes[0].plot( + aligned_pose[0], + aligned_pose[1], + "o", + color="#d62728", + markersize=8, + markeredgecolor="k", + label="ndt_align result", + ) + axes[0].plot( + 0.0, + 0.0, + "s", + color="#ff7f0e", + markersize=8, + markeredgecolor="k", + label="initial guess", + ) axes[0].set_xlabel("translation x [m]") axes[0].set_ylabel("translation y [m]") axes[0].set_title( @@ -350,10 +403,14 @@ def plot_ndt_score_surface(voxel_size: float = 1.0, # Slice through the optimum so the basin and the roughness are both visible. row_at_truth = int(np.argmin(np.abs(offsets - TRUE_MOTION[1]))) - axes[1].plot(offsets, scores[row_at_truth, :], "-", color="#1f77b4", - linewidth=1.6) - axes[1].axvline(TRUE_MOTION[0], color="#d62728", linestyle="--", - linewidth=1.2, label="true alignment") + axes[1].plot(offsets, scores[row_at_truth, :], "-", color="#1f77b4", linewidth=1.6) + axes[1].axvline( + TRUE_MOTION[0], + color="#d62728", + linestyle="--", + linewidth=1.2, + label="true alignment", + ) axes[1].legend(fontsize=8) axes[1].set_xlabel( f"translation x [m] (y = {offsets[row_at_truth]:.2f} m, true yaw)" @@ -379,8 +436,9 @@ def plot_ndt_score_surface(voxel_size: float = 1.0, return fig -def plot_convergence_basin(grid: int = 9, span: float = 1.6, - tolerance: float = 0.25) -> plt.Figure: +def plot_convergence_basin( + grid: int = 9, span: float = 1.6, tolerance: float = 0.25 +) -> plt.Figure: """Figure 4: which initial guesses each method recovers from. Args: @@ -403,8 +461,10 @@ def plot_convergence_basin(grid: int = 9, span: float = 1.6, guess = np.array([dx, dy, 0.0]) pose, _, _, _ = icp_point_to_point( - source, target, initial_pose=guess.copy(), - max_correspondence_distance=1.0 + source, + target, + initial_pose=guess.copy(), + max_correspondence_distance=1.0, ) icp_ok[row, col] = np.linalg.norm(pose[:2] - truth) < tolerance @@ -422,8 +482,10 @@ def plot_convergence_basin(grid: int = 9, span: float = 1.6, for dy in offsets: for dx in offsets: pose, _, _, _ = ndt_align( - source, target, initial_pose=np.array([dx, dy, 0.0]), - voxel_size=voxel_size + source, + target, + initial_pose=np.array([dx, dy, 0.0]), + voxel_size=voxel_size, ) count += np.linalg.norm(pose[:2] - truth) < tolerance ndt_counts.append(count) @@ -433,10 +495,18 @@ def plot_convergence_basin(grid: int = 9, span: float = 1.6, (axes[0], icp_ok, "ICP, max correspondence 1.0 m"), (axes[1], ndt_ok, "NDT, voxel 1.0 m"), ): - ax.pcolormesh(offsets, offsets, mask.astype(float), shading="auto", - cmap="RdYlGn", vmin=0.0, vmax=1.0) - ax.plot(truth[0], truth[1], "*", color="white", markersize=16, - markeredgecolor="k") + ax.pcolormesh( + offsets, + offsets, + mask.astype(float), + shading="auto", + cmap="RdYlGn", + vmin=0.0, + vmax=1.0, + ) + ax.plot( + truth[0], truth[1], "*", color="white", markersize=16, markeredgecolor="k" + ) ax.set_aspect("equal") ax.set_xlabel("initial x offset [m]") ax.set_title( @@ -446,10 +516,16 @@ def plot_convergence_basin(grid: int = 9, span: float = 1.6, ) axes[0].set_ylabel("initial y offset [m]") - axes[2].plot(voxel_sizes, ndt_counts, "o-", color="#1f77b4", - markersize=6, label="NDT") - axes[2].axhline(icp_ok.sum(), color="#d62728", linestyle="--", - linewidth=1.4, label="ICP (for reference)") + axes[2].plot( + voxel_sizes, ndt_counts, "o-", color="#1f77b4", markersize=6, label="NDT" + ) + axes[2].axhline( + icp_ok.sum(), + color="#d62728", + linestyle="--", + linewidth=1.4, + label="ICP (for reference)", + ) axes[2].set_xlabel("NDT voxel size [m]") axes[2].set_ylabel(f"starts converging (of {icp_ok.size})") axes[2].set_ylim(0, icp_ok.size) @@ -470,8 +546,7 @@ def plot_convergence_basin(grid: int = 9, span: float = 1.6, return fig -def animate_icp_convergence(max_iterations: int = 18, - max_pairs: int = 45) -> tuple: +def animate_icp_convergence(max_iterations: int = 18, max_pairs: int = 45) -> tuple: """Build the ICP convergence animation, Section 7.3.1. The static figure shows the first and last iteration; what it cannot show @@ -517,15 +592,32 @@ def update(frame: int): moved, target, max_distance=1.0 ) - axes[0].plot(target[:, 0], target[:, 1], ".", color="#1f77b4", - markersize=3, label="target scan") - axes[0].plot(moved[:, 0], moved[:, 1], ".", color="#d62728", - markersize=3, label="source scan") + axes[0].plot( + target[:, 0], + target[:, 1], + ".", + color="#1f77b4", + markersize=3, + label="target scan", + ) + axes[0].plot( + moved[:, 0], + moved[:, 1], + ".", + color="#d62728", + markersize=3, + label="source scan", + ) stride = max(1, len(matched_source) // max_pairs) - for src_pt, tgt_pt in zip(matched_source[::stride], - matched_target[::stride]): - axes[0].plot([src_pt[0], tgt_pt[0]], [src_pt[1], tgt_pt[1]], - "-", color="0.4", linewidth=0.7, alpha=0.8) + for src_pt, tgt_pt in zip(matched_source[::stride], matched_target[::stride]): + axes[0].plot( + [src_pt[0], tgt_pt[0]], + [src_pt[1], tgt_pt[1]], + "-", + color="0.4", + linewidth=0.7, + alpha=0.8, + ) axes[0].set_xlim(-6.5, 6.5) axes[0].set_ylim(-5.5, 5.5) axes[0].set_aspect("equal") @@ -539,16 +631,19 @@ def update(frame: int): fontsize=10, ) - axes[1].semilogy(np.arange(1, frame + 2), residuals[: frame + 1], - "o-", color="#2ca02c", markersize=4) + axes[1].semilogy( + np.arange(1, frame + 2), + residuals[: frame + 1], + "o-", + color="#2ca02c", + markersize=4, + ) axes[1].set_xlim(0.5, n_frames + 0.5) axes[1].set_ylim(min(residuals) * 0.6, max(residuals) * 1.6) axes[1].grid(alpha=0.3, which="both") axes[1].set_xlabel("iteration") axes[1].set_ylabel("sum of squared distances") - axes[1].set_title( - f"Eq. (7.10) objective: {residuals[frame]:.2f}", fontsize=10 - ) + axes[1].set_title(f"Eq. (7.10) objective: {residuals[frame]:.2f}", fontsize=10) fig.suptitle( "ICP convergence, Eqs. (7.10)-(7.11): correspondences are " @@ -570,16 +665,20 @@ def main() -> None: "--out-dir", default=str(FIGS_DIR), help="Output directory for figures" ) parser.add_argument( - "--animate", action="store_true", default=False, - help="Also render the ICP convergence GIF (slower)" + "--animate", + action="store_true", + default=False, + help="Also render the ICP convergence GIF (slower)", ) args = parser.parse_args() print("=" * 70) print("Chapter 7, Section 7.3: Scan Matching Visualization") print("=" * 70) - print(f"True motion between scans: dx={TRUE_MOTION[0]:.2f} m, " - f"dy={TRUE_MOTION[1]:.2f} m, dyaw={np.rad2deg(TRUE_MOTION[2]):.1f} deg") + print( + f"True motion between scans: dx={TRUE_MOTION[0]:.2f} m, " + f"dy={TRUE_MOTION[1]:.2f} m, dyaw={np.rad2deg(TRUE_MOTION[2]):.1f} deg" + ) print() figures = [ @@ -595,8 +694,9 @@ def main() -> None: if args.animate: fig, update, n_frames = animate_icp_convergence() - path = save_animation(fig, update, n_frames, args.out_dir, - "ch7_icp_convergence", fps=4) + path = save_animation( + fig, update, n_frames, args.out_dir, "ch7_icp_convergence", fps=4 + ) plt.close(fig) size_mb = path.stat().st_size / (1024 * 1024) print(f" saved {path.name}: {n_frames} frames, {size_mb:.2f} MB") diff --git a/ch7_slam/example_slam_frontend.py b/ch7_slam/example_slam_frontend.py index 5c06425..35b5e7b 100644 --- a/ch7_slam/example_slam_frontend.py +++ b/ch7_slam/example_slam_frontend.py @@ -44,10 +44,10 @@ def generate_simple_trajectory(n_poses: int = 10) -> list: """Generate simple straight-line trajectory. - + Args: n_poses: Number of poses. - + Returns: List of poses [x, y, yaw]. """ @@ -60,11 +60,11 @@ def generate_simple_trajectory(n_poses: int = 10) -> list: def generate_wall_scan(pose: np.ndarray, wall_x: float = 5.0) -> np.ndarray: """Generate synthetic scan of a wall parallel to Y-axis. - + Args: pose: Robot pose [x, y, yaw]. wall_x: X-coordinate of wall in map frame. - + Returns: Scan points in robot frame. """ @@ -91,9 +91,7 @@ def generate_wall_scan(pose: np.ndarray, wall_x: float = 5.0) -> np.ndarray: return scan -def run_frontend_demo( - n_poses: int = 10, seed: int = DEFAULT_SEED -) -> Dict[str, object]: +def run_frontend_demo(n_poses: int = 10, seed: int = DEFAULT_SEED) -> Dict[str, object]: """Run the front-end loop over a synthetic straight-line walk. Kept separate from ``main`` so the figure -- and the tests that pin what @@ -118,11 +116,13 @@ def run_frontend_demo( odom_poses = [true_poses[0].copy()] for i in range(1, n_poses): true_delta = se2_relative(true_poses[i - 1], true_poses[i]) - noisy_delta = true_delta + np.array([ - np.random.normal(0, 0.05), - np.random.normal(0, 0.02), - np.random.normal(0, 0.01), - ]) + noisy_delta = true_delta + np.array( + [ + np.random.normal(0, 0.05), + np.random.normal(0, 0.02), + np.random.normal(0, 0.01), + ] + ) # Simplified composition, valid only because this walk holds yaw at 0. odom_poses.append(odom_poses[-1] + noisy_delta) @@ -139,7 +139,7 @@ def run_frontend_demo( odom_delta = se2_relative(odom_poses[i - 1], odom_poses[i]) result = frontend.step(i, odom_delta, scans[i]) - frontend_poses.append(result['pose_est']) + frontend_poses.append(result["pose_est"]) steps.append(result) true_xy = np.array([pose[:2] for pose in true_poses]) @@ -147,13 +147,13 @@ def run_frontend_demo( frontend_xy = np.array([pose[:2] for pose in frontend_poses]) return { - 'true_xy': true_xy, - 'odom_xy': odom_xy, - 'frontend_xy': frontend_xy, - 'odom_errors': np.linalg.norm(odom_xy - true_xy, axis=1), - 'frontend_errors': np.linalg.norm(frontend_xy - true_xy, axis=1), - 'scans': scans, - 'steps': steps, + "true_xy": true_xy, + "odom_xy": odom_xy, + "frontend_xy": frontend_xy, + "odom_errors": np.linalg.norm(odom_xy - true_xy, axis=1), + "frontend_errors": np.linalg.norm(frontend_xy - true_xy, axis=1), + "scans": scans, + "steps": steps, } @@ -176,28 +176,28 @@ def build_figure(demo: Dict[str, object]) -> plt.Figure: # labels the axes as not-to-scale in exchange, which is the right trade # here: the reader is being shown a cross-track deviation, not a shape. plot_trajectory_2d( - demo['true_xy'], + demo["true_xy"], { - 'Odometry (Drift)': demo['odom_xy'], - 'Frontend (Scan-to-Map)': demo['frontend_xy'], + "Odometry (Drift)": demo["odom_xy"], + "Frontend (Scan-to-Map)": demo["frontend_xy"], }, - title='SLAM Front-End: Trajectories', - axis_labels=('X [m]', 'Y [m]'), + title="SLAM Front-End: Trajectories", + axis_labels=("X [m]", "Y [m]"), ax=axes[0], equal_aspect=False, ) plot_error_magnitude_time( { - 'Odometry Error': demo['odom_errors'], - 'Frontend Error': demo['frontend_errors'], + "Odometry Error": demo["odom_errors"], + "Frontend Error": demo["frontend_errors"], }, - t=np.arange(len(demo['odom_errors'])), - title='Position Error Over Time', + t=np.arange(len(demo["odom_errors"])), + title="Position Error Over Time", ax=axes[1], ) # This series is indexed by pose, not seconds, so correct the shared label. - axes[1].set_xlabel('Step Index', fontsize=12) + axes[1].set_xlabel("Step Index", fontsize=12) fig.tight_layout() return fig @@ -219,9 +219,9 @@ def main(): n_poses = 10 demo = run_frontend_demo(n_poses=n_poses) - true_xy = demo['true_xy'] - odom_xy = demo['odom_xy'] - scans = demo['scans'] + true_xy = demo["true_xy"] + odom_xy = demo["odom_xy"] + scans = demo["scans"] print("1. Generating trajectory...") print(f" Generated {n_poses} poses (straight line)") @@ -231,22 +231,28 @@ def main(): print(f" Odometry drift: {odom_drift:.3f} m") print("\n3. Generating LiDAR scans...") - print(f" Generated {n_poses} scans " - f"(avg {np.mean([len(s) for s in scans]):.1f} points/scan)") + print( + f" Generated {n_poses} scans " + f"(avg {np.mean([len(s) for s in scans]):.1f} points/scan)" + ) print("\n4. Running SLAM front-end...") print("=" * 80) - print(f"{'Step':<6} {'Pred X':<10} {'Est X':<10} {'Correction':<12} {'Residual':<10} {'Converged'}") + print( + f"{'Step':<6} {'Pred X':<10} {'Est X':<10} {'Correction':<12} {'Residual':<10} {'Converged'}" + ) print("=" * 80) - for i, result in enumerate(demo['steps']): - pred = result['pose_pred'] - est = result['pose_est'] - mq = result['match_quality'] + for i, result in enumerate(demo["steps"]): + pred = result["pose_pred"] + est = result["pose_est"] + mq = result["match_quality"] - print(f"{i:<6} {pred[0]:<10.3f} {est[0]:<10.3f} " - f"{result['correction_magnitude']:<12.4f} " - f"{mq.residual:<10.4f} {str(mq.converged)}") + print( + f"{i:<6} {pred[0]:<10.3f} {est[0]:<10.3f} " + f"{result['correction_magnitude']:<12.4f} " + f"{mq.residual:<10.4f} {str(mq.converged)}" + ) print("=" * 80) print() @@ -254,8 +260,8 @@ def main(): # Evaluate results print("5. Evaluating results...") - odom_rmse = np.sqrt(np.mean(demo['odom_errors'] ** 2)) - frontend_rmse = np.sqrt(np.mean(demo['frontend_errors'] ** 2)) + odom_rmse = np.sqrt(np.mean(demo["odom_errors"] ** 2)) + frontend_rmse = np.sqrt(np.mean(demo["frontend_errors"] ** 2)) print(f" Odometry RMSE: {odom_rmse:.4f} m") print(f" Frontend RMSE: {frontend_rmse:.4f} m") diff --git a/ch8_sensor_fusion/__init__.py b/ch8_sensor_fusion/__init__.py index 7583f44..21ac4a0 100644 --- a/ch8_sensor_fusion/__init__.py +++ b/ch8_sensor_fusion/__init__.py @@ -12,5 +12,3 @@ """ __all__ = [] - - diff --git a/ch8_sensor_fusion/example_anchor_outage.py b/ch8_sensor_fusion/example_anchor_outage.py index 0aa581a..ac21c3e 100644 --- a/ch8_sensor_fusion/example_anchor_outage.py +++ b/ch8_sensor_fusion/example_anchor_outage.py @@ -132,8 +132,9 @@ def _position_error(result, truth): return t, np.hypot(p[:, 0] - east, p[:, 1] - north) -def run_outage_scenario(data_dir=DEFAULT_DATA, window=OUTAGE_WINDOW, - keep=ANCHORS_KEPT, verbose=True): +def run_outage_scenario( + data_dir=DEFAULT_DATA, window=OUTAGE_WINDOW, keep=ANCHORS_KEPT, verbose=True +): """Run LC and TC over a dataset with a constructed anchor outage. Args: @@ -207,27 +208,45 @@ def update(frame: int): ax.clear() in_outage = window[0] <= now <= window[1] - n_visible = int( - visibility[max(np.searchsorted(t_uwb, now) - 1, 0)] - ) + n_visible = int(visibility[max(np.searchsorted(t_uwb, now) - 1, 0)]) # --- trajectory, with anchors switching off during the outage k_truth = np.searchsorted(t_truth, now) + 1 k_lc = np.searchsorted(t_lc, now) + 1 k_tc = np.searchsorted(t_tc, now) + 1 - axes[0].plot(p_truth[:k_truth, 0], p_truth[:k_truth, 1], - color=COLOR_TRUTH, linewidth=2.0, label="ground truth") - axes[0].plot(p_lc[:k_lc, 0], p_lc[:k_lc, 1], color=COLOR_LC, - linewidth=1.5, label="loosely coupled") - axes[0].plot(p_tc[:k_tc, 0], p_tc[:k_tc, 1], color=COLOR_TC, - linewidth=1.5, label="tightly coupled") + axes[0].plot( + p_truth[:k_truth, 0], + p_truth[:k_truth, 1], + color=COLOR_TRUTH, + linewidth=2.0, + label="ground truth", + ) + axes[0].plot( + p_lc[:k_lc, 0], + p_lc[:k_lc, 1], + color=COLOR_LC, + linewidth=1.5, + label="loosely coupled", + ) + axes[0].plot( + p_tc[:k_tc, 0], + p_tc[:k_tc, 1], + color=COLOR_TC, + linewidth=1.5, + label="tightly coupled", + ) for index, anchor in enumerate(anchors): visible = (not in_outage) or index < ANCHORS_KEPT axes[0].scatter( - anchor[0], anchor[1], s=140, marker="^", + anchor[0], + anchor[1], + s=140, + marker="^", c="red" if visible else "none", - edgecolors="darkred", linewidths=2, zorder=5, + edgecolors="darkred", + linewidths=2, + zorder=5, ) axes[0].set_aspect("equal") axes[0].grid(alpha=0.25) @@ -242,10 +261,12 @@ def update(frame: int): # --- anchor visibility over time shown = t_uwb <= now - axes[1].step(t_uwb[shown], visibility[shown], where="post", - color="0.25", linewidth=1.6) - axes[1].axhline(3, color="red", linestyle="--", linewidth=1.4, - label="LC needs 3 for a fix") + axes[1].step( + t_uwb[shown], visibility[shown], where="post", color="0.25", linewidth=1.6 + ) + axes[1].axhline( + 3, color="red", linestyle="--", linewidth=1.4, label="LC needs 3 for a fix" + ) axes[1].axvspan(window[0], min(now, window[1]), color="0.85", zorder=0) axes[1].set_xlim(t_truth[0], t_end) axes[1].set_ylim(0, len(anchors) + 0.5) @@ -256,10 +277,20 @@ def update(frame: int): axes[1].set_title("anchor visibility", fontsize=10) # --- error, the payoff - axes[2].plot(t_lc[:k_lc], error_lc[:k_lc], color=COLOR_LC, - linewidth=1.6, label="loosely coupled") - axes[2].plot(t_tc[:k_tc], error_tc[:k_tc], color=COLOR_TC, - linewidth=1.6, label="tightly coupled") + axes[2].plot( + t_lc[:k_lc], + error_lc[:k_lc], + color=COLOR_LC, + linewidth=1.6, + label="loosely coupled", + ) + axes[2].plot( + t_tc[:k_tc], + error_tc[:k_tc], + color=COLOR_TC, + linewidth=1.6, + label="tightly coupled", + ) axes[2].axvspan(window[0], min(now, window[1]), color="0.85", zorder=0) axes[2].set_xlim(t_truth[0], t_end) axes[2].set_ylim(0, max_error) @@ -274,8 +305,11 @@ def update(frame: int): fontsize=10, ) - state = (f"OUTAGE: {n_visible} anchors -- LC cannot solve a fix at all" - if in_outage else "anchors nominal") + state = ( + f"OUTAGE: {n_visible} anchors -- LC cannot solve a fix at all" + if in_outage + else "anchors nominal" + ) fig.suptitle( f"Loose vs tight coupling under anchor outage - {state}", fontsize=11, @@ -291,10 +325,16 @@ def plot_outage_summary(scenario) -> plt.Figure: window = scenario["window"] fig, axes = plt.subplots(2, 1, figsize=(10, 6.5), sharex=True) - axes[0].step(scenario["t_uwb"], scenario["visibility"], where="post", - color="0.25", linewidth=1.6) - axes[0].axhline(3, color="red", linestyle="--", linewidth=1.4, - label="LC needs 3 for a fix") + axes[0].step( + scenario["t_uwb"], + scenario["visibility"], + where="post", + color="0.25", + linewidth=1.6, + ) + axes[0].axhline( + 3, color="red", linestyle="--", linewidth=1.4, label="LC needs 3 for a fix" + ) axes[0].axvspan(*window, color="0.85", zorder=0) axes[0].set_ylabel("anchors visible") axes[0].legend(fontsize=9, loc="lower left") @@ -311,12 +351,20 @@ def plot_outage_summary(scenario) -> plt.Figure: # spike. The error is positive and spans two decades, which is what a log # axis is for. floor = 1e-3 # a log axis cannot show an exact zero - axes[1].semilogy(scenario["t_lc"], - np.maximum(scenario["error_lc"], floor), - color=COLOR_LC, linewidth=1.6, label="loosely coupled") - axes[1].semilogy(scenario["t_tc"], - np.maximum(scenario["error_tc"], floor), - color=COLOR_TC, linewidth=1.6, label="tightly coupled") + axes[1].semilogy( + scenario["t_lc"], + np.maximum(scenario["error_lc"], floor), + color=COLOR_LC, + linewidth=1.6, + label="loosely coupled", + ) + axes[1].semilogy( + scenario["t_tc"], + np.maximum(scenario["error_tc"], floor), + color=COLOR_TC, + linewidth=1.6, + label="tightly coupled", + ) axes[1].axvspan(*window, color="0.85", zorder=0) axes[1].set_xlabel("time [s]") axes[1].set_ylabel("horizontal position error [m], log") @@ -339,21 +387,29 @@ def main() -> None: parser = argparse.ArgumentParser( description="Anchor outage: loose vs tight coupling (Chapter 8)" ) - parser.add_argument("--data", default=DEFAULT_DATA, - help="Fusion dataset directory") - parser.add_argument("--out-dir", default=str(FIGS_DIR), - help="Output directory for figures") - parser.add_argument("--animate", action="store_true", default=False, - help="Also render the outage animation GIF (slower)") + parser.add_argument("--data", default=DEFAULT_DATA, help="Fusion dataset directory") + parser.add_argument( + "--out-dir", default=str(FIGS_DIR), help="Output directory for figures" + ) + parser.add_argument( + "--animate", + action="store_true", + default=False, + help="Also render the outage animation GIF (slower)", + ) args = parser.parse_args() print("=" * 70) print("Chapter 8: Anchor Outage -- Loose vs Tight Coupling") print("=" * 70) - print(f"Constructed outage: at most {ANCHORS_KEPT} of 4 anchors between " - f"t = {OUTAGE_WINDOW[0]:.0f} s and {OUTAGE_WINDOW[1]:.0f} s") - print("(the shipped dataset's own dropouts are single isolated epochs " - "and do not stress the difference)\n") + print( + f"Constructed outage: at most {ANCHORS_KEPT} of 4 anchors between " + f"t = {OUTAGE_WINDOW[0]:.0f} s and {OUTAGE_WINDOW[1]:.0f} s" + ) + print( + "(the shipped dataset's own dropouts are single isolated epochs " + "and do not stress the difference)\n" + ) scenario = run_outage_scenario(args.data, verbose=False) @@ -366,11 +422,15 @@ def in_window(times): rmse_lc = np.sqrt(np.mean(scenario["error_lc"] ** 2)) rmse_tc = np.sqrt(np.mean(scenario["error_tc"] ** 2)) - print(f" LC position fixes that failed outright: " - f"{scenario['lc']['n_uwb_failed']}") + print( + f" LC position fixes that failed outright: " + f"{scenario['lc']['n_uwb_failed']}" + ) print(f" RMSE over the run: LC {rmse_lc:.3f} m TC {rmse_tc:.3f} m") - print(f" Peak error in outage: LC {peak_lc:.2f} m TC {peak_tc:.2f} m " - f"({peak_lc / peak_tc:.0f}x)") + print( + f" Peak error in outage: LC {peak_lc:.2f} m TC {peak_tc:.2f} m " + f"({peak_lc / peak_tc:.0f}x)" + ) # The other half of the story, and it is a geometry problem rather than a # bad measurement. The surviving anchors are collinear with the leg being @@ -380,30 +440,42 @@ def in_window(times): est = np.asarray(scenario["tc"]["x_est"])[worst, :2] truth = scenario["dataset"]["truth"] t_worst = scenario["t_tc"][worst] - truth_xy = np.array([ - np.interp(t_worst, truth["t"], truth["p_xy"][:, 0]), - np.interp(t_worst, truth["t"], truth["p_xy"][:, 1]), - ]) - print(f" TC's peak is a mirror-branch flip at t=" - f"{scenario['t_tc'][worst]:.1f} s: estimate " - f"({est[0]:.1f}, {est[1]:.1f}) against truth " - f"({truth_xy[0]:.1f}, {truth_xy[1]:.1f}),") - print(" reflected across the y = 0 baseline joining the two " - "surviving anchors. It lasts under a second, and it is what " - "puts TC's") - print(" whole-run RMSE above LC's at this window. Other outage " - "windows do not trigger it -- see the module docstring.") + truth_xy = np.array( + [ + np.interp(t_worst, truth["t"], truth["p_xy"][:, 0]), + np.interp(t_worst, truth["t"], truth["p_xy"][:, 1]), + ] + ) + print( + f" TC's peak is a mirror-branch flip at t=" + f"{scenario['t_tc'][worst]:.1f} s: estimate " + f"({est[0]:.1f}, {est[1]:.1f}) against truth " + f"({truth_xy[0]:.1f}, {truth_xy[1]:.1f})," + ) + print( + " reflected across the y = 0 baseline joining the two " + "surviving anchors. It lasts under a second, and it is what " + "puts TC's" + ) + print( + " whole-run RMSE above LC's at this window. Other outage " + "windows do not trigger it -- see the module docstring." + ) print() - paths = save_figure(plot_outage_summary(scenario), args.out_dir, - "ch8_anchor_outage") - print(f" saved ch8_anchor_outage: " - f"{', '.join(p.suffix.lstrip('.') for p in paths)}") + paths = save_figure( + plot_outage_summary(scenario), args.out_dir, "ch8_anchor_outage" + ) + print( + f" saved ch8_anchor_outage: " + f"{', '.join(p.suffix.lstrip('.') for p in paths)}" + ) if args.animate: fig, update, n_frames = animate_anchor_outage(scenario) - path = save_animation(fig, update, n_frames, args.out_dir, - "ch8_anchor_outage", fps=5) + path = save_animation( + fig, update, n_frames, args.out_dir, "ch8_anchor_outage", fps=5 + ) plt.close(fig) size_mb = path.stat().st_size / (1024 * 1024) print(f" saved {path.name}: {n_frames} frames, {size_mb:.2f} MB") diff --git a/ch8_sensor_fusion/example_calibration.py b/ch8_sensor_fusion/example_calibration.py index c9bd532..49ab563 100644 --- a/ch8_sensor_fusion/example_calibration.py +++ b/ch8_sensor_fusion/example_calibration.py @@ -44,30 +44,30 @@ def wrap_angle_deg(angle_deg: float) -> float: number, but a calibration near +/-180 deg would otherwise report a 2 deg misalignment as 358. """ - return float(np.degrees(np.arctan2( - np.sin(np.radians(angle_deg)), np.cos(np.radians(angle_deg)) - ))) + return float( + np.degrees( + np.arctan2(np.sin(np.radians(angle_deg)), np.cos(np.radians(angle_deg))) + ) + ) def estimate_imu_bias_stationary( - accel_samples: np.ndarray, - gyro_samples: np.ndarray, - gravity_magnitude: float = 9.81 + accel_samples: np.ndarray, gyro_samples: np.ndarray, gravity_magnitude: float = 9.81 ) -> Dict: """Estimate IMU biases from stationary measurements. - + During a stationary period: - Gyroscope should read zero (any reading is bias) - Accelerometer should read gravity vector (deviation is bias) - + This is the simplest intrinsic IMU calibration method mentioned in the book (Section 8.4.1.3). - + Args: accel_samples: Accelerometer samples (N, 3) in m/s² gyro_samples: Gyroscope samples (N, 3) in rad/s gravity_magnitude: Expected gravity magnitude (default 9.81 m/s²) - + Returns: Dictionary with: - 'accel_bias': Estimated accelerometer bias (3,) in m/s² @@ -75,7 +75,7 @@ def estimate_imu_bias_stationary( - 'accel_std': Standard deviation of accel samples - 'gyro_std': Standard deviation of gyro samples - 'gravity_axis': Identified gravity axis (0=x, 1=y, 2=z) - + References: Chapter 8, Section 8.4.1.3: IMU Intrinsic Calibration """ @@ -98,39 +98,38 @@ def estimate_imu_bias_stationary( accel_bias = accel_mean - expected_accel return { - 'accel_bias': accel_bias, - 'gyro_bias': gyro_bias, - 'accel_std': accel_std, - 'gyro_std': gyro_std, - 'gravity_axis': gravity_axis, - 'accel_mean': accel_mean, - 'n_samples': len(accel_samples) + "accel_bias": accel_bias, + "gyro_bias": gyro_bias, + "accel_std": accel_std, + "gyro_std": gyro_std, + "gravity_axis": gravity_axis, + "accel_mean": accel_mean, + "n_samples": len(accel_samples), } def calibrate_extrinsic_2d_least_squares( - p_sensor1: np.ndarray, - p_sensor2: np.ndarray + p_sensor1: np.ndarray, p_sensor2: np.ndarray ) -> Tuple[np.ndarray, np.ndarray]: """Estimate 2D extrinsic calibration between two sensors. - + Estimates the relative pose (translation + rotation) between two sensors observing the same motion or scene. - + Model: p_sensor2 = R @ p_sensor1 + t - + This uses least-squares fitting to estimate R (2x2 rotation) and t (2D translation/lever-arm). - + Args: p_sensor1: Positions from sensor 1 (N, 2) p_sensor2: Positions from sensor 2 (N, 2) at same timestamps - + Returns: Tuple of (R, t): R: 2D rotation matrix (2, 2) t: 2D translation vector (2,) - lever-arm - + References: Chapter 8, Section 8.4.2: Extrinsic Calibration """ @@ -165,10 +164,10 @@ def generate_synthetic_imu_stationary( accel_bias: np.ndarray = None, gyro_bias: np.ndarray = None, accel_noise_std: float = 0.01, - gyro_noise_std: float = 0.001 + gyro_noise_std: float = 0.001, ) -> Dict: """Generate synthetic stationary IMU data for calibration testing. - + Args: duration: Duration in seconds rate: Sampling rate in Hz @@ -176,7 +175,7 @@ def generate_synthetic_imu_stationary( gyro_bias: True gyroscope bias (3,) in rad/s accel_noise_std: Accelerometer noise std gyro_noise_std: Gyroscope noise std - + Returns: Dictionary with 't', 'accel', 'gyro', 'true_accel_bias', 'true_gyro_bias' """ @@ -201,11 +200,11 @@ def generate_synthetic_imu_stationary( gyro[i] = gyro_bias + np.random.randn(3) * gyro_noise_std return { - 't': t, - 'accel': accel, - 'gyro': gyro, - 'true_accel_bias': accel_bias, - 'true_gyro_bias': gyro_bias + "t": t, + "accel": accel, + "gyro": gyro, + "true_accel_bias": accel_bias, + "true_gyro_bias": gyro_bias, } @@ -213,19 +212,19 @@ def generate_synthetic_extrinsic_data( duration: float = 30.0, rate: float = 10.0, lever_arm: np.ndarray = None, - rotation_angle: float = np.pi / 6 # 30 degrees + rotation_angle: float = np.pi / 6, # 30 degrees ) -> Dict: """Generate synthetic data for 2D extrinsic calibration. - + Simulates two sensors observing the same trajectory with known relative pose (lever-arm + rotation). - + Args: duration: Duration in seconds rate: Sampling rate in Hz lever_arm: True lever-arm (2,) in meters rotation_angle: Rotation angle in radians - + Returns: Dictionary with positions from both sensors and true calibration """ @@ -246,10 +245,12 @@ def generate_synthetic_extrinsic_data( p_sensor1[i, 1] = radius * np.sin(angle) # Apply transformation to get sensor 2 positions - R_true = np.array([ - [np.cos(rotation_angle), -np.sin(rotation_angle)], - [np.sin(rotation_angle), np.cos(rotation_angle)] - ]) + R_true = np.array( + [ + [np.cos(rotation_angle), -np.sin(rotation_angle)], + [np.sin(rotation_angle), np.cos(rotation_angle)], + ] + ) p_sensor2 = (R_true @ p_sensor1.T).T + lever_arm @@ -259,25 +260,21 @@ def generate_synthetic_extrinsic_data( p_sensor2 += np.random.randn(n_samples, 2) * noise_std return { - 't': t, - 'p_sensor1': p_sensor1, - 'p_sensor2': p_sensor2, - 'true_R': R_true, - 'true_t': lever_arm, - 'true_rotation_angle': rotation_angle, + "t": t, + "p_sensor1": p_sensor1, + "p_sensor2": p_sensor2, + "true_R": R_true, + "true_t": lever_arm, + "true_rotation_angle": rotation_angle, # Exposed so the caller can derive the expected alignment residual # instead of quoting a number that has to be kept in sync by hand. - 'noise_std': noise_std, + "noise_std": noise_std, } -def plot_imu_calibration( - data: Dict, - calibration: Dict, - save_path: str = None -): +def plot_imu_calibration(data: Dict, calibration: Dict, save_path: str = None): """Plot IMU calibration results. - + Args: data: IMU data dictionary calibration: Calibration results @@ -288,69 +285,79 @@ def plot_imu_calibration( # Accelerometer data ax1 = fig.add_subplot(gs[0, :]) - for i, axis in enumerate(['X', 'Y', 'Z']): - ax1.plot(data['t'], data['accel'][:, i], label=f'Accel {axis}', - alpha=0.7, linewidth=0.5) - - ax1.axhline(0, color='k', linestyle='--', alpha=0.3) - ax1.axhline(-9.81, color='r', linestyle='--', alpha=0.3, label='Gravity') - ax1.set_xlabel('Time [s]') - ax1.set_ylabel('Acceleration [m/s²]') - ax1.set_title('Accelerometer Raw Data (Stationary)') + for i, axis in enumerate(["X", "Y", "Z"]): + ax1.plot( + data["t"], + data["accel"][:, i], + label=f"Accel {axis}", + alpha=0.7, + linewidth=0.5, + ) + + ax1.axhline(0, color="k", linestyle="--", alpha=0.3) + ax1.axhline(-9.81, color="r", linestyle="--", alpha=0.3, label="Gravity") + ax1.set_xlabel("Time [s]") + ax1.set_ylabel("Acceleration [m/s²]") + ax1.set_title("Accelerometer Raw Data (Stationary)") ax1.legend(ncol=4) ax1.grid(True, alpha=0.3) # Gyroscope data ax2 = fig.add_subplot(gs[1, :]) - for i, axis in enumerate(['X', 'Y', 'Z']): - ax2.plot(data['t'], data['gyro'][:, i] * 180/np.pi, - label=f'Gyro {axis}', alpha=0.7, linewidth=0.5) - - ax2.axhline(0, color='k', linestyle='--', alpha=0.3) - ax2.set_xlabel('Time [s]') - ax2.set_ylabel('Angular Rate [deg/s]') - ax2.set_title('Gyroscope Raw Data (Stationary)') + for i, axis in enumerate(["X", "Y", "Z"]): + ax2.plot( + data["t"], + data["gyro"][:, i] * 180 / np.pi, + label=f"Gyro {axis}", + alpha=0.7, + linewidth=0.5, + ) + + ax2.axhline(0, color="k", linestyle="--", alpha=0.3) + ax2.set_xlabel("Time [s]") + ax2.set_ylabel("Angular Rate [deg/s]") + ax2.set_title("Gyroscope Raw Data (Stationary)") ax2.legend(ncol=3) ax2.grid(True, alpha=0.3) # Bias estimation results ax3 = fig.add_subplot(gs[2, 0]) - axes = ['X', 'Y', 'Z'] + axes = ["X", "Y", "Z"] x_pos = np.arange(3) - true_bias = data['true_accel_bias'] - est_bias = calibration['accel_bias'] + true_bias = data["true_accel_bias"] + est_bias = calibration["accel_bias"] width = 0.35 - ax3.bar(x_pos - width/2, true_bias, width, label='True Bias', alpha=0.7) - ax3.bar(x_pos + width/2, est_bias, width, label='Estimated Bias', alpha=0.7) + ax3.bar(x_pos - width / 2, true_bias, width, label="True Bias", alpha=0.7) + ax3.bar(x_pos + width / 2, est_bias, width, label="Estimated Bias", alpha=0.7) - ax3.set_xlabel('Axis') - ax3.set_ylabel('Bias [m/s²]') - ax3.set_title('Accelerometer Bias Estimation') + ax3.set_xlabel("Axis") + ax3.set_ylabel("Bias [m/s²]") + ax3.set_title("Accelerometer Bias Estimation") ax3.set_xticks(x_pos) ax3.set_xticklabels(axes) ax3.legend() - ax3.grid(True, alpha=0.3, axis='y') + ax3.grid(True, alpha=0.3, axis="y") ax4 = fig.add_subplot(gs[2, 1]) - true_bias_gyro = data['true_gyro_bias'] * 180/np.pi - est_bias_gyro = calibration['gyro_bias'] * 180/np.pi + true_bias_gyro = data["true_gyro_bias"] * 180 / np.pi + est_bias_gyro = calibration["gyro_bias"] * 180 / np.pi - ax4.bar(x_pos - width/2, true_bias_gyro, width, label='True Bias', alpha=0.7) - ax4.bar(x_pos + width/2, est_bias_gyro, width, label='Estimated Bias', alpha=0.7) + ax4.bar(x_pos - width / 2, true_bias_gyro, width, label="True Bias", alpha=0.7) + ax4.bar(x_pos + width / 2, est_bias_gyro, width, label="Estimated Bias", alpha=0.7) - ax4.set_xlabel('Axis') - ax4.set_ylabel('Bias [deg/s]') - ax4.set_title('Gyroscope Bias Estimation') + ax4.set_xlabel("Axis") + ax4.set_ylabel("Bias [deg/s]") + ax4.set_title("Gyroscope Bias Estimation") ax4.set_xticks(x_pos) ax4.set_xticklabels(axes) ax4.legend() - ax4.grid(True, alpha=0.3, axis='y') + ax4.grid(True, alpha=0.3, axis="y") - plt.suptitle('IMU Intrinsic Calibration (Section 8.4.1.3)', fontsize=14, y=0.995) + plt.suptitle("IMU Intrinsic Calibration (Section 8.4.1.3)", fontsize=14, y=0.995) if save_path: # save_figure takes a directory and a stem, and writes svg/pdf/png @@ -363,13 +370,10 @@ def plot_imu_calibration( def plot_extrinsic_calibration( - data: Dict, - R_est: np.ndarray, - t_est: np.ndarray, - save_path: str = None + data: Dict, R_est: np.ndarray, t_est: np.ndarray, save_path: str = None ): """Plot extrinsic calibration results. - + Args: data: Extrinsic calibration data R_est: Estimated rotation matrix @@ -380,42 +384,64 @@ def plot_extrinsic_calibration( # Trajectories ax = axes[0, 0] - ax.plot(data['p_sensor1'][:, 0], data['p_sensor1'][:, 1], - 'b-', label='Sensor 1', alpha=0.7) - ax.plot(data['p_sensor2'][:, 0], data['p_sensor2'][:, 1], - 'r-', label='Sensor 2', alpha=0.7) - ax.scatter(0, 0, c='black', s=100, marker='x', label='Origin', zorder=5) - ax.set_xlabel('X [m]') - ax.set_ylabel('Y [m]') - ax.set_title('Sensor Trajectories') + ax.plot( + data["p_sensor1"][:, 0], + data["p_sensor1"][:, 1], + "b-", + label="Sensor 1", + alpha=0.7, + ) + ax.plot( + data["p_sensor2"][:, 0], + data["p_sensor2"][:, 1], + "r-", + label="Sensor 2", + alpha=0.7, + ) + ax.scatter(0, 0, c="black", s=100, marker="x", label="Origin", zorder=5) + ax.set_xlabel("X [m]") + ax.set_ylabel("Y [m]") + ax.set_title("Sensor Trajectories") ax.legend() ax.grid(True, alpha=0.3) - ax.axis('equal') + ax.axis("equal") # Aligned trajectories ax = axes[0, 1] # Transform sensor 1 to sensor 2 frame using estimated calibration - p1_transformed = (R_est @ data['p_sensor1'].T).T + t_est - - ax.plot(data['p_sensor2'][:, 0], data['p_sensor2'][:, 1], - 'r-', label='Sensor 2 (reference)', alpha=0.7, linewidth=2) - ax.plot(p1_transformed[:, 0], p1_transformed[:, 1], - 'b--', label='Sensor 1 (transformed)', alpha=0.7, linewidth=2) - ax.set_xlabel('X [m]') - ax.set_ylabel('Y [m]') - ax.set_title('After Calibration (Aligned)') + p1_transformed = (R_est @ data["p_sensor1"].T).T + t_est + + ax.plot( + data["p_sensor2"][:, 0], + data["p_sensor2"][:, 1], + "r-", + label="Sensor 2 (reference)", + alpha=0.7, + linewidth=2, + ) + ax.plot( + p1_transformed[:, 0], + p1_transformed[:, 1], + "b--", + label="Sensor 1 (transformed)", + alpha=0.7, + linewidth=2, + ) + ax.set_xlabel("X [m]") + ax.set_ylabel("Y [m]") + ax.set_title("After Calibration (Aligned)") ax.legend() ax.grid(True, alpha=0.3) - ax.axis('equal') + ax.axis("equal") # Calibration parameters ax = axes[1, 0] - ax.axis('off') + ax.axis("off") - t_true = data['true_t'] - angle_true = data['true_rotation_angle'] * 180/np.pi - angle_est = np.arctan2(R_est[1, 0], R_est[0, 0]) * 180/np.pi + t_true = data["true_t"] + angle_true = data["true_rotation_angle"] * 180 / np.pi + angle_est = np.arctan2(R_est[1, 0], R_est[0, 0]) * 180 / np.pi info_text = f""" Extrinsic Calibration Results: @@ -433,22 +459,25 @@ def plot_extrinsic_calibration( [{R_est[1,0]:>7.4f}, {R_est[1,1]:>7.4f}] """ - ax.text(0.1, 0.5, info_text, fontsize=11, family='monospace', - verticalalignment='center') + ax.text( + 0.1, 0.5, info_text, fontsize=11, family="monospace", verticalalignment="center" + ) # Residual errors ax = axes[1, 1] - residuals = data['p_sensor2'] - p1_transformed + residuals = data["p_sensor2"] - p1_transformed residual_norms = np.linalg.norm(residuals, axis=1) - ax.plot(data['t'], residual_norms, 'g-', linewidth=1) - ax.set_xlabel('Time [s]') - ax.set_ylabel('Alignment Error [m]') - ax.set_title(f'Calibration Residuals (RMSE: {np.sqrt(np.mean(residual_norms**2)):.4f} m)') + ax.plot(data["t"], residual_norms, "g-", linewidth=1) + ax.set_xlabel("Time [s]") + ax.set_ylabel("Alignment Error [m]") + ax.set_title( + f"Calibration Residuals (RMSE: {np.sqrt(np.mean(residual_norms**2)):.4f} m)" + ) ax.grid(True, alpha=0.3) - plt.suptitle('2D Extrinsic Calibration (Section 8.4.2)', fontsize=14) + plt.suptitle("2D Extrinsic Calibration (Section 8.4.2)", fontsize=14) if save_path: # save_figure takes a directory and a stem, and writes svg/pdf/png @@ -466,29 +495,20 @@ def main(): description="Calibration Demo: Intrinsic and Extrinsic Calibration" ) parser.add_argument( - "--skip-intrinsic", - action="store_true", - help="Skip intrinsic calibration demo" - ) - parser.add_argument( - "--skip-extrinsic", - action="store_true", - help="Skip extrinsic calibration demo" + "--skip-intrinsic", action="store_true", help="Skip intrinsic calibration demo" ) parser.add_argument( - "--seed", - type=int, - default=42, - help="Random seed" + "--skip-extrinsic", action="store_true", help="Skip extrinsic calibration demo" ) + parser.add_argument("--seed", type=int, default=42, help="Random seed") args = parser.parse_args() np.random.seed(args.seed) - print("\n" + "="*70) + print("\n" + "=" * 70) print("Calibration Demonstration (Chapter 8, Section 8.4)") - print("="*70) + print("=" * 70) # ===================================================================== # Part 1: Intrinsic IMU Calibration @@ -503,10 +523,7 @@ def main(): print("") print("Generating synthetic stationary IMU data...") - imu_data = generate_synthetic_imu_stationary( - duration=10.0, - rate=100.0 - ) + imu_data = generate_synthetic_imu_stationary(duration=10.0, rate=100.0) print(f" Duration: {imu_data['t'][-1]:.1f}s") print(f" Samples: {len(imu_data['t'])}") @@ -515,32 +532,35 @@ def main(): print("") print("Estimating biases from stationary window...") - calibration = estimate_imu_bias_stationary( - imu_data['accel'], - imu_data['gyro'] - ) + calibration = estimate_imu_bias_stationary(imu_data["accel"], imu_data["gyro"]) - print("\n" + "="*70) + print("\n" + "=" * 70) print("IMU Calibration Results") - print("="*70) + print("=" * 70) print(f"{'Parameter':<30} {'Estimated':>15} {'True':>15} {'Error':>10}") print("-" * 70) - for i, axis in enumerate(['X', 'Y', 'Z']): - true_val = imu_data['true_accel_bias'][i] - est_val = calibration['accel_bias'][i] + for i, axis in enumerate(["X", "Y", "Z"]): + true_val = imu_data["true_accel_bias"][i] + est_val = calibration["accel_bias"][i] error = abs(est_val - true_val) - print(f"Accel Bias {axis} [m/s^2] {est_val:>15.4f} {true_val:>15.4f} {error:>10.5f}") + print( + f"Accel Bias {axis} [m/s^2] {est_val:>15.4f} {true_val:>15.4f} {error:>10.5f}" + ) print("") - for i, axis in enumerate(['X', 'Y', 'Z']): - true_val = imu_data['true_gyro_bias'][i] * 180/np.pi - est_val = calibration['gyro_bias'][i] * 180/np.pi + for i, axis in enumerate(["X", "Y", "Z"]): + true_val = imu_data["true_gyro_bias"][i] * 180 / np.pi + est_val = calibration["gyro_bias"][i] * 180 / np.pi error = abs(est_val - true_val) - print(f"Gyro Bias {axis} [deg/s] {est_val:>15.4f} {true_val:>15.4f} {error:>10.5f}") + print( + f"Gyro Bias {axis} [deg/s] {est_val:>15.4f} {true_val:>15.4f} {error:>10.5f}" + ) - print("="*70) - print(f"\nGravity identified along axis: {['X', 'Y', 'Z'][calibration['gravity_axis']]}") + print("=" * 70) + print( + f"\nGravity identified along axis: {['X', 'Y', 'Z'][calibration['gravity_axis']]}" + ) print(f"Number of samples used: {calibration['n_samples']}") # Plot @@ -562,10 +582,7 @@ def main(): print("") print("Generating synthetic dual-sensor data...") - ext_data = generate_synthetic_extrinsic_data( - duration=30.0, - rate=10.0 - ) + ext_data = generate_synthetic_extrinsic_data(duration=30.0, rate=10.0) print(f" Duration: {ext_data['t'][-1]:.1f}s") print(f" Samples: {len(ext_data['t'])}") @@ -575,26 +592,31 @@ def main(): print("Estimating extrinsic calibration (least-squares)...") R_est, t_est = calibrate_extrinsic_2d_least_squares( - ext_data['p_sensor1'], - ext_data['p_sensor2'] + ext_data["p_sensor1"], ext_data["p_sensor2"] ) - angle_est = np.arctan2(R_est[1, 0], R_est[0, 0]) * 180/np.pi - angle_true = ext_data['true_rotation_angle'] * 180/np.pi + angle_est = np.arctan2(R_est[1, 0], R_est[0, 0]) * 180 / np.pi + angle_true = ext_data["true_rotation_angle"] * 180 / np.pi - print("\n" + "="*70) + print("\n" + "=" * 70) print("Extrinsic Calibration Results") - print("="*70) + print("=" * 70) print(f"{'Parameter':<30} {'Estimated':>15} {'True':>15} {'Error':>10}") print("-" * 70) - print(f"Rotation Angle [deg] {angle_est:>15.2f} {angle_true:>15.2f} {abs(wrap_angle_deg(angle_est - angle_true)):>10.4f}") - print(f"Lever-arm X [m] {t_est[0]:>15.4f} {ext_data['true_t'][0]:>15.4f} {abs(t_est[0] - ext_data['true_t'][0]):>10.5f}") - print(f"Lever-arm Y [m] {t_est[1]:>15.4f} {ext_data['true_t'][1]:>15.4f} {abs(t_est[1] - ext_data['true_t'][1]):>10.5f}") - print("="*70) + print( + f"Rotation Angle [deg] {angle_est:>15.2f} {angle_true:>15.2f} {abs(wrap_angle_deg(angle_est - angle_true)):>10.4f}" + ) + print( + f"Lever-arm X [m] {t_est[0]:>15.4f} {ext_data['true_t'][0]:>15.4f} {abs(t_est[0] - ext_data['true_t'][0]):>10.5f}" + ) + print( + f"Lever-arm Y [m] {t_est[1]:>15.4f} {ext_data['true_t'][1]:>15.4f} {abs(t_est[1] - ext_data['true_t'][1]):>10.5f}" + ) + print("=" * 70) # Compute RMSE after calibration - p1_transformed = (R_est @ ext_data['p_sensor1'].T).T + t_est - residuals = ext_data['p_sensor2'] - p1_transformed + p1_transformed = (R_est @ ext_data["p_sensor1"].T).T + t_est + residuals = ext_data["p_sensor2"] - p1_transformed rmse = np.sqrt(np.mean(np.sum(residuals**2, axis=1))) # Derive the expectation rather than quoting the sensor noise. The @@ -602,22 +624,26 @@ def main(): # its per-axis std is sigma*sqrt(2), and this RMSE is the 2-D magnitude, # which brings another sqrt(2): 2*sigma, not sigma. Printing "~0.05 m" # made a calibration that is right to 2% look twice as bad as expected. - sigma = ext_data['noise_std'] + sigma = ext_data["noise_std"] expected = 2.0 * sigma print(f"\nAlignment RMSE after calibration: {rmse:.4f} m") - print(f"(Expected {expected:.4f} m = 2 x the {sigma:.2f} m per-axis " - f"sensor noise: sqrt(2) for differencing two noisy sensors,") - print(f" and sqrt(2) again because this is a 2-D magnitude rather than " - f"one axis. Measured/expected = {rmse / expected:.2f}.)") + print( + f"(Expected {expected:.4f} m = 2 x the {sigma:.2f} m per-axis " + f"sensor noise: sqrt(2) for differencing two noisy sensors," + ) + print( + f" and sqrt(2) again because this is a 2-D magnitude rather than " + f"one axis. Measured/expected = {rmse / expected:.2f}.)" + ) # Plot save_path = "ch8_sensor_fusion/figs/extrinsic_calibration.svg" Path(save_path).parent.mkdir(parents=True, exist_ok=True) plot_extrinsic_calibration(ext_data, R_est, t_est, save_path=save_path) - print("\n" + "="*70) + print("\n" + "=" * 70) print("Calibration Demo Complete") - print("="*70) + print("=" * 70) print("\nKey Takeaways:") print(" 1. Intrinsic calibration corrects sensor-specific errors (biases)") print(" 2. Extrinsic calibration aligns multi-sensor coordinate frames") @@ -626,6 +652,5 @@ def main(): print("") -if __name__ == '__main__': +if __name__ == "__main__": main() - diff --git a/ch8_sensor_fusion/example_comparison.py b/ch8_sensor_fusion/example_comparison.py index 885990b..a3f10db 100644 --- a/ch8_sensor_fusion/example_comparison.py +++ b/ch8_sensor_fusion/example_comparison.py @@ -41,117 +41,113 @@ def run_both_fusions( dataset: Dict, use_gating: bool = True, gate_confidence: float = 0.95, - verbose: bool = True + verbose: bool = True, ) -> Tuple[Dict, Dict]: """Run both LC and TC fusion on the same dataset. - + Args: dataset: Dataset dictionary use_gating: Whether to apply chi-square gating gate_confidence: Gating confidence level (default 0.95 for 95% confidence) verbose: Print progress - + Returns: Tuple of (lc_results, tc_results) """ if verbose: - print("\n" + "="*70) + print("\n" + "=" * 70) print("Running LC vs TC Comparison") - print("="*70) + print("=" * 70) # Run LC fusion if verbose: print("\n[1/2] Running Loosely Coupled Fusion...") lc_results = run_lc_fusion( - dataset, - use_gating=use_gating, - gate_confidence=gate_confidence, - verbose=verbose + dataset, use_gating=use_gating, gate_confidence=gate_confidence, verbose=verbose ) # Run TC fusion if verbose: print("\n[2/2] Running Tightly Coupled Fusion...") tc_results = run_tc_fusion( - dataset, - use_gating=use_gating, - gate_confidence=gate_confidence, - verbose=verbose + dataset, use_gating=use_gating, gate_confidence=gate_confidence, verbose=verbose ) return lc_results, tc_results def compute_comparative_metrics( - dataset: Dict, - lc_results: Dict, - tc_results: Dict + dataset: Dict, lc_results: Dict, tc_results: Dict ) -> Dict: """Compute comparison metrics for LC and TC. - + Args: dataset: Dataset dictionary lc_results: LC fusion results tc_results: TC fusion results - + Returns: Dictionary with comparative metrics """ - truth = dataset['truth'] + truth = dataset["truth"] # Interpolate truth to estimated timestamps def interpolate_truth(t_est): - return np.column_stack([ - np.interp(t_est, truth['t'], truth['p_xy'][:, 0]), - np.interp(t_est, truth['t'], truth['p_xy'][:, 1]) - ]) + return np.column_stack( + [ + np.interp(t_est, truth["t"], truth["p_xy"][:, 0]), + np.interp(t_est, truth["t"], truth["p_xy"][:, 1]), + ] + ) # LC metrics - p_true_lc = interpolate_truth(lc_results['t']) - p_est_lc = lc_results['x_est'][:, :2] + p_true_lc = interpolate_truth(lc_results["t"]) + p_est_lc = lc_results["x_est"][:, :2] errors_lc = compute_position_errors(p_true_lc, p_est_lc) rmse_lc = compute_position_rmse(errors_lc) # TC metrics - p_true_tc = interpolate_truth(tc_results['t']) - p_est_tc = tc_results['x_est'][:, :2] + p_true_tc = interpolate_truth(tc_results["t"]) + p_est_tc = tc_results["x_est"][:, :2] errors_tc = compute_position_errors(p_true_tc, p_est_tc) rmse_tc = compute_position_rmse(errors_tc) metrics = { - 'lc': { - 'rmse_2d': rmse_lc, - 'rmse_x': np.sqrt(np.mean(errors_lc[:, 0]**2)), - 'rmse_y': np.sqrt(np.mean(errors_lc[:, 1]**2)), - 'max_error': np.max(np.linalg.norm(errors_lc, axis=1)), - 'mean_error': np.mean(np.linalg.norm(errors_lc, axis=1)), - 'final_error': np.linalg.norm(errors_lc[-1]), - 'n_updates': lc_results['n_uwb_accepted'], - 'n_rejected': lc_results['n_uwb_rejected'], - 'n_failed': lc_results['n_uwb_failed'], - 'acceptance_rate': ( - 100 * lc_results['n_uwb_accepted'] / - (lc_results['n_uwb_accepted'] + lc_results['n_uwb_rejected']) - if (lc_results['n_uwb_accepted'] + lc_results['n_uwb_rejected']) > 0 + "lc": { + "rmse_2d": rmse_lc, + "rmse_x": np.sqrt(np.mean(errors_lc[:, 0] ** 2)), + "rmse_y": np.sqrt(np.mean(errors_lc[:, 1] ** 2)), + "max_error": np.max(np.linalg.norm(errors_lc, axis=1)), + "mean_error": np.mean(np.linalg.norm(errors_lc, axis=1)), + "final_error": np.linalg.norm(errors_lc[-1]), + "n_updates": lc_results["n_uwb_accepted"], + "n_rejected": lc_results["n_uwb_rejected"], + "n_failed": lc_results["n_uwb_failed"], + "acceptance_rate": ( + 100 + * lc_results["n_uwb_accepted"] + / (lc_results["n_uwb_accepted"] + lc_results["n_uwb_rejected"]) + if (lc_results["n_uwb_accepted"] + lc_results["n_uwb_rejected"]) > 0 else 0.0 ), }, - 'tc': { - 'rmse_2d': rmse_tc, - 'rmse_x': np.sqrt(np.mean(errors_tc[:, 0]**2)), - 'rmse_y': np.sqrt(np.mean(errors_tc[:, 1]**2)), - 'max_error': np.max(np.linalg.norm(errors_tc, axis=1)), - 'mean_error': np.mean(np.linalg.norm(errors_tc, axis=1)), - 'final_error': np.linalg.norm(errors_tc[-1]), - 'n_updates': tc_results['n_uwb_accepted'], - 'n_rejected': tc_results['n_uwb_rejected'], - 'acceptance_rate': ( - 100 * tc_results['n_uwb_accepted'] / - (tc_results['n_uwb_accepted'] + tc_results['n_uwb_rejected']) - if (tc_results['n_uwb_accepted'] + tc_results['n_uwb_rejected']) > 0 + "tc": { + "rmse_2d": rmse_tc, + "rmse_x": np.sqrt(np.mean(errors_tc[:, 0] ** 2)), + "rmse_y": np.sqrt(np.mean(errors_tc[:, 1] ** 2)), + "max_error": np.max(np.linalg.norm(errors_tc, axis=1)), + "mean_error": np.mean(np.linalg.norm(errors_tc, axis=1)), + "final_error": np.linalg.norm(errors_tc[-1]), + "n_updates": tc_results["n_uwb_accepted"], + "n_rejected": tc_results["n_uwb_rejected"], + "acceptance_rate": ( + 100 + * tc_results["n_uwb_accepted"] + / (tc_results["n_uwb_accepted"] + tc_results["n_uwb_rejected"]) + if (tc_results["n_uwb_accepted"] + tc_results["n_uwb_rejected"]) > 0 else 0.0 ), - } + }, } return metrics @@ -159,57 +155,81 @@ def interpolate_truth(t_est): def print_comparison_table(metrics: Dict) -> None: """Print comparison metrics table. - + Args: metrics: Dictionary with 'lc' and 'tc' metrics """ - print("\n" + "="*70) + print("\n" + "=" * 70) print("LC vs TC Performance Comparison") - print("="*70) + print("=" * 70) print(f"{'Metric':<25} {'LC Fusion':>15} {'TC Fusion':>15} {'Difference':>12}") - print("-"*70) + print("-" * 70) # Position accuracy - lc = metrics['lc'] - tc = metrics['tc'] - - print(f"{'RMSE 2D (m)':<25} {lc['rmse_2d']:>15.3f} {tc['rmse_2d']:>15.3f} " - f"{lc['rmse_2d'] - tc['rmse_2d']:>+11.3f}") - print(f"{'RMSE X (m)':<25} {lc['rmse_x']:>15.3f} {tc['rmse_x']:>15.3f} " - f"{lc['rmse_x'] - tc['rmse_x']:>+11.3f}") - print(f"{'RMSE Y (m)':<25} {lc['rmse_y']:>15.3f} {tc['rmse_y']:>15.3f} " - f"{lc['rmse_y'] - tc['rmse_y']:>+11.3f}") - print(f"{'Max Error (m)':<25} {lc['max_error']:>15.3f} {tc['max_error']:>15.3f} " - f"{lc['max_error'] - tc['max_error']:>+11.3f}") - print(f"{'Mean Error (m)':<25} {lc['mean_error']:>15.3f} {tc['mean_error']:>15.3f} " - f"{lc['mean_error'] - tc['mean_error']:>+11.3f}") - print(f"{'Final Error (m)':<25} {lc['final_error']:>15.3f} {tc['final_error']:>15.3f} " - f"{lc['final_error'] - tc['final_error']:>+11.3f}") - - print("-"*70) + lc = metrics["lc"] + tc = metrics["tc"] + + print( + f"{'RMSE 2D (m)':<25} {lc['rmse_2d']:>15.3f} {tc['rmse_2d']:>15.3f} " + f"{lc['rmse_2d'] - tc['rmse_2d']:>+11.3f}" + ) + print( + f"{'RMSE X (m)':<25} {lc['rmse_x']:>15.3f} {tc['rmse_x']:>15.3f} " + f"{lc['rmse_x'] - tc['rmse_x']:>+11.3f}" + ) + print( + f"{'RMSE Y (m)':<25} {lc['rmse_y']:>15.3f} {tc['rmse_y']:>15.3f} " + f"{lc['rmse_y'] - tc['rmse_y']:>+11.3f}" + ) + print( + f"{'Max Error (m)':<25} {lc['max_error']:>15.3f} {tc['max_error']:>15.3f} " + f"{lc['max_error'] - tc['max_error']:>+11.3f}" + ) + print( + f"{'Mean Error (m)':<25} {lc['mean_error']:>15.3f} {tc['mean_error']:>15.3f} " + f"{lc['mean_error'] - tc['mean_error']:>+11.3f}" + ) + print( + f"{'Final Error (m)':<25} {lc['final_error']:>15.3f} {tc['final_error']:>15.3f} " + f"{lc['final_error'] - tc['final_error']:>+11.3f}" + ) + + print("-" * 70) # Update statistics - print(f"{'UWB Updates Accepted':<25} {lc['n_updates']:>15d} {tc['n_updates']:>15d} " - f"{lc['n_updates'] - tc['n_updates']:>+11d}") - print(f"{'UWB Updates Rejected':<25} {lc['n_rejected']:>15d} {tc['n_rejected']:>15d} " - f"{lc['n_rejected'] - tc['n_rejected']:>+11d}") - if 'n_failed' in lc: + print( + f"{'UWB Updates Accepted':<25} {lc['n_updates']:>15d} {tc['n_updates']:>15d} " + f"{lc['n_updates'] - tc['n_updates']:>+11d}" + ) + print( + f"{'UWB Updates Rejected':<25} {lc['n_rejected']:>15d} {tc['n_rejected']:>15d} " + f"{lc['n_rejected'] - tc['n_rejected']:>+11d}" + ) + if "n_failed" in lc: print(f"{'LC Solver Failures':<25} {lc['n_failed']:>15d} {'N/A':>15} {'':>12}") - print(f"{'Acceptance Rate (%)':<25} {lc['acceptance_rate']:>15.1f} {tc['acceptance_rate']:>15.1f} " - f"{lc['acceptance_rate'] - tc['acceptance_rate']:>+11.1f}") + print( + f"{'Acceptance Rate (%)':<25} {lc['acceptance_rate']:>15.1f} {tc['acceptance_rate']:>15.1f} " + f"{lc['acceptance_rate'] - tc['acceptance_rate']:>+11.1f}" + ) - print("="*70) + print("=" * 70) # Summary - better_rmse = "LC" if lc['rmse_2d'] < tc['rmse_2d'] else "TC" - better_accept = "LC" if lc['acceptance_rate'] > tc['acceptance_rate'] else "TC" + better_rmse = "LC" if lc["rmse_2d"] < tc["rmse_2d"] else "TC" + better_accept = "LC" if lc["acceptance_rate"] > tc["acceptance_rate"] else "TC" print("\nSummary:") - print(f" - {better_rmse} has lower RMSE ({abs(lc['rmse_2d'] - tc['rmse_2d']):.3f}m difference)") - print(f" - {better_accept} has higher acceptance rate " - f"({abs(lc['acceptance_rate'] - tc['acceptance_rate']):.1f}% difference)") - print(f" - LC: {lc['n_updates']} updates, TC: {tc['n_updates']} updates " - f"(TC has {tc['n_updates'] - lc['n_updates']:+d} more)") + print( + f" - {better_rmse} has lower RMSE ({abs(lc['rmse_2d'] - tc['rmse_2d']):.3f}m difference)" + ) + print( + f" - {better_accept} has higher acceptance rate " + f"({abs(lc['acceptance_rate'] - tc['acceptance_rate']):.1f}% difference)" + ) + print( + f" - LC: {lc['n_updates']} updates, TC: {tc['n_updates']} updates " + f"(TC has {tc['n_updates'] - lc['n_updates']:+d} more)" + ) # The trade-off is in the numbers above but was not being said. TC wins on # typical error and loses on the worst case, and the mechanism is the same @@ -217,15 +237,21 @@ def print_comparison_table(metrics: Dict) -> None: # when there are too few for a fix, and a bad range also reaches the filter # undiluted. LC solves for a position first, which needs three anchors but # absorbs some of the damage on the way. - if tc['max_error'] > lc['max_error']: - print(f" - ...but TC's worst case is larger: {tc['max_error']:.2f} m " - f"against {lc['max_error']:.2f} m, while its mean error is the") - print(f" smaller of the two ({tc['mean_error']:.2f} m against " - f"{lc['mean_error']:.2f} m). Fusing raw ranges gives TC more " - f"updates and no") - print(" solver failures, and exposes it directly to a bad range; " - "LC's least-squares step needs three anchors but absorbs part " - "of the outlier.") + if tc["max_error"] > lc["max_error"]: + print( + f" - ...but TC's worst case is larger: {tc['max_error']:.2f} m " + f"against {lc['max_error']:.2f} m, while its mean error is the" + ) + print( + f" smaller of the two ({tc['mean_error']:.2f} m against " + f"{lc['mean_error']:.2f} m). Fusing raw ranges gives TC more " + f"updates and no" + ) + print( + " solver failures, and exposes it directly to a bad range; " + "LC's least-squares step needs three anchors but absorbs part " + "of the outlier." + ) print() @@ -234,10 +260,10 @@ def plot_comparison( lc_results: Dict, tc_results: Dict, metrics: Dict, - save_path: str = None + save_path: str = None, ) -> None: """Generate comprehensive LC vs TC comparison plots. - + Args: dataset: Dataset dictionary lc_results: LC fusion results @@ -245,124 +271,246 @@ def plot_comparison( metrics: Comparison metrics save_path: Path to save figure """ - truth = dataset['truth'] - anchors = dataset['uwb_anchors'] + truth = dataset["truth"] + anchors = dataset["uwb_anchors"] # Create figure with custom layout fig = plt.figure(figsize=(18, 12)) gs = GridSpec(3, 3, figure=fig, hspace=0.3, wspace=0.3) # Color scheme - color_truth = 'black' - color_lc = 'tab:blue' - color_tc = 'tab:orange' + color_truth = "black" + color_lc = "tab:blue" + color_tc = "tab:orange" # ========== Row 1: Trajectories ========== # 1. LC Trajectory ax1 = fig.add_subplot(gs[0, 0]) - ax1.plot(truth['p_xy'][:, 0], truth['p_xy'][:, 1], - color=color_truth, linewidth=2, label='Ground Truth', zorder=3) - ax1.plot(lc_results['x_est'][:, 0], lc_results['x_est'][:, 1], - color=color_lc, linewidth=1.5, alpha=0.7, label='LC Estimate', zorder=2) - if len(lc_results.get('uwb_positions', [])) > 0: - ax1.scatter(lc_results['uwb_positions'][:, 0], lc_results['uwb_positions'][:, 1], - s=10, c='cyan', alpha=0.2, label='UWB Fixes', zorder=1) - ax1.scatter(anchors[:, 0], anchors[:, 1], s=150, c='red', marker='^', - edgecolors='darkred', linewidths=2, label='UWB Anchors', zorder=5) - ax1.scatter(truth['p_xy'][0, 0], truth['p_xy'][0, 1], s=200, c='green', - marker='*', edgecolors='darkgreen', linewidths=2, label='Start', zorder=4) - ax1.set_xlabel('X [m]') - ax1.set_ylabel('Y [m]') + ax1.plot( + truth["p_xy"][:, 0], + truth["p_xy"][:, 1], + color=color_truth, + linewidth=2, + label="Ground Truth", + zorder=3, + ) + ax1.plot( + lc_results["x_est"][:, 0], + lc_results["x_est"][:, 1], + color=color_lc, + linewidth=1.5, + alpha=0.7, + label="LC Estimate", + zorder=2, + ) + if len(lc_results.get("uwb_positions", [])) > 0: + ax1.scatter( + lc_results["uwb_positions"][:, 0], + lc_results["uwb_positions"][:, 1], + s=10, + c="cyan", + alpha=0.2, + label="UWB Fixes", + zorder=1, + ) + ax1.scatter( + anchors[:, 0], + anchors[:, 1], + s=150, + c="red", + marker="^", + edgecolors="darkred", + linewidths=2, + label="UWB Anchors", + zorder=5, + ) + ax1.scatter( + truth["p_xy"][0, 0], + truth["p_xy"][0, 1], + s=200, + c="green", + marker="*", + edgecolors="darkgreen", + linewidths=2, + label="Start", + zorder=4, + ) + ax1.set_xlabel("X [m]") + ax1.set_ylabel("Y [m]") ax1.set_title(f'LC Trajectory (RMSE: {metrics["lc"]["rmse_2d"]:.2f}m)') - ax1.legend(loc='upper right', fontsize=8) + ax1.legend(loc="upper right", fontsize=8) ax1.grid(True, alpha=0.3) - ax1.axis('equal') + ax1.axis("equal") # 2. TC Trajectory ax2 = fig.add_subplot(gs[0, 1]) - ax2.plot(truth['p_xy'][:, 0], truth['p_xy'][:, 1], - color=color_truth, linewidth=2, label='Ground Truth', zorder=3) - ax2.plot(tc_results['x_est'][:, 0], tc_results['x_est'][:, 1], - color=color_tc, linewidth=1.5, alpha=0.7, label='TC Estimate', zorder=2) - ax2.scatter(anchors[:, 0], anchors[:, 1], s=150, c='red', marker='^', - edgecolors='darkred', linewidths=2, label='UWB Anchors', zorder=5) - ax2.scatter(truth['p_xy'][0, 0], truth['p_xy'][0, 1], s=200, c='green', - marker='*', edgecolors='darkgreen', linewidths=2, label='Start', zorder=4) - ax2.set_xlabel('X [m]') - ax2.set_ylabel('Y [m]') + ax2.plot( + truth["p_xy"][:, 0], + truth["p_xy"][:, 1], + color=color_truth, + linewidth=2, + label="Ground Truth", + zorder=3, + ) + ax2.plot( + tc_results["x_est"][:, 0], + tc_results["x_est"][:, 1], + color=color_tc, + linewidth=1.5, + alpha=0.7, + label="TC Estimate", + zorder=2, + ) + ax2.scatter( + anchors[:, 0], + anchors[:, 1], + s=150, + c="red", + marker="^", + edgecolors="darkred", + linewidths=2, + label="UWB Anchors", + zorder=5, + ) + ax2.scatter( + truth["p_xy"][0, 0], + truth["p_xy"][0, 1], + s=200, + c="green", + marker="*", + edgecolors="darkgreen", + linewidths=2, + label="Start", + zorder=4, + ) + ax2.set_xlabel("X [m]") + ax2.set_ylabel("Y [m]") ax2.set_title(f'TC Trajectory (RMSE: {metrics["tc"]["rmse_2d"]:.2f}m)') - ax2.legend(loc='upper right', fontsize=8) + ax2.legend(loc="upper right", fontsize=8) ax2.grid(True, alpha=0.3) - ax2.axis('equal') + ax2.axis("equal") # 3. Overlay Comparison ax3 = fig.add_subplot(gs[0, 2]) - ax3.plot(truth['p_xy'][:, 0], truth['p_xy'][:, 1], - color=color_truth, linewidth=2, label='Ground Truth', zorder=4) - ax3.plot(lc_results['x_est'][:, 0], lc_results['x_est'][:, 1], - color=color_lc, linewidth=1.5, alpha=0.6, label='LC', zorder=3) - ax3.plot(tc_results['x_est'][:, 0], tc_results['x_est'][:, 1], - color=color_tc, linewidth=1.5, alpha=0.6, label='TC', zorder=2) - ax3.scatter(anchors[:, 0], anchors[:, 1], s=150, c='red', marker='^', - edgecolors='darkred', linewidths=2, label='Anchors', zorder=5) - ax3.set_xlabel('X [m]') - ax3.set_ylabel('Y [m]') - ax3.set_title('LC vs TC Overlay') - ax3.legend(loc='upper right', fontsize=8) + ax3.plot( + truth["p_xy"][:, 0], + truth["p_xy"][:, 1], + color=color_truth, + linewidth=2, + label="Ground Truth", + zorder=4, + ) + ax3.plot( + lc_results["x_est"][:, 0], + lc_results["x_est"][:, 1], + color=color_lc, + linewidth=1.5, + alpha=0.6, + label="LC", + zorder=3, + ) + ax3.plot( + tc_results["x_est"][:, 0], + tc_results["x_est"][:, 1], + color=color_tc, + linewidth=1.5, + alpha=0.6, + label="TC", + zorder=2, + ) + ax3.scatter( + anchors[:, 0], + anchors[:, 1], + s=150, + c="red", + marker="^", + edgecolors="darkred", + linewidths=2, + label="Anchors", + zorder=5, + ) + ax3.set_xlabel("X [m]") + ax3.set_ylabel("Y [m]") + ax3.set_title("LC vs TC Overlay") + ax3.legend(loc="upper right", fontsize=8) ax3.grid(True, alpha=0.3) - ax3.axis('equal') + ax3.axis("equal") # ========== Row 2: Position Errors ========== # Compute errors def interpolate_truth(t_est): - return np.column_stack([ - np.interp(t_est, truth['t'], truth['p_xy'][:, 0]), - np.interp(t_est, truth['t'], truth['p_xy'][:, 1]) - ]) - - p_true_lc = interpolate_truth(lc_results['t']) - errors_lc = lc_results['x_est'][:, :2] - p_true_lc + return np.column_stack( + [ + np.interp(t_est, truth["t"], truth["p_xy"][:, 0]), + np.interp(t_est, truth["t"], truth["p_xy"][:, 1]), + ] + ) + + p_true_lc = interpolate_truth(lc_results["t"]) + errors_lc = lc_results["x_est"][:, :2] - p_true_lc error_norm_lc = np.linalg.norm(errors_lc, axis=1) - p_true_tc = interpolate_truth(tc_results['t']) - errors_tc = tc_results['x_est'][:, :2] - p_true_tc + p_true_tc = interpolate_truth(tc_results["t"]) + errors_tc = tc_results["x_est"][:, :2] - p_true_tc error_norm_tc = np.linalg.norm(errors_tc, axis=1) # 4. LC Position Error ax4 = fig.add_subplot(gs[1, 0]) - ax4.plot(lc_results['t'], error_norm_lc, color=color_lc, linewidth=1) - ax4.axhline(metrics['lc']['rmse_2d'], color='red', linestyle='--', - linewidth=1.5, label=f'RMSE: {metrics["lc"]["rmse_2d"]:.2f}m') - ax4.set_xlabel('Time [s]') - ax4.set_ylabel('Position Error [m]') - ax4.set_title('LC Position Error') + ax4.plot(lc_results["t"], error_norm_lc, color=color_lc, linewidth=1) + ax4.axhline( + metrics["lc"]["rmse_2d"], + color="red", + linestyle="--", + linewidth=1.5, + label=f'RMSE: {metrics["lc"]["rmse_2d"]:.2f}m', + ) + ax4.set_xlabel("Time [s]") + ax4.set_ylabel("Position Error [m]") + ax4.set_title("LC Position Error") ax4.legend() ax4.grid(True, alpha=0.3) # 5. TC Position Error ax5 = fig.add_subplot(gs[1, 1]) - ax5.plot(tc_results['t'], error_norm_tc, color=color_tc, linewidth=1) - ax5.axhline(metrics['tc']['rmse_2d'], color='red', linestyle='--', - linewidth=1.5, label=f'RMSE: {metrics["tc"]["rmse_2d"]:.2f}m') - ax5.set_xlabel('Time [s]') - ax5.set_ylabel('Position Error [m]') - ax5.set_title('TC Position Error') + ax5.plot(tc_results["t"], error_norm_tc, color=color_tc, linewidth=1) + ax5.axhline( + metrics["tc"]["rmse_2d"], + color="red", + linestyle="--", + linewidth=1.5, + label=f'RMSE: {metrics["tc"]["rmse_2d"]:.2f}m', + ) + ax5.set_xlabel("Time [s]") + ax5.set_ylabel("Position Error [m]") + ax5.set_title("TC Position Error") ax5.legend() ax5.grid(True, alpha=0.3) # 6. Error Comparison ax6 = fig.add_subplot(gs[1, 2]) - ax6.plot(lc_results['t'], error_norm_lc, color=color_lc, - linewidth=1, alpha=0.7, label='LC') - ax6.plot(tc_results['t'], error_norm_tc, color=color_tc, - linewidth=1, alpha=0.7, label='TC') - ax6.axhline(metrics['lc']['rmse_2d'], color=color_lc, linestyle='--', linewidth=1) - ax6.axhline(metrics['tc']['rmse_2d'], color=color_tc, linestyle='--', linewidth=1) - ax6.set_xlabel('Time [s]') - ax6.set_ylabel('Position Error [m]') - ax6.set_title('Position Error Comparison') + ax6.plot( + lc_results["t"], + error_norm_lc, + color=color_lc, + linewidth=1, + alpha=0.7, + label="LC", + ) + ax6.plot( + tc_results["t"], + error_norm_tc, + color=color_tc, + linewidth=1, + alpha=0.7, + label="TC", + ) + ax6.axhline(metrics["lc"]["rmse_2d"], color=color_lc, linestyle="--", linewidth=1) + ax6.axhline(metrics["tc"]["rmse_2d"], color=color_tc, linestyle="--", linewidth=1) + ax6.set_xlabel("Time [s]") + ax6.set_ylabel("Position Error [m]") + ax6.set_title("Position Error Comparison") ax6.legend() ax6.grid(True, alpha=0.3) @@ -370,46 +518,70 @@ def interpolate_truth(t_est): # 7. LC NIS ax7 = fig.add_subplot(gs[2, 0]) - if len(lc_results['nis']) > 0: - nis_lc = np.array(lc_results['nis']) - accepted_lc = np.array(lc_results['gated']) - ax7.plot(np.arange(len(nis_lc))[accepted_lc], nis_lc[accepted_lc], - 'g.', markersize=3, label='Accepted', alpha=0.5) + if len(lc_results["nis"]) > 0: + nis_lc = np.array(lc_results["nis"]) + accepted_lc = np.array(lc_results["gated"]) + ax7.plot( + np.arange(len(nis_lc))[accepted_lc], + nis_lc[accepted_lc], + "g.", + markersize=3, + label="Accepted", + alpha=0.5, + ) if np.any(~accepted_lc): - ax7.plot(np.arange(len(nis_lc))[~accepted_lc], nis_lc[~accepted_lc], - 'rx', markersize=4, label='Rejected') + ax7.plot( + np.arange(len(nis_lc))[~accepted_lc], + nis_lc[~accepted_lc], + "rx", + markersize=4, + label="Rejected", + ) # Chi-square bounds for m=2 DOF (position) from core.fusion import chi_square_bounds + lower, upper = chi_square_bounds(dof=2, confidence=0.95) - ax7.axhline(upper, color='r', linestyle='--', linewidth=1.5, label='95% bounds') - ax7.axhline(lower, color='r', linestyle='--', linewidth=1.5) + ax7.axhline(upper, color="r", linestyle="--", linewidth=1.5, label="95% bounds") + ax7.axhline(lower, color="r", linestyle="--", linewidth=1.5) - ax7.set_xlabel('UWB Update Index') - ax7.set_ylabel('NIS (2 DOF)') + ax7.set_xlabel("UWB Update Index") + ax7.set_ylabel("NIS (2 DOF)") ax7.set_title(f'LC NIS ({metrics["lc"]["acceptance_rate"]:.1f}% accepted)') ax7.legend(fontsize=8) ax7.grid(True, alpha=0.3) # 8. TC NIS ax8 = fig.add_subplot(gs[2, 1]) - if len(tc_results['nis']) > 0: - nis_tc = np.array(tc_results['nis']) - accepted_tc = np.array(tc_results['gated']) - ax8.plot(np.arange(len(nis_tc))[accepted_tc], nis_tc[accepted_tc], - 'g.', markersize=3, label='Accepted', alpha=0.5) + if len(tc_results["nis"]) > 0: + nis_tc = np.array(tc_results["nis"]) + accepted_tc = np.array(tc_results["gated"]) + ax8.plot( + np.arange(len(nis_tc))[accepted_tc], + nis_tc[accepted_tc], + "g.", + markersize=3, + label="Accepted", + alpha=0.5, + ) if np.any(~accepted_tc): - ax8.plot(np.arange(len(nis_tc))[~accepted_tc], nis_tc[~accepted_tc], - 'rx', markersize=4, label='Rejected') + ax8.plot( + np.arange(len(nis_tc))[~accepted_tc], + nis_tc[~accepted_tc], + "rx", + markersize=4, + label="Rejected", + ) # Chi-square bounds for m=1 DOF (range) from core.fusion import chi_square_bounds + lower, upper = chi_square_bounds(dof=1, confidence=0.95) - ax8.axhline(upper, color='r', linestyle='--', linewidth=1.5, label='95% bounds') - ax8.axhline(lower, color='r', linestyle='--', linewidth=1.5) + ax8.axhline(upper, color="r", linestyle="--", linewidth=1.5, label="95% bounds") + ax8.axhline(lower, color="r", linestyle="--", linewidth=1.5) - ax8.set_xlabel('UWB Update Index') - ax8.set_ylabel('NIS (1 DOF)') + ax8.set_xlabel("UWB Update Index") + ax8.set_ylabel("NIS (1 DOF)") ax8.set_title(f'TC NIS ({metrics["tc"]["acceptance_rate"]:.1f}% accepted)') ax8.legend(fontsize=8) ax8.grid(True, alpha=0.3) @@ -417,41 +589,48 @@ def interpolate_truth(t_est): # 9. Metrics Comparison Bar Chart ax9 = fig.add_subplot(gs[2, 2]) - metric_names = ['RMSE\n[m]', 'Max Err\n[m]', 'Updates\n[×100]', 'Accept\n[%]'] + metric_names = ["RMSE\n[m]", "Max Err\n[m]", "Updates\n[×100]", "Accept\n[%]"] lc_values = [ - metrics['lc']['rmse_2d'], - metrics['lc']['max_error'], - metrics['lc']['n_updates'] / 100, - metrics['lc']['acceptance_rate'] + metrics["lc"]["rmse_2d"], + metrics["lc"]["max_error"], + metrics["lc"]["n_updates"] / 100, + metrics["lc"]["acceptance_rate"], ] tc_values = [ - metrics['tc']['rmse_2d'], - metrics['tc']['max_error'], - metrics['tc']['n_updates'] / 100, - metrics['tc']['acceptance_rate'] + metrics["tc"]["rmse_2d"], + metrics["tc"]["max_error"], + metrics["tc"]["n_updates"] / 100, + metrics["tc"]["acceptance_rate"], ] x = np.arange(len(metric_names)) width = 0.35 - ax9.bar(x - width/2, lc_values, width, label='LC', color=color_lc, alpha=0.8) - ax9.bar(x + width/2, tc_values, width, label='TC', color=color_tc, alpha=0.8) + ax9.bar(x - width / 2, lc_values, width, label="LC", color=color_lc, alpha=0.8) + ax9.bar(x + width / 2, tc_values, width, label="TC", color=color_tc, alpha=0.8) - ax9.set_ylabel('Value') - ax9.set_title('Performance Metrics Comparison') + ax9.set_ylabel("Value") + ax9.set_title("Performance Metrics Comparison") ax9.set_xticks(x) ax9.set_xticklabels(metric_names, fontsize=9) ax9.legend() - ax9.grid(True, alpha=0.3, axis='y') + ax9.grid(True, alpha=0.3, axis="y") # Add value labels on bars for i, (lc_val, tc_val) in enumerate(zip(lc_values, tc_values)): - ax9.text(i - width/2, lc_val, f'{lc_val:.1f}', ha='center', va='bottom', fontsize=8) - ax9.text(i + width/2, tc_val, f'{tc_val:.1f}', ha='center', va='bottom', fontsize=8) + ax9.text( + i - width / 2, lc_val, f"{lc_val:.1f}", ha="center", va="bottom", fontsize=8 + ) + ax9.text( + i + width / 2, tc_val, f"{tc_val:.1f}", ha="center", va="bottom", fontsize=8 + ) # Overall title - fig.suptitle('Loosely Coupled vs Tightly Coupled Fusion Comparison', - fontsize=16, fontweight='bold') + fig.suptitle( + "Loosely Coupled vs Tightly Coupled Fusion Comparison", + fontsize=16, + fontweight="bold", + ) if save_path: # save_figure takes a directory and a stem, and writes svg/pdf/png @@ -464,14 +643,10 @@ def interpolate_truth(t_est): def save_comparison_report( - dataset: Dict, - lc_results: Dict, - tc_results: Dict, - metrics: Dict, - output_path: str + dataset: Dict, lc_results: Dict, tc_results: Dict, metrics: Dict, output_path: str ) -> None: """Save comparison report to JSON. - + Args: dataset: Dataset dictionary lc_results: LC fusion results @@ -481,21 +656,28 @@ def save_comparison_report( set, so that a run's report and figure land together. """ report = { - 'dataset': { - 'path': str(dataset.get('path', 'unknown')), - 'config': dataset['config'], - 'n_imu_samples': len(dataset['imu']['t']), - 'n_uwb_epochs': len(dataset['uwb']['t']), - 'duration': float(dataset['truth']['t'][-1]), + "dataset": { + "path": str(dataset.get("path", "unknown")), + "config": dataset["config"], + "n_imu_samples": len(dataset["imu"]["t"]), + "n_uwb_epochs": len(dataset["uwb"]["t"]), + "duration": float(dataset["truth"]["t"][-1]), + }, + "lc_fusion": metrics["lc"], + "tc_fusion": metrics["tc"], + "comparison": { + "rmse_difference": float( + metrics["lc"]["rmse_2d"] - metrics["tc"]["rmse_2d"] + ), + "better_rmse": ( + "LC" if metrics["lc"]["rmse_2d"] < metrics["tc"]["rmse_2d"] else "TC" + ), + "update_ratio": ( + float(metrics["lc"]["n_updates"] / metrics["tc"]["n_updates"]) + if metrics["tc"]["n_updates"] > 0 + else 0.0 + ), }, - 'lc_fusion': metrics['lc'], - 'tc_fusion': metrics['tc'], - 'comparison': { - 'rmse_difference': float(metrics['lc']['rmse_2d'] - metrics['tc']['rmse_2d']), - 'better_rmse': 'LC' if metrics['lc']['rmse_2d'] < metrics['tc']['rmse_2d'] else 'TC', - 'update_ratio': float(metrics['lc']['n_updates'] / metrics['tc']['n_updates']) - if metrics['tc']['n_updates'] > 0 else 0.0, - } } # The figure written by --save goes through save_figure and so honours @@ -503,7 +685,7 @@ def save_comparison_report( # same run could scatter its two outputs across two directories. Resolve # the parent the same way. output_path = resolve_figs_dir(Path(output_path).parent) / Path(output_path).name - with open(output_path, 'w') as f: + with open(output_path, "w") as f: json.dump(report, f, indent=2) print(f"Saved comparison report: {output_path}") @@ -518,30 +700,22 @@ def main(): "--data", type=str, default="data/sim/ch8_fusion_2d_imu_uwb", - help="Path to fusion dataset directory" + help="Path to fusion dataset directory", ) parser.add_argument( - "--no-gating", - action="store_true", - help="Disable chi-square gating" + "--no-gating", action="store_true", help="Disable chi-square gating" ) parser.add_argument( "--confidence", type=float, default=0.95, - help="Gating confidence level (default: 0.95 for 95%% confidence)" + help="Gating confidence level (default: 0.95 for 95%% confidence)", ) parser.add_argument( - "--save", - type=str, - default=None, - help="Path to save comparison figure" + "--save", type=str, default=None, help="Path to save comparison figure" ) parser.add_argument( - "--report", - type=str, - default=None, - help="Path to save comparison report (JSON)" + "--report", type=str, default=None, help="Path to save comparison report (JSON)" ) args = parser.parse_args() @@ -549,14 +723,14 @@ def main(): # Load dataset print(f"\nLoading dataset from: {args.data}") dataset = load_fusion_dataset(args.data) - dataset['path'] = args.data + dataset["path"] = args.data # Run both fusions lc_results, tc_results = run_both_fusions( dataset, use_gating=not args.no_gating, gate_confidence=args.confidence, - verbose=True + verbose=True, ) # Compute metrics @@ -570,11 +744,12 @@ def main(): save_comparison_report(dataset, lc_results, tc_results, metrics, args.report) # Generate comparison plots - save_path = args.save if args.save else "ch8_sensor_fusion/figs/lc_tc_comparison.svg" + save_path = ( + args.save if args.save else "ch8_sensor_fusion/figs/lc_tc_comparison.svg" + ) Path(save_path).parent.mkdir(parents=True, exist_ok=True) plot_comparison(dataset, lc_results, tc_results, metrics, save_path=save_path) if __name__ == "__main__": main() - diff --git a/ch8_sensor_fusion/example_lc_fusion.py b/ch8_sensor_fusion/example_lc_fusion.py index 12c783a..8e71d45 100644 --- a/ch8_sensor_fusion/example_lc_fusion.py +++ b/ch8_sensor_fusion/example_lc_fusion.py @@ -44,27 +44,29 @@ def evaluate_results(dataset: Dict, history: Dict) -> Dict: """Evaluate fusion results against ground truth.""" - truth = dataset['truth'] + truth = dataset["truth"] # Interpolate truth to estimated timestamps - p_true_interp = np.column_stack([ - np.interp(history['t'], truth['t'], truth['p_xy'][:, 0]), - np.interp(history['t'], truth['t'], truth['p_xy'][:, 1]) - ]) + p_true_interp = np.column_stack( + [ + np.interp(history["t"], truth["t"], truth["p_xy"][:, 0]), + np.interp(history["t"], truth["t"], truth["p_xy"][:, 1]), + ] + ) # Extract estimated positions - p_est = history['x_est'][:, :2] + p_est = history["x_est"][:, :2] # Compute errors errors = compute_position_errors(p_true_interp, p_est) rmse = compute_position_rmse(errors) metrics = { - 'rmse_2d': rmse, - 'rmse_x': np.sqrt(np.mean(errors[:, 0]**2)), - 'rmse_y': np.sqrt(np.mean(errors[:, 1]**2)), - 'max_error': np.max(np.linalg.norm(errors, axis=1)), - 'final_error': np.linalg.norm(errors[-1]) + "rmse_2d": rmse, + "rmse_x": np.sqrt(np.mean(errors[:, 0] ** 2)), + "rmse_y": np.sqrt(np.mean(errors[:, 1] ** 2)), + "max_error": np.max(np.linalg.norm(errors, axis=1)), + "final_error": np.linalg.norm(errors[-1]), } return metrics @@ -72,73 +74,97 @@ def evaluate_results(dataset: Dict, history: Dict) -> Dict: def plot_results(dataset: Dict, history: Dict, save_path: str = None) -> None: """Generate LC fusion results plots.""" - truth = dataset['truth'] - anchors = dataset['uwb_anchors'] + truth = dataset["truth"] + anchors = dataset["uwb_anchors"] fig, axes = plt.subplots(2, 2, figsize=(14, 10)) # 1. Trajectory plot ax = axes[0, 0] - ax.plot(truth['p_xy'][:, 0], truth['p_xy'][:, 1], 'k-', label='Truth', linewidth=2) - ax.plot(history['x_est'][:, 0], history['x_est'][:, 1], 'b-', label='LC EKF', alpha=0.7) + ax.plot(truth["p_xy"][:, 0], truth["p_xy"][:, 1], "k-", label="Truth", linewidth=2) + ax.plot( + history["x_est"][:, 0], history["x_est"][:, 1], "b-", label="LC EKF", alpha=0.7 + ) # Plot UWB position fixes - if len(history['uwb_positions']) > 0: - ax.scatter(history['uwb_positions'][:, 0], history['uwb_positions'][:, 1], - s=20, c='orange', alpha=0.3, label='UWB Fixes', zorder=2) - - ax.scatter(anchors[:, 0], anchors[:, 1], s=100, c='red', marker='^', - label='UWB Anchors', zorder=5) - ax.set_xlabel('X [m]') - ax.set_ylabel('Y [m]') - ax.set_title('Trajectory: LC IMU + UWB Fusion') + if len(history["uwb_positions"]) > 0: + ax.scatter( + history["uwb_positions"][:, 0], + history["uwb_positions"][:, 1], + s=20, + c="orange", + alpha=0.3, + label="UWB Fixes", + zorder=2, + ) + + ax.scatter( + anchors[:, 0], + anchors[:, 1], + s=100, + c="red", + marker="^", + label="UWB Anchors", + zorder=5, + ) + ax.set_xlabel("X [m]") + ax.set_ylabel("Y [m]") + ax.set_title("Trajectory: LC IMU + UWB Fusion") ax.legend() ax.grid(True) - ax.axis('equal') + ax.axis("equal") # 2. Position error ax = axes[0, 1] - p_true_interp = np.column_stack([ - np.interp(history['t'], truth['t'], truth['p_xy'][:, 0]), - np.interp(history['t'], truth['t'], truth['p_xy'][:, 1]) - ]) - errors = history['x_est'][:, :2] - p_true_interp + p_true_interp = np.column_stack( + [ + np.interp(history["t"], truth["t"], truth["p_xy"][:, 0]), + np.interp(history["t"], truth["t"], truth["p_xy"][:, 1]), + ] + ) + errors = history["x_est"][:, :2] - p_true_interp error_norm = np.linalg.norm(errors, axis=1) - ax.plot(history['t'], error_norm, 'b-') - ax.set_xlabel('Time [s]') - ax.set_ylabel('Position Error [m]') - ax.set_title('Position Error vs Time') + ax.plot(history["t"], error_norm, "b-") + ax.set_xlabel("Time [s]") + ax.set_ylabel("Position Error [m]") + ax.set_title("Position Error vs Time") ax.grid(True) # 3. NIS plot ax = axes[1, 0] - if len(history['nis']) > 0: - nis = np.array(history['nis']) - accepted = np.array(history['gated']) + if len(history["nis"]) > 0: + nis = np.array(history["nis"]) + accepted = np.array(history["gated"]) - ax.plot(nis[accepted], 'g.', label='Accepted', markersize=4) + ax.plot(nis[accepted], "g.", label="Accepted", markersize=4) if np.any(~accepted): - ax.plot(np.where(~accepted)[0], nis[~accepted], 'rx', - label='Rejected', markersize=6) + ax.plot( + np.where(~accepted)[0], + nis[~accepted], + "rx", + label="Rejected", + markersize=6, + ) # Chi-square bounds for m=2 DOF (position is 2D) from core.fusion import chi_square_bounds + lower, upper = chi_square_bounds(dof=2, confidence=0.95) - ax.axhline(upper, color='r', linestyle='--', label='95% bounds') - ax.axhline(lower, color='r', linestyle='--') + ax.axhline(upper, color="r", linestyle="--", label="95% bounds") + ax.axhline(lower, color="r", linestyle="--") - ax.set_xlabel('UWB Update Index') - ax.set_ylabel('NIS (Normalized Innovation Squared)') - ax.set_title('Innovation Consistency (NIS) - 2 DOF') + ax.set_xlabel("UWB Update Index") + ax.set_ylabel("NIS (Normalized Innovation Squared)") + ax.set_title("Innovation Consistency (NIS) - 2 DOF") ax.legend() ax.grid(True) # 4. Covariance trace ax = axes[1, 1] - ax.plot(history['t'], history['P_trace'], 'b-') - ax.set_xlabel('Time [s]') - ax.set_ylabel('Trace(P)') - ax.set_title('Covariance Trace') + ax.plot(history["t"], history["P_trace"], "b-") + ax.set_xlabel("Time [s]") + ax.set_ylabel("Trace(P)") + ax.set_title("Covariance Trace") ax.grid(True) plt.tight_layout() @@ -162,24 +188,19 @@ def main(): "--data", type=str, default="data/sim/ch8_fusion_2d_imu_uwb", - help="Path to fusion dataset directory" + help="Path to fusion dataset directory", ) parser.add_argument( - "--no-gating", - action="store_true", - help="Disable chi-square gating" + "--no-gating", action="store_true", help="Disable chi-square gating" ) parser.add_argument( "--confidence", type=float, default=0.95, - help="Gating confidence level (default: 0.95 for 95%% confidence)" + help="Gating confidence level (default: 0.95 for 95%% confidence)", ) parser.add_argument( - "--save", - type=str, - default=None, - help="Path to save results figure" + "--save", type=str, default=None, help="Path to save results figure" ) args = parser.parse_args() @@ -193,13 +214,13 @@ def main(): dataset, use_gating=not args.no_gating, gate_confidence=args.confidence, - verbose=True + verbose=True, ) # Evaluate - print("\n" + "="*70) + print("\n" + "=" * 70) print("Evaluation Metrics") - print("="*70) + print("=" * 70) metrics = evaluate_results(dataset, history) print(f" RMSE (2D) : {metrics['rmse_2d']:.3f} m") print(f" RMSE (X) : {metrics['rmse_x']:.3f} m") @@ -209,11 +230,12 @@ def main(): print("") # Plot - save_path = args.save if args.save else "ch8_sensor_fusion/figs/lc_uwb_imu_results.svg" + save_path = ( + args.save if args.save else "ch8_sensor_fusion/figs/lc_uwb_imu_results.svg" + ) Path(save_path).parent.mkdir(parents=True, exist_ok=True) plot_results(dataset, history, save_path=save_path) if __name__ == "__main__": main() - diff --git a/ch8_sensor_fusion/example_observability.py b/ch8_sensor_fusion/example_observability.py index 009e45a..59951b3 100644 --- a/ch8_sensor_fusion/example_observability.py +++ b/ch8_sensor_fusion/example_observability.py @@ -39,17 +39,15 @@ def generate_trajectory( - duration: float = 30.0, - speed: float = 1.0, - dt: float = 0.1 + duration: float = 30.0, speed: float = 1.0, dt: float = 0.1 ) -> Dict: """Generate a simple 2D trajectory for observability demo. - + Args: duration: Trajectory duration in seconds speed: Constant speed in m/s dt: Time step in seconds - + Returns: Dictionary with 't', 'p_xy', 'v_xy' """ @@ -70,27 +68,24 @@ def generate_trajectory( v_xy[i, 0] = 5 * (2 * np.pi / duration) * np.cos(angle) v_xy[i, 1] = 5 * 2 * (2 * np.pi / duration) * np.cos(2 * angle) - return {'t': t, 'p_xy': p_xy, 'v_xy': v_xy} + return {"t": t, "p_xy": p_xy, "v_xy": v_xy} -def generate_odometry_measurements( - trajectory: Dict, - noise_std: float = 0.05 -) -> Dict: +def generate_odometry_measurements(trajectory: Dict, noise_std: float = 0.05) -> Dict: """Generate odometry measurements (relative displacements). - + Odometry measures INCREMENTS, not absolute position. This is why absolute translation is unobservable from odometry alone. - + Args: trajectory: Trajectory dictionary with 'p_xy' noise_std: Odometry noise standard deviation (m) - + Returns: Dictionary with 't', 'delta_p' (incremental displacement) """ - p_xy = trajectory['p_xy'] - t = trajectory['t'] + p_xy = trajectory["p_xy"] + t = trajectory["t"] # Compute increments (odometry measures displacement between steps) delta_p = np.diff(p_xy, axis=0) @@ -100,31 +95,29 @@ def generate_odometry_measurements( delta_p_noisy = delta_p + noise return { - 't': t[1:], # Odometry starts at t[1] - 'delta_p': delta_p_noisy, - 'noise_std': noise_std + "t": t[1:], # Odometry starts at t[1] + "delta_p": delta_p_noisy, + "noise_std": noise_std, } def generate_absolute_fixes( - trajectory: Dict, - fix_rate: float = 1.0, # Hz - noise_std: float = 0.5 # meters + trajectory: Dict, fix_rate: float = 1.0, noise_std: float = 0.5 # Hz # meters ) -> Dict: """Generate occasional absolute position fixes (e.g., UWB, GPS). - + These measurements observe ABSOLUTE position, making translation observable. - + Args: trajectory: Trajectory dictionary fix_rate: Fix rate in Hz noise_std: Position fix noise standard deviation (m) - + Returns: Dictionary with 't_fix', 'p_fix' """ - t = trajectory['t'] - p_xy = trajectory['p_xy'] + t = trajectory["t"] + p_xy = trajectory["p_xy"] dt = t[1] - t[0] # Sample at fix_rate @@ -134,41 +127,35 @@ def generate_absolute_fixes( t_fix = t[fix_indices] p_fix = p_xy[fix_indices] + np.random.randn(len(fix_indices), 2) * noise_std - return { - 't_fix': t_fix, - 'p_fix': p_fix, - 'noise_std': noise_std - } + return {"t_fix": t_fix, "p_fix": p_fix, "noise_std": noise_std} def compute_observability_matrix( - H_sequence: list, - F_sequence: list, - max_steps: int = None + H_sequence: list, F_sequence: list, max_steps: int = None ) -> Tuple[np.ndarray, int, np.ndarray]: """Compute EKF observability matrix per Equation 8.3. - + Implements the discrete-time EKF observability matrix: O_EKF = [H_0; H_1 * Φ(1,0); H_2 * Φ(2,0); ... H_k * Φ(k,0)] - + where H_i is the measurement Jacobian at step i and Φ(k,0) is the state transition matrix from step 0 to k. - + Args: H_sequence: List of measurement Jacobians H_i (each m_i x n) F_sequence: List of state transition Jacobians F_i (each n x n) max_steps: Maximum number of steps to include (default: all) - + Returns: Tuple of (O_EKF, rank, singular_values): O_EKF: Observability matrix (stacked rows) rank: Numerical rank of O_EKF singular_values: Singular values for analysis - + References: Chapter 8, Equation (8.3): EKF Observability Matrix """ @@ -214,8 +201,7 @@ def compute_observability_matrix( def compute_fgo_observability_matrix( - graph, - max_factors: int = None + graph, max_factors: int = None ) -> Tuple[np.ndarray, int, np.ndarray]: """Compute the FGO observability matrix per Equation 8.4. @@ -299,22 +285,19 @@ def compute_fgo_observability_matrix( def analyze_unobservable_states( - O_EKF: np.ndarray, - rank: int, - state_names: list = None, - tolerance: float = None + O_EKF: np.ndarray, rank: int, state_names: list = None, tolerance: float = None ) -> Dict: """Analyze unobservable states from observability matrix. - + Uses SVD to identify the null space of O_EKF, which corresponds to unobservable directions in the state space. - + Args: O_EKF: Observability matrix rank: Numerical rank of O_EKF state_names: Names of state variables (default: x0, x1, ...) tolerance: Threshold for determining zero singular values - + Returns: Dictionary with: - 'n_states': Total number of states @@ -322,14 +305,14 @@ def analyze_unobservable_states( - 'n_unobservable': Number of unobservable states - 'unobservable_modes': Null space basis vectors (each column is a mode) - 'state_names': Names of state variables - + References: Chapter 8, Section 8.2: Observability Analysis """ n_states = O_EKF.shape[1] if state_names is None: - state_names = [f'x{i}' for i in range(n_states)] + state_names = [f"x{i}" for i in range(n_states)] # Perform SVD to find null space U, s, Vt = np.linalg.svd(O_EKF, full_matrices=True) @@ -349,45 +332,47 @@ def analyze_unobservable_states( unobservable_modes = np.array([]).reshape(n_states, 0) return { - 'n_states': n_states, - 'n_observable': rank, - 'n_unobservable': n_unobservable, - 'unobservable_modes': unobservable_modes, - 'singular_values': s, - 'state_names': state_names, + "n_states": n_states, + "n_observable": rank, + "n_unobservable": n_unobservable, + "unobservable_modes": unobservable_modes, + "singular_values": s, + "state_names": state_names, } def run_odometry_only_fusion( trajectory: Dict, odometry: Dict, - translation_offset: np.ndarray = np.array([0.0, 0.0]) + translation_offset: np.ndarray = np.array([0.0, 0.0]), ) -> Dict: """Run fusion with odometry only (no absolute position fixes). - + State: [px, py, vx, vy] (4D) Measurement: [delta_px, delta_py] (odometry increment) - + This demonstrates that absolute translation is UNOBSERVABLE. - + Args: trajectory: Ground truth trajectory odometry: Odometry measurements translation_offset: Initial position offset (unobservable!) - + Returns: Fusion results dictionary """ # Initial state (with translation offset) - true_p0 = trajectory['p_xy'][0] - x0 = np.array([ - true_p0[0] + translation_offset[0], - true_p0[1] + translation_offset[1], - trajectory['v_xy'][0, 0], - trajectory['v_xy'][0, 1] - ]) + true_p0 = trajectory["p_xy"][0] + x0 = np.array( + [ + true_p0[0] + translation_offset[0], + true_p0[1] + translation_offset[1], + trajectory["v_xy"][0, 0], + trajectory["v_xy"][0, 1], + ] + ) - P0 = np.diag([1.0, 1.0, 0.5, 0.5])**2 # Initial covariance + P0 = np.diag([1.0, 1.0, 0.5, 0.5]) ** 2 # Initial covariance # Process model: constant velocity def process_model(x, u, dt): @@ -395,23 +380,20 @@ def process_model(x, u, dt): return np.array([px + vx * dt, py + vy * dt, vx, vy]) def process_jacobian(x, u, dt): - F = np.array([ - [1, 0, dt, 0], - [0, 1, 0, dt], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) + F = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) return F def process_noise_cov(dt): # Process noise (small velocity perturbations) q_v = 0.1**2 - Q = np.array([ - [0.25 * q_v * dt**4, 0, 0.5 * q_v * dt**3, 0], - [0, 0.25 * q_v * dt**4, 0, 0.5 * q_v * dt**3], - [0.5 * q_v * dt**3, 0, q_v * dt**2, 0], - [0, 0.5 * q_v * dt**3, 0, q_v * dt**2] - ]) + Q = np.array( + [ + [0.25 * q_v * dt**4, 0, 0.5 * q_v * dt**3, 0], + [0, 0.25 * q_v * dt**4, 0, 0.5 * q_v * dt**3], + [0.5 * q_v * dt**3, 0, q_v * dt**2, 0], + [0, 0.5 * q_v * dt**3, 0, q_v * dt**2], + ] + ) return Q # Measurement model: odometry increment @@ -423,14 +405,11 @@ def measurement_model(x): def measurement_jacobian(x): # H observes velocity only (position is unobservable!) - H = np.array([ - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) + H = np.array([[0, 0, 1, 0], [0, 0, 0, 1]]) return H def measurement_noise_cov(): - R = np.eye(2) * odometry['noise_std']**2 + R = np.eye(2) * odometry["noise_std"] ** 2 return R # Initialize EKF @@ -442,45 +421,45 @@ def measurement_noise_cov(): Q=process_noise_cov, R=measurement_noise_cov, x0=x0, - P0=P0 + P0=P0, ) # Run fusion - dt = trajectory['t'][1] - trajectory['t'][0] + dt = trajectory["t"][1] - trajectory["t"][0] history = { - 't': [], - 'x_est': [], - 'P_trace': [], - 'H_sequence': [], # For observability analysis - 'F_sequence': [] # For observability analysis + "t": [], + "x_est": [], + "P_trace": [], + "H_sequence": [], # For observability analysis + "F_sequence": [], # For observability analysis } # Initial state - history['t'].append(trajectory['t'][0]) - history['x_est'].append(ekf.state.copy()) - history['P_trace'].append(np.trace(ekf.covariance)) + history["t"].append(trajectory["t"][0]) + history["x_est"].append(ekf.state.copy()) + history["P_trace"].append(np.trace(ekf.covariance)) - for i, delta_p in enumerate(odometry['delta_p']): + for i, delta_p in enumerate(odometry["delta_p"]): # Predict F = process_jacobian(ekf.state, None, dt) - history['F_sequence'].append(F) + history["F_sequence"].append(F) ekf.predict(u=None, dt=dt) # Update with odometry (velocity measurement as proxy for increment) z = delta_p / dt # Convert increment to velocity H = measurement_jacobian(ekf.state) - history['H_sequence'].append(H) + history["H_sequence"].append(H) ekf.update(z) # Log - history['t'].append(odometry['t'][i]) - history['x_est'].append(ekf.state.copy()) - history['P_trace'].append(np.trace(ekf.covariance)) + history["t"].append(odometry["t"][i]) + history["x_est"].append(ekf.state.copy()) + history["P_trace"].append(np.trace(ekf.covariance)) # Convert to arrays - history['t'] = np.array(history['t']) - history['x_est'] = np.array(history['x_est']) - history['P_trace'] = np.array(history['P_trace']) + history["t"] = np.array(history["t"]) + history["x_est"] = np.array(history["x_est"]) + history["P_trace"] = np.array(history["P_trace"]) return history @@ -489,32 +468,34 @@ def run_odometry_with_fixes_fusion( trajectory: Dict, odometry: Dict, absolute_fixes: Dict, - translation_offset: np.ndarray = np.array([0.0, 0.0]) + translation_offset: np.ndarray = np.array([0.0, 0.0]), ) -> Dict: """Run fusion with odometry + absolute position fixes. - + This demonstrates that absolute translation becomes OBSERVABLE when absolute position measurements are available. - + Args: trajectory: Ground truth trajectory odometry: Odometry measurements absolute_fixes: Absolute position fix measurements translation_offset: Initial position offset (will be corrected!) - + Returns: Fusion results dictionary """ # Same initialization as odometry-only - true_p0 = trajectory['p_xy'][0] - x0 = np.array([ - true_p0[0] + translation_offset[0], - true_p0[1] + translation_offset[1], - trajectory['v_xy'][0, 0], - trajectory['v_xy'][0, 1] - ]) + true_p0 = trajectory["p_xy"][0] + x0 = np.array( + [ + true_p0[0] + translation_offset[0], + true_p0[1] + translation_offset[1], + trajectory["v_xy"][0, 0], + trajectory["v_xy"][0, 1], + ] + ) - P0 = np.diag([1.0, 1.0, 0.5, 0.5])**2 + P0 = np.diag([1.0, 1.0, 0.5, 0.5]) ** 2 # Process model (same as before) def process_model(x, u, dt): @@ -522,21 +503,18 @@ def process_model(x, u, dt): return np.array([px + vx * dt, py + vy * dt, vx, vy]) def process_jacobian(x, u, dt): - return np.array([ - [1, 0, dt, 0], - [0, 1, 0, dt], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) + return np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) def process_noise_cov(dt): q_v = 0.1**2 - Q = np.array([ - [0.25 * q_v * dt**4, 0, 0.5 * q_v * dt**3, 0], - [0, 0.25 * q_v * dt**4, 0, 0.5 * q_v * dt**3], - [0.5 * q_v * dt**3, 0, q_v * dt**2, 0], - [0, 0.5 * q_v * dt**3, 0, q_v * dt**2] - ]) + Q = np.array( + [ + [0.25 * q_v * dt**4, 0, 0.5 * q_v * dt**3, 0], + [0, 0.25 * q_v * dt**4, 0, 0.5 * q_v * dt**3], + [0.5 * q_v * dt**3, 0, q_v * dt**2, 0], + [0, 0.5 * q_v * dt**3, 0, q_v * dt**2], + ] + ) return Q # Odometry measurement model @@ -544,13 +522,10 @@ def odom_measurement_model(x): return np.array([x[2], x[3]]) def odom_measurement_jacobian(x): - return np.array([ - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) + return np.array([[0, 0, 1, 0], [0, 0, 0, 1]]) def odom_measurement_noise_cov(): - return np.eye(2) * odometry['noise_std']**2 + return np.eye(2) * odometry["noise_std"] ** 2 # Absolute position measurement model def pos_measurement_model(x): @@ -558,13 +533,10 @@ def pos_measurement_model(x): def pos_measurement_jacobian(x): # H observes position directly (makes translation observable!) - return np.array([ - [1, 0, 0, 0], - [0, 1, 0, 0] - ]) + return np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) def pos_measurement_noise_cov(): - return np.eye(2) * absolute_fixes['noise_std']**2 + return np.eye(2) * absolute_fixes["noise_std"] ** 2 # Initialize EKF ekf = ExtendedKalmanFilter( @@ -575,7 +547,7 @@ def pos_measurement_noise_cov(): Q=process_noise_cov, R=odom_measurement_noise_cov, x0=x0, - P0=P0 + P0=P0, ) # Merge odometry and fixes by timestamp @@ -584,59 +556,63 @@ def pos_measurement_noise_cov(): measurements = [] # Add odometry - for i in range(len(odometry['t'])): - measurements.append(StampedMeasurement( - t=odometry['t'][i], - sensor='odometry', - z=odometry['delta_p'][i], - R=np.eye(2) * odometry['noise_std']**2, - meta={} - )) + for i in range(len(odometry["t"])): + measurements.append( + StampedMeasurement( + t=odometry["t"][i], + sensor="odometry", + z=odometry["delta_p"][i], + R=np.eye(2) * odometry["noise_std"] ** 2, + meta={}, + ) + ) # Add absolute fixes - for i in range(len(absolute_fixes['t_fix'])): - measurements.append(StampedMeasurement( - t=absolute_fixes['t_fix'][i], - sensor='position_fix', - z=absolute_fixes['p_fix'][i], - R=np.eye(2) * absolute_fixes['noise_std']**2, - meta={} - )) + for i in range(len(absolute_fixes["t_fix"])): + measurements.append( + StampedMeasurement( + t=absolute_fixes["t_fix"][i], + sensor="position_fix", + z=absolute_fixes["p_fix"][i], + R=np.eye(2) * absolute_fixes["noise_std"] ** 2, + meta={}, + ) + ) # Sort by timestamp measurements.sort(key=lambda m: m.t) # Run fusion history = { - 't': [], - 'x_est': [], - 'P_trace': [], - 'fix_times': [], - 'H_sequence': [], # For observability analysis - 'F_sequence': [] # For observability analysis + "t": [], + "x_est": [], + "P_trace": [], + "fix_times": [], + "H_sequence": [], # For observability analysis + "F_sequence": [], # For observability analysis } # Initial state - history['t'].append(trajectory['t'][0]) - history['x_est'].append(ekf.state.copy()) - history['P_trace'].append(np.trace(ekf.covariance)) + history["t"].append(trajectory["t"][0]) + history["x_est"].append(ekf.state.copy()) + history["P_trace"].append(np.trace(ekf.covariance)) - t_prev = trajectory['t'][0] + t_prev = trajectory["t"][0] for meas in measurements: dt_step = meas.t - t_prev # Predict F = process_jacobian(ekf.state, None, dt_step) - history['F_sequence'].append(F) + history["F_sequence"].append(F) ekf.predict(u=None, dt=dt_step) # Update based on sensor type - if meas.sensor == 'odometry': + if meas.sensor == "odometry": # Odometry update (velocity) z_odom = meas.z / dt_step H_odom = odom_measurement_jacobian(ekf.state) - history['H_sequence'].append(H_odom) + history["H_sequence"].append(H_odom) R_odom = odom_measurement_noise_cov() # Manual update (simpler than switching models) @@ -646,11 +622,11 @@ def pos_measurement_noise_cov(): ekf.state = ekf.state + K @ y ekf.covariance = (np.eye(4) - K @ H_odom) @ ekf.covariance - elif meas.sensor == 'position_fix': + elif meas.sensor == "position_fix": # Position fix update (absolute position) z_pos = meas.z H_pos = pos_measurement_jacobian(ekf.state) - history['H_sequence'].append(H_pos) + history["H_sequence"].append(H_pos) R_pos = pos_measurement_noise_cov() y = z_pos - pos_measurement_model(ekf.state) @@ -659,20 +635,20 @@ def pos_measurement_noise_cov(): ekf.state = ekf.state + K @ y ekf.covariance = (np.eye(4) - K @ H_pos) @ ekf.covariance - history['fix_times'].append(meas.t) + history["fix_times"].append(meas.t) # Log - history['t'].append(meas.t) - history['x_est'].append(ekf.state.copy()) - history['P_trace'].append(np.trace(ekf.covariance)) + history["t"].append(meas.t) + history["x_est"].append(ekf.state.copy()) + history["P_trace"].append(np.trace(ekf.covariance)) t_prev = meas.t # Convert to arrays - history['t'] = np.array(history['t']) - history['x_est'] = np.array(history['x_est']) - history['P_trace'] = np.array(history['P_trace']) - history['fix_times'] = np.array(history['fix_times']) + history["t"] = np.array(history["t"]) + history["x_est"] = np.array(history["x_est"]) + history["P_trace"] = np.array(history["P_trace"]) + history["fix_times"] = np.array(history["fix_times"]) return history @@ -683,10 +659,10 @@ def plot_example_observability( odom_only_2: Dict, odom_with_fixes: Dict, translation_offset: np.ndarray, - save_path: str = None + save_path: str = None, ) -> None: """Generate observability demonstration plots. - + Args: trajectory: Ground truth odom_only_1: Odometry-only fusion (offset 1) @@ -701,125 +677,224 @@ def plot_example_observability( gs = GridSpec(2, 3, figure=fig, hspace=0.3, wspace=0.3) # Color scheme - color_truth = 'black' - color_odom1 = 'tab:red' - color_odom2 = 'tab:purple' - color_fixes = 'tab:green' + color_truth = "black" + color_odom1 = "tab:red" + color_odom2 = "tab:purple" + color_fixes = "tab:green" # 1. Odometry-only: Two translations ax1 = fig.add_subplot(gs[0, 0]) - ax1.plot(trajectory['p_xy'][:, 0], trajectory['p_xy'][:, 1], - color=color_truth, linewidth=2, label='Ground Truth', zorder=3) - ax1.plot(odom_only_1['x_est'][:, 0], odom_only_1['x_est'][:, 1], - color=color_odom1, linewidth=1.5, alpha=0.7, - label='Odom-only (offset=[0,0])', zorder=2) - ax1.plot(odom_only_2['x_est'][:, 0], odom_only_2['x_est'][:, 1], - color=color_odom2, linewidth=1.5, alpha=0.7, - label=f'Odom-only (offset={translation_offset})', zorder=1) - ax1.scatter(trajectory['p_xy'][0, 0], trajectory['p_xy'][0, 1], - s=200, c='green', marker='*', edgecolors='darkgreen', - linewidths=2, label='True Start', zorder=5) - ax1.set_xlabel('X [m]') - ax1.set_ylabel('Y [m]') - ax1.set_title('Odometry-Only: Translation is Unobservable') + ax1.plot( + trajectory["p_xy"][:, 0], + trajectory["p_xy"][:, 1], + color=color_truth, + linewidth=2, + label="Ground Truth", + zorder=3, + ) + ax1.plot( + odom_only_1["x_est"][:, 0], + odom_only_1["x_est"][:, 1], + color=color_odom1, + linewidth=1.5, + alpha=0.7, + label="Odom-only (offset=[0,0])", + zorder=2, + ) + ax1.plot( + odom_only_2["x_est"][:, 0], + odom_only_2["x_est"][:, 1], + color=color_odom2, + linewidth=1.5, + alpha=0.7, + label=f"Odom-only (offset={translation_offset})", + zorder=1, + ) + ax1.scatter( + trajectory["p_xy"][0, 0], + trajectory["p_xy"][0, 1], + s=200, + c="green", + marker="*", + edgecolors="darkgreen", + linewidths=2, + label="True Start", + zorder=5, + ) + ax1.set_xlabel("X [m]") + ax1.set_ylabel("Y [m]") + ax1.set_title("Odometry-Only: Translation is Unobservable") ax1.legend(fontsize=8) ax1.grid(True, alpha=0.3) - ax1.axis('equal') + ax1.axis("equal") # 2. Odometry + Fixes ax2 = fig.add_subplot(gs[0, 1]) - ax2.plot(trajectory['p_xy'][:, 0], trajectory['p_xy'][:, 1], - color=color_truth, linewidth=2, label='Ground Truth', zorder=3) - ax2.plot(odom_with_fixes['x_est'][:, 0], odom_with_fixes['x_est'][:, 1], - color=color_fixes, linewidth=1.5, alpha=0.7, - label='Odom + Absolute Fixes', zorder=2) + ax2.plot( + trajectory["p_xy"][:, 0], + trajectory["p_xy"][:, 1], + color=color_truth, + linewidth=2, + label="Ground Truth", + zorder=3, + ) + ax2.plot( + odom_with_fixes["x_est"][:, 0], + odom_with_fixes["x_est"][:, 1], + color=color_fixes, + linewidth=1.5, + alpha=0.7, + label="Odom + Absolute Fixes", + zorder=2, + ) # Mark position fix times - fix_indices = [np.argmin(np.abs(odom_with_fixes['t'] - t_fix)) - for t_fix in odom_with_fixes['fix_times']] - ax2.scatter(odom_with_fixes['x_est'][fix_indices, 0], - odom_with_fixes['x_est'][fix_indices, 1], - s=50, c='orange', marker='o', alpha=0.5, - label='Position Fixes', zorder=4) - ax2.scatter(trajectory['p_xy'][0, 0], trajectory['p_xy'][0, 1], - s=200, c='green', marker='*', edgecolors='darkgreen', - linewidths=2, label='True Start', zorder=5) - ax2.set_xlabel('X [m]') - ax2.set_ylabel('Y [m]') - ax2.set_title('Odometry + Fixes: Translation is Observable') + fix_indices = [ + np.argmin(np.abs(odom_with_fixes["t"] - t_fix)) + for t_fix in odom_with_fixes["fix_times"] + ] + ax2.scatter( + odom_with_fixes["x_est"][fix_indices, 0], + odom_with_fixes["x_est"][fix_indices, 1], + s=50, + c="orange", + marker="o", + alpha=0.5, + label="Position Fixes", + zorder=4, + ) + ax2.scatter( + trajectory["p_xy"][0, 0], + trajectory["p_xy"][0, 1], + s=200, + c="green", + marker="*", + edgecolors="darkgreen", + linewidths=2, + label="True Start", + zorder=5, + ) + ax2.set_xlabel("X [m]") + ax2.set_ylabel("Y [m]") + ax2.set_title("Odometry + Fixes: Translation is Observable") ax2.legend(fontsize=8) ax2.grid(True, alpha=0.3) - ax2.axis('equal') + ax2.axis("equal") # 3. Comparison ax3 = fig.add_subplot(gs[0, 2]) - ax3.plot(trajectory['p_xy'][:, 0], trajectory['p_xy'][:, 1], - color=color_truth, linewidth=2, label='Truth', zorder=4) - ax3.plot(odom_only_2['x_est'][:, 0], odom_only_2['x_est'][:, 1], - color=color_odom2, linewidth=1.5, alpha=0.6, - label='Odom-only (drifted)', zorder=2) - ax3.plot(odom_with_fixes['x_est'][:, 0], odom_with_fixes['x_est'][:, 1], - color=color_fixes, linewidth=1.5, alpha=0.8, - label='Odom + Fixes (corrected)', zorder=3) - ax3.set_xlabel('X [m]') - ax3.set_ylabel('Y [m]') - ax3.set_title('Direct Comparison') + ax3.plot( + trajectory["p_xy"][:, 0], + trajectory["p_xy"][:, 1], + color=color_truth, + linewidth=2, + label="Truth", + zorder=4, + ) + ax3.plot( + odom_only_2["x_est"][:, 0], + odom_only_2["x_est"][:, 1], + color=color_odom2, + linewidth=1.5, + alpha=0.6, + label="Odom-only (drifted)", + zorder=2, + ) + ax3.plot( + odom_with_fixes["x_est"][:, 0], + odom_with_fixes["x_est"][:, 1], + color=color_fixes, + linewidth=1.5, + alpha=0.8, + label="Odom + Fixes (corrected)", + zorder=3, + ) + ax3.set_xlabel("X [m]") + ax3.set_ylabel("Y [m]") + ax3.set_title("Direct Comparison") ax3.legend(fontsize=8) ax3.grid(True, alpha=0.3) - ax3.axis('equal') + ax3.axis("equal") # 4. Position errors (Odom-only) ax4 = fig.add_subplot(gs[1, 0]) + # Interpolate truth def get_errors(history): - p_true_interp = np.column_stack([ - np.interp(history['t'], trajectory['t'], trajectory['p_xy'][:, 0]), - np.interp(history['t'], trajectory['t'], trajectory['p_xy'][:, 1]) - ]) - errors = history['x_est'][:, :2] - p_true_interp + p_true_interp = np.column_stack( + [ + np.interp(history["t"], trajectory["t"], trajectory["p_xy"][:, 0]), + np.interp(history["t"], trajectory["t"], trajectory["p_xy"][:, 1]), + ] + ) + errors = history["x_est"][:, :2] - p_true_interp return np.linalg.norm(errors, axis=1) error_odom1 = get_errors(odom_only_1) error_odom2 = get_errors(odom_only_2) - ax4.plot(odom_only_1['t'], error_odom1, color=color_odom1, - linewidth=1.5, label='Offset=[0,0]') - ax4.plot(odom_only_2['t'], error_odom2, color=color_odom2, - linewidth=1.5, label=f'Offset={translation_offset}') - ax4.set_xlabel('Time [s]') - ax4.set_ylabel('Position Error [m]') - ax4.set_title('Odometry-Only: Constant Translation Error') + ax4.plot( + odom_only_1["t"], + error_odom1, + color=color_odom1, + linewidth=1.5, + label="Offset=[0,0]", + ) + ax4.plot( + odom_only_2["t"], + error_odom2, + color=color_odom2, + linewidth=1.5, + label=f"Offset={translation_offset}", + ) + ax4.set_xlabel("Time [s]") + ax4.set_ylabel("Position Error [m]") + ax4.set_title("Odometry-Only: Constant Translation Error") ax4.legend() ax4.grid(True, alpha=0.3) # 5. Position error (Odom + Fixes) ax5 = fig.add_subplot(gs[1, 1]) error_fixes = get_errors(odom_with_fixes) - ax5.plot(odom_with_fixes['t'], error_fixes, color=color_fixes, linewidth=1.5) + ax5.plot(odom_with_fixes["t"], error_fixes, color=color_fixes, linewidth=1.5) # Mark position fix times - for t_fix in odom_with_fixes['fix_times']: - ax5.axvline(t_fix, color='orange', linestyle='--', alpha=0.3, linewidth=1) - ax5.set_xlabel('Time [s]') - ax5.set_ylabel('Position Error [m]') - ax5.set_title('Odom + Fixes: Error Corrected at Fixes') + for t_fix in odom_with_fixes["fix_times"]: + ax5.axvline(t_fix, color="orange", linestyle="--", alpha=0.3, linewidth=1) + ax5.set_xlabel("Time [s]") + ax5.set_ylabel("Position Error [m]") + ax5.set_title("Odom + Fixes: Error Corrected at Fixes") ax5.grid(True, alpha=0.3) # 6. Covariance trace ax6 = fig.add_subplot(gs[1, 2]) - ax6.plot(odom_only_1['t'], odom_only_1['P_trace'], - color=color_odom1, linewidth=1.5, label='Odom-only', alpha=0.7) - ax6.plot(odom_with_fixes['t'], odom_with_fixes['P_trace'], - color=color_fixes, linewidth=1.5, label='Odom + Fixes') + ax6.plot( + odom_only_1["t"], + odom_only_1["P_trace"], + color=color_odom1, + linewidth=1.5, + label="Odom-only", + alpha=0.7, + ) + ax6.plot( + odom_with_fixes["t"], + odom_with_fixes["P_trace"], + color=color_fixes, + linewidth=1.5, + label="Odom + Fixes", + ) # Mark fix times - for t_fix in odom_with_fixes['fix_times']: - ax6.axvline(t_fix, color='orange', linestyle='--', alpha=0.3, linewidth=1) - ax6.set_xlabel('Time [s]') - ax6.set_ylabel('Trace(P) [m²]') - ax6.set_title('Covariance: Fixes Reduce Uncertainty') + for t_fix in odom_with_fixes["fix_times"]: + ax6.axvline(t_fix, color="orange", linestyle="--", alpha=0.3, linewidth=1) + ax6.set_xlabel("Time [s]") + ax6.set_ylabel("Trace(P) [m²]") + ax6.set_title("Covariance: Fixes Reduce Uncertainty") ax6.legend() ax6.grid(True, alpha=0.3) - fig.suptitle('Observability Demo: Odometry-Only vs Odometry + Absolute Fixes', - fontsize=16, fontweight='bold') + fig.suptitle( + "Observability Demo: Odometry-Only vs Odometry + Absolute Fixes", + fontsize=16, + fontweight="bold", + ) if save_path: # Via save_figure, not plt.savefig: it writes the book's svg/pdf @@ -839,25 +914,17 @@ def main(): description="Observability Demo: Odometry-Only vs Odometry + Absolute Fixes" ) parser.add_argument( - "--save", - type=str, - default=None, - help="Path to save results figure" - ) - parser.add_argument( - "--seed", - type=int, - default=42, - help="Random seed" + "--save", type=str, default=None, help="Path to save results figure" ) + parser.add_argument("--seed", type=int, default=42, help="Random seed") args = parser.parse_args() np.random.seed(args.seed) - print("\n" + "="*70) + print("\n" + "=" * 70) print("Observability Demonstration (Chapter 8)") - print("="*70) + print("=" * 70) print("\nKey Concept:") print(" Odometry measures INCREMENTS, not absolute position.") print(" -> Translation is UNOBSERVABLE from odometry alone.") @@ -894,22 +961,20 @@ def main(): ) # Perform observability analysis (Eq. 8.3) - print("\n" + "="*70) + print("\n" + "=" * 70) print("Observability Analysis (Equation 8.3)") - print("="*70) + print("=" * 70) - state_names = ['px', 'py', 'vx', 'vy'] + state_names = ["px", "py", "vx", "vy"] print("\n[A] Odometry-Only System:") print("-" * 70) # Limit analysis to first 50 steps for computational efficiency - max_steps = min(50, len(odom_only_1['H_sequence'])) + max_steps = min(50, len(odom_only_1["H_sequence"])) O_odom, rank_odom, s_odom = compute_observability_matrix( - odom_only_1['H_sequence'], - odom_only_1['F_sequence'], - max_steps=max_steps + odom_only_1["H_sequence"], odom_only_1["F_sequence"], max_steps=max_steps ) obs_analysis_odom = analyze_unobservable_states( @@ -922,16 +987,15 @@ def main(): print(f" Observability matrix shape: {O_odom.shape}") print(f" Rank: {rank_odom} / {obs_analysis_odom['n_states']}") - if obs_analysis_odom['n_unobservable'] > 0: + if obs_analysis_odom["n_unobservable"] > 0: print("\n Unobservable modes (null space basis):") - for i in range(obs_analysis_odom['n_unobservable']): - mode = obs_analysis_odom['unobservable_modes'][:, i] + for i in range(obs_analysis_odom["n_unobservable"]): + mode = obs_analysis_odom["unobservable_modes"][:, i] # Round for display. The raw entries are numpy scalars, which # numpy 2 reprs as np.float64(-0.9999999999999998) -- accurate and # unreadable in a table the chapter asks the reader to interpret. components = { - name: round(float(value), 3) - for name, value in zip(state_names, mode) + name: round(float(value), 3) for name, value in zip(state_names, mode) } print(f" Mode {i+1}: {components}") # Identify dominant components. Taking argsort()[-2:] unconditionally @@ -942,9 +1006,9 @@ def main(): # not, that is the wrong claim to print. magnitudes = np.abs(mode) significant = magnitudes >= 0.1 * magnitudes.max() - dominant_idx = [ - j for j in np.argsort(magnitudes)[::-1] if significant[j] - ][:2] + dominant_idx = [j for j in np.argsort(magnitudes)[::-1] if significant[j]][ + :2 + ] dominant_states = [state_names[j] for j in dominant_idx] print(f" (dominant: {', '.join(dominant_states)})") @@ -953,12 +1017,12 @@ def main(): print("\n[B] Odometry + Absolute Fixes System:") print("-" * 70) - max_steps_fixes = min(50, len(odom_with_fixes['H_sequence'])) + max_steps_fixes = min(50, len(odom_with_fixes["H_sequence"])) O_fixes, rank_fixes, s_fixes = compute_observability_matrix( - odom_with_fixes['H_sequence'], - odom_with_fixes['F_sequence'], - max_steps=max_steps_fixes + odom_with_fixes["H_sequence"], + odom_with_fixes["F_sequence"], + max_steps=max_steps_fixes, ) obs_analysis_fixes = analyze_unobservable_states( @@ -971,10 +1035,10 @@ def main(): print(f" Observability matrix shape: {O_fixes.shape}") print(f" Rank: {rank_fixes} / {obs_analysis_fixes['n_states']}") - if obs_analysis_fixes['n_unobservable'] > 0: + if obs_analysis_fixes["n_unobservable"] > 0: print("\n Unobservable modes:") - for i in range(obs_analysis_fixes['n_unobservable']): - mode = obs_analysis_fixes['unobservable_modes'][:, i] + for i in range(obs_analysis_fixes["n_unobservable"]): + mode = obs_analysis_fixes["unobservable_modes"][:, i] print(f" Mode {i+1}: {dict(zip(state_names, mode))}") else: print("\n System is FULLY OBSERVABLE!") @@ -983,67 +1047,90 @@ def main(): print("\n[C] Key Observation:") print("-" * 70) - if obs_analysis_odom['n_unobservable'] > obs_analysis_fixes['n_unobservable']: - print(f" * Odometry-only has {obs_analysis_odom['n_unobservable']} unobservable directions") - print(f" * Adding absolute fixes reduces this to {obs_analysis_fixes['n_unobservable']}") + if obs_analysis_odom["n_unobservable"] > obs_analysis_fixes["n_unobservable"]: + print( + f" * Odometry-only has {obs_analysis_odom['n_unobservable']} unobservable directions" + ) + print( + f" * Adding absolute fixes reduces this to {obs_analysis_fixes['n_unobservable']}" + ) print(" * The unobservable directions correspond to constant translation") print(" * This matches the book's observability analysis (Section 8.2)") # Compute final errors def final_error(history): - p_true_final = trajectory['p_xy'][-1] - p_est_final = history['x_est'][-1, :2] + p_true_final = trajectory["p_xy"][-1] + p_est_final = history["x_est"][-1, :2] return np.linalg.norm(p_true_final - p_est_final) error_odom1 = final_error(odom_only_1) error_odom2 = final_error(odom_only_2) error_fixes = final_error(odom_with_fixes) - print("\n" + "="*70) + print("\n" + "=" * 70) print("Results") - print("="*70) + print("=" * 70) print(f"{'Method':<35} {'Final Error [m]':>15}") - print("-"*70) + print("-" * 70) print(f"{'Odometry-only (offset [0, 0])':<35} {error_odom1:>15.3f}") print(f"{'Odometry-only (offset [3, 2])':<35} {error_odom2:>15.3f}") print(f"{'Odometry + Absolute Fixes':<35} {error_fixes:>15.3f}") - print("="*70) + print("=" * 70) # Theory against measurement, side by side. This used to print the offset # magnitude formatted as though it were the measured error, directly under # a table giving a different number for the same quantity. predicted = float(np.linalg.norm(translation_offset)) - fix_noise = absolute_fixes['noise_std'] + fix_noise = absolute_fixes["noise_std"] print("\nInterpretation:") - print(" * Translation is unobservable from odometry, so the initial " - "offset should survive to the end untouched.") - print(f" Predicted final error {predicted:.2f} m (the offset " - f"magnitude); measured {error_odom2:.2f} m. They agree to within the " - f"{error_odom1:.2f} m") - print(" of drift that odometry accumulates anyway, which is the " - "offset-free run above.") - print(f" * Adding absolute fixes: {error_fixes:.2f} m. The offset is gone " - f"-- that is what observability buys.") - print(f" * Note what it does NOT buy. The offset-free odometry run scores " - f"{error_odom1:.2f} m, better than the {error_fixes:.2f} m with " - f"fixes,") - print(f" because each fix carries {fix_noise:.1f} m of noise. Absolute " - f"measurements make the state observable; over a run this short they " - f"do not make it") - print(" more precise. Observability is about which errors can be " - "corrected at all, not about how small they end up.") + print( + " * Translation is unobservable from odometry, so the initial " + "offset should survive to the end untouched." + ) + print( + f" Predicted final error {predicted:.2f} m (the offset " + f"magnitude); measured {error_odom2:.2f} m. They agree to within the " + f"{error_odom1:.2f} m" + ) + print( + " of drift that odometry accumulates anyway, which is the " + "offset-free run above." + ) + print( + f" * Adding absolute fixes: {error_fixes:.2f} m. The offset is gone " + f"-- that is what observability buys." + ) + print( + f" * Note what it does NOT buy. The offset-free odometry run scores " + f"{error_odom1:.2f} m, better than the {error_fixes:.2f} m with " + f"fixes," + ) + print( + f" because each fix carries {fix_noise:.1f} m of noise. Absolute " + f"measurements make the state observable; over a run this short they " + f"do not make it" + ) + print( + " more precise. Observability is about which errors can be " + "corrected at all, not about how small they end up." + ) print("") # Plot - save_path = args.save if args.save else "ch8_sensor_fusion/figs/observability_demo.svg" + save_path = ( + args.save if args.save else "ch8_sensor_fusion/figs/observability_demo.svg" + ) Path(save_path).parent.mkdir(parents=True, exist_ok=True) plot_example_observability( - trajectory, odom_only_1, odom_only_2, odom_with_fixes, - translation_offset, save_path=save_path + trajectory, + odom_only_1, + odom_only_2, + odom_with_fixes, + translation_offset, + save_path=save_path, ) if __name__ == "__main__": main() - diff --git a/ch8_sensor_fusion/example_robust_tuning.py b/ch8_sensor_fusion/example_robust_tuning.py index 8735d32..11f93a9 100644 --- a/ch8_sensor_fusion/example_robust_tuning.py +++ b/ch8_sensor_fusion/example_robust_tuning.py @@ -79,10 +79,10 @@ def run_fusion_with_strategy( use_gating: bool = False, gate_confidence: float = 0.95, robust_threshold: float = 2.0, - verbose: bool = False + verbose: bool = False, ) -> Dict: """Run TC fusion with different tuning/robust strategies. - + Args: dataset: Dataset dictionary strategy: One of 'baseline', 'gating', 'huber', 'cauchy' @@ -91,7 +91,7 @@ def run_fusion_with_strategy( gate_confidence: Gating confidence level (default 0.95 for 95% confidence) robust_threshold: Threshold for robust loss (Huber/Cauchy) verbose: Print progress - + Returns: Results dictionary """ @@ -99,25 +99,27 @@ def run_fusion_with_strategy( print(f"\nRunning fusion with strategy: {strategy.upper()}") print(f" R scale: {R_scale}") print(f" Gating: {'Enabled' if use_gating else 'Disabled'}") - if strategy in ['huber', 'cauchy']: + if strategy in ["huber", "cauchy"]: print(f" Robust threshold: {robust_threshold}") - truth = dataset['truth'] - imu = dataset['imu'] - uwb = dataset['uwb'] - anchors = dataset['uwb_anchors'] + truth = dataset["truth"] + imu = dataset["imu"] + uwb = dataset["uwb"] + anchors = dataset["uwb_anchors"] # Initial state: [px, py, vx, vy, yaw] (follows StateIndex convention) - x0 = np.array([ - truth['p_xy'][0, 0], # px - truth['p_xy'][0, 1], # py - truth['v_xy'][0, 0], # vx - truth['v_xy'][0, 1], # vy - truth['yaw'][0] # yaw - ]) + x0 = np.array( + [ + truth["p_xy"][0, 0], # px + truth["p_xy"][0, 1], # py + truth["v_xy"][0, 0], # vx + truth["v_xy"][0, 1], # vy + truth["yaw"][0], # yaw + ] + ) # P0: covariances for [px, py, vx, vy, yaw] - P0 = np.diag([0.1, 0.1, 0.5, 0.5, 0.1])**2 + P0 = np.diag([0.1, 0.1, 0.5, 0.5, 0.1]) ** 2 # Process noise accel_noise_std = 0.1 @@ -135,7 +137,7 @@ def run_fusion_with_strategy( Q=lambda dt: tc_process_noise_covariance(dt, accel_noise_std, gyro_noise_std), R=lambda: np.eye(4) * uwb_range_noise_std**2, x0=x0, - P0=P0 + P0=P0, ) # Prepare measurements @@ -144,39 +146,43 @@ def run_fusion_with_strategy( measurements: List[StampedMeasurement] = [] # Add IMU - for i in range(len(imu['t'])): - measurements.append(StampedMeasurement( - t=imu['t'][i], - sensor='imu', - z=np.hstack([imu['accel_xy'][i], imu['gyro_z'][i]]), - R=np.eye(3), - meta={} - )) + for i in range(len(imu["t"])): + measurements.append( + StampedMeasurement( + t=imu["t"][i], + sensor="imu", + z=np.hstack([imu["accel_xy"][i], imu["gyro_z"][i]]), + R=np.eye(3), + meta={}, + ) + ) # Add UWB (per anchor) - for i in range(len(uwb['t'])): + for i in range(len(uwb["t"])): for j in range(anchors.shape[0]): - if not np.isnan(uwb['ranges'][i, j]): - measurements.append(StampedMeasurement( - t=uwb['t'][i], - sensor='uwb', - z=np.array([uwb['ranges'][i, j]]), - R=np.array([[uwb_range_noise_std**2]]), - meta={'anchor_id': j, 'anchor_pos': anchors[j]} - )) + if not np.isnan(uwb["ranges"][i, j]): + measurements.append( + StampedMeasurement( + t=uwb["t"][i], + sensor="uwb", + z=np.array([uwb["ranges"][i, j]]), + R=np.array([[uwb_range_noise_std**2]]), + meta={"anchor_id": j, "anchor_pos": anchors[j]}, + ) + ) # Sort by timestamp measurements.sort(key=lambda m: m.t) # Run fusion history = { - 't': [], - 'x_est': [], - 'P_trace': [], - 'innovations': [], - 'nis': [], - 'gated': [], - 'robust_scales': [], # Renamed from robust_weights for clarity + "t": [], + "x_est": [], + "P_trace": [], + "innovations": [], + "nis": [], + "gated": [], + "robust_scales": [], # Renamed from robust_weights for clarity } n_uwb_accepted = 0 @@ -186,14 +192,14 @@ def run_fusion_with_strategy( for meas in measurements: dt = meas.t - t_prev - if meas.sensor == 'imu': + if meas.sensor == "imu": # Propagate u = meas.z ekf.predict(u=u, dt=dt) - elif meas.sensor == 'uwb': + elif meas.sensor == "uwb": # UWB range update - anchor_pos = meas.meta['anchor_pos'] + anchor_pos = meas.meta["anchor_pos"] # Predict range to this anchor state_pos = ekf.state[:2] @@ -211,10 +217,10 @@ def run_fusion_with_strategy( # Apply robust covariance inflation if requested (Eq. 8.7) # Outliers get INFLATED covariance (R_scale >= 1) R_scale = 1.0 - if strategy == 'huber': + if strategy == "huber": R_scale = huber_R_scale(y[0], delta=robust_threshold) R_robust = R_scale * R_base # Eq. 8.7: R <- w_R * R - elif strategy == 'cauchy': + elif strategy == "cauchy": R_scale = cauchy_R_scale(y[0], c=robust_threshold) R_robust = R_scale * R_base # Eq. 8.7: R <- w_R * R else: @@ -225,7 +231,7 @@ def run_fusion_with_strategy( # Gating accept = True - if use_gating or strategy == 'gating': + if use_gating or strategy == "gating": accept = chi_square_gate(y, S, confidence=gate_confidence) if accept: @@ -238,29 +244,31 @@ def run_fusion_with_strategy( n_uwb_rejected += 1 # Log - history['innovations'].append(float(np.abs(y[0]))) - history['nis'].append(mahalanobis_distance_squared(y, S)) - history['gated'].append(accept) - history['robust_scales'].append(R_scale) + history["innovations"].append(float(np.abs(y[0]))) + history["nis"].append(mahalanobis_distance_squared(y, S)) + history["gated"].append(accept) + history["robust_scales"].append(R_scale) # Record state - history['t'].append(meas.t) - history['x_est'].append(ekf.state.copy()) - history['P_trace'].append(np.trace(ekf.covariance)) + history["t"].append(meas.t) + history["x_est"].append(ekf.state.copy()) + history["P_trace"].append(np.trace(ekf.covariance)) t_prev = meas.t # Convert to arrays - history['t'] = np.array(history['t']) - history['x_est'] = np.array(history['x_est']) - history['P_trace'] = np.array(history['P_trace']) - history['n_uwb_accepted'] = n_uwb_accepted - history['n_uwb_rejected'] = n_uwb_rejected + history["t"] = np.array(history["t"]) + history["x_est"] = np.array(history["x_est"]) + history["P_trace"] = np.array(history["P_trace"]) + history["n_uwb_accepted"] = n_uwb_accepted + history["n_uwb_rejected"] = n_uwb_rejected if verbose: print(f" Accepted: {n_uwb_accepted}") print(f" Rejected: {n_uwb_rejected}") - print(f" Acceptance rate: {100*n_uwb_accepted/(n_uwb_accepted+n_uwb_rejected):.1f}%") + print( + f" Acceptance rate: {100*n_uwb_accepted/(n_uwb_accepted+n_uwb_rejected):.1f}%" + ) return history @@ -271,10 +279,10 @@ def plot_tuning_comparison( gating: Dict, huber: Dict, cauchy: Dict, - save_path: str = None + save_path: str = None, ) -> None: """Generate tuning and robust loss comparison plots. - + Args: dataset: Dataset dictionary baseline: Baseline results (no gating, no robust) @@ -283,90 +291,166 @@ def plot_tuning_comparison( cauchy: Cauchy robust loss results save_path: Path to save figure """ - truth = dataset['truth'] - anchors = dataset['uwb_anchors'] + truth = dataset["truth"] + anchors = dataset["uwb_anchors"] fig = plt.figure(figsize=(18, 12)) gs = GridSpec(3, 3, figure=fig, hspace=0.3, wspace=0.3) # Color scheme - color_truth = 'black' - color_baseline = 'tab:red' - color_gating = 'tab:blue' - color_huber = 'tab:orange' - color_cauchy = 'tab:green' + color_truth = "black" + color_baseline = "tab:red" + color_gating = "tab:blue" + color_huber = "tab:orange" + color_cauchy = "tab:green" # Helper function for errors def get_errors(history): - p_true_interp = np.column_stack([ - np.interp(history['t'], truth['t'], truth['p_xy'][:, 0]), - np.interp(history['t'], truth['t'], truth['p_xy'][:, 1]) - ]) - errors = history['x_est'][:, :2] - p_true_interp + p_true_interp = np.column_stack( + [ + np.interp(history["t"], truth["t"], truth["p_xy"][:, 0]), + np.interp(history["t"], truth["t"], truth["p_xy"][:, 1]), + ] + ) + errors = history["x_est"][:, :2] - p_true_interp return np.linalg.norm(errors, axis=1) # 1. Baseline trajectory ax1 = fig.add_subplot(gs[0, 0]) - ax1.plot(truth['p_xy'][:, 0], truth['p_xy'][:, 1], - color=color_truth, linewidth=2, label='Truth', zorder=3) - ax1.plot(baseline['x_est'][:, 0], baseline['x_est'][:, 1], - color=color_baseline, linewidth=1.5, alpha=0.7, - label='Baseline (no gating)', zorder=2) - ax1.scatter(anchors[:, 0], anchors[:, 1], s=150, c='red', marker='^', - edgecolors='darkred', linewidths=2, label='Anchors', zorder=5) - ax1.set_xlabel('X [m]') - ax1.set_ylabel('Y [m]') - ax1.set_title('Baseline (No Gating)') + ax1.plot( + truth["p_xy"][:, 0], + truth["p_xy"][:, 1], + color=color_truth, + linewidth=2, + label="Truth", + zorder=3, + ) + ax1.plot( + baseline["x_est"][:, 0], + baseline["x_est"][:, 1], + color=color_baseline, + linewidth=1.5, + alpha=0.7, + label="Baseline (no gating)", + zorder=2, + ) + ax1.scatter( + anchors[:, 0], + anchors[:, 1], + s=150, + c="red", + marker="^", + edgecolors="darkred", + linewidths=2, + label="Anchors", + zorder=5, + ) + ax1.set_xlabel("X [m]") + ax1.set_ylabel("Y [m]") + ax1.set_title("Baseline (No Gating)") ax1.legend(fontsize=8) ax1.grid(True, alpha=0.3) - ax1.axis('equal') + ax1.axis("equal") # 2. Gating trajectory ax2 = fig.add_subplot(gs[0, 1]) - ax2.plot(truth['p_xy'][:, 0], truth['p_xy'][:, 1], - color=color_truth, linewidth=2, label='Truth', zorder=3) - ax2.plot(gating['x_est'][:, 0], gating['x_est'][:, 1], - color=color_gating, linewidth=1.5, alpha=0.7, - label='Chi-Square Gating', zorder=2) - ax2.scatter(anchors[:, 0], anchors[:, 1], s=150, c='red', marker='^', - edgecolors='darkred', linewidths=2, label='Anchors', zorder=5) - ax2.set_xlabel('X [m]') - ax2.set_ylabel('Y [m]') - ax2.set_title('Chi-Square Gating: diverges (see NIS panel)') + ax2.plot( + truth["p_xy"][:, 0], + truth["p_xy"][:, 1], + color=color_truth, + linewidth=2, + label="Truth", + zorder=3, + ) + ax2.plot( + gating["x_est"][:, 0], + gating["x_est"][:, 1], + color=color_gating, + linewidth=1.5, + alpha=0.7, + label="Chi-Square Gating", + zorder=2, + ) + ax2.scatter( + anchors[:, 0], + anchors[:, 1], + s=150, + c="red", + marker="^", + edgecolors="darkred", + linewidths=2, + label="Anchors", + zorder=5, + ) + ax2.set_xlabel("X [m]") + ax2.set_ylabel("Y [m]") + ax2.set_title("Chi-Square Gating: diverges (see NIS panel)") # Without this the panel reads as "gating is broken". It is not: the gate # is only as valid as the covariance it tests against, and this filter is # over-confident. Say so on the panel, because a reader looks at the # picture and not at the terminal. Placed inside the axes -- below them # lands on the next row of the GridSpec, over the NIS panel. ax2.text( - 0.5, 0.03, - 'hard rejection + over-confident R -> starves the filter -> drift', - transform=ax2.transAxes, ha='center', va='bottom', - fontsize=7.5, style='italic', color=color_gating, - bbox=dict(facecolor='white', alpha=0.75, edgecolor='none', pad=1.5), + 0.5, + 0.03, + "hard rejection + over-confident R -> starves the filter -> drift", + transform=ax2.transAxes, + ha="center", + va="bottom", + fontsize=7.5, + style="italic", + color=color_gating, + bbox=dict(facecolor="white", alpha=0.75, edgecolor="none", pad=1.5), ) ax2.legend(fontsize=8) ax2.grid(True, alpha=0.3) - ax2.axis('equal') + ax2.axis("equal") # 3. Robust losses comparison ax3 = fig.add_subplot(gs[0, 2]) - ax3.plot(truth['p_xy'][:, 0], truth['p_xy'][:, 1], - color=color_truth, linewidth=2, label='Truth', zorder=4) - ax3.plot(huber['x_est'][:, 0], huber['x_est'][:, 1], - color=color_huber, linewidth=1.5, alpha=0.7, - label='Huber Loss', zorder=3) - ax3.plot(cauchy['x_est'][:, 0], cauchy['x_est'][:, 1], - color=color_cauchy, linewidth=1.5, alpha=0.7, - label='Cauchy Loss', zorder=2) - ax3.scatter(anchors[:, 0], anchors[:, 1], s=150, c='red', marker='^', - edgecolors='darkred', linewidths=2, label='Anchors', zorder=5) - ax3.set_xlabel('X [m]') - ax3.set_ylabel('Y [m]') - ax3.set_title('Robust Loss Functions') + ax3.plot( + truth["p_xy"][:, 0], + truth["p_xy"][:, 1], + color=color_truth, + linewidth=2, + label="Truth", + zorder=4, + ) + ax3.plot( + huber["x_est"][:, 0], + huber["x_est"][:, 1], + color=color_huber, + linewidth=1.5, + alpha=0.7, + label="Huber Loss", + zorder=3, + ) + ax3.plot( + cauchy["x_est"][:, 0], + cauchy["x_est"][:, 1], + color=color_cauchy, + linewidth=1.5, + alpha=0.7, + label="Cauchy Loss", + zorder=2, + ) + ax3.scatter( + anchors[:, 0], + anchors[:, 1], + s=150, + c="red", + marker="^", + edgecolors="darkred", + linewidths=2, + label="Anchors", + zorder=5, + ) + ax3.set_xlabel("X [m]") + ax3.set_ylabel("Y [m]") + ax3.set_title("Robust Loss Functions") ax3.legend(fontsize=8) ax3.grid(True, alpha=0.3) - ax3.axis('equal') + ax3.axis("equal") # 4. Position errors comparison ax4 = fig.add_subplot(gs[1, 0]) @@ -375,25 +459,49 @@ def get_errors(history): error_huber = get_errors(huber) error_cauchy = get_errors(cauchy) - ax4.plot(baseline['t'], error_baseline, color=color_baseline, - linewidth=1, alpha=0.7, label='Baseline') - ax4.plot(gating['t'], error_gating, color=color_gating, - linewidth=1, alpha=0.7, label='Gating') - ax4.plot(huber['t'], error_huber, color=color_huber, - linewidth=1, alpha=0.7, label='Huber') - ax4.plot(cauchy['t'], error_cauchy, color=color_cauchy, - linewidth=1, alpha=0.7, label='Cauchy') - ax4.set_xlabel('Time [s]') - ax4.set_ylabel('Position Error [m]') - ax4.set_title('Position Error Comparison') + ax4.plot( + baseline["t"], + error_baseline, + color=color_baseline, + linewidth=1, + alpha=0.7, + label="Baseline", + ) + ax4.plot( + gating["t"], + error_gating, + color=color_gating, + linewidth=1, + alpha=0.7, + label="Gating", + ) + ax4.plot( + huber["t"], + error_huber, + color=color_huber, + linewidth=1, + alpha=0.7, + label="Huber", + ) + ax4.plot( + cauchy["t"], + error_cauchy, + color=color_cauchy, + linewidth=1, + alpha=0.7, + label="Cauchy", + ) + ax4.set_xlabel("Time [s]") + ax4.set_ylabel("Position Error [m]") + ax4.set_title("Position Error Comparison") ax4.legend() ax4.grid(True, alpha=0.3) # 5. NIS comparison ax5 = fig.add_subplot(gs[1, 1]) - if len(baseline['nis']) > 0: - nis_baseline = np.array(baseline['nis']) - nis_gating = np.array(gating['nis']) + if len(baseline["nis"]) > 0: + nis_baseline = np.array(baseline["nis"]) + nis_gating = np.array(gating["nis"]) # Log axis and dots rather than a linear axis and lines. NIS here # spans four decades -- baseline median 28, gating reaching 1e4 -- @@ -403,50 +511,87 @@ def get_errors(history): # explanation for the gating result; it has to be readable. step = max(1, len(nis_baseline) // 500) floor = 1e-2 # log scale cannot show an exact zero - ax5.semilogy(np.maximum(nis_baseline[::step], floor), '.', - color=color_baseline, markersize=1.5, alpha=0.5, - label='Baseline') - ax5.semilogy(np.maximum(nis_gating[::step], floor), '.', - color=color_gating, markersize=1.5, alpha=0.5, - label='Gating') + ax5.semilogy( + np.maximum(nis_baseline[::step], floor), + ".", + color=color_baseline, + markersize=1.5, + alpha=0.5, + label="Baseline", + ) + ax5.semilogy( + np.maximum(nis_gating[::step], floor), + ".", + color=color_gating, + markersize=1.5, + alpha=0.5, + label="Gating", + ) # Chi-square bound from core.fusion import chi_square_threshold + threshold = chi_square_threshold(dof=1, confidence=0.95) - ax5.axhline(threshold, color='r', linestyle='--', - linewidth=1.5, label=f'95% bound (chi^2={threshold:.2f})') + ax5.axhline( + threshold, + color="r", + linestyle="--", + linewidth=1.5, + label=f"95% bound (chi^2={threshold:.2f})", + ) # ...and where a *consistent* filter would sit. The gap between this # line and the baseline cloud is the over-confidence that makes the # gate reject good measurements. - ax5.axhline(0.4549, color='0.35', linestyle=':', linewidth=1.2, - label='median if consistent (0.45)') + ax5.axhline( + 0.4549, + color="0.35", + linestyle=":", + linewidth=1.2, + label="median if consistent (0.45)", + ) - ax5.set_xlabel('UWB Update Index') - ax5.set_ylabel('NIS (1 DOF, log)') + ax5.set_xlabel("UWB Update Index") + ax5.set_ylabel("NIS (1 DOF, log)") ax5.set_title( - f'NIS: baseline median {np.median(nis_baseline):.0f} vs 0.45 ' - f'if consistent' + f"NIS: baseline median {np.median(nis_baseline):.0f} vs 0.45 " + f"if consistent" ) - ax5.legend(fontsize=7, loc='upper left') + ax5.legend(fontsize=7, loc="upper left") ax5.grid(True, alpha=0.3) # 6. Robust covariance scales ax6 = fig.add_subplot(gs[1, 2]) - if len(huber['robust_scales']) > 0: - scales_huber = np.array(huber['robust_scales']) - scales_cauchy = np.array(cauchy['robust_scales']) + if len(huber["robust_scales"]) > 0: + scales_huber = np.array(huber["robust_scales"]) + scales_cauchy = np.array(cauchy["robust_scales"]) step = max(1, len(scales_huber) // 500) - ax6.plot(scales_huber[::step], color=color_huber, - linewidth=0.5, alpha=0.7, label='Huber') - ax6.plot(scales_cauchy[::step], color=color_cauchy, - linewidth=0.5, alpha=0.7, label='Cauchy') - - ax6.axhline(1.0, color='gray', linestyle='--', linewidth=1, alpha=0.5, - label='No inflation (inlier)') - ax6.set_xlabel('UWB Update Index') - ax6.set_ylabel('R Scale Factor w_R') - ax6.set_title('Robust Covariance Inflation (Eq. 8.7): higher = more inflation') + ax6.plot( + scales_huber[::step], + color=color_huber, + linewidth=0.5, + alpha=0.7, + label="Huber", + ) + ax6.plot( + scales_cauchy[::step], + color=color_cauchy, + linewidth=0.5, + alpha=0.7, + label="Cauchy", + ) + + ax6.axhline( + 1.0, + color="gray", + linestyle="--", + linewidth=1, + alpha=0.5, + label="No inflation (inlier)", + ) + ax6.set_xlabel("UWB Update Index") + ax6.set_ylabel("R Scale Factor w_R") + ax6.set_title("Robust Covariance Inflation (Eq. 8.7): higher = more inflation") ax6.set_ylim([0.5, max(10, np.percentile(scales_cauchy, 95))]) ax6.legend() ax6.grid(True, alpha=0.3) @@ -457,62 +602,95 @@ def get_errors(history): compute_rmse(get_errors(baseline)), compute_rmse(get_errors(gating)), compute_rmse(get_errors(huber)), - compute_rmse(get_errors(cauchy)) + compute_rmse(get_errors(cauchy)), ] - methods = ['Baseline', 'Gating', 'Huber', 'Cauchy'] + methods = ["Baseline", "Gating", "Huber", "Cauchy"] colors = [color_baseline, color_gating, color_huber, color_cauchy] bars = ax7.bar(methods, rmses, color=colors, alpha=0.7) - ax7.set_ylabel('RMSE [m]') - ax7.set_title('RMSE Comparison') - ax7.grid(True, alpha=0.3, axis='y') + ax7.set_ylabel("RMSE [m]") + ax7.set_title("RMSE Comparison") + ax7.grid(True, alpha=0.3, axis="y") # Add value labels for bar, rmse in zip(bars, rmses): height = bar.get_height() - ax7.text(bar.get_x() + bar.get_width()/2., height, - f'{rmse:.2f}m', ha='center', va='bottom', fontsize=9) + ax7.text( + bar.get_x() + bar.get_width() / 2.0, + height, + f"{rmse:.2f}m", + ha="center", + va="bottom", + fontsize=9, + ) # 8. Acceptance rate ax8 = fig.add_subplot(gs[2, 1]) acceptance_rates = [ - 100 * baseline['n_uwb_accepted'] / (baseline['n_uwb_accepted'] + baseline['n_uwb_rejected']), - 100 * gating['n_uwb_accepted'] / (gating['n_uwb_accepted'] + gating['n_uwb_rejected']), - 100 * huber['n_uwb_accepted'] / (huber['n_uwb_accepted'] + huber['n_uwb_rejected']), - 100 * cauchy['n_uwb_accepted'] / (cauchy['n_uwb_accepted'] + cauchy['n_uwb_rejected']) + 100 + * baseline["n_uwb_accepted"] + / (baseline["n_uwb_accepted"] + baseline["n_uwb_rejected"]), + 100 + * gating["n_uwb_accepted"] + / (gating["n_uwb_accepted"] + gating["n_uwb_rejected"]), + 100 + * huber["n_uwb_accepted"] + / (huber["n_uwb_accepted"] + huber["n_uwb_rejected"]), + 100 + * cauchy["n_uwb_accepted"] + / (cauchy["n_uwb_accepted"] + cauchy["n_uwb_rejected"]), ] bars = ax8.bar(methods, acceptance_rates, color=colors, alpha=0.7) - ax8.set_ylabel('Acceptance Rate [%]') - ax8.set_title('Measurement Acceptance Rate') + ax8.set_ylabel("Acceptance Rate [%]") + ax8.set_title("Measurement Acceptance Rate") ax8.set_ylim([0, 105]) - ax8.grid(True, alpha=0.3, axis='y') + ax8.grid(True, alpha=0.3, axis="y") # Add value labels for bar, rate in zip(bars, acceptance_rates): height = bar.get_height() - ax8.text(bar.get_x() + bar.get_width()/2., height, - f'{rate:.1f}%', ha='center', va='bottom', fontsize=9) + ax8.text( + bar.get_x() + bar.get_width() / 2.0, + height, + f"{rate:.1f}%", + ha="center", + va="bottom", + fontsize=9, + ) # 9. Innovation distribution ax9 = fig.add_subplot(gs[2, 2]) - if len(baseline['innovations']) > 0: - innov_baseline = np.array(baseline['innovations']) - innov_gating = np.array(gating['innovations'])[np.array(gating['gated'])] - - ax9.hist(innov_baseline, bins=50, alpha=0.5, color=color_baseline, - label='Baseline (all)', density=True) - ax9.hist(innov_gating, bins=50, alpha=0.5, color=color_gating, - label='Gating (accepted)', density=True) - - ax9.set_xlabel('Innovation Magnitude [m]') - ax9.set_ylabel('Density') - ax9.set_title('Innovation Distribution') + if len(baseline["innovations"]) > 0: + innov_baseline = np.array(baseline["innovations"]) + innov_gating = np.array(gating["innovations"])[np.array(gating["gated"])] + + ax9.hist( + innov_baseline, + bins=50, + alpha=0.5, + color=color_baseline, + label="Baseline (all)", + density=True, + ) + ax9.hist( + innov_gating, + bins=50, + alpha=0.5, + color=color_gating, + label="Gating (accepted)", + density=True, + ) + + ax9.set_xlabel("Innovation Magnitude [m]") + ax9.set_ylabel("Density") + ax9.set_title("Innovation Distribution") ax9.legend() ax9.grid(True, alpha=0.3) - fig.suptitle('Tuning & Robust Loss Comparison (NLOS Dataset)', - fontsize=16, fontweight='bold') + fig.suptitle( + "Tuning & Robust Loss Comparison (NLOS Dataset)", fontsize=16, fontweight="bold" + ) if save_path: # save_figure takes a directory and a stem, and writes svg/pdf/png @@ -533,32 +711,26 @@ def main(): "--data", type=str, default="data/sim/ch8_fusion_2d_imu_uwb_nlos", - help="Path to NLOS dataset directory" + help="Path to NLOS dataset directory", ) parser.add_argument( - "--save", - type=str, - default=None, - help="Path to save results figure" - ) - parser.add_argument( - "--seed", - type=int, - default=42, - help="Random seed" + "--save", type=str, default=None, help="Path to save results figure" ) + parser.add_argument("--seed", type=int, default=42, help="Random seed") args = parser.parse_args() np.random.seed(args.seed) - print("\n" + "="*70) + print("\n" + "=" * 70) print("Tuning and Robust Loss Demo (Chapter 8)") - print("="*70) + print("=" * 70) print("\nKey Concepts (Eq. 8.7: R_k <- w_R * R_k):") print(" 1. Baseline (no gating): Accepts all measurements (including outliers)") print(" 2. Chi-square gating: Hard rejection based on Mahalanobis distance") - print(" 3. Huber loss: Soft inflation of R for outliers (w_R = |r|/delta for |r|>delta)") + print( + " 3. Huber loss: Soft inflation of R for outliers (w_R = |r|/delta for |r|>delta)" + ) print(" 4. Cauchy loss: Strong inflation of R for outliers (w_R = 1+(r/c)^2)") print("\n Note: Outliers get INFLATED covariance (w_R >= 1), reducing their") print(" influence in the Kalman gain without complete rejection.") @@ -578,32 +750,34 @@ def main(): # Run different strategies print("[1/4] Running baseline (no gating, no robust)...") baseline = run_fusion_with_strategy( - dataset, strategy='baseline', use_gating=False, verbose=True + dataset, strategy="baseline", use_gating=False, verbose=True ) print("[2/4] Running with chi-square gating...") gating = run_fusion_with_strategy( - dataset, strategy='gating', use_gating=True, gate_confidence=0.95, verbose=True + dataset, strategy="gating", use_gating=True, gate_confidence=0.95, verbose=True ) print("[3/4] Running with Huber robust loss...") huber = run_fusion_with_strategy( - dataset, strategy='huber', robust_threshold=1.5, verbose=True + dataset, strategy="huber", robust_threshold=1.5, verbose=True ) print("[4/4] Running with Cauchy robust loss...") cauchy = run_fusion_with_strategy( - dataset, strategy='cauchy', robust_threshold=2.5, verbose=True + dataset, strategy="cauchy", robust_threshold=2.5, verbose=True ) # Compute RMSE def compute_final_rmse(history): - truth = dataset['truth'] - p_true_interp = np.column_stack([ - np.interp(history['t'], truth['t'], truth['p_xy'][:, 0]), - np.interp(history['t'], truth['t'], truth['p_xy'][:, 1]) - ]) - errors = history['x_est'][:, :2] - p_true_interp + truth = dataset["truth"] + p_true_interp = np.column_stack( + [ + np.interp(history["t"], truth["t"], truth["p_xy"][:, 0]), + np.interp(history["t"], truth["t"], truth["p_xy"][:, 1]), + ] + ) + errors = history["x_est"][:, :2] - p_true_interp # Norm first, then RMS. Passing the (N, 2) vectors straight to # compute_rmse averages over 2N components instead of N positions, # which is the per-axis RMS and is smaller by exactly sqrt(2) -- this @@ -616,37 +790,52 @@ def compute_final_rmse(history): rmse_huber = compute_final_rmse(huber) rmse_cauchy = compute_final_rmse(cauchy) - print("\n" + "="*70) + print("\n" + "=" * 70) print("Results Summary") - print("="*70) + print("=" * 70) print(f"{'Method':<25} {'RMSE [m]':>12} {'Accepted':>12} {'Rejected':>12}") - print("-"*70) - print(f"{'Baseline (no gating)':<25} {rmse_baseline:>12.3f} " - f"{baseline['n_uwb_accepted']:>12d} {baseline['n_uwb_rejected']:>12d}") - print(f"{'Chi-Square Gating':<25} {rmse_gating:>12.3f} " - f"{gating['n_uwb_accepted']:>12d} {gating['n_uwb_rejected']:>12d}") - print(f"{'Huber Loss':<25} {rmse_huber:>12.3f} " - f"{huber['n_uwb_accepted']:>12d} {huber['n_uwb_rejected']:>12d}") - print(f"{'Cauchy Loss':<25} {rmse_cauchy:>12.3f} " - f"{cauchy['n_uwb_accepted']:>12d} {cauchy['n_uwb_rejected']:>12d}") - print("="*70) + print("-" * 70) + print( + f"{'Baseline (no gating)':<25} {rmse_baseline:>12.3f} " + f"{baseline['n_uwb_accepted']:>12d} {baseline['n_uwb_rejected']:>12d}" + ) + print( + f"{'Chi-Square Gating':<25} {rmse_gating:>12.3f} " + f"{gating['n_uwb_accepted']:>12d} {gating['n_uwb_rejected']:>12d}" + ) + print( + f"{'Huber Loss':<25} {rmse_huber:>12.3f} " + f"{huber['n_uwb_accepted']:>12d} {huber['n_uwb_rejected']:>12d}" + ) + print( + f"{'Cauchy Loss':<25} {rmse_cauchy:>12.3f} " + f"{cauchy['n_uwb_accepted']:>12d} {cauchy['n_uwb_rejected']:>12d}" + ) + print("=" * 70) best_method = min( - [('Gating', rmse_gating), ('Huber', rmse_huber), ('Cauchy', rmse_cauchy)], - key=lambda x: x[1] + [("Gating", rmse_gating), ("Huber", rmse_huber), ("Cauchy", rmse_cauchy)], + key=lambda x: x[1], )[0] - improvement = 100 * (rmse_baseline - min(rmse_gating, rmse_huber, rmse_cauchy)) / rmse_baseline + improvement = ( + 100 + * (rmse_baseline - min(rmse_gating, rmse_huber, rmse_cauchy)) + / rmse_baseline + ) # Why gating fails here, measured rather than asserted. For a filter whose # covariance is right, NIS follows chi-square with 1 DOF: median 0.45, and # 95% of samples below the 3.84 gate. Reading the baseline -- which never # gates, so it cannot have been driven off course by its own rejections -- # tells us whether the gate is judging against a covariance worth trusting. - nis_baseline = np.asarray(baseline['nis']) + nis_baseline = np.asarray(baseline["nis"]) nis_median = float(np.median(nis_baseline)) frac_inside = 100.0 * float(np.mean(nis_baseline < 3.84)) - gate_rate = 100.0 * gating['n_uwb_accepted'] / ( - gating['n_uwb_accepted'] + gating['n_uwb_rejected']) + gate_rate = ( + 100.0 + * gating["n_uwb_accepted"] + / (gating["n_uwb_accepted"] + gating["n_uwb_rejected"]) + ) print("\nKey Findings:") print(f" * Best method: {best_method}") @@ -655,28 +844,45 @@ def compute_final_rmse(history): print(" * Huber: linear inflation, Cauchy: quadratic inflation") print("") print(f" Why chi-square gating collapses here (RMSE {rmse_gating:.2f} m):") - print(f" - The filter is over-confident. Ungated NIS has median " - f"{nis_median:.1f}, against {0.45:.2f} for a consistent 1-DOF filter,") - print(f" and only {frac_inside:.0f}% of samples fall inside the 3.84 " - f"gate. R is set from the line-of-sight noise, while roughly half") - print(" this dataset's ranges carry an NLOS bias an order of " - "magnitude larger.") - print(f" - So the gate is not discarding outliers, it is discarding " - f"most measurements: {gate_rate:.0f}% accepted.") - print(" - Starved of updates the state drifts, drift inflates the next " - "innovation, and the rejection feeds itself.") - print(" - The lesson is not that gating is inferior. A hard gate is " - "only as good as the covariance it tests against; the robust") - print(" losses survive the same mis-specified R because they scale " - "an outlier's influence down instead of removing it.") + print( + f" - The filter is over-confident. Ungated NIS has median " + f"{nis_median:.1f}, against {0.45:.2f} for a consistent 1-DOF filter," + ) + print( + f" and only {frac_inside:.0f}% of samples fall inside the 3.84 " + f"gate. R is set from the line-of-sight noise, while roughly half" + ) + print( + " this dataset's ranges carry an NLOS bias an order of " + "magnitude larger." + ) + print( + f" - So the gate is not discarding outliers, it is discarding " + f"most measurements: {gate_rate:.0f}% accepted." + ) + print( + " - Starved of updates the state drifts, drift inflates the next " + "innovation, and the rejection feeds itself." + ) + print( + " - The lesson is not that gating is inferior. A hard gate is " + "only as good as the covariance it tests against; the robust" + ) + print( + " losses survive the same mis-specified R because they scale " + "an outlier's influence down instead of removing it." + ) print("") # Plot - save_path = args.save if args.save else "ch8_sensor_fusion/figs/tuning_robust_demo.svg" + save_path = ( + args.save if args.save else "ch8_sensor_fusion/figs/tuning_robust_demo.svg" + ) Path(save_path).parent.mkdir(parents=True, exist_ok=True) - plot_tuning_comparison(dataset, baseline, gating, huber, cauchy, save_path=save_path) + plot_tuning_comparison( + dataset, baseline, gating, huber, cauchy, save_path=save_path + ) if __name__ == "__main__": main() - diff --git a/ch8_sensor_fusion/example_temporal_calibration.py b/ch8_sensor_fusion/example_temporal_calibration.py index cafd509..33f6b1c 100644 --- a/ch8_sensor_fusion/example_temporal_calibration.py +++ b/ch8_sensor_fusion/example_temporal_calibration.py @@ -73,10 +73,10 @@ def run_fusion_with_time_sync( apply_correction: bool = False, use_gating: bool = False, gate_confidence: float = 0.95, - verbose: bool = False + verbose: bool = False, ) -> Dict: """Run TC fusion with or without temporal calibration. - + Args: dataset: Dataset dictionary apply_correction: Whether to apply TimeSyncModel correction @@ -90,7 +90,7 @@ def run_fusion_with_time_sync( example_robust_tuning, which is where that failure mode belongs. gate_confidence: Gating confidence level (default 0.95 for 95% confidence) verbose: Print progress - + Returns: Results dictionary """ @@ -99,15 +99,15 @@ def run_fusion_with_time_sync( print(f" Temporal correction: {'ENABLED' if apply_correction else 'DISABLED'}") print(f" Gating: {'Enabled' if use_gating else 'Disabled'}") - truth = dataset['truth'] - imu = dataset['imu'] - uwb = dataset['uwb'] - anchors = dataset['uwb_anchors'] - config = dataset['config'] + truth = dataset["truth"] + imu = dataset["imu"] + uwb = dataset["uwb"] + anchors = dataset["uwb_anchors"] + config = dataset["config"] # Get time offset and drift from config - time_offset = config['temporal_calibration']['time_offset_sec'] - clock_drift = config['temporal_calibration']['clock_drift'] + time_offset = config["temporal_calibration"]["time_offset_sec"] + clock_drift = config["temporal_calibration"]["clock_drift"] if verbose and apply_correction: print(f" Time offset: {time_offset*1000:.1f} ms") @@ -125,16 +125,18 @@ def run_fusion_with_time_sync( uwb_time_sync = TimeSyncModel(offset=0.0, drift=0.0) # Initial state: [px, py, vx, vy, yaw] (follows StateIndex convention) - x0 = np.array([ - truth['p_xy'][0, 0], # px - truth['p_xy'][0, 1], # py - truth['v_xy'][0, 0], # vx - truth['v_xy'][0, 1], # vy - truth['yaw'][0] # yaw - ]) + x0 = np.array( + [ + truth["p_xy"][0, 0], # px + truth["p_xy"][0, 1], # py + truth["v_xy"][0, 0], # vx + truth["v_xy"][0, 1], # vy + truth["yaw"][0], # yaw + ] + ) # P0: covariances for [px, py, vx, vy, yaw] - P0 = np.diag([0.1, 0.1, 0.5, 0.5, 0.1])**2 + P0 = np.diag([0.1, 0.1, 0.5, 0.5, 0.1]) ** 2 # Process and measurement noise accel_noise_std = 0.1 @@ -150,34 +152,36 @@ def run_fusion_with_time_sync( Q=lambda dt: tc_process_noise_covariance(dt, accel_noise_std, gyro_noise_std), R=lambda: np.eye(4) * uwb_range_noise_std**2, x0=x0, - P0=P0 + P0=P0, ) # Prepare IMU data arrays (for interpolation) - t_imu_all = imu['t'] - accel_xy_all = imu['accel_xy'] - gyro_z_all = imu['gyro_z'] + t_imu_all = imu["t"] + accel_xy_all = imu["accel_xy"] + gyro_z_all = imu["gyro_z"] # Prepare UWB measurements with corrected timestamps uwb_measurements: List[StampedMeasurement] = [] # Add UWB (apply time sync correction if requested) - for i in range(len(uwb['t'])): + for i in range(len(uwb["t"])): # Convert UWB sensor time to fusion time if apply_correction: - t_fusion = uwb_time_sync.to_fusion_time(uwb['t'][i]) + t_fusion = uwb_time_sync.to_fusion_time(uwb["t"][i]) else: - t_fusion = uwb['t'][i] # Use raw (incorrect) time + t_fusion = uwb["t"][i] # Use raw (incorrect) time for j in range(anchors.shape[0]): - if not np.isnan(uwb['ranges'][i, j]): - uwb_measurements.append(StampedMeasurement( - t=t_fusion, - sensor='uwb', - z=np.array([uwb['ranges'][i, j]]), - R=np.array([[uwb_range_noise_std**2]]), - meta={'anchor_id': j, 'anchor_pos': anchors[j]} - )) + if not np.isnan(uwb["ranges"][i, j]): + uwb_measurements.append( + StampedMeasurement( + t=t_fusion, + sensor="uwb", + z=np.array([uwb["ranges"][i, j]]), + R=np.array([[uwb_range_noise_std**2]]), + meta={"anchor_id": j, "anchor_pos": anchors[j]}, + ) + ) # Sort UWB by timestamp uwb_measurements.sort(key=lambda m: m.t) @@ -186,12 +190,12 @@ def run_fusion_with_time_sync( from core.fusion import chi_square_gate, innovation, innovation_covariance history = { - 't': [], - 'x_est': [], - 'P_trace': [], - 'innovations': [], - 'nis': [], - 'gated': [], + "t": [], + "x_est": [], + "P_trace": [], + "innovations": [], + "nis": [], + "gated": [], } n_uwb_accepted = 0 @@ -209,11 +213,13 @@ def run_fusion_with_time_sync( while imu_idx < len(t_imu_all) - 1 and t_imu_all[imu_idx + 1] <= t_uwb: # Propagate using this IMU sample dt = t_imu_all[imu_idx + 1] - t_state - u = np.array([ - accel_xy_all[imu_idx + 1, 0], - accel_xy_all[imu_idx + 1, 1], - gyro_z_all[imu_idx + 1] - ]) + u = np.array( + [ + accel_xy_all[imu_idx + 1, 0], + accel_xy_all[imu_idx + 1, 1], + gyro_z_all[imu_idx + 1], + ] + ) ekf.predict(u=u, dt=dt) t_state = t_imu_all[imu_idx + 1] imu_idx += 1 @@ -235,7 +241,7 @@ def run_fusion_with_time_sync( continue # UWB range update - anchor_pos = uwb_meas.meta['anchor_pos'] + anchor_pos = uwb_meas.meta["anchor_pos"] # Predict range state_pos = ekf.state[:2] @@ -266,51 +272,52 @@ def run_fusion_with_time_sync( n_uwb_rejected += 1 # Log - history['innovations'].append(float(np.abs(y[0]))) - history['nis'].append(float(y @ np.linalg.inv(S) @ y)) - history['gated'].append(accept) + history["innovations"].append(float(np.abs(y[0]))) + history["nis"].append(float(y @ np.linalg.inv(S) @ y)) + history["gated"].append(accept) # Record state at UWB measurement time - history['t'].append(t_uwb) - history['x_est'].append(ekf.state.copy()) - history['P_trace'].append(np.trace(ekf.covariance)) + history["t"].append(t_uwb) + history["x_est"].append(ekf.state.copy()) + history["P_trace"].append(np.trace(ekf.covariance)) # Propagate through remaining IMU samples while imu_idx < len(t_imu_all) - 1: dt = t_imu_all[imu_idx + 1] - t_state - u = np.array([ - accel_xy_all[imu_idx + 1, 0], - accel_xy_all[imu_idx + 1, 1], - gyro_z_all[imu_idx + 1] - ]) + u = np.array( + [ + accel_xy_all[imu_idx + 1, 0], + accel_xy_all[imu_idx + 1, 1], + gyro_z_all[imu_idx + 1], + ] + ) ekf.predict(u=u, dt=dt) t_state = t_imu_all[imu_idx + 1] imu_idx += 1 # Convert to arrays - history['t'] = np.array(history['t']) - history['x_est'] = np.array(history['x_est']) - history['P_trace'] = np.array(history['P_trace']) - history['n_uwb_accepted'] = n_uwb_accepted - history['n_uwb_rejected'] = n_uwb_rejected + history["t"] = np.array(history["t"]) + history["x_est"] = np.array(history["x_est"]) + history["P_trace"] = np.array(history["P_trace"]) + history["n_uwb_accepted"] = n_uwb_accepted + history["n_uwb_rejected"] = n_uwb_rejected if verbose: print(f" Accepted: {n_uwb_accepted}") print(f" Rejected: {n_uwb_rejected}") if n_uwb_accepted + n_uwb_rejected > 0: - print(f" Acceptance rate: {100*n_uwb_accepted/(n_uwb_accepted+n_uwb_rejected):.1f}%") + print( + f" Acceptance rate: {100*n_uwb_accepted/(n_uwb_accepted+n_uwb_rejected):.1f}%" + ) return history def plot_temporal_calibration( - dataset: Dict, - no_correction: Dict, - with_correction: Dict, - save_path: str = None + dataset: Dict, no_correction: Dict, with_correction: Dict, save_path: str = None ) -> None: """Generate temporal calibration comparison plots. - + Args: dataset: Dataset dictionary no_correction: Results without time sync correction @@ -319,118 +326,216 @@ def plot_temporal_calibration( are used -- ``core.eval.save_figure`` writes svg, pdf and png, so any extension given here is ignored. """ - truth = dataset['truth'] - anchors = dataset['uwb_anchors'] - config = dataset['config'] + truth = dataset["truth"] + anchors = dataset["uwb_anchors"] + config = dataset["config"] - time_offset_ms = config['temporal_calibration']['time_offset_sec'] * 1000 - clock_drift_ppm = config['temporal_calibration']['clock_drift'] * 1e6 + time_offset_ms = config["temporal_calibration"]["time_offset_sec"] * 1000 + clock_drift_ppm = config["temporal_calibration"]["clock_drift"] * 1e6 fig = plt.figure(figsize=(16, 10)) gs = GridSpec(2, 3, figure=fig, hspace=0.3, wspace=0.3) # Color scheme - color_truth = 'black' - color_no_corr = 'tab:red' - color_with_corr = 'tab:green' + color_truth = "black" + color_no_corr = "tab:red" + color_with_corr = "tab:green" # Helper function for errors def get_errors(history): - p_true_interp = np.column_stack([ - np.interp(history['t'], truth['t'], truth['p_xy'][:, 0]), - np.interp(history['t'], truth['t'], truth['p_xy'][:, 1]) - ]) - errors = history['x_est'][:, :2] - p_true_interp + p_true_interp = np.column_stack( + [ + np.interp(history["t"], truth["t"], truth["p_xy"][:, 0]), + np.interp(history["t"], truth["t"], truth["p_xy"][:, 1]), + ] + ) + errors = history["x_est"][:, :2] - p_true_interp return np.linalg.norm(errors, axis=1) # 1. Trajectory without correction ax1 = fig.add_subplot(gs[0, 0]) - ax1.plot(truth['p_xy'][:, 0], truth['p_xy'][:, 1], - color=color_truth, linewidth=2, label='Ground Truth', zorder=3) - ax1.plot(no_correction['x_est'][:, 0], no_correction['x_est'][:, 1], - color=color_no_corr, linewidth=1.5, alpha=0.7, - label='No Time Correction', zorder=2) - ax1.scatter(anchors[:, 0], anchors[:, 1], s=150, c='red', marker='^', - edgecolors='darkred', linewidths=2, label='UWB Anchors', zorder=5) - ax1.set_xlabel('X [m]') - ax1.set_ylabel('Y [m]') - ax1.set_title(f'Without Correction (offset={time_offset_ms:.0f}ms, drift={clock_drift_ppm:.0f}ppm)') + ax1.plot( + truth["p_xy"][:, 0], + truth["p_xy"][:, 1], + color=color_truth, + linewidth=2, + label="Ground Truth", + zorder=3, + ) + ax1.plot( + no_correction["x_est"][:, 0], + no_correction["x_est"][:, 1], + color=color_no_corr, + linewidth=1.5, + alpha=0.7, + label="No Time Correction", + zorder=2, + ) + ax1.scatter( + anchors[:, 0], + anchors[:, 1], + s=150, + c="red", + marker="^", + edgecolors="darkred", + linewidths=2, + label="UWB Anchors", + zorder=5, + ) + ax1.set_xlabel("X [m]") + ax1.set_ylabel("Y [m]") + ax1.set_title( + f"Without Correction (offset={time_offset_ms:.0f}ms, drift={clock_drift_ppm:.0f}ppm)" + ) ax1.legend(fontsize=8) ax1.grid(True, alpha=0.3) - ax1.axis('equal') + ax1.axis("equal") # 2. Trajectory with correction ax2 = fig.add_subplot(gs[0, 1]) - ax2.plot(truth['p_xy'][:, 0], truth['p_xy'][:, 1], - color=color_truth, linewidth=2, label='Ground Truth', zorder=3) - ax2.plot(with_correction['x_est'][:, 0], with_correction['x_est'][:, 1], - color=color_with_corr, linewidth=1.5, alpha=0.7, - label='With Time Correction', zorder=2) - ax2.scatter(anchors[:, 0], anchors[:, 1], s=150, c='red', marker='^', - edgecolors='darkred', linewidths=2, label='UWB Anchors', zorder=5) - ax2.set_xlabel('X [m]') - ax2.set_ylabel('Y [m]') - ax2.set_title('With TimeSyncModel Correction') + ax2.plot( + truth["p_xy"][:, 0], + truth["p_xy"][:, 1], + color=color_truth, + linewidth=2, + label="Ground Truth", + zorder=3, + ) + ax2.plot( + with_correction["x_est"][:, 0], + with_correction["x_est"][:, 1], + color=color_with_corr, + linewidth=1.5, + alpha=0.7, + label="With Time Correction", + zorder=2, + ) + ax2.scatter( + anchors[:, 0], + anchors[:, 1], + s=150, + c="red", + marker="^", + edgecolors="darkred", + linewidths=2, + label="UWB Anchors", + zorder=5, + ) + ax2.set_xlabel("X [m]") + ax2.set_ylabel("Y [m]") + ax2.set_title("With TimeSyncModel Correction") ax2.legend(fontsize=8) ax2.grid(True, alpha=0.3) - ax2.axis('equal') + ax2.axis("equal") # 3. Overlay comparison ax3 = fig.add_subplot(gs[0, 2]) - ax3.plot(truth['p_xy'][:, 0], truth['p_xy'][:, 1], - color=color_truth, linewidth=2, label='Truth', zorder=4) - ax3.plot(no_correction['x_est'][:, 0], no_correction['x_est'][:, 1], - color=color_no_corr, linewidth=1.5, alpha=0.6, - label='No Correction', zorder=2) - ax3.plot(with_correction['x_est'][:, 0], with_correction['x_est'][:, 1], - color=color_with_corr, linewidth=1.5, alpha=0.8, - label='With Correction', zorder=3) - ax3.scatter(anchors[:, 0], anchors[:, 1], s=150, c='red', marker='^', - edgecolors='darkred', linewidths=2, zorder=5) - ax3.set_xlabel('X [m]') - ax3.set_ylabel('Y [m]') - ax3.set_title('Direct Comparison') + ax3.plot( + truth["p_xy"][:, 0], + truth["p_xy"][:, 1], + color=color_truth, + linewidth=2, + label="Truth", + zorder=4, + ) + ax3.plot( + no_correction["x_est"][:, 0], + no_correction["x_est"][:, 1], + color=color_no_corr, + linewidth=1.5, + alpha=0.6, + label="No Correction", + zorder=2, + ) + ax3.plot( + with_correction["x_est"][:, 0], + with_correction["x_est"][:, 1], + color=color_with_corr, + linewidth=1.5, + alpha=0.8, + label="With Correction", + zorder=3, + ) + ax3.scatter( + anchors[:, 0], + anchors[:, 1], + s=150, + c="red", + marker="^", + edgecolors="darkred", + linewidths=2, + zorder=5, + ) + ax3.set_xlabel("X [m]") + ax3.set_ylabel("Y [m]") + ax3.set_title("Direct Comparison") ax3.legend(fontsize=8) ax3.grid(True, alpha=0.3) - ax3.axis('equal') + ax3.axis("equal") # 4. Position error comparison ax4 = fig.add_subplot(gs[1, 0]) error_no_corr = get_errors(no_correction) error_with_corr = get_errors(with_correction) - ax4.plot(no_correction['t'], error_no_corr, color=color_no_corr, - linewidth=1.5, label='No Correction') - ax4.plot(with_correction['t'], error_with_corr, color=color_with_corr, - linewidth=1.5, label='With Correction') - ax4.set_xlabel('Time [s]') - ax4.set_ylabel('Position Error [m]') - ax4.set_title('Position Error vs Time') + ax4.plot( + no_correction["t"], + error_no_corr, + color=color_no_corr, + linewidth=1.5, + label="No Correction", + ) + ax4.plot( + with_correction["t"], + error_with_corr, + color=color_with_corr, + linewidth=1.5, + label="With Correction", + ) + ax4.set_xlabel("Time [s]") + ax4.set_ylabel("Position Error [m]") + ax4.set_title("Position Error vs Time") ax4.legend() ax4.grid(True, alpha=0.3) # 5. NIS comparison ax5 = fig.add_subplot(gs[1, 1]) - if len(no_correction['nis']) > 0: - nis_no_corr = np.array(no_correction['nis']) - nis_with_corr = np.array(with_correction['nis']) + if len(no_correction["nis"]) > 0: + nis_no_corr = np.array(no_correction["nis"]) + nis_with_corr = np.array(with_correction["nis"]) # Downsample for visibility step = max(1, len(nis_no_corr) // 500) - ax5.plot(nis_no_corr[::step], color=color_no_corr, - linewidth=0.5, alpha=0.5, label='No Correction') - ax5.plot(nis_with_corr[::step], color=color_with_corr, - linewidth=0.5, alpha=0.5, label='With Correction') + ax5.plot( + nis_no_corr[::step], + color=color_no_corr, + linewidth=0.5, + alpha=0.5, + label="No Correction", + ) + ax5.plot( + nis_with_corr[::step], + color=color_with_corr, + linewidth=0.5, + alpha=0.5, + label="With Correction", + ) # Chi-square bound from core.fusion import chi_square_threshold - threshold = chi_square_threshold(dof=1, confidence=0.95) - ax5.axhline(threshold, color='r', linestyle='--', - linewidth=1.5, label=f'95% bound (chi^2={threshold:.2f})') - ax5.set_xlabel('UWB Update Index') - ax5.set_ylabel('NIS (1 DOF)') - ax5.set_title('Innovation Consistency (NIS)') + threshold = chi_square_threshold(dof=1, confidence=0.95) + ax5.axhline( + threshold, + color="r", + linestyle="--", + linewidth=1.5, + label=f"95% bound (chi^2={threshold:.2f})", + ) + + ax5.set_xlabel("UWB Update Index") + ax5.set_ylabel("NIS (1 DOF)") + ax5.set_title("Innovation Consistency (NIS)") ax5.set_ylim([0, min(20, np.percentile(nis_no_corr, 99))]) ax5.legend() ax5.grid(True, alpha=0.3) @@ -441,45 +546,76 @@ def get_errors(history): rmse_no_corr = compute_rmse(error_no_corr) rmse_with_corr = compute_rmse(error_with_corr) - metrics = ['RMSE\n[m]', 'Max Error\n[m]', 'Accept\nRate [%]'] + metrics = ["RMSE\n[m]", "Max Error\n[m]", "Accept\nRate [%]"] no_corr_vals = [ rmse_no_corr, np.max(error_no_corr), - 100 * no_correction['n_uwb_accepted'] / (no_correction['n_uwb_accepted'] + no_correction['n_uwb_rejected']) + 100 + * no_correction["n_uwb_accepted"] + / (no_correction["n_uwb_accepted"] + no_correction["n_uwb_rejected"]), ] with_corr_vals = [ rmse_with_corr, np.max(error_with_corr), - 100 * with_correction['n_uwb_accepted'] / (with_correction['n_uwb_accepted'] + with_correction['n_uwb_rejected']) + 100 + * with_correction["n_uwb_accepted"] + / (with_correction["n_uwb_accepted"] + with_correction["n_uwb_rejected"]), ] x = np.arange(len(metrics)) width = 0.35 - bars1 = ax6.bar(x - width/2, no_corr_vals, width, - label='No Correction', color=color_no_corr, alpha=0.7) - bars2 = ax6.bar(x + width/2, with_corr_vals, width, - label='With Correction', color=color_with_corr, alpha=0.7) + bars1 = ax6.bar( + x - width / 2, + no_corr_vals, + width, + label="No Correction", + color=color_no_corr, + alpha=0.7, + ) + bars2 = ax6.bar( + x + width / 2, + with_corr_vals, + width, + label="With Correction", + color=color_with_corr, + alpha=0.7, + ) - ax6.set_ylabel('Value') - ax6.set_title('Metrics Comparison') + ax6.set_ylabel("Value") + ax6.set_title("Metrics Comparison") ax6.set_xticks(x) ax6.set_xticklabels(metrics, fontsize=9) ax6.legend() - ax6.grid(True, alpha=0.3, axis='y') + ax6.grid(True, alpha=0.3, axis="y") # Add value labels for bar, val in zip(bars1, no_corr_vals): height = bar.get_height() - ax6.text(bar.get_x() + bar.get_width()/2., height, - f'{val:.1f}', ha='center', va='bottom', fontsize=8) + ax6.text( + bar.get_x() + bar.get_width() / 2.0, + height, + f"{val:.1f}", + ha="center", + va="bottom", + fontsize=8, + ) for bar, val in zip(bars2, with_corr_vals): height = bar.get_height() - ax6.text(bar.get_x() + bar.get_width()/2., height, - f'{val:.1f}', ha='center', va='bottom', fontsize=8) - - fig.suptitle(f'Temporal Calibration Demo (offset={time_offset_ms:.0f}ms, drift={clock_drift_ppm:.0f}ppm)', - fontsize=16, fontweight='bold') + ax6.text( + bar.get_x() + bar.get_width() / 2.0, + height, + f"{val:.1f}", + ha="center", + va="bottom", + fontsize=8, + ) + + fig.suptitle( + f"Temporal Calibration Demo (offset={time_offset_ms:.0f}ms, drift={clock_drift_ppm:.0f}ppm)", + fontsize=16, + fontweight="bold", + ) if save_path: # Via save_figure, not plt.savefig: it writes the book's svg/pdf @@ -502,28 +638,20 @@ def main(): "--data", type=str, default="data/sim/ch8_fusion_2d_imu_uwb_timeoffset", - help="Path to time-offset dataset directory" - ) - parser.add_argument( - "--save", - type=str, - default=None, - help="Path to save results figure" + help="Path to time-offset dataset directory", ) parser.add_argument( - "--seed", - type=int, - default=42, - help="Random seed" + "--save", type=str, default=None, help="Path to save results figure" ) + parser.add_argument("--seed", type=int, default=42, help="Random seed") args = parser.parse_args() np.random.seed(args.seed) - print("\n" + "="*70) + print("\n" + "=" * 70) print("Temporal Calibration Demo (Chapter 8)") - print("="*70) + print("=" * 70) print("\nKey Concept:") print(" Sensor clocks are not perfectly synchronized.") print(" -> Time offsets and clock drift cause fusion errors.") @@ -535,8 +663,8 @@ def main(): print(f"Loading time-offset dataset from: {args.data}") dataset = load_fusion_dataset(args.data) - time_offset_ms = dataset['config']['temporal_calibration']['time_offset_sec'] * 1000 - clock_drift_ppm = dataset['config']['temporal_calibration']['clock_drift'] * 1e6 + time_offset_ms = dataset["config"]["temporal_calibration"]["time_offset_sec"] * 1000 + clock_drift_ppm = dataset["config"]["temporal_calibration"]["clock_drift"] * 1e6 print("\nDataset info:") print(f" IMU samples: {len(dataset['imu']['t'])}") @@ -561,26 +689,30 @@ def main(): # Compute RMSE def compute_final_rmse(history): - truth = dataset['truth'] - p_true_interp = np.column_stack([ - np.interp(history['t'], truth['t'], truth['p_xy'][:, 0]), - np.interp(history['t'], truth['t'], truth['p_xy'][:, 1]) - ]) - errors = history['x_est'][:, :2] - p_true_interp + truth = dataset["truth"] + p_true_interp = np.column_stack( + [ + np.interp(history["t"], truth["t"], truth["p_xy"][:, 0]), + np.interp(history["t"], truth["t"], truth["p_xy"][:, 1]), + ] + ) + errors = history["x_est"][:, :2] - p_true_interp return compute_position_rmse(errors) rmse_no_corr = compute_final_rmse(no_correction) rmse_with_corr = compute_final_rmse(with_correction) - print("\n" + "="*70) + print("\n" + "=" * 70) print("Results Summary") - print("="*70) + print("=" * 70) print(f"{'Method':<30} {'RMSE [m]':>12} {'Improvement':>15}") - print("-"*70) + print("-" * 70) print(f"{'Without Time Correction':<30} {rmse_no_corr:>12.3f} {'(baseline)':>15}") - print(f"{'With TimeSyncModel':<30} {rmse_with_corr:>12.3f} " - f"{100*(rmse_no_corr-rmse_with_corr)/rmse_no_corr:>14.1f}%") - print("="*70) + print( + f"{'With TimeSyncModel':<30} {rmse_with_corr:>12.3f} " + f"{100*(rmse_no_corr-rmse_with_corr)/rmse_no_corr:>14.1f}%" + ) + print("=" * 70) improvement = 100 * (rmse_no_corr - rmse_with_corr) / rmse_no_corr @@ -589,30 +721,45 @@ def compute_final_rmse(history): # What the offset costs is the difference between the two runs, and the # sanity check on it is kinematic -- a platform moving at v with its # ranges stamped Dt late is being fused against a position v*Dt away. - speed = float(np.mean(np.linalg.norm(dataset['truth']['v_xy'], axis=1))) + speed = float(np.mean(np.linalg.norm(dataset["truth"]["v_xy"], axis=1))) expected = speed * abs(time_offset_ms) / 1000.0 print("\nKey Findings:") - print(f" * Uncorrected: {rmse_no_corr:.3f} m RMSE; corrected: " - f"{rmse_with_corr:.3f} m") - print(f" * So a {time_offset_ms:.1f} ms offset costs " - f"{rmse_no_corr - rmse_with_corr:.3f} m, and TimeSyncModel recovers " - f"it: {improvement:.1f}% better") - print(f" * That is the order the kinematics predict: {speed:.2f} m/s for " - f"{abs(time_offset_ms):.0f} ms displaces the platform " - f"{expected:.3f} m") - print(" * Small here because the platform is slow. The cost scales with " - "speed, so the same clock error on a vehicle at 15 m/s is metres,") - print(" not centimetres -- that is why temporal alignment matters, " - "rather than any large number printed by this demo.") + print( + f" * Uncorrected: {rmse_no_corr:.3f} m RMSE; corrected: " + f"{rmse_with_corr:.3f} m" + ) + print( + f" * So a {time_offset_ms:.1f} ms offset costs " + f"{rmse_no_corr - rmse_with_corr:.3f} m, and TimeSyncModel recovers " + f"it: {improvement:.1f}% better" + ) + print( + f" * That is the order the kinematics predict: {speed:.2f} m/s for " + f"{abs(time_offset_ms):.0f} ms displaces the platform " + f"{expected:.3f} m" + ) + print( + " * Small here because the platform is slow. The cost scales with " + "speed, so the same clock error on a vehicle at 15 m/s is metres," + ) + print( + " not centimetres -- that is why temporal alignment matters, " + "rather than any large number printed by this demo." + ) print("") # Plot - save_path = args.save if args.save else "ch8_sensor_fusion/figs/temporal_calibration_demo.svg" + save_path = ( + args.save + if args.save + else "ch8_sensor_fusion/figs/temporal_calibration_demo.svg" + ) Path(save_path).parent.mkdir(parents=True, exist_ok=True) - plot_temporal_calibration(dataset, no_correction, with_correction, save_path=save_path) + plot_temporal_calibration( + dataset, no_correction, with_correction, save_path=save_path + ) if __name__ == "__main__": main() - diff --git a/core/estimators/__init__.py b/core/estimators/__init__.py index 9bf6f2f..5a4f1ae 100644 --- a/core/estimators/__init__.py +++ b/core/estimators/__init__.py @@ -58,4 +58,3 @@ "Factor", "FactorGraph", ] - diff --git a/core/estimators/base.py b/core/estimators/base.py index 8e1a9bc..d096089 100644 --- a/core/estimators/base.py +++ b/core/estimators/base.py @@ -76,6 +76,3 @@ def estimate( Tuple of (state_estimate, covariance_matrix). """ pass - - - diff --git a/core/estimators/extended_kalman_filter.py b/core/estimators/extended_kalman_filter.py index 8abef28..891b5eb 100644 --- a/core/estimators/extended_kalman_filter.py +++ b/core/estimators/extended_kalman_filter.py @@ -46,14 +46,18 @@ class ExtendedKalmanFilter(StateEstimator): def __init__( self, process_model: Callable[[np.ndarray, Optional[np.ndarray], float], np.ndarray], - process_jacobian: Callable[[np.ndarray, Optional[np.ndarray], float], np.ndarray], + process_jacobian: Callable[ + [np.ndarray, Optional[np.ndarray], float], np.ndarray + ], measurement_model: Callable[[np.ndarray], np.ndarray], measurement_jacobian: Callable[[np.ndarray], np.ndarray], Q: Callable[[float], np.ndarray], R: Callable[[], np.ndarray], x0: np.ndarray, P0: np.ndarray, - innovation_func: Optional[Callable[[np.ndarray, np.ndarray], np.ndarray]] = None, + innovation_func: Optional[ + Callable[[np.ndarray, np.ndarray], np.ndarray] + ] = None, ): """ Initialize Extended Kalman Filter. @@ -231,41 +235,34 @@ def check_ekf_range_only_tracking(): # Process model: constant velocity in 2D def process_model(x, u, dt): - F = np.array([ - [1, 0, dt, 0], - [0, 1, 0, dt], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) + F = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) return F @ x def process_jacobian(x, u, dt): - return np.array([ - [1, 0, dt, 0], - [0, 1, 0, dt], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) + return np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) # Measurement model: range from origin (nonlinear) def measurement_model(x): - return np.array([np.sqrt(x[0]**2 + x[1]**2)]) + return np.array([np.sqrt(x[0] ** 2 + x[1] ** 2)]) def measurement_jacobian(x): - r = np.sqrt(x[0]**2 + x[1]**2) + r = np.sqrt(x[0] ** 2 + x[1] ** 2) if r < 1e-6: return np.array([[0, 0, 0, 0]]) - return np.array([[x[0]/r, x[1]/r, 0, 0]]) + return np.array([[x[0] / r, x[1] / r, 0, 0]]) # Noise covariances q = 0.1 + def Q_func(dt): - return q * np.array([ - [dt**3/3, 0, dt**2/2, 0], - [0, dt**3/3, 0, dt**2/2], - [dt**2/2, 0, dt, 0], - [0, dt**2/2, 0, dt] - ]) + return q * np.array( + [ + [dt**3 / 3, 0, dt**2 / 2, 0], + [0, dt**3 / 3, 0, dt**2 / 2], + [dt**2 / 2, 0, dt, 0], + [0, dt**2 / 2, 0, dt], + ] + ) def R_func(): return np.array([[0.5]]) @@ -276,9 +273,14 @@ def R_func(): # Create EKF ekf = ExtendedKalmanFilter( - process_model, process_jacobian, - measurement_model, measurement_jacobian, - Q_func, R_func, x0, P0 + process_model, + process_jacobian, + measurement_model, + measurement_jacobian, + Q_func, + R_func, + x0, + P0, ) # Generate true trajectory @@ -337,40 +339,33 @@ def check_ekf_bearing_only_tracking(): # Same process model as before def process_model(x, u, dt): - F = np.array([ - [1, 0, dt, 0], - [0, 1, 0, dt], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) + F = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) return F @ x def process_jacobian(x, u, dt): - return np.array([ - [1, 0, dt, 0], - [0, 1, 0, dt], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) + return np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) # Measurement model: bearing angle from origin def measurement_model(x): return np.array([np.arctan2(x[1], x[0])]) def measurement_jacobian(x): - r_sq = x[0]**2 + x[1]**2 + r_sq = x[0] ** 2 + x[1] ** 2 if r_sq < 1e-6: return np.array([[0, 0, 0, 0]]) - return np.array([[-x[1]/r_sq, x[0]/r_sq, 0, 0]]) + return np.array([[-x[1] / r_sq, x[0] / r_sq, 0, 0]]) q = 0.1 + def Q_func(dt): - return q * np.array([ - [dt**3/3, 0, dt**2/2, 0], - [0, dt**3/3, 0, dt**2/2], - [dt**2/2, 0, dt, 0], - [0, dt**2/2, 0, dt] - ]) + return q * np.array( + [ + [dt**3 / 3, 0, dt**2 / 2, 0], + [0, dt**3 / 3, 0, dt**2 / 2], + [dt**2 / 2, 0, dt, 0], + [0, dt**2 / 2, 0, dt], + ] + ) def R_func(): return np.array([[0.05]]) # 0.05 rad^2 variance @@ -384,9 +379,14 @@ def R_func(): # takes `innovation_func` for exactly this and the docstring says so; this # demo simply never passed one. ekf = ExtendedKalmanFilter( - process_model, process_jacobian, - measurement_model, measurement_jacobian, - Q_func, R_func, x0, P0, + process_model, + process_jacobian, + measurement_model, + measurement_jacobian, + Q_func, + R_func, + x0, + P0, innovation_func=lambda z, z_pred: angle_diff(z, z_pred), ) @@ -443,10 +443,9 @@ def process_model(x, u, dt): def process_jacobian(x, u, dt): """Jacobian depends on x[0], so evaluation point matters!""" - F = np.array([ - [1.0 + 0.2 * x[0] * dt, dt], # ∂f_0/∂x_0 = 1 + 0.2*x*dt - [0.0, 1.0] - ]) + F = np.array( + [[1.0 + 0.2 * x[0] * dt, dt], [0.0, 1.0]] # ∂f_0/∂x_0 = 1 + 0.2*x*dt + ) return F # Simple linear measurement (position only) @@ -468,9 +467,14 @@ def R_func(): # Create EKF ekf = ExtendedKalmanFilter( - process_model, process_jacobian, - measurement_model, measurement_jacobian, - Q_func, R_func, x0, P0 + process_model, + process_jacobian, + measurement_model, + measurement_jacobian, + Q_func, + R_func, + x0, + P0, ) # Record pre-prediction state @@ -508,10 +512,9 @@ def R_func(): ) # Verify it's NOT the wrong covariance - assert not np.allclose(ekf.covariance, P_wrong, atol=1e-10) or \ - np.allclose(P_correct, P_wrong, atol=1e-10), ( - "EKF appears to use post-prediction state for Jacobian (violates Eq. 3.22)" - ) + assert not np.allclose(ekf.covariance, P_wrong, atol=1e-10) or np.allclose( + P_correct, P_wrong, atol=1e-10 + ), "EKF appears to use post-prediction state for Jacobian (violates Eq. 3.22)" print("Jacobian Evaluation Point Test (Eq. 3.22):") print(f" Pre-prediction state x_{{k-1}}: {x_pre}") @@ -540,6 +543,3 @@ def R_func(): print("=" * 70) print("ALL CHECKS PASSED") print("=" * 70) - - - diff --git a/core/estimators/factor_graph.py b/core/estimators/factor_graph.py index 45c29e8..7486c1a 100644 --- a/core/estimators/factor_graph.py +++ b/core/estimators/factor_graph.py @@ -589,6 +589,7 @@ def check_fgo_simple_ls(): # Add measurement factors for z in measurements: + def residual_func(x_vars, z=z): return np.array([x_vars[0][0] - z]) @@ -715,9 +716,7 @@ def jacobian_func(x_vars, anchor=anchor): # Optimize with LM optimized_vars, error_history = graph.optimize( - method="levenberg_marquardt", - max_iterations=50, - initial_mu=1e-2 + method="levenberg_marquardt", max_iterations=50, initial_mu=1e-2 ) estimated_pos = optimized_vars[0] @@ -733,8 +732,11 @@ def jacobian_func(x_vars, anchor=anchor): # Check monotonic decrease (allowing for rejected steps) # Count how many times error increased - increases = sum(1 for i in range(1, len(error_history)) - if error_history[i] > error_history[i-1] + 1e-10) + increases = sum( + 1 + for i in range(1, len(error_history)) + if error_history[i] > error_history[i - 1] + 1e-10 + ) print(f" Error increases (should be 0 or small): {increases}") # LM should converge @@ -794,8 +796,9 @@ def jacobian_func(x_vars, anchor=anchor): # Check error decreases monotonically (line search guarantees this) for i in range(1, len(error_history)): - assert error_history[i] <= error_history[i-1] + 1e-10, \ - f"Line search should guarantee decrease: {error_history[i]} > {error_history[i-1]}" + assert ( + error_history[i] <= error_history[i - 1] + 1e-10 + ), f"Line search should guarantee decrease: {error_history[i]} > {error_history[i-1]}" assert position_error < 1e-5, f"Position error {position_error} too large" print(" [PASS] Test passed") @@ -870,9 +873,7 @@ def jacobian_func_b(x_vars, anchor=anchor): # Optimize with LM optimized_vars, error_history = graph.optimize( - method="levenberg_marquardt", - max_iterations=30, - initial_mu=1e-2 + method="levenberg_marquardt", max_iterations=30, initial_mu=1e-2 ) estimated_pos = optimized_vars[0] @@ -885,8 +886,9 @@ def jacobian_func_b(x_vars, anchor=anchor): print(f" Final error: {error_history[-1]:.6e}") # LM should NOT diverge - assert error_history[-1] <= error_history[0] * 1.1, \ - "LM should not diverge significantly" + assert ( + error_history[-1] <= error_history[0] * 1.1 + ), "LM should not diverge significantly" # Should converge reasonably assert position_error < 0.5, f"Position error {position_error} too large" @@ -980,13 +982,16 @@ def jacobian_func(x_vars, anchor=anchor): # Check error decreases monotonically with line search for i in range(1, len(error_history_ls)): - assert error_history_ls[i] <= error_history_ls[i-1] + 1e-10, \ - f"Line search should guarantee decrease: {error_history_ls[i]} > {error_history_ls[i-1]}" + assert ( + error_history_ls[i] <= error_history_ls[i - 1] + 1e-10 + ), f"Line search should guarantee decrease: {error_history_ls[i]} > {error_history_ls[i-1]}" print(" [PASS] Line search guarantees monotonic decrease") # Line search should generally do better or equal to fixed step - print(f" Line search improvement: {error_history_fixed[-1] / max(error_history_ls[-1], 1e-15):.1f}x") + print( + f" Line search improvement: {error_history_fixed[-1] / max(error_history_ls[-1], 1e-15):.1f}x" + ) print(" [PASS] Test passed") @@ -1013,6 +1018,3 @@ def jacobian_func(x_vars, anchor=anchor): print("=" * 70) print("ALL CHECKS PASSED") print("=" * 70) - - - diff --git a/core/estimators/iterated_extended_kalman_filter.py b/core/estimators/iterated_extended_kalman_filter.py index 829b87d..f595568 100644 --- a/core/estimators/iterated_extended_kalman_filter.py +++ b/core/estimators/iterated_extended_kalman_filter.py @@ -69,7 +69,9 @@ class IteratedExtendedKalmanFilter(StateEstimator): def __init__( self, process_model: Callable[[np.ndarray, Optional[np.ndarray], float], np.ndarray], - process_jacobian: Callable[[np.ndarray, Optional[np.ndarray], float], np.ndarray], + process_jacobian: Callable[ + [np.ndarray, Optional[np.ndarray], float], np.ndarray + ], measurement_model: Callable[[np.ndarray], np.ndarray], measurement_jacobian: Callable[[np.ndarray], np.ndarray], Q: Callable[[float], np.ndarray], @@ -78,7 +80,9 @@ def __init__( P0: np.ndarray, max_iterations: int = 5, convergence_tol: float = 1e-6, - innovation_func: Optional[Callable[[np.ndarray, np.ndarray], np.ndarray]] = None, + innovation_func: Optional[ + Callable[[np.ndarray, np.ndarray], np.ndarray] + ] = None, ): """ Initialize Iterated Extended Kalman Filter. @@ -316,9 +320,14 @@ def R_func(): P0 = np.eye(2) iekf = IteratedExtendedKalmanFilter( - process_model, process_jacobian, - measurement_model, measurement_jacobian, - Q_func, R_func, x0, P0, + process_model, + process_jacobian, + measurement_model, + measurement_jacobian, + Q_func, + R_func, + x0, + P0, max_iterations=10, convergence_tol=1e-8, ) @@ -350,21 +359,11 @@ def check_iekf_vs_ekf_high_nonlinearity(): # Process model def process_model(x, u, dt): - F = np.array([ - [1, 0, dt, 0], - [0, 1, 0, dt], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) + F = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) return F @ x def process_jacobian(x, u, dt): - return np.array([ - [1, 0, dt, 0], - [0, 1, 0, dt], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) + return np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) # Highly nonlinear measurement: range from origin def measurement_model(x): @@ -379,12 +378,14 @@ def measurement_jacobian(x): q = 0.1 def Q_func(dt): - return q * np.array([ - [dt ** 3 / 3, 0, dt ** 2 / 2, 0], - [0, dt ** 3 / 3, 0, dt ** 2 / 2], - [dt ** 2 / 2, 0, dt, 0], - [0, dt ** 2 / 2, 0, dt] - ]) + return q * np.array( + [ + [dt**3 / 3, 0, dt**2 / 2, 0], + [0, dt**3 / 3, 0, dt**2 / 2], + [dt**2 / 2, 0, dt, 0], + [0, dt**2 / 2, 0, dt], + ] + ) def R_func(): return np.array([[0.5]]) @@ -396,16 +397,26 @@ def R_func(): # Create both filters ekf = ExtendedKalmanFilter( - process_model, process_jacobian, - measurement_model, measurement_jacobian, - Q_func, R_func, x0_est.copy(), P0.copy() + process_model, + process_jacobian, + measurement_model, + measurement_jacobian, + Q_func, + R_func, + x0_est.copy(), + P0.copy(), ) iekf = IteratedExtendedKalmanFilter( - process_model, process_jacobian, - measurement_model, measurement_jacobian, - Q_func, R_func, x0_est.copy(), P0.copy(), - max_iterations=5 + process_model, + process_jacobian, + measurement_model, + measurement_jacobian, + Q_func, + R_func, + x0_est.copy(), + P0.copy(), + max_iterations=5, ) # Run simulation @@ -444,12 +455,14 @@ def R_func(): print("\nIEKF vs EKF Comparison Test:") print(f" Mean EKF position error: {mean_ekf_error:.4f} m") print(f" Mean IEKF position error: {mean_iekf_error:.4f} m") - print(f" IEKF improvement: {(mean_ekf_error - mean_iekf_error) / mean_ekf_error * 100:.1f}%") + print( + f" IEKF improvement: {(mean_ekf_error - mean_iekf_error) / mean_ekf_error * 100:.1f}%" + ) # IEKF should be at least as good as EKF (usually better) - assert mean_iekf_error <= mean_ekf_error * 1.1, ( - "IEKF should not be significantly worse than EKF" - ) + assert ( + mean_iekf_error <= mean_ekf_error * 1.1 + ), "IEKF should not be significantly worse than EKF" print(" [PASS] IEKF performs at least as well as EKF") @@ -467,4 +480,3 @@ def R_func(): print("=" * 70) print("ALL CHECKS PASSED") print("=" * 70) - diff --git a/core/estimators/kalman_filter.py b/core/estimators/kalman_filter.py index 8eac032..b6528e3 100644 --- a/core/estimators/kalman_filter.py +++ b/core/estimators/kalman_filter.py @@ -81,7 +81,9 @@ def __init__( elif x0 is not None: state_dim = len(x0) else: - raise ValueError("Must provide either F as ndarray or x0 to determine state_dim") + raise ValueError( + "Must provide either F as ndarray or x0 to determine state_dim" + ) super().__init__(state_dim) @@ -100,9 +102,7 @@ def __init__( f"P0 shape {self.covariance.shape} inconsistent with state_dim {state_dim}" ) - def _get_matrix( - self, mat: Union[np.ndarray, Callable], *args - ) -> np.ndarray: + def _get_matrix(self, mat: Union[np.ndarray, Callable], *args) -> np.ndarray: """ Helper to get matrix value (handles both constant and callable). @@ -394,4 +394,3 @@ def R_func(): print("=" * 70) print("ALL CHECKS PASSED") print("=" * 70) - diff --git a/core/estimators/least_squares.py b/core/estimators/least_squares.py index 349e742..c257a49 100644 --- a/core/estimators/least_squares.py +++ b/core/estimators/least_squares.py @@ -57,7 +57,9 @@ def linear_least_squares( """ # Validate inputs if A.ndim != 2 or b.ndim != 1: - raise ValueError(f"A must be 2D and b must be 1D. Got A: {A.shape}, b: {b.shape}") + raise ValueError( + f"A must be 2D and b must be 1D. Got A: {A.shape}, b: {b.shape}" + ) m, n = A.shape if m < n: @@ -70,7 +72,8 @@ def linear_least_squares( rank = np.linalg.matrix_rank(A) if rank < n: raise ValueError( - f"A is rank deficient: rank={rank} < n={n}. " f"System has no unique solution." + f"A is rank deficient: rank={rank} < n={n}. " + f"System has no unique solution." ) # Compute normal equations: A'A x = A'b @@ -162,9 +165,7 @@ def weighted_least_squares( m, n = A.shape if len(b) != m: - raise ValueError( - f"Dimension mismatch: A has {m} rows, b has {len(b)} elements" - ) + raise ValueError(f"Dimension mismatch: A has {m} rows, b has {len(b)} elements") # Process W_or_sigma into full weight matrix W W_or_sigma = np.asarray(W_or_sigma) @@ -180,7 +181,7 @@ def weighted_least_squares( # Convert σᵢ to wᵢ = 1/σᵢ² if np.any(W_or_sigma <= 0): raise ValueError("Sigma values must be positive") - weights = 1.0 / (W_or_sigma ** 2) + weights = 1.0 / (W_or_sigma**2) else: weights = W_or_sigma if np.any(weights < 0): @@ -206,9 +207,7 @@ def weighted_least_squares( if np.any(eigenvalues < -1e-10): # Allow small numerical errors raise ValueError("Weight matrix W must be positive semi-definite") else: - raise ValueError( - f"W_or_sigma must be 1D or 2D array, got {W_or_sigma.ndim}D" - ) + raise ValueError(f"W_or_sigma must be 1D or 2D array, got {W_or_sigma.ndim}D") # Compute weighted normal equations: A'WA x = A'Wb ATWA = A.T @ W @ A @@ -428,7 +427,9 @@ def robust_least_squares( """ # Validate inputs if A.ndim != 2 or b.ndim != 1: - raise ValueError(f"A must be 2D and b must be 1D. Got A: {A.shape}, b: {b.shape}") + raise ValueError( + f"A must be 2D and b must be 1D. Got A: {A.shape}, b: {b.shape}" + ) m, n = A.shape if len(b) != m: @@ -524,18 +525,18 @@ def _compute_robust_weights( elif method == "cauchy": # Cauchy loss: ρ(r) = ½ ln(1 + r²) # Weight: w = 1 / (1 + r²) - weights = 1.0 / (1.0 + u ** 2) + weights = 1.0 / (1.0 + u**2) elif method == "gm": # Geman-McClure (G-M) loss: ρ(r) = ½ r² / (1 + r²) # Weight: w = 1 / (1 + r²)² - weights = 1.0 / (1.0 + u ** 2) ** 2 + weights = 1.0 / (1.0 + u**2) ** 2 elif method == "tukey": # Tukey biweight (*Extra, not in book Table 3.1*) # ρ(r) = { (c²/6)[1-(1-(r/c)²)³] if |r|≤c; c²/6 otherwise } # Weight: w = { (1-(r/c)²)² if |r|≤c; 0 otherwise } - weights = np.where(abs_u <= 1.0, (1.0 - u ** 2) ** 2, 0.0) + weights = np.where(abs_u <= 1.0, (1.0 - u**2) ** 2, 0.0) # Add small epsilon to avoid singular weight matrix weights = np.maximum(weights, 1e-10) diff --git a/core/estimators/nonlinear_least_squares.py b/core/estimators/nonlinear_least_squares.py index 780d019..b3142db 100644 --- a/core/estimators/nonlinear_least_squares.py +++ b/core/estimators/nonlinear_least_squares.py @@ -254,9 +254,16 @@ def robust_gauss_newton( # L2 is just standard Gauss-Newton if loss == "l2": - result = gauss_newton(h, jacobian, y, x0, weights=None, - max_iter=max_iter, tol=tol, - return_covariance=return_covariance) + result = gauss_newton( + h, + jacobian, + y, + x0, + weights=None, + max_iter=max_iter, + tol=tol, + return_covariance=return_covariance, + ) result.weights = np.ones(m) return result @@ -265,7 +272,10 @@ def robust_gauss_newton( for irls_iter in range(max_irls_iter): # Run weighted Gauss-Newton with current weights result = gauss_newton( - h, jacobian, y, x, + h, + jacobian, + y, + x, weights=weights, max_iter=max_iter, tol=tol, @@ -298,7 +308,10 @@ def robust_gauss_newton( # Final solve with converged weights result = gauss_newton( - h, jacobian, y, x, + h, + jacobian, + y, + x, weights=weights, max_iter=max_iter, tol=tol, @@ -483,15 +496,15 @@ def _compute_robust_weights(u: np.ndarray, loss: str) -> np.ndarray: elif loss == "cauchy": # Cauchy: w = 1 / (1 + u²) - weights = 1.0 / (1.0 + u ** 2) + weights = 1.0 / (1.0 + u**2) elif loss == "gm" or loss == "geman_mcclure": # Geman-McClure: w = 1 / (1 + u²)² - weights = 1.0 / (1.0 + u ** 2) ** 2 + weights = 1.0 / (1.0 + u**2) ** 2 elif loss == "tukey": # Tukey biweight: w = (1-u²)² if |u| ≤ 1, else 0 - weights = np.where(abs_u <= 1.0, (1.0 - u ** 2) ** 2, 0.0) + weights = np.where(abs_u <= 1.0, (1.0 - u**2) ** 2, 0.0) weights = np.maximum(weights, 1e-10) # Avoid singularity else: @@ -608,4 +621,3 @@ def solve_nonlinear_ls( ) else: raise ValueError(f"Unknown method: {method}. Use 'gn' or 'lm'.") - diff --git a/core/estimators/particle_filter.py b/core/estimators/particle_filter.py index 348becd..a22b4b7 100644 --- a/core/estimators/particle_filter.py +++ b/core/estimators/particle_filter.py @@ -80,9 +80,7 @@ def __init__( self._rng = np.random if rng is None else rng # Initialize particles from initial distribution - self.particles = self._rng.multivariate_normal( - x0, P0, size=n_particles - ) + self.particles = self._rng.multivariate_normal(x0, P0, size=n_particles) # Initialize weights uniformly self.weights = np.ones(n_particles) / n_particles @@ -227,7 +225,7 @@ def check_particle_filter_1d(): def process_model(x, u, dt): F = np.array([[1.0, dt], [0.0, 1.0]]) process_noise = np.random.multivariate_normal( - [0, 0], 0.1 * np.array([[dt**3/3, dt**2/2], [dt**2/2, dt]]) + [0, 0], 0.1 * np.array([[dt**3 / 3, dt**2 / 2], [dt**2 / 2, dt]]) ) return F @ x + process_noise @@ -256,7 +254,9 @@ def likelihood_func(z, x): for _ in range(n_steps): true_state = np.array([[1.0, dt], [0.0, 1.0]]) @ true_state - true_state += np.random.multivariate_normal([0, 0], 0.1 * np.array([[dt**3/3, dt**2/2], [dt**2/2, dt]])) + true_state += np.random.multivariate_normal( + [0, 0], 0.1 * np.array([[dt**3 / 3, dt**2 / 2], [dt**2 / 2, dt]]) + ) # Generate measurement z = true_state[0] + np.random.normal(0, 0.5) @@ -296,10 +296,7 @@ def check_particle_filter_nonlinear(): # Nonlinear process model def process_model(x, u, dt): # Simple nonlinear dynamics with bimodal noise - x_new = np.array([ - x[0] + x[1] * dt + 0.1 * np.sin(x[0]), - x[1] * 0.95 - ]) + x_new = np.array([x[0] + x[1] * dt + 0.1 * np.sin(x[0]), x[1] * 0.95]) # Add bimodal noise if np.random.random() > 0.5: x_new += np.random.normal(0, 0.1, size=2) @@ -310,7 +307,7 @@ def process_model(x, u, dt): # Nonlinear measurement model def likelihood_func(z, x): # Range measurement from origin - z_pred = np.sqrt(x[0]**2 + x[1]**2) + z_pred = np.sqrt(x[0] ** 2 + x[1] ** 2) measurement_std = 0.5 return float( np.exp(-0.5 * np.sum(((z - z_pred) / measurement_std) ** 2)) @@ -328,14 +325,16 @@ def likelihood_func(z, x): for _ in range(n_steps): # Simple dynamics for true state - true_state = np.array([ - true_state[0] + true_state[1] * dt + 0.1 * np.sin(true_state[0]), - true_state[1] * 0.95 - ]) + true_state = np.array( + [ + true_state[0] + true_state[1] * dt + 0.1 * np.sin(true_state[0]), + true_state[1] * 0.95, + ] + ) true_state += np.random.normal(0, 0.1, size=2) # Generate range measurement - z = np.sqrt(true_state[0]**2 + true_state[1]**2) + np.random.normal(0, 0.5) + z = np.sqrt(true_state[0] ** 2 + true_state[1] ** 2) + np.random.normal(0, 0.5) pf.predict(dt=dt) pf.update(np.array([z])) @@ -368,6 +367,3 @@ def likelihood_func(z, x): print("=" * 70) print("ALL CHECKS PASSED") print("=" * 70) - - - diff --git a/core/estimators/unscented_kalman_filter.py b/core/estimators/unscented_kalman_filter.py index 260c110..3cff721 100644 --- a/core/estimators/unscented_kalman_filter.py +++ b/core/estimators/unscented_kalman_filter.py @@ -60,7 +60,9 @@ def __init__( alpha: float = 1e-3, beta: float = 2.0, kappa: Optional[float] = None, - innovation_func: Optional[Callable[[np.ndarray, np.ndarray], np.ndarray]] = None, + innovation_func: Optional[ + Callable[[np.ndarray, np.ndarray], np.ndarray] + ] = None, ): """ Initialize Unscented Kalman Filter. @@ -131,9 +133,7 @@ def _compute_weights(self) -> None: self.lambda_ = lambda_ - def _generate_sigma_points( - self, x: np.ndarray, P: np.ndarray - ) -> np.ndarray: + def _generate_sigma_points(self, x: np.ndarray, P: np.ndarray) -> np.ndarray: """ Generate sigma points using the Unscented Transform. @@ -162,7 +162,11 @@ def _generate_sigma_points( except np.linalg.LinAlgError: # If Cholesky fails, use eigendecomposition eigenvalues, eigenvectors = np.linalg.eig(P) - L = eigenvectors @ np.diag(np.sqrt(np.maximum(eigenvalues, 0))) * np.sqrt(n + self.lambda_) + L = ( + eigenvectors + @ np.diag(np.sqrt(np.maximum(eigenvalues, 0))) + * np.sqrt(n + self.lambda_) + ) # Eq. (3.24): χᵢ = x̂ + δᵢ and χ_{i+n} = x̂ - δᵢ for i in range(n): @@ -190,7 +194,11 @@ def _unscented_transform( # Weighted covariance diff = sigma_points - mean - covariance = (weights_c[:, np.newaxis, np.newaxis] * diff[:, :, np.newaxis] * diff[:, np.newaxis, :]).sum(axis=0) + covariance = ( + weights_c[:, np.newaxis, np.newaxis] + * diff[:, :, np.newaxis] + * diff[:, np.newaxis, :] + ).sum(axis=0) return mean, covariance @@ -217,9 +225,9 @@ def predict(self, u: Optional[np.ndarray] = None, dt: float = 1.0) -> None: sigma_points = self._generate_sigma_points(self.state, self.covariance) # Eq. (3.25): Propagate sigma points through process model - sigma_points_pred = np.array([ - self.process_model(sp, u, dt) for sp in sigma_points - ]) + sigma_points_pred = np.array( + [self.process_model(sp, u, dt) for sp in sigma_points] + ) # Compute predicted state and covariance using Unscented Transform self.state, P_pred = self._unscented_transform( @@ -258,14 +266,12 @@ def update(self, z: np.ndarray) -> None: sigma_points = self._generate_sigma_points(self.state, self.covariance) # Propagate sigma points through measurement model - sigma_points_meas = np.array([ - self.measurement_model(sp) for sp in sigma_points - ]) + sigma_points_meas = np.array( + [self.measurement_model(sp) for sp in sigma_points] + ) # Predicted measurement mean and covariance - z_pred, Pzz = self._unscented_transform( - sigma_points_meas, self.Wm, self.Wc - ) + z_pred, Pzz = self._unscented_transform(sigma_points_meas, self.Wm, self.Wc) # Add measurement noise R = self.R() @@ -274,7 +280,11 @@ def update(self, z: np.ndarray) -> None: # Cross-covariance between state and measurement diff_x = sigma_points - self.state diff_z = sigma_points_meas - z_pred - Pxz = (self.Wc[:, np.newaxis, np.newaxis] * diff_x[:, :, np.newaxis] * diff_z[:, np.newaxis, :]).sum(axis=0) + Pxz = ( + self.Wc[:, np.newaxis, np.newaxis] + * diff_x[:, :, np.newaxis] + * diff_z[:, np.newaxis, :] + ).sum(axis=0) # Eq. (3.30): Kalman gain K_k K = Pxz @ np.linalg.inv(Pzz) @@ -304,26 +314,24 @@ def check_ukf_range_only_tracking(): # Process model: constant velocity in 2D def process_model(x, u, dt): - F = np.array([ - [1, 0, dt, 0], - [0, 1, 0, dt], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) + F = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) return F @ x # Measurement model: range from origin def measurement_model(x): - return np.array([np.sqrt(x[0]**2 + x[1]**2)]) + return np.array([np.sqrt(x[0] ** 2 + x[1] ** 2)]) q = 0.1 + def Q_func(dt): - return q * np.array([ - [dt**3/3, 0, dt**2/2, 0], - [0, dt**3/3, 0, dt**2/2], - [dt**2/2, 0, dt, 0], - [0, dt**2/2, 0, dt] - ]) + return q * np.array( + [ + [dt**3 / 3, 0, dt**2 / 2, 0], + [0, dt**3 / 3, 0, dt**2 / 2], + [dt**2 / 2, 0, dt, 0], + [0, dt**2 / 2, 0, dt], + ] + ) def R_func(): return np.array([[0.5]]) @@ -333,8 +341,7 @@ def R_func(): # Create UKF ukf = UnscentedKalmanFilter( - process_model, measurement_model, - Q_func, R_func, x0, P0 + process_model, measurement_model, Q_func, R_func, x0, P0 ) # Generate true trajectory @@ -372,25 +379,23 @@ def check_ukf_bearing_only_tracking(): n_steps = 50 def process_model(x, u, dt): - F = np.array([ - [1, 0, dt, 0], - [0, 1, 0, dt], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) + F = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) return F @ x def measurement_model(x): return np.array([np.arctan2(x[1], x[0])]) q = 0.1 + def Q_func(dt): - return q * np.array([ - [dt**3/3, 0, dt**2/2, 0], - [0, dt**3/3, 0, dt**2/2], - [dt**2/2, 0, dt, 0], - [0, dt**2/2, 0, dt] - ]) + return q * np.array( + [ + [dt**3 / 3, 0, dt**2 / 2, 0], + [0, dt**3 / 3, 0, dt**2 / 2], + [dt**2 / 2, 0, dt, 0], + [0, dt**2 / 2, 0, dt], + ] + ) def R_func(): return np.array([[0.05]]) @@ -401,8 +406,12 @@ def R_func(): # Bearing-only: wrap the innovation, or the branch cut reports 358 deg # of error where the truth is 2. Same gap as the EKF demo next door. ukf = UnscentedKalmanFilter( - process_model, measurement_model, - Q_func, R_func, x0, P0, + process_model, + measurement_model, + Q_func, + R_func, + x0, + P0, innovation_func=lambda z, z_pred: angle_diff(z, z_pred), ) @@ -443,6 +452,3 @@ def R_func(): print("=" * 70) print("ALL CHECKS PASSED") print("=" * 70) - - - diff --git a/core/eval/__init__.py b/core/eval/__init__.py index 013c280..658fb23 100644 --- a/core/eval/__init__.py +++ b/core/eval/__init__.py @@ -60,6 +60,3 @@ "show_figures_if_requested", "set_axes_equal_3d", ] - - - diff --git a/core/eval/plots.py b/core/eval/plots.py index 68cd63a..af5bc6d 100644 --- a/core/eval/plots.py +++ b/core/eval/plots.py @@ -359,9 +359,7 @@ def plot_position_error_time( ax.set_xlabel("Time (s)", fontsize=11) ax.set_ylabel(f"{axis_label} Error (m)", fontsize=11) - ax.set_title( - f"{axis_label}-axis Error", fontsize=12, fontweight="bold" - ) + ax.set_title(f"{axis_label}-axis Error", fontsize=12, fontweight="bold") ax.legend(fontsize=9) ax.grid(True, alpha=0.3) ax.axhline(y=0, color="k", linestyle="--", linewidth=0.8, alpha=0.5) @@ -947,8 +945,12 @@ def save_animation( path = out_dir / f"{name}.gif" animation = FuncAnimation( - fig, update, frames=n_frames, init_func=init, - interval=1000 // max(fps, 1), blit=False, + fig, + update, + frames=n_frames, + init_func=init, + interval=1000 // max(fps, 1), + blit=False, ) animation.save(path, writer=PillowWriter(fps=fps), dpi=dpi) @@ -1012,6 +1014,3 @@ def save_figure( plt.rcParams["svg.hashsalt"] = previous_salt return paths - - - diff --git a/core/fingerprinting/__init__.py b/core/fingerprinting/__init__.py index 2fbf167..aa44804 100644 --- a/core/fingerprinting/__init__.py +++ b/core/fingerprinting/__init__.py @@ -22,10 +22,10 @@ ... ) >>> db = load_fingerprint_database('data/sim/ch5_wifi_fingerprint_grid') >>> z_query = np.array([-50, -60, -70]) - >>> + >>> >>> # Deterministic >>> x_hat_nn = nn_localize(z_query, db, floor_id=0) - >>> + >>> >>> # Probabilistic >>> model = fit_gaussian_naive_bayes(db) >>> x_hat_map = map_localize(z_query, model, floor_id=0) @@ -101,4 +101,3 @@ ] __version__ = "0.1.0" - diff --git a/core/fingerprinting/classification.py b/core/fingerprinting/classification.py index 2bb17e7..f00c93e 100644 --- a/core/fingerprinting/classification.py +++ b/core/fingerprinting/classification.py @@ -43,11 +43,11 @@ class ClassificationLocalizer: """ Classification-based fingerprinting localizer. - + This localizer treats positioning as a classification problem, as discussed in Chapter 5, Section 5.2. The classifier predicts a discrete location class (RP index, zone ID, or grid cell) from the fingerprint features. - + Attributes: classifier: Trained scikit-learn classifier (RandomForest, SVM, etc.) locations: Reference point locations, shape (M, d) @@ -55,7 +55,7 @@ class ClassificationLocalizer: floor_ids: Floor identifiers for each class label_encoder: Encoder for class labels meta: Metadata dictionary - + References: Chapter 5, Section 5.2: Pattern Recognition Approaches """ @@ -75,13 +75,13 @@ def predict( ) -> Tuple[Location, dict]: """ Predict location using classification. - + Args: z: Query fingerprint, shape (N,) floor_id: Optional floor constraint. If provided, only considers classes on that floor. return_proba: If True, returns class probabilities in info dict. - + Returns: Tuple of (predicted_location, info_dict) """ @@ -115,10 +115,10 @@ def fit_classifier( ) -> ClassificationLocalizer: """ Fit a classification-based localizer from fingerprint database. - + This function implements the classification approach discussed in Chapter 5, Section 5.2, where positioning is framed as a pattern recognition problem. - + Args: db: FingerprintDatabase containing training data classifier_type: Type of classifier: @@ -131,14 +131,14 @@ def fit_classifier( floor_id: Optional floor to train on (None = all floors) **classifier_kwargs: Additional arguments for the classifier (e.g., n_estimators for RandomForest) - + Returns: Trained ClassificationLocalizer - + Raises: ImportError: If scikit-learn is not installed ValueError: If zone_type is invalid or insufficient data - + Examples: >>> # Direct classification (each RP is a class) >>> classifier = fit_classifier( @@ -147,7 +147,7 @@ def fit_classifier( ... zone_type="rp", ... n_estimators=100 ... ) - + >>> # Zone-based classification >>> classifier = fit_classifier( ... db, @@ -155,7 +155,7 @@ def fit_classifier( ... zone_type="grid", ... floor_id=0 ... ) - + References: Chapter 5, Section 5.2: Pattern Recognition for fingerprinting mentions Random Forests, Decision Trees, and SVM classifiers. @@ -287,7 +287,8 @@ def fit_floor_classifier( floor_labels = db.floor_ids clf = RandomForestClassifier( - n_estimators=n_estimators, random_state=random_state, + n_estimators=n_estimators, + random_state=random_state, ) clf.fit(features, floor_labels) return clf @@ -418,5 +419,3 @@ def hierarchical_localize( info["fine_position"] = pos return pos, info - - diff --git a/core/fingerprinting/dataset.py b/core/fingerprinting/dataset.py index ccb3f90..ce6e999 100644 --- a/core/fingerprinting/dataset.py +++ b/core/fingerprinting/dataset.py @@ -280,7 +280,9 @@ def print_database_summary(db: FingerprintDatabase) -> None: # Show within-RP variability within_rp_stds = db.get_std_features(min_std=0.0) print(f" Within-RP Std (mean): {np.mean(within_rp_stds):.2f}") - print(f" Within-RP Std (range): [{np.min(within_rp_stds):.2f}, {np.max(within_rp_stds):.2f}]") + print( + f" Within-RP Std (range): [{np.min(within_rp_stds):.2f}, {np.max(within_rp_stds):.2f}]" + ) print() # Location bounds @@ -299,4 +301,3 @@ def print_database_summary(db: FingerprintDatabase) -> None: if len(value_str) > 60: value_str = value_str[:57] + "..." print(f" {key}: {value_str}") - diff --git a/core/fingerprinting/deterministic.py b/core/fingerprinting/deterministic.py index 2c074ec..3212a2b 100644 --- a/core/fingerprinting/deterministic.py +++ b/core/fingerprinting/deterministic.py @@ -24,7 +24,7 @@ def distance(z: np.ndarray, f: np.ndarray, metric: str = "euclidean") -> float: This function implements the distance metric D(·, ·) used in Eq. (5.1) and Eq. (5.2) of Chapter 5. - + **Missing AP Handling:** If either z or f contains NaN values (representing missing AP readings), the distance is computed only over dimensions where both values are present. @@ -48,7 +48,7 @@ def distance(z: np.ndarray, f: np.ndarray, metric: str = "euclidean") -> float: >>> d_manh = distance(z, f, metric='manhattan') >>> print(f"Euclidean: {d_eucl:.2f}, Manhattan: {d_manh:.2f}") Euclidean: 3.46, Manhattan: 6.00 - + >>> # With missing values (NaN) >>> z_missing = np.array([-50, np.nan, -70]) >>> f_missing = np.array([-52, -58, np.nan]) @@ -98,7 +98,7 @@ def pairwise_distances( This function evaluates the distance metric required in Eq. (5.1) across all reference fingerprints i = 1, ..., M. - + **Missing AP Handling:** If z or any row of F contains NaN values, distances are computed only over dimensions where both values are present. If no overlapping dimensions @@ -125,7 +125,7 @@ def pairwise_distances( >>> distances = pairwise_distances(z, F, metric='euclidean') >>> print(distances) [3.46 4.47 7.07] - + >>> # With missing values >>> z_missing = np.array([-50, np.nan, -70]) >>> F_missing = np.array([[-52, -58, np.nan], # Only AP1 overlaps @@ -292,13 +292,13 @@ def knn_localize( Examples: >>> db = load_fingerprint_database('data/sim/ch5_wifi_fingerprint_grid') >>> z_query = np.array([-51, -61, -71]) - >>> + >>> >>> # Standard k-NN with k=3 >>> x_hat = knn_localize(z_query, db, k=3, floor_id=0) - >>> + >>> >>> # k-NN with uniform weights (simple average) >>> x_hat_uniform = knn_localize(z_query, db, k=5, weighting='uniform') - >>> + >>> >>> # k-NN with inverse distance weighting (default) >>> x_hat_weighted = knn_localize(z_query, db, k=5, weighting='inverse_distance') @@ -376,4 +376,3 @@ def knn_localize( x_hat = np.sum(weights[:, np.newaxis] * k_locations, axis=0) / weights_sum return x_hat - diff --git a/core/fingerprinting/pattern_recognition.py b/core/fingerprinting/pattern_recognition.py index 3e40f9d..14cfca3 100644 --- a/core/fingerprinting/pattern_recognition.py +++ b/core/fingerprinting/pattern_recognition.py @@ -130,10 +130,10 @@ def fit( Examples: >>> db = load_fingerprint_database('data/sim/ch5_wifi_fingerprint_grid') - >>> + >>> >>> # Train on floor 0 only >>> model = LinearRegressionLocalizer.fit(db, floor_id=0) - >>> + >>> >>> # Train on all floors with regularization >>> model_all = LinearRegressionLocalizer.fit(db, regularization=1.0) @@ -316,7 +316,7 @@ def score(self, db: FingerprintDatabase, floor_id: Optional[int] = None) -> floa Examples: >>> # Train on floor 0 >>> model = LinearRegressionLocalizer.fit(train_db, floor_id=0) - >>> + >>> >>> # Evaluate on test set (same floor) >>> r2 = model.score(test_db, floor_id=0) >>> print(f"R² score: {r2:.3f}") @@ -361,7 +361,9 @@ def score(self, db: FingerprintDatabase, floor_id: Optional[int] = None) -> floa def __repr__(self) -> str: """Readable string representation.""" - floor_str = f"floor={self.floor_id}" if self.floor_id is not None else "all_floors" + floor_str = ( + f"floor={self.floor_id}" if self.floor_id is not None else "all_floors" + ) return ( f"LinearRegressionLocalizer(" f"location_dim={self.location_dim}, " @@ -369,4 +371,3 @@ def __repr__(self) -> str: f"n_training_samples={self.n_training_samples}, " f"{floor_str})" ) - diff --git a/core/fingerprinting/preprocess.py b/core/fingerprinting/preprocess.py index c2d27cb..f4c8fa8 100644 --- a/core/fingerprinting/preprocess.py +++ b/core/fingerprinting/preprocess.py @@ -98,9 +98,7 @@ def average_scans( # Trimmed mean: remove extreme values, then average # Good balance between robustness and efficiency if not 0.0 <= trim_percent < 0.5: - raise ValueError( - f"trim_percent must be in [0.0, 0.5), got {trim_percent}" - ) + raise ValueError(f"trim_percent must be in [0.0, 0.5), got {trim_percent}") S, N = scans.shape result = np.zeros(N) @@ -219,7 +217,9 @@ def normalize_fingerprint( # Per-feature normalization (ref_mean is array) pass else: - raise ValueError(f"ref_mean shape {ref_mean.shape} incompatible with z shape {z.shape}") + raise ValueError( + f"ref_mean shape {ref_mean.shape} incompatible with z shape {z.shape}" + ) if ref_std is None: ref_std = np.nanstd(z, ddof=1) @@ -235,7 +235,9 @@ def normalize_fingerprint( # Replace zeros/NaNs with 1.0 ref_std = np.where((ref_std == 0) | np.isnan(ref_std), 1.0, ref_std) else: - raise ValueError(f"ref_std shape {ref_std.shape} incompatible with z shape {z.shape}") + raise ValueError( + f"ref_std shape {ref_std.shape} incompatible with z shape {z.shape}" + ) # Normalize z_norm = (z - ref_mean) / ref_std @@ -263,14 +265,18 @@ def normalize_fingerprint( else: ref_min = np.asarray(ref_min) if ref_min.ndim > 0 and ref_min.shape != z.shape: - raise ValueError(f"ref_min shape {ref_min.shape} incompatible with z shape {z.shape}") + raise ValueError( + f"ref_min shape {ref_min.shape} incompatible with z shape {z.shape}" + ) if ref_max is None: ref_max = np.nanmax(z) else: ref_max = np.asarray(ref_max) if ref_max.ndim > 0 and ref_max.shape != z.shape: - raise ValueError(f"ref_max shape {ref_max.shape} incompatible with z shape {z.shape}") + raise ValueError( + f"ref_max shape {ref_max.shape} incompatible with z shape {z.shape}" + ) # Avoid division by zero range_val = ref_max - ref_min @@ -289,7 +295,9 @@ def normalize_fingerprint( "method": "minmax", "min": ref_min.copy(), "max": ref_max.copy(), - "range": range_val.copy() if isinstance(range_val, np.ndarray) else range_val, + "range": ( + range_val.copy() if isinstance(range_val, np.ndarray) else range_val + ), } else: params = { @@ -461,4 +469,3 @@ def compute_normalization_params( else: raise ValueError(f"Unknown method '{method}'. Use 'zscore' or 'minmax'.") - diff --git a/core/fingerprinting/probabilistic.py b/core/fingerprinting/probabilistic.py index 796f3e8..5e6a501 100644 --- a/core/fingerprinting/probabilistic.py +++ b/core/fingerprinting/probabilistic.py @@ -148,7 +148,7 @@ def fit_gaussian_naive_bayes( >>> model = fit_gaussian_naive_bayes(db, min_std=2.0) >>> print(f"Trained model with {model.n_reference_points} RPs") >>> # All stds will be 2.0 dBm - + >>> # Multi-sample database (actual variance estimation) >>> db_multi = load_fingerprint_database('data/sim/ch5_wifi_fp_multisamples') >>> model = fit_gaussian_naive_bayes(db_multi, min_std=1.0) @@ -196,7 +196,7 @@ def log_likelihood( = Σ_j [-0.5 log(2π σ_ij²) - 0.5 (z_j - μ_ij)² / σ_ij²] where the sum is over all features j = 1, ..., N. - + **Missing AP Handling:** If z contains NaN values (representing missing AP readings), the sum includes only terms for observed (non-NaN) features. If no observed @@ -220,7 +220,7 @@ def log_likelihood( >>> z_query = np.array([-51, -61, -71]) >>> log_probs = log_likelihood(z_query, model, floor_id=0) >>> print(f"Log-likelihoods: {log_probs}") - + >>> # With missing values (NaN) >>> z_missing = np.array([-51, np.nan, -71]) # AP2 missing >>> log_probs_missing = log_likelihood(z_missing, model, floor_id=0) @@ -470,10 +470,10 @@ def posterior_mean_localize( >>> db = load_fingerprint_database('data/sim/ch5_wifi_fingerprint_grid') >>> model = fit_gaussian_naive_bayes(db, min_std=2.0) >>> z_query = np.array([-51, -61, -71]) - + >>> # Full posterior mean (all RPs) >>> x_hat_full = posterior_mean_localize(z_query, model, floor_id=0) - + >>> # Top-k posterior mean (faster, typically sufficient) >>> x_hat_topk = posterior_mean_localize(z_query, model, floor_id=0, top_k=10) >>> # Results are nearly identical but top-k is faster @@ -531,4 +531,3 @@ def posterior_mean_localize( x_hat = np.sum(posteriors[:, np.newaxis] * model.locations, axis=0) return x_hat - diff --git a/core/fingerprinting/types.py b/core/fingerprinting/types.py index 4811a2c..868a2a3 100644 --- a/core/fingerprinting/types.py +++ b/core/fingerprinting/types.py @@ -64,7 +64,7 @@ class FingerprintDatabase: ... ) >>> print(f"Database has {db.n_reference_points} RPs on {db.n_floors} floor(s)") Database has 3 RPs on 1 floor(s) - + >>> # Create database with multiple samples per RP (shape: M, S, N) >>> features_multi = np.array([ ... [[-50, -60, -70], [-51, -59, -71], [-49, -61, -69]], # RP1: 3 samples @@ -154,7 +154,7 @@ def n_features(self) -> int: def n_samples_per_rp(self) -> Optional[int]: """ Number of samples (S) per reference point. - + Returns: int: Number of samples if features is 3D (M, S, N). None: If features is 2D (M, N) indicating single sample. @@ -229,10 +229,10 @@ def filter_by_floor(self, floor_id: int) -> "FingerprintDatabase": def get_mean_features(self) -> np.ndarray: """ Get mean features across samples. - + Handles NaN values (missing AP readings) using nanmean, which computes the mean while ignoring NaN values. - + Returns: Mean feature array of shape (M, N). If single-sample format, returns features as-is. @@ -246,20 +246,22 @@ def get_mean_features(self) -> np.ndarray: def get_std_features(self, min_std: float = 0.0) -> np.ndarray: """ Get standard deviation of features across samples. - + Handles NaN values (missing AP readings) using nanstd, which computes the standard deviation while ignoring NaN values. - + Args: min_std: Minimum std to return (floor for numerical stability). - + Returns: Std array of shape (M, N). If single-sample format, returns array filled with min_std. If multi-sample format, returns std over samples axis (ignoring NaN). """ if self.has_multiple_samples: - stds = np.nanstd(self.features, axis=1, ddof=1) # Sample std over S, ignore NaN + stds = np.nanstd( + self.features, axis=1, ddof=1 + ) # Sample std over S, ignore NaN # Apply floor stds = np.maximum(stds, min_std) # If all samples at an RP for a feature are NaN, nanstd returns NaN @@ -272,7 +274,11 @@ def get_std_features(self, min_std: float = 0.0) -> np.ndarray: def __repr__(self) -> str: """Readable string representation.""" - samples_str = f", samples_per_rp={self.n_samples_per_rp}" if self.has_multiple_samples else "" + samples_str = ( + f", samples_per_rp={self.n_samples_per_rp}" + if self.has_multiple_samples + else "" + ) return ( f"FingerprintDatabase(" f"n_rps={self.n_reference_points}, " @@ -280,4 +286,3 @@ def __repr__(self) -> str: f"location_dim={self.location_dim}, " f"floors={self.floor_list.tolist()})" ) - diff --git a/core/fusion/__init__.py b/core/fusion/__init__.py index a7c9a19..9f653d7 100644 --- a/core/fusion/__init__.py +++ b/core/fusion/__init__.py @@ -70,5 +70,3 @@ "create_adaptive_manager_for_tc", "create_adaptive_manager_for_lc", ] - - diff --git a/core/fusion/adaptive.py b/core/fusion/adaptive.py index c5db7d9..0845397 100644 --- a/core/fusion/adaptive.py +++ b/core/fusion/adaptive.py @@ -20,30 +20,30 @@ class AdaptiveGatingManager: """Manages adaptive gating mechanisms for robust sensor fusion. - + This class implements practical robustness features to prevent chi-square gating from starving the filter: - + 1. **Consecutive Reject Tracking**: If a sensor stream is rejected too many times in a row, apply covariance inflation or widen gate. - + 2. **NIS Consistency Monitoring**: Track rolling mean of NIS values. If NIS >> DOF consistently, the filter is overconfident → scale up R or Q. - + 3. **Adaptive Recovery**: Automatically adjust parameters to restore filter consistency when gating becomes too aggressive. - + Usage: >>> manager = AdaptiveGatingManager(dof=4, consecutive_reject_limit=5) - >>> + >>> >>> # In fusion loop: >>> accept, action = manager.update(nis_value, gated_accept) - >>> + >>> >>> if action == 'inflate_P': >>> P = manager.inflate_covariance(P) >>> elif action == 'scale_R': >>> R = manager.get_R_scale() * R - + References: Chapter 8, Section 8.3.2: Filter Tuning and Consistency Checking """ @@ -60,7 +60,7 @@ def __init__( max_R_scale: float = 5.0, ): """Initialize adaptive gating manager. - + Args: dof: Degrees of freedom (measurement dimension) consecutive_reject_limit: Max consecutive rejects before adaptation @@ -90,16 +90,14 @@ def __init__( self.total_adaptations = 0 def update( - self, - nis_value: float, - gated_accept: bool + self, nis_value: float, gated_accept: bool ) -> tuple[bool, Optional[str]]: """Update adaptive gating state and determine if action needed. - + Args: nis_value: Current Normalized Innovation Squared (NIS) gated_accept: Whether measurement was accepted by gate - + Returns: Tuple of (final_accept, action): final_accept: Whether to accept measurement (may override gate) @@ -125,7 +123,7 @@ def update( # Check for consecutive reject limit if self.consecutive_rejects >= self.consecutive_reject_limit: # Apply covariance inflation to prevent filter starvation - action = 'inflate_P' + action = "inflate_P" self.consecutive_rejects = 0 # Reset after adaptation self.total_adaptations += 1 # Force accept this measurement with inflated uncertainty @@ -139,29 +137,27 @@ def update( # If mean NIS >> expected, filter is overconfident if mean_nis > self.nis_scale_threshold * expected_nis: if action is None: # Don't override P inflation - action = 'scale_R' + action = "scale_R" # Gradually increase R scale self.current_R_scale = min( - self.current_R_scale * self.R_scale_factor, - self.max_R_scale + self.current_R_scale * self.R_scale_factor, self.max_R_scale ) elif mean_nis < 0.7 * expected_nis: # Filter is too conservative, reduce R scale self.current_R_scale = max( - self.current_R_scale / self.R_scale_factor, - self.min_R_scale + self.current_R_scale / self.R_scale_factor, self.min_R_scale ) return gated_accept, action def inflate_covariance(self, P: np.ndarray) -> np.ndarray: """Apply covariance inflation: P <- λP. - + Used when consecutive rejects suggest filter is overconfident. - + Args: P: Current state covariance (n x n) - + Returns: Inflated covariance P_inflated = λ * P """ @@ -169,7 +165,7 @@ def inflate_covariance(self, P: np.ndarray) -> np.ndarray: def get_R_scale(self) -> float: """Get current R scale factor based on NIS monitoring. - + Returns: Scale factor w_R >= 1.0 to apply to measurement covariance R """ @@ -177,7 +173,7 @@ def get_R_scale(self) -> float: def get_stats(self) -> dict: """Get diagnostic statistics for logging. - + Returns: Dictionary with acceptance rate, NIS stats, etc. """ @@ -190,15 +186,15 @@ def get_stats(self) -> dict: mean_nis = np.mean(self.nis_history) if self.nis_history else 0.0 return { - 'total_measurements': self.total_measurements, - 'total_accepts': self.total_accepts, - 'total_rejects': self.total_rejects, - 'acceptance_rate': acceptance_rate, - 'consecutive_rejects': self.consecutive_rejects, - 'mean_nis': mean_nis, - 'expected_nis': self.dof, - 'current_R_scale': self.current_R_scale, - 'total_adaptations': self.total_adaptations, + "total_measurements": self.total_measurements, + "total_accepts": self.total_accepts, + "total_rejects": self.total_rejects, + "acceptance_rate": acceptance_rate, + "consecutive_rejects": self.consecutive_rejects, + "mean_nis": mean_nis, + "expected_nis": self.dof, + "current_R_scale": self.current_R_scale, + "total_adaptations": self.total_adaptations, } def reset(self): @@ -212,13 +208,15 @@ def reset(self): self.total_adaptations = 0 -def create_adaptive_manager_for_tc(n_anchors: int = 4, **kwargs) -> AdaptiveGatingManager: +def create_adaptive_manager_for_tc( + n_anchors: int = 4, **kwargs +) -> AdaptiveGatingManager: """Create adaptive gating manager for TC fusion (per-anchor updates). - + Args: n_anchors: Number of UWB anchors **kwargs: Additional parameters for AdaptiveGatingManager - + Returns: Configured AdaptiveGatingManager instance """ @@ -228,13 +226,12 @@ def create_adaptive_manager_for_tc(n_anchors: int = 4, **kwargs) -> AdaptiveGati def create_adaptive_manager_for_lc(**kwargs) -> AdaptiveGatingManager: """Create adaptive gating manager for LC fusion (position updates). - + Args: **kwargs: Additional parameters for AdaptiveGatingManager - + Returns: Configured AdaptiveGatingManager instance """ # LC fusion: position fix is 2D measurement return AdaptiveGatingManager(dof=2, **kwargs) - diff --git a/core/fusion/gating.py b/core/fusion/gating.py index b86917d..f88abbb 100644 --- a/core/fusion/gating.py +++ b/core/fusion/gating.py @@ -16,44 +16,41 @@ from scipy import stats -def mahalanobis_distance_squared( - y: np.ndarray, - S: np.ndarray -) -> float: +def mahalanobis_distance_squared(y: np.ndarray, S: np.ndarray) -> float: """Compute squared Mahalanobis distance of innovation. - + Implements Eq. (8.8) in Chapter 8: d_k^2 = y_k^T S_k^{-1} y_k - + This is the same quantity as the Normalized Innovation Squared (NIS) computed in core.eval.metrics.compute_nis. The squared Mahalanobis distance follows a chi-square distribution under the hypothesis that the measurement is consistent with the predicted state. - + Args: y: Innovation vector (m,). S: Innovation covariance matrix (m × m), must be positive definite. - + Returns: Squared Mahalanobis distance d^2 (scalar). - + Raises: ValueError: If dimensions are incompatible or S is not positive definite. - + Example: >>> y = np.array([1.0, 0.0]) >>> S = np.diag([1.0, 1.0]) >>> d_sq = mahalanobis_distance_squared(y, S) >>> np.allclose(d_sq, 1.0) True - + >>> # Larger innovation or smaller covariance → larger distance >>> y = np.array([3.0, 4.0]) >>> S = np.diag([1.0, 1.0]) >>> d_sq = mahalanobis_distance_squared(y, S) >>> np.allclose(d_sq, 25.0) # 3^2 + 4^2 True - + References: Eq. (8.8) in Chapter 8 See also core.eval.metrics.compute_nis (equivalent computation) @@ -84,20 +81,17 @@ def mahalanobis_distance_squared( def chi_square_gate( - y: np.ndarray, - S: np.ndarray, - confidence: float = None, - alpha: float = None + y: np.ndarray, S: np.ndarray, confidence: float = None, alpha: float = None ) -> bool: """Chi-square gating decision for measurement validation. - + Implements Eq. (8.9) in Chapter 8: Accept measurement if d_k^2 < χ²(m, α) Reject measurement if d_k^2 ≥ χ²(m, α) - + where m is the measurement dimension and χ²(m, α) is the chi-square critical value at confidence level α (e.g., α=0.95 for 95% confidence). - + Args: y: Innovation vector (m,). S: Innovation covariance matrix (m × m), must be positive definite. @@ -108,28 +102,28 @@ def chi_square_gate( - 0.90 (90% confidence, less conservative) alpha: DEPRECATED. Use 'confidence' instead. If provided, treated as significance level (1 - confidence) for backward compatibility. - + Returns: True if measurement should be accepted (innovation is consistent). False if measurement should be rejected (likely outlier). - + Raises: ValueError: If dimensions are incompatible, S is not positive definite, or confidence is not in (0, 1). - + Example: >>> # Small innovation → accept (95% confidence) >>> y = np.array([0.1, 0.2]) >>> S = np.diag([1.0, 1.0]) >>> chi_square_gate(y, S, confidence=0.95) True - + >>> # Large innovation → reject (95% confidence) >>> y = np.array([5.0, 5.0]) >>> S = np.diag([1.0, 1.0]) >>> chi_square_gate(y, S, confidence=0.95) False - + >>> # More conservative gating (higher confidence) → easier to reject >>> y = np.array([2.5, 0.0]) >>> S = np.diag([1.0, 1.0]) @@ -137,16 +131,16 @@ def chi_square_gate( True >>> chi_square_gate(y, S, confidence=0.99) # 99% confidence False - + Notes: The chi-square critical values for common cases (95% confidence): - m=1, α=0.95: χ² ≈ 3.841 - m=2, α=0.95: χ² ≈ 5.991 - m=3, α=0.95: χ² ≈ 7.815 - + Higher confidence (larger α) leads to higher critical values, making it harder to reject measurements (more conservative). - + References: Eq. (8.9) in Chapter 8 """ @@ -158,7 +152,7 @@ def chi_square_gate( "level (1 - confidence). To maintain equivalent behavior, use " f"confidence={1.0 - alpha:.2f} instead of alpha={alpha:.2f}.", DeprecationWarning, - stacklevel=2 + stacklevel=2, ) confidence = 1.0 - alpha elif confidence is None: @@ -166,9 +160,7 @@ def chi_square_gate( confidence = 0.95 if not (0 < confidence < 1): - raise ValueError( - f"Confidence level must be in (0, 1), got {confidence}" - ) + raise ValueError(f"Confidence level must be in (0, 1), got {confidence}") # Compute squared Mahalanobis distance (Eq. 8.8) d_squared = mahalanobis_distance_squared(y, S) @@ -187,18 +179,16 @@ def chi_square_gate( def chi_square_threshold( - dof: int, - confidence: float = None, - alpha: float = None + dof: int, confidence: float = None, alpha: float = None ) -> float: """Get chi-square critical value for a given confidence level. - + Computes χ²(m, α), the critical value for chi-square gating with m degrees of freedom at confidence level α (Chapter 8, Eq. 8.9). - + In the book's notation, α is the upper quantile (e.g., α=0.95 for 95% confidence), not the significance level. - + Args: dof: Degrees of freedom m (measurement dimension). confidence: Confidence level α (default 0.95 for 95% confidence). @@ -208,25 +198,25 @@ def chi_square_threshold( - 0.90 (90% confidence, less conservative) alpha: DEPRECATED. Use 'confidence' instead. If provided, treated as significance level (1 - confidence) for backward compatibility. - + Returns: Chi-square critical value χ²(m, α). - + Example: >>> # Standard 95% confidence for 1 DOF >>> threshold = chi_square_threshold(dof=1, confidence=0.95) >>> np.allclose(threshold, 3.841, atol=0.01) True - + >>> # Standard 95% confidence for 2 DOF >>> threshold = chi_square_threshold(dof=2, confidence=0.95) >>> np.allclose(threshold, 5.991, atol=0.01) True - + >>> # More conservative (higher confidence) → higher threshold >>> chi_square_threshold(dof=2, confidence=0.99) 9.21... - + References: Eq. (8.9) in Chapter 8: Accept if d_k^2 < χ²(m, α) """ @@ -238,7 +228,7 @@ def chi_square_threshold( "level (1 - confidence). To maintain equivalent behavior, use " f"confidence={1.0 - alpha:.2f} instead of alpha={alpha:.2f}.", DeprecationWarning, - stacklevel=2 + stacklevel=2, ) confidence = 1.0 - alpha elif confidence is None: @@ -248,9 +238,7 @@ def chi_square_threshold( if dof < 1: raise ValueError(f"Degrees of freedom must be positive, got {dof}") if not (0 < confidence < 1): - raise ValueError( - f"Confidence level must be in (0, 1), got {confidence}" - ) + raise ValueError(f"Confidence level must be in (0, 1), got {confidence}") # Book notation: α is the upper quantile (confidence level) # scipy.stats.chi2.ppf(confidence, dof) gives the α-quantile @@ -258,15 +246,13 @@ def chi_square_threshold( def chi_square_bounds( - dof: int, - confidence: float = None, - alpha: float = None + dof: int, confidence: float = None, alpha: float = None ) -> tuple[float, float]: """Get lower and upper chi-square bounds for consistency monitoring. - + Computes the symmetric confidence interval [χ²_lower, χ²_upper] for chi-square distributed statistics. Useful for NIS/NEES consistency plots. - + Args: dof: Degrees of freedom m. confidence: Confidence level (default 0.95 for 95% confidence). @@ -274,10 +260,10 @@ def chi_square_bounds( probability mass. alpha: DEPRECATED. Use 'confidence' instead. If provided, treated as significance level (1 - confidence) for backward compatibility. - + Returns: Tuple (lower_bound, upper_bound). - + Example: >>> lower, upper = chi_square_bounds(dof=2, confidence=0.95) >>> # For 2 DOF, 95% interval is approximately [0.05, 5.99] @@ -285,23 +271,23 @@ def chi_square_bounds( True >>> 5.5 < upper < 6.5 True - + >>> # For 1 DOF, 95% interval >>> lower, upper = chi_square_bounds(dof=1, confidence=0.95) >>> 0.0 < lower < 0.01 True >>> 3.5 < upper < 4.0 True - + Notes: For consistency monitoring (e.g., NIS plots), the statistic should fall within these bounds approximately 'confidence'% of the time if the filter is well-tuned. - + The bounds are computed as the symmetric two-sided interval: - lower = ppf((1 - confidence) / 2) - upper = ppf((1 + confidence) / 2) - + References: Chapter 8, Section 8.3 (Filter Consistency Monitoring) """ @@ -313,7 +299,7 @@ def chi_square_bounds( "level (1 - confidence). To maintain equivalent behavior, use " f"confidence={1.0 - alpha:.2f} instead of alpha={alpha:.2f}.", DeprecationWarning, - stacklevel=2 + stacklevel=2, ) confidence = 1.0 - alpha elif confidence is None: @@ -323,9 +309,7 @@ def chi_square_bounds( if dof < 1: raise ValueError(f"Degrees of freedom must be positive, got {dof}") if not (0 < confidence < 1): - raise ValueError( - f"Confidence level must be in (0, 1), got {confidence}" - ) + raise ValueError(f"Confidence level must be in (0, 1), got {confidence}") # Two-sided interval: [(1-conf)/2, (1+conf)/2] # This is the central 'confidence' interval @@ -333,5 +317,3 @@ def chi_square_bounds( upper = float(stats.chi2.ppf((1.0 + confidence) / 2.0, dof)) return lower, upper - - diff --git a/core/fusion/lc_models.py b/core/fusion/lc_models.py index 248adff..ca6cec8 100644 --- a/core/fusion/lc_models.py +++ b/core/fusion/lc_models.py @@ -35,15 +35,15 @@ def solve_uwb_position_wls( cov_floor_std: float = 0.2, ) -> Tuple[Optional[np.ndarray], Optional[np.ndarray], bool]: """Solve for 2D position from UWB ranges using Weighted Least Squares. - + This implements the iterative WLS position solver from Chapter 4, with proper measurement covariance handling for LC fusion. - + Key improvements over naive WLS: - Uses proper measurement covariance: W = R^{-1} where R = diag(σ_i²) - Supports anchor-dependent noise levels (e.g., based on SNR, NLOS flags) - Enforces covariance floor to prevent absurd certainty - + Args: ranges: Range measurements to each anchor (A,), NaN for dropouts anchor_positions: Anchor positions (A, 2) @@ -55,23 +55,23 @@ def solve_uwb_position_wls( tolerance: Convergence tolerance (meters, default 0.01) cov_floor_std: Minimum position std (meters, default 0.2) Prevents overconfident covariance estimates - + Returns: Tuple of (position, covariance, converged): position: Estimated 2D position (2,) or None if failed covariance: Position covariance (2, 2) or None if failed converged: True if solver converged - + Example: >>> ranges = np.array([5.0, 7.0, 8.5, 6.2]) >>> anchors = np.array([[0, 0], [20, 0], [20, 15], [0, 15]]) >>> # Uniform noise >>> pos, cov, ok = solve_uwb_position_wls(ranges, anchors, range_noise_std=0.05) - >>> + >>> >>> # Anchor-dependent noise (e.g., anchor 1 has NLOS) >>> anchor_stds = np.array([0.05, 0.5, 0.05, 0.05]) # Anchor 1 degraded >>> pos, cov, ok = solve_uwb_position_wls(ranges, anchors, anchor_noise_std=anchor_stds) - + References: Chapter 4, Section 4.2: TOA Positioning with Iterative WLS Equations (4.14)-(4.23): Nonlinear TOA I-WLS @@ -186,35 +186,36 @@ def solve_uwb_position_wls( def create_lc_process_model( - process_noise_std: np.ndarray = None + process_noise_std: np.ndarray = None, ) -> Tuple[Callable, Callable, Callable]: """Create process model for LC fusion (same as TC). - + Args: process_noise_std: [σ_p, σ_v, σ_yaw] (default: [0.01, 0.05, 0.01]) - + Returns: Tuple of (process_model, process_jacobian, process_noise_cov) """ # Reuse TC process model from core.fusion.tc_models import create_process_model + return create_process_model(process_noise_std) def create_lc_position_measurement_model( - position_noise_std: np.ndarray = None + position_noise_std: np.ndarray = None, ) -> Tuple[Callable, Callable, Callable]: """Create position measurement model for LC fusion. - + In LC fusion, the UWB position fix is treated as a 2D position measurement. - + Measurement: z = [px_meas, py_meas] Model: h(x) = [px, py] (direct observation of position state) - + Args: position_noise_std: Position measurement noise [σ_x, σ_y] Default: [0.5, 0.5] meters - + Returns: Tuple of (measurement_model, measurement_jacobian, measurement_noise_cov) """ @@ -233,10 +234,7 @@ def measurement_jacobian(x: np.ndarray) -> np.ndarray: # h(x) = [px, py] = [x[0], x[1]] # H = [[1, 0, 0, 0, 0], # [0, 1, 0, 0, 0]] - H = np.array([ - [1.0, 0.0, 0.0, 0.0, 0.0], - [0.0, 1.0, 0.0, 0.0, 0.0] - ]) + H = np.array([[1.0, 0.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0, 0.0]]) return H def measurement_noise_cov() -> np.ndarray: @@ -252,12 +250,12 @@ def create_lc_fusion_ekf( process_noise_std: np.ndarray = None, ) -> any: """Create and initialize loosely coupled fusion EKF. - + Args: initial_state: Initial state [px, py, vx, vy, yaw] (5,) initial_cov: Initial covariance (5, 5) process_noise_std: Process noise std [σ_p, σ_v, σ_yaw] - + Returns: Initialized ExtendedKalmanFilter instance """ @@ -284,8 +282,7 @@ def dummy_R(): Q=process_Q, R=dummy_R, x0=initial_state.copy(), - P0=initial_cov.copy() + P0=initial_cov.copy(), ) return ekf - diff --git a/core/fusion/loosely_coupled.py b/core/fusion/loosely_coupled.py index 86001a7..b37b2d2 100644 --- a/core/fusion/loosely_coupled.py +++ b/core/fusion/loosely_coupled.py @@ -31,16 +31,16 @@ def run_lc_fusion( dataset: Dict, use_gating: bool = True, gate_confidence: float = 0.95, - verbose: bool = True + verbose: bool = True, ) -> Dict: """Run loosely coupled IMU + UWB fusion. - + Args: dataset: Dataset dictionary from load_fusion_dataset use_gating: Whether to apply chi-square gating gate_confidence: Gating confidence level (default 0.95 for 95% confidence) verbose: Print progress - + Returns: Results dictionary with: - 't': timestamps (N,) @@ -54,40 +54,41 @@ def run_lc_fusion( - 'n_uwb_failed': number of UWB position solves that failed """ if verbose: - print("="*70) + print("=" * 70) print("Loosely Coupled IMU + UWB EKF Fusion") - print("="*70) + print("=" * 70) # Extract data - truth = dataset['truth'] - imu = dataset['imu'] - uwb = dataset['uwb'] - anchors = dataset['uwb_anchors'] + truth = dataset["truth"] + imu = dataset["imu"] + uwb = dataset["uwb"] + anchors = dataset["uwb_anchors"] # Initialize EKF at true starting position - x0 = np.array([ - truth['p_xy'][0, 0], # px - truth['p_xy'][0, 1], # py - truth['v_xy'][0, 0], # vx - truth['v_xy'][0, 1], # vy - truth['yaw'][0] # yaw - ]) + x0 = np.array( + [ + truth["p_xy"][0, 0], # px + truth["p_xy"][0, 1], # py + truth["v_xy"][0, 0], # vx + truth["v_xy"][0, 1], # vy + truth["yaw"][0], # yaw + ] + ) # Increase initial uncertainty to be more conservative (per book guidance on P0) # This prevents overconfidence in early stages before sufficient observations - P0 = np.diag([1.0, 1.0, 1.0, 1.0, 0.5])**2 # Larger initial uncertainty + P0 = np.diag([1.0, 1.0, 1.0, 1.0, 0.5]) ** 2 # Larger initial uncertainty - ekf = create_lc_fusion_ekf( - initial_state=x0, - initial_cov=P0 - ) + ekf = create_lc_fusion_ekf(initial_state=x0, initial_cov=P0) if verbose: print("\nInitialization:") print(f" State: {x0}") print(f" Gating: {'Enabled' if use_gating else 'Disabled'}") if use_gating: - print(f" Confidence: {gate_confidence} ({gate_confidence*100:.0f}% confidence)") + print( + f" Confidence: {gate_confidence} ({gate_confidence*100:.0f}% confidence)" + ) # Create position measurement model h, H_func, R_func = create_lc_position_measurement_model() @@ -96,24 +97,28 @@ def run_lc_fusion( measurements: List[StampedMeasurement] = [] # Add IMU measurements - for i in range(len(imu['t'])): - measurements.append(StampedMeasurement( - t=imu['t'][i], - sensor='imu', - z=np.hstack([imu['accel_xy'][i], imu['gyro_z'][i]]), # [ax, ay, gz] - R=np.eye(3), # Not used - meta={} - )) + for i in range(len(imu["t"])): + measurements.append( + StampedMeasurement( + t=imu["t"][i], + sensor="imu", + z=np.hstack([imu["accel_xy"][i], imu["gyro_z"][i]]), # [ax, ay, gz] + R=np.eye(3), # Not used + meta={}, + ) + ) # Add UWB measurements (aggregate by timestamp) - for i in range(len(uwb['t'])): - measurements.append(StampedMeasurement( - t=uwb['t'][i], - sensor='uwb', - z=uwb['ranges'][i, :], # All ranges at this timestamp - R=np.eye(anchors.shape[0]), # Not used (WLS computes own cov) - meta={'epoch_idx': i} - )) + for i in range(len(uwb["t"])): + measurements.append( + StampedMeasurement( + t=uwb["t"][i], + sensor="uwb", + z=uwb["ranges"][i, :], # All ranges at this timestamp + R=np.eye(anchors.shape[0]), # Not used (WLS computes own cov) + meta={"epoch_idx": i}, + ) + ) # Sort by timestamp measurements.sort(key=lambda m: m.t) @@ -137,14 +142,14 @@ def run_lc_fusion( # Run fusion history = { - 't': [], - 'x_est': [], - 'P_trace': [], - 'innovations': [], - 'nis': [], - 'gated': [], - 'uwb_positions': [], # Store solved UWB positions for analysis - 'R_scales': [], + "t": [], + "x_est": [], + "P_trace": [], + "innovations": [], + "nis": [], + "gated": [], + "uwb_positions": [], # Store solved UWB positions for analysis + "R_scales": [], } n_uwb_accepted = 0 @@ -155,12 +160,12 @@ def run_lc_fusion( for idx, meas in enumerate(measurements): dt = meas.t - t_prev - if meas.sensor == 'imu': + if meas.sensor == "imu": # Propagate with IMU u = meas.z # [ax, ay, gyro_z] ekf.predict(u=u, dt=dt) - elif meas.sensor == 'uwb': + elif meas.sensor == "uwb": # Solve for UWB position fix ranges = meas.z # All ranges at this epoch @@ -181,7 +186,7 @@ def run_lc_fusion( continue # Store solved position - history['uwb_positions'].append(pos_uwb) + history["uwb_positions"].append(pos_uwb) # Compute innovation (position residual) z_pred = h(ekf.state) # Predicted position [px, py] @@ -215,7 +220,7 @@ def run_lc_fusion( accept, action = adaptive_mgr.update(nis_value, gate_accept) # Handle adaptive actions - if action == 'inflate_P': + if action == "inflate_P": # Apply covariance inflation to prevent filter starvation ekf.covariance = adaptive_mgr.inflate_covariance(ekf.covariance) # 'scale_R' action is handled automatically via get_R_scale() @@ -230,27 +235,27 @@ def run_lc_fusion( n_uwb_rejected += 1 # Log - history['innovations'].append(np.linalg.norm(y)) # 2D innovation norm - history['nis'].append(nis_value) - history['gated'].append(accept) - history['R_scales'].append(R_scale) + history["innovations"].append(np.linalg.norm(y)) # 2D innovation norm + history["nis"].append(nis_value) + history["gated"].append(accept) + history["R_scales"].append(R_scale) # Record state - history['t'].append(meas.t) - history['x_est'].append(ekf.state.copy()) - history['P_trace'].append(np.trace(ekf.covariance)) + history["t"].append(meas.t) + history["x_est"].append(ekf.state.copy()) + history["P_trace"].append(np.trace(ekf.covariance)) t_prev = meas.t # Convert to arrays - history['t'] = np.array(history['t']) - history['x_est'] = np.array(history['x_est']) - history['P_trace'] = np.array(history['P_trace']) - if history['uwb_positions']: - history['uwb_positions'] = np.array(history['uwb_positions']) - history['n_uwb_accepted'] = n_uwb_accepted - history['n_uwb_rejected'] = n_uwb_rejected - history['n_uwb_failed'] = n_uwb_failed + history["t"] = np.array(history["t"]) + history["x_est"] = np.array(history["x_est"]) + history["P_trace"] = np.array(history["P_trace"]) + if history["uwb_positions"]: + history["uwb_positions"] = np.array(history["uwb_positions"]) + history["n_uwb_accepted"] = n_uwb_accepted + history["n_uwb_rejected"] = n_uwb_rejected + history["n_uwb_failed"] = n_uwb_failed if verbose: print("\nFusion complete:") @@ -259,13 +264,17 @@ def run_lc_fusion( print(f" UWB fixes rejected: {n_uwb_rejected}") print(f" UWB solver failures: {n_uwb_failed}") if n_uwb_accepted + n_uwb_rejected > 0: - print(f" Acceptance rate: {100*n_uwb_accepted/(n_uwb_accepted+n_uwb_rejected):.1f}%") + print( + f" Acceptance rate: {100*n_uwb_accepted/(n_uwb_accepted+n_uwb_rejected):.1f}%" + ) # Print adaptive gating stats if enabled if adaptive_mgr is not None: stats = adaptive_mgr.get_stats() print("\nAdaptive Gating Stats:") - print(f" Mean NIS: {stats['mean_nis']:.2f} (expected: {stats['expected_nis']:.0f})") + print( + f" Mean NIS: {stats['mean_nis']:.2f} (expected: {stats['expected_nis']:.0f})" + ) print(f" Final R scale: {stats['current_R_scale']:.2f}x") print(f" Covariance inflations: {stats['total_adaptations']}") diff --git a/core/fusion/tc_models.py b/core/fusion/tc_models.py index 9bb2102..09d4ffb 100644 --- a/core/fusion/tc_models.py +++ b/core/fusion/tc_models.py @@ -29,25 +29,25 @@ def interpolate_imu_measurements( gyro_z: np.ndarray, ) -> Tuple[np.ndarray, float]: """Interpolate IMU measurements at a query time (Section 8.5.2 - Direct Interpolation). - + Implements direct linear interpolation method from Chapter 8, Section 8.5.2 for handling asynchronous measurement timestamps. When a measurement arrives at time t that doesn't align with IMU samples, we interpolate IMU inputs. - + Args: t_query: Query time for interpolation (seconds). t_imu: IMU timestamps array (N,) - must be sorted. accel_xy: Accelerometer measurements (N, 2) in m/s². gyro_z: Gyroscope measurements (N,) in rad/s. - + Returns: Tuple of (u_interp, dt): u_interp: Interpolated control input [ax, ay, gyro_z] (3,) dt: Time since last IMU sample (for propagation) - + Raises: ValueError: If t_query is outside the range of t_imu. - + Example: >>> t_imu = np.array([0.0, 0.01, 0.02]) >>> accel = np.array([[1.0, 0.5], [1.1, 0.6], [1.2, 0.7]]) @@ -55,16 +55,16 @@ def interpolate_imu_measurements( >>> u, dt = interpolate_imu_measurements(0.015, t_imu, accel, gyro) >>> # At t=0.015 (halfway between 0.01 and 0.02): >>> # u ≈ [1.15, 0.65, 0.175] - + Notes: This implements the simplest interpolation method (linear). More sophisticated methods from Section 8.5.2: - Continuous-time propagation (integrate between samples) - Physics-based interpolation (use motion model) - + Linear interpolation is sufficient for high-rate IMU (≥100 Hz) when measurements arrive within ±10ms of IMU samples. - + References: Chapter 8, Section 8.5.2 (Measurement Timing and Interpolation) """ @@ -75,7 +75,7 @@ def interpolate_imu_measurements( ) # Find the interval [t_imu[idx], t_imu[idx+1]] containing t_query - idx = np.searchsorted(t_imu, t_query, side='right') - 1 + idx = np.searchsorted(t_imu, t_query, side="right") - 1 # Handle edge case: t_query exactly equals last IMU timestamp if idx >= len(t_imu) - 1: @@ -106,19 +106,20 @@ def interpolate_imu_measurements( @dataclass(frozen=True) class StateIndex: """State vector indices for TC fusion. - + Enforces single convention: x = [px, py, vx, vy, yaw] - + Usage: x = np.array([1.0, 2.0, 0.5, 0.3, 0.1]) position = x[StateIndex.PX:StateIndex.PY+1] # [1.0, 2.0] velocity = x[StateIndex.VX:StateIndex.VY+1] # [0.5, 0.3] yaw = x[StateIndex.YAW] # 0.1 """ - PX: int = 0 # Position X - PY: int = 1 # Position Y - VX: int = 2 # Velocity X - VY: int = 3 # Velocity Y + + PX: int = 0 # Position X + PY: int = 1 # Position Y + VX: int = 2 # Velocity X + VY: int = 3 # Velocity Y YAW: int = 4 # Yaw angle @staticmethod @@ -128,16 +129,16 @@ def state_dim() -> int: def create_process_model( - process_noise_std: np.ndarray = None + process_noise_std: np.ndarray = None, ) -> Tuple[Callable, Callable, Callable]: """Create process model functions for 2D IMU-based dead reckoning. - + State: x = [px, py, vx, vy, yaw] (5D) Control: u = [ax, ay, gyro_z] (3D) - + Args: process_noise_std: [σ_p, σ_v, σ_yaw] (default: [0.01, 0.05, 0.01]) - + Returns: Tuple of (process_model, process_jacobian, process_noise_cov) """ @@ -182,13 +183,15 @@ def process_jacobian(x: np.ndarray, u: np.ndarray, dt: float) -> np.ndarray: dax_map_dyaw = -ax * sin_yaw - ay * cos_yaw day_map_dyaw = ax * cos_yaw - ay * sin_yaw - F = np.array([ - [1, 0, dt, 0, 0], - [0, 1, 0, dt, 0], - [0, 0, 1, 0, dax_map_dyaw * dt], - [0, 0, 0, 1, day_map_dyaw * dt], - [0, 0, 0, 0, 1] - ]) + F = np.array( + [ + [1, 0, dt, 0, 0], + [0, 1, 0, dt, 0], + [0, 0, 1, 0, dax_map_dyaw * dt], + [0, 0, 0, 1, day_map_dyaw * dt], + [0, 0, 0, 0, 1], + ] + ) return F @@ -196,13 +199,15 @@ def process_noise_cov(dt: float) -> np.ndarray: """Compute process noise covariance Q(dt).""" σ_p, σ_v, σ_yaw = process_noise_std - Q = np.diag([ - (σ_p * dt)**2, - (σ_p * dt)**2, - (σ_v * dt)**2, - (σ_v * dt)**2, - (σ_yaw * dt)**2 - ]) + Q = np.diag( + [ + (σ_p * dt) ** 2, + (σ_p * dt) ** 2, + (σ_v * dt) ** 2, + (σ_v * dt) ** 2, + (σ_yaw * dt) ** 2, + ] + ) return Q @@ -210,18 +215,17 @@ def process_noise_cov(dt: float) -> np.ndarray: def create_uwb_range_measurement_model( - anchor_position: np.ndarray, - range_noise_std: float = 0.05 + anchor_position: np.ndarray, range_noise_std: float = 0.05 ) -> Tuple[Callable, Callable, Callable]: """Create UWB range measurement model for a single anchor. - + Measurement: z = range to anchor Model: h(x) = ||p - anchor|| - + Args: anchor_position: Anchor position [x, y] (2,) range_noise_std: Range noise std (meters) - + Returns: Tuple of (measurement_model, measurement_jacobian, measurement_noise_cov) """ @@ -249,13 +253,17 @@ def measurement_jacobian(x: np.ndarray) -> np.ndarray: if range_pred < 1e-6: range_pred = 1e-6 - H = np.array([[ - dx / range_pred, # ∂h/∂px - dy / range_pred, # ∂h/∂py - 0.0, # ∂h/∂vx - 0.0, # ∂h/∂vy - 0.0 # ∂h/∂yaw - ]]) + H = np.array( + [ + [ + dx / range_pred, # ∂h/∂px + dy / range_pred, # ∂h/∂py + 0.0, # ∂h/∂vx + 0.0, # ∂h/∂vy + 0.0, # ∂h/∂yaw + ] + ] + ) return H @@ -272,12 +280,12 @@ def create_tc_fusion_ekf( process_noise_std: np.ndarray = None, ) -> any: """Create and initialize tightly coupled fusion EKF. - + Args: initial_state: Initial state [px, py, vx, vy, yaw] (5,) initial_cov: Initial covariance (5, 5) process_noise_std: Process noise std [σ_p, σ_v, σ_yaw] - + Returns: Initialized ExtendedKalmanFilter instance """ @@ -305,7 +313,7 @@ def dummy_R(): Q=process_Q, R=dummy_R, x0=initial_state.copy(), - P0=initial_cov.copy() + P0=initial_cov.copy(), ) return ekf @@ -320,7 +328,7 @@ def dummy_R(): def tc_process_model(x: np.ndarray, u: np.ndarray, dt: float) -> np.ndarray: """Legacy wrapper: Process model for demos. - + State: x = [px, py, vx, vy, yaw] Control: u = [ax, ay, gyro_z] """ @@ -330,7 +338,7 @@ def tc_process_model(x: np.ndarray, u: np.ndarray, dt: float) -> np.ndarray: def tc_process_jacobian(x: np.ndarray, u: np.ndarray, dt: float) -> np.ndarray: """Legacy wrapper: Process Jacobian for demos. - + State: x = [px, py, vx, vy, yaw] Control: u = [ax, ay, gyro_z] """ @@ -339,27 +347,23 @@ def tc_process_jacobian(x: np.ndarray, u: np.ndarray, dt: float) -> np.ndarray: def tc_process_noise_covariance( - dt: float, - accel_noise_std: float = 0.1, - gyro_noise_std: float = 0.01 + dt: float, accel_noise_std: float = 0.1, gyro_noise_std: float = 0.01 ) -> np.ndarray: """Legacy wrapper: Process noise covariance for demos. - + Args: dt: Time step accel_noise_std: Acceleration noise std (m/s²) gyro_noise_std: Gyro noise std (rad/s) - + Returns: Q: Process noise covariance (5, 5) """ # Map accel/gyro noise to state noise # For simplicity: σ_p ~ σ_accel * dt², σ_v ~ σ_accel * dt, σ_yaw ~ σ_gyro * dt - process_noise_std = np.array([ - accel_noise_std, # σ_p - accel_noise_std, # σ_v - gyro_noise_std # σ_yaw - ]) + process_noise_std = np.array( + [accel_noise_std, accel_noise_std, gyro_noise_std] # σ_p # σ_v # σ_yaw + ) _, _, Q = create_process_model(process_noise_std) return Q(dt) @@ -367,11 +371,11 @@ def tc_process_noise_covariance( def tc_uwb_measurement_model(x: np.ndarray, anchors: np.ndarray) -> np.ndarray: """Legacy wrapper: Predict ranges to all anchors. - + Args: x: State [px, py, vx, vy, yaw] anchors: Anchor positions (n_anchors, 2) - + Returns: Predicted ranges (n_anchors,) """ @@ -384,11 +388,11 @@ def tc_uwb_measurement_model(x: np.ndarray, anchors: np.ndarray) -> np.ndarray: def tc_uwb_measurement_jacobian(x: np.ndarray, anchors: np.ndarray) -> np.ndarray: """Legacy wrapper: Measurement Jacobian for all anchors. - + Args: x: State [px, py, vx, vy, yaw] anchors: Anchor positions (n_anchors, 2) - + Returns: H: Measurement Jacobian (n_anchors, 5) """ diff --git a/core/fusion/tuning.py b/core/fusion/tuning.py index cea95d5..78ea567 100644 --- a/core/fusion/tuning.py +++ b/core/fusion/tuning.py @@ -5,7 +5,7 @@ Robust Covariance Scaling (Eq. 8.7): R_k ← w_R(y_k) * R_k - + Where w_R >= 1 is a covariance scale factor that **inflates** R for outliers. This reduces the influence of measurements with large innovations without completely rejecting them. @@ -21,30 +21,30 @@ def innovation(z: np.ndarray, z_pred: np.ndarray) -> np.ndarray: """Compute measurement innovation (residual). - + Implements Eq. (8.5) in Chapter 8: y_k = z_k - h(x̂_{k|k-1}) - + The innovation represents the difference between the actual measurement and the predicted measurement from the current state estimate. - + Args: z: Actual measurement vector (m,). z_pred: Predicted measurement h(x̂_{k|k-1}) (m,). - + Returns: Innovation vector y_k (m,). - + Raises: ValueError: If z and z_pred have different shapes. - + Example: >>> z = np.array([5.2, 3.1]) >>> z_pred = np.array([5.0, 3.0]) >>> y = innovation(z, z_pred) >>> np.allclose(y, [0.2, 0.1]) True - + References: Eq. (8.5) in Chapter 8 """ @@ -61,29 +61,27 @@ def innovation(z: np.ndarray, z_pred: np.ndarray) -> np.ndarray: def innovation_covariance( - H: np.ndarray, - P_pred: np.ndarray, - R: np.ndarray + H: np.ndarray, P_pred: np.ndarray, R: np.ndarray ) -> np.ndarray: """Compute innovation covariance matrix. - + Implements Eq. (8.6) in Chapter 8: S_k = H_k P_{k|k-1} H_k^T + R_k - + The innovation covariance quantifies the uncertainty in the innovation, combining prediction uncertainty (P) and measurement noise (R). - + Args: H: Measurement Jacobian matrix (m × n). P_pred: Predicted state covariance P_{k|k-1} (n × n). R: Measurement noise covariance R_k (m × m). - + Returns: Innovation covariance S_k (m × m). - + Raises: ValueError: If matrix dimensions are incompatible. - + Example: >>> H = np.array([[1.0, 0.0], [0.0, 1.0]]) # identity observation >>> P_pred = np.diag([0.5, 0.3]) @@ -91,7 +89,7 @@ def innovation_covariance( >>> S = innovation_covariance(H, P_pred, R) >>> np.allclose(S, [[0.6, 0.0], [0.0, 0.4]]) True - + References: Eq. (8.6) in Chapter 8 """ @@ -130,58 +128,55 @@ def innovation_covariance( return S -def scale_measurement_covariance( - R: np.ndarray, - scale_factor: float -) -> np.ndarray: +def scale_measurement_covariance(R: np.ndarray, scale_factor: float) -> np.ndarray: """Apply robust scaling to measurement covariance (Eq. 8.7). - + Implements Eq. (8.7) in Chapter 8: R_k ← w_R(y_k) * R_k - + where w_R >= 1 is a covariance scale factor that **inflates** R for outliers. This reduces the influence of measurements with large innovations without completely rejecting them (softer alternative to chi-square gating). - + Args: R: Original measurement covariance (m × m). scale_factor: Covariance inflation factor w_R >= 1. - w_R = 1: no inflation (inlier) - w_R > 1: inflate covariance (outlier) Typical robust functions return values in [1, ∞). - + Returns: Scaled covariance R_scaled = w_R * R (m × m). - + Raises: ValueError: If scale_factor < 1 or R is not 2D. - + Example: >>> R = np.diag([0.1, 0.2]) - + >>> # Inlier: no scaling >>> R_inlier = scale_measurement_covariance(R, 1.0) >>> np.allclose(R_inlier, R) True - + >>> # Moderate outlier: inflate by 2x >>> R_scaled = scale_measurement_covariance(R, 2.0) >>> np.allclose(R_scaled, [[0.2, 0.0], [0.0, 0.4]]) True - + >>> # Strong outlier: inflate by 100x (nearly reject) >>> R_outlier = scale_measurement_covariance(R, 100.0) >>> np.allclose(R_outlier, [[10.0, 0.0], [0.0, 20.0]]) True - + Notes: **Key Point:** Outliers get **larger** covariance (inflated R), which reduces their weight in the Kalman gain K = P H^T S^{-1}. - + Use the companion functions to compute scale factors: - `huber_R_scale(r, delta)` for Huber robust loss - `cauchy_R_scale(r, c)` for Cauchy robust loss - + References: Eq. (8.7) in Chapter 8 """ @@ -205,49 +200,46 @@ def scale_measurement_covariance( return R_scaled -def huber_R_scale( - residual: float, - delta: float = 1.345 -) -> float: +def huber_R_scale(residual: float, delta: float = 1.345) -> float: """Compute Huber covariance scale factor for Eq. 8.7. - + Returns a scale factor w_R >= 1 that inflates measurement covariance R for large residuals (outliers). Implements the Huber robust loss function as a covariance inflation strategy. - + Scale factor: w_R(r) = 1 if |r| ≤ δ (inlier: no inflation) w_R(r) = |r| / δ if |r| > δ (outlier: inflate by ratio) - + Args: residual: Normalized residual r (e.g., innovation / sqrt(variance)). delta: Huber threshold δ (default 1.345 for 95% efficiency on Gaussian data). Common values: 1.345 (standard), 2.0-3.0 (more tolerant). - + Returns: Covariance scale factor w_R >= 1. - + Example: >>> # Inlier: no inflation >>> huber_R_scale(0.5, delta=1.345) 1.0 - + >>> # Moderate outlier: inflate proportionally >>> huber_R_scale(2.69, delta=1.345) 2.0 - + >>> # Strong outlier: large inflation >>> scale = huber_R_scale(10.0, delta=1.345) >>> scale > 7.0 True - + Notes: For Eq. 8.7 application: R_robust = huber_R_scale(r, delta) * R - + The Huber function provides a **linear** inflation for outliers, making it less aggressive than Cauchy. - + References: Chapter 8, Section 8.3.2 (Robust Loss Functions) Eq. (8.7): R_k ← w_R(y_k) * R_k @@ -261,49 +253,46 @@ def huber_R_scale( return abs_residual / delta -def cauchy_R_scale( - residual: float, - c: float = 2.385 -) -> float: +def cauchy_R_scale(residual: float, c: float = 2.385) -> float: """Compute Cauchy covariance scale factor for Eq. 8.7. - + Returns a scale factor w_R >= 1 that inflates measurement covariance R for large residuals (outliers). Implements the Cauchy robust loss function as a covariance inflation strategy. - + Scale factor: w_R(r) = 1 + (r / c)² - + Args: residual: Normalized residual r (e.g., innovation / sqrt(variance)). c: Cauchy scale parameter (default 2.385 for 95% efficiency on Gaussian data). Larger values are more tolerant of outliers. - + Returns: Covariance scale factor w_R >= 1. - + Example: >>> # Inlier: minimal inflation >>> cauchy_R_scale(0.0, c=2.385) 1.0 - + >>> # Moderate outlier >>> scale = cauchy_R_scale(2.385, c=2.385) >>> np.isclose(scale, 2.0) True - + >>> # Strong outlier: quadratic inflation >>> scale = cauchy_R_scale(10.0, c=2.385) >>> scale > 17.0 True - + Notes: For Eq. 8.7 application: R_robust = cauchy_R_scale(r, c) * R - + The Cauchy function provides **quadratic** inflation for outliers, making it more aggressive than Huber. It strongly down-weights measurements with large innovations. - + References: Chapter 8, Section 8.3.2 (Robust Loss Functions) Eq. (8.7): R_k ← w_R(y_k) * R_k @@ -313,30 +302,27 @@ def cauchy_R_scale( return 1.0 + normalized**2 -def huber_weight( - residual: float, - threshold: float -) -> float: +def huber_weight(residual: float, threshold: float) -> float: """Compute Huber robust weight for a scalar residual. - + **DEPRECATED:** Use `huber_R_scale()` for Eq. 8.7 covariance inflation. - + This function returns IRLS-style weights w ∈ (0, 1] that are used in iterative optimization. For Kalman filtering with Eq. 8.7, use the inverse relationship: R_scale = 1 / w, which gives inflation factors >= 1. - + The Huber weight function: w(r) = 1 if |r| ≤ k (inlier) w(r) = k / |r| if |r| > k (outlier: weight < 1) - + Args: residual: Normalized residual (e.g., innovation / sqrt(variance)). threshold: Huber threshold k (typical values: 1.345 for 95% efficiency on Gaussian data, or 2.0-3.0 for more tolerant). - + Returns: IRLS weight w(r) ∈ (0, 1]. For Eq. 8.7, use R_scale = 1/w. - + Example: >>> huber_weight(0.5, threshold=1.345) # inlier 1.0 @@ -344,7 +330,7 @@ def huber_weight( >>> np.isclose(w, 0.448, atol=0.01) True >>> # For Eq. 8.7: R_scale = 1/w ≈ 2.23 (inflate R) - + References: Chapter 8, Section 8.3 (Robust Loss Functions) """ @@ -353,7 +339,7 @@ def huber_weight( "covariance inflation. Use huber_R_scale() instead for Eq. 8.7, which " "returns scale factors >= 1 that directly inflate R.", DeprecationWarning, - stacklevel=2 + stacklevel=2, ) abs_residual = abs(residual) @@ -364,28 +350,25 @@ def huber_weight( return threshold / abs_residual -def cauchy_weight( - residual: float, - scale: float -) -> float: +def cauchy_weight(residual: float, scale: float) -> float: """Compute Cauchy robust weight for a scalar residual. - + **DEPRECATED:** Use `cauchy_R_scale()` for Eq. 8.7 covariance inflation. - + This function returns IRLS-style weights w ∈ (0, 1] that are used in iterative optimization. For Kalman filtering with Eq. 8.7, use the inverse relationship: R_scale = 1 / w, which gives inflation factors >= 1. - + The Cauchy weight function: w(r) = 1 / (1 + (r/c)²) (outliers get weight << 1) - + Args: residual: Normalized residual (e.g., innovation / sqrt(variance)). scale: Cauchy scale parameter c (typical values: 2.385 for 95% efficiency). - + Returns: IRLS weight w(r) ∈ (0, 1]. For Eq. 8.7, use R_scale = 1/w. - + Example: >>> cauchy_weight(0.0, scale=2.385) 1.0 @@ -395,7 +378,7 @@ def cauchy_weight( >>> w < 0.06 True >>> # For Eq. 8.7: R_scale = 1/w ≈ 18.6 (strongly inflate R) - + References: Chapter 8, Section 8.3 (Robust Loss Functions) """ @@ -404,50 +387,47 @@ def cauchy_weight( "covariance inflation. Use cauchy_R_scale() instead for Eq. 8.7, which " "returns scale factors >= 1 that directly inflate R.", DeprecationWarning, - stacklevel=2 + stacklevel=2, ) normalized = residual / scale return 1.0 / (1.0 + normalized**2) -def compute_normalized_innovation( - y: np.ndarray, - S: np.ndarray -) -> np.ndarray: +def compute_normalized_innovation(y: np.ndarray, S: np.ndarray) -> np.ndarray: """Compute normalized (whitened) innovation for robust weighting. - + Normalizes the innovation vector by the innovation covariance, producing a dimensionless residual suitable for robust weight computation. - + For a scalar measurement, this is simply y / sqrt(S). For vector measurements, this uses Cholesky decomposition: y_norm = L^{-1} y where S = L L^T. - + Args: y: Innovation vector (m,). S: Innovation covariance (m × m), must be positive definite. - + Returns: Normalized innovation vector (m,). - + Raises: ValueError: If dimensions are incompatible or S is not positive definite. - + Example: >>> y = np.array([2.0]) >>> S = np.array([[4.0]]) >>> y_norm = compute_normalized_innovation(y, S) >>> np.allclose(y_norm, [1.0]) # 2.0 / sqrt(4.0) = 1.0 True - + >>> # Vector case >>> y = np.array([2.0, 1.0]) >>> S = np.diag([4.0, 1.0]) >>> y_norm = compute_normalized_innovation(y, S) >>> np.allclose(y_norm, [1.0, 1.0]) True - + Notes: This function is used to prepare innovations for element-wise robust weight computation (Huber, Cauchy, etc.). @@ -476,5 +456,3 @@ def compute_normalized_innovation( y_normalized = np.linalg.solve(L, y) return y_normalized - - diff --git a/core/fusion/types.py b/core/fusion/types.py index bbe6d74..61f2143 100644 --- a/core/fusion/types.py +++ b/core/fusion/types.py @@ -16,11 +16,11 @@ @dataclass(frozen=True) class StampedMeasurement: """Generic time-stamped measurement packet used by fusion demos. - + This structure provides a unified interface for multi-sensor fusion, supporting different sensor types with varying measurement dimensions and covariances. - + Attributes: t: Timestamp in seconds (float, monotonic time). sensor: Sensor identifier (e.g., 'imu', 'uwb_range', 'lidar_odom'). @@ -28,7 +28,7 @@ class StampedMeasurement: R: Measurement covariance matrix (m x m where m = len(z)). meta: Optional metadata dictionary for sensor-specific information (e.g., anchor_id for UWB, frame_id for camera). - + Example: >>> # UWB range measurement to anchor 3 >>> uwb_meas = StampedMeasurement( @@ -38,7 +38,7 @@ class StampedMeasurement: ... R=np.array([[0.01]]), ... meta={'anchor_id': 3} ... ) - + >>> # IMU acceleration measurement >>> imu_meas = StampedMeasurement( ... t=1.234, @@ -71,7 +71,9 @@ def __post_init__(self) -> None: if not isinstance(self.z, np.ndarray): raise TypeError(f"Measurement z must be numpy array, got {type(self.z)}") if self.z.ndim != 1: - raise ValueError(f"Measurement z must be 1D array, got shape {self.z.shape}") + raise ValueError( + f"Measurement z must be 1D array, got shape {self.z.shape}" + ) # Validate covariance matrix if not isinstance(self.R, np.ndarray): @@ -93,37 +95,39 @@ def __post_init__(self) -> None: # Check positive semi-definite (all eigenvalues >= 0) eigvals = np.linalg.eigvalsh(self.R) if np.any(eigvals < -1e-10): # small negative tolerance for numerical errors - raise ValueError(f"Covariance R must be positive semi-definite, got eigenvalues {eigvals}") + raise ValueError( + f"Covariance R must be positive semi-definite, got eigenvalues {eigvals}" + ) @dataclass(frozen=True) class TimeSyncModel: """Map sensor-local time to a common fusion time. - + This model handles temporal calibration between sensors by accounting for constant time offsets and clock drift. Essential for Chapter 8 temporal calibration demos (Section 8.5). - + The transformation is: t_fusion = (1 + drift) * t_sensor + offset - + Attributes: offset: Constant time offset in seconds. Positive offset means the sensor clock is ahead of the fusion clock. drift: Clock drift rate in seconds/second (dimensionless). A drift of 0.001 means the sensor gains 1 ms per second. - + Example: >>> # Sensor clock is 0.5 seconds behind fusion clock >>> sync = TimeSyncModel(offset=-0.5, drift=0.0) >>> sync.to_fusion_time(10.0) # sensor time 9.5 # fusion time - + >>> # Sensor clock drifts +1 ms per second and is 0.2s ahead >>> sync = TimeSyncModel(offset=0.2, drift=0.001) >>> sync.to_fusion_time(100.0) 100.3 # = 100 * 1.001 + 0.2 - + References: Chapter 8, Section 8.5 (Temporal Calibration and Synchronization) """ @@ -142,21 +146,22 @@ def __post_init__(self) -> None: # Warn about unrealistic drift values (typically < 100 ppm = 0.0001) if abs(self.drift) > 0.01: import warnings + warnings.warn( f"Clock drift of {self.drift} (= {self.drift * 1e6:.0f} ppm) " f"is unusually large. Typical values are < 100 ppm (0.0001).", - UserWarning + UserWarning, ) def to_fusion_time(self, t_sensor: float) -> float: """Convert sensor-local time to fusion time. - + Args: t_sensor: Timestamp in sensor-local time (seconds). - + Returns: Timestamp in fusion time (seconds). - + Example: >>> sync = TimeSyncModel(offset=0.5, drift=0.001) >>> sync.to_fusion_time(10.0) @@ -166,13 +171,13 @@ def to_fusion_time(self, t_sensor: float) -> float: def to_sensor_time(self, t_fusion: float) -> float: """Convert fusion time to sensor-local time (inverse operation). - + Args: t_fusion: Timestamp in fusion time (seconds). - + Returns: Timestamp in sensor-local time (seconds). - + Example: >>> sync = TimeSyncModel(offset=0.5, drift=0.001) >>> t_fus = sync.to_fusion_time(10.0) @@ -183,13 +188,13 @@ def to_sensor_time(self, t_fusion: float) -> float: def is_synchronized(self, tolerance: float = 1e-6) -> bool: """Check if the sensor is already synchronized (identity transform). - + Args: tolerance: Tolerance for offset and drift (default 1 microsecond). - + Returns: True if both offset and drift are within tolerance of zero. - + Example: >>> TimeSyncModel(offset=0.0, drift=0.0).is_synchronized() True @@ -197,5 +202,3 @@ def is_synchronized(self, tolerance: float = 1e-6) -> bool: False """ return abs(self.offset) < tolerance and abs(self.drift) < tolerance - - diff --git a/core/models/__init__.py b/core/models/__init__.py index 29e1396..455ffac 100644 --- a/core/models/__init__.py +++ b/core/models/__init__.py @@ -15,28 +15,25 @@ ConstantVelocity2D, ConstantVelocity1D, ConstantAcceleration2D, - create_process_noise_continuous_white_acceleration + create_process_noise_continuous_white_acceleration, ) from .measurement_models import ( RangeMeasurement2D, RangeBearingMeasurement2D, PositionMeasurement2D, - validate_measurement_inputs + validate_measurement_inputs, ) __all__ = [ # Motion models - 'ConstantVelocity2D', - 'ConstantVelocity1D', - 'ConstantAcceleration2D', - 'create_process_noise_continuous_white_acceleration', - + "ConstantVelocity2D", + "ConstantVelocity1D", + "ConstantAcceleration2D", + "create_process_noise_continuous_white_acceleration", # Measurement models - 'RangeMeasurement2D', - 'RangeBearingMeasurement2D', - 'PositionMeasurement2D', - 'validate_measurement_inputs', + "RangeMeasurement2D", + "RangeBearingMeasurement2D", + "PositionMeasurement2D", + "validate_measurement_inputs", ] - - diff --git a/core/models/measurement_models.py b/core/models/measurement_models.py index 3c9cd3a..810cd7c 100644 --- a/core/models/measurement_models.py +++ b/core/models/measurement_models.py @@ -19,16 +19,16 @@ class RangeMeasurement2D: """ Range-only measurement model for 2D positioning. - + Measurement: z = ||p - anchor|| + noise where p = [px, py] is position from state x - + Used in: - TOA/TDOA positioning - UWB ranging - ch3_estimators examples - ch8_sensor_fusion (tightly coupled) - + Example: >>> anchors = np.array([[0, 0], [10, 0], [10, 10], [0, 10]]) >>> model = RangeMeasurement2D(anchors) @@ -38,17 +38,21 @@ class RangeMeasurement2D: (4,) # One range per anchor """ - def __init__(self, anchors: np.ndarray, state_position_indices: Tuple[int, int] = (0, 1)): + def __init__( + self, anchors: np.ndarray, state_position_indices: Tuple[int, int] = (0, 1) + ): """ Initialize range measurement model. - + Args: anchors: Anchor positions, shape (N, 2) state_position_indices: Indices of [px, py] in state vector (default: (0, 1)) """ self.anchors = np.asarray(anchors) if self.anchors.ndim != 2 or self.anchors.shape[1] != 2: - raise ValueError(f"Anchors must be (N, 2) array, got shape {self.anchors.shape}") + raise ValueError( + f"Anchors must be (N, 2) array, got shape {self.anchors.shape}" + ) self.n_anchors = len(self.anchors) self.pos_idx = state_position_indices @@ -56,10 +60,10 @@ def __init__(self, anchors: np.ndarray, state_position_indices: Tuple[int, int] def h(self, x: np.ndarray) -> np.ndarray: """ Measurement function: predicted ranges. - + Args: x: State vector (must contain position at self.pos_idx) - + Returns: Predicted ranges to all anchors, shape (N,) """ @@ -70,10 +74,10 @@ def h(self, x: np.ndarray) -> np.ndarray: def H(self, x: np.ndarray) -> np.ndarray: """ Measurement Jacobian with singularity handling. - + Args: x: State vector - + Returns: Jacobian matrix, shape (N, len(x)) """ @@ -98,16 +102,16 @@ def H(self, x: np.ndarray) -> np.ndarray: class RangeBearingMeasurement2D: """ Range and bearing measurement model for 2D positioning. - + Measurements: - Range: z_r = ||p - landmark|| - Bearing: z_θ = atan2(ly - py, lx - px) - + Used in: - Robot localization - Landmark-based navigation - ch3_estimators/example_ekf_range_bearing.py - + Example: >>> landmarks = np.array([[0, 0], [10, 0], [10, 10]]) >>> model = RangeBearingMeasurement2D(landmarks) @@ -117,17 +121,21 @@ class RangeBearingMeasurement2D: (6,) # [r0, θ0, r1, θ1, r2, θ2] """ - def __init__(self, landmarks: np.ndarray, state_position_indices: Tuple[int, int] = (0, 1)): + def __init__( + self, landmarks: np.ndarray, state_position_indices: Tuple[int, int] = (0, 1) + ): """ Initialize range-bearing measurement model. - + Args: landmarks: Landmark positions, shape (N, 2) state_position_indices: Indices of [px, py] in state (default: (0, 1)) """ self.landmarks = np.asarray(landmarks) if self.landmarks.ndim != 2 or self.landmarks.shape[1] != 2: - raise ValueError(f"Landmarks must be (N, 2) array, got shape {self.landmarks.shape}") + raise ValueError( + f"Landmarks must be (N, 2) array, got shape {self.landmarks.shape}" + ) self.n_landmarks = len(self.landmarks) self.pos_idx = state_position_indices @@ -135,10 +143,10 @@ def __init__(self, landmarks: np.ndarray, state_position_indices: Tuple[int, int def h(self, x: np.ndarray) -> np.ndarray: """ Measurement function: [range_0, bearing_0, range_1, bearing_1, ...]. - + Args: x: State vector - + Returns: Measurements [r0, θ0, r1, θ1, ...], shape (2*N,) """ @@ -157,10 +165,10 @@ def h(self, x: np.ndarray) -> np.ndarray: def H(self, x: np.ndarray) -> np.ndarray: """ Measurement Jacobian with singularity handling. - + Args: x: State vector - + Returns: Jacobian matrix, shape (2*N, len(x)) """ @@ -199,13 +207,13 @@ def H(self, x: np.ndarray) -> np.ndarray: def innovation(self, z_measured: np.ndarray, z_predicted: np.ndarray) -> np.ndarray: """ Compute innovation with proper angle wrapping for bearings. - + CRITICAL: Bearing innovations must be wrapped to [-π, π]. - + Args: z_measured: Measured [r0, θ0, r1, θ1, ...] z_predicted: Predicted [r0, θ0, r1, θ1, ...] - + Returns: Innovation vector with wrapped bearing differences """ @@ -221,15 +229,15 @@ def innovation(self, z_measured: np.ndarray, z_predicted: np.ndarray) -> np.ndar class PositionMeasurement2D: """ Direct position measurement model (e.g., GPS, UWB position fix). - + Measurement: z = [px, py] + noise - + Used in: - GPS updates - UWB position fixes (loosely coupled) - Absolute position corrections - ch8_sensor_fusion (loosely coupled) - + Example: >>> model = PositionMeasurement2D() >>> x = np.array([5, 7, 1, 0.5]) # [px, py, vx, vy] @@ -241,7 +249,7 @@ class PositionMeasurement2D: def __init__(self, state_position_indices: Tuple[int, int] = (0, 1)): """ Initialize position measurement model. - + Args: state_position_indices: Indices of [px, py] in state (default: (0, 1)) """ @@ -250,10 +258,10 @@ def __init__(self, state_position_indices: Tuple[int, int] = (0, 1)): def h(self, x: np.ndarray) -> np.ndarray: """ Measurement function: extract position from state. - + Args: x: State vector - + Returns: Position [px, py] """ @@ -262,10 +270,10 @@ def h(self, x: np.ndarray) -> np.ndarray: def H(self, x: np.ndarray) -> np.ndarray: """ Measurement Jacobian (trivial for linear measurement). - + Args: x: State vector - + Returns: Jacobian matrix, shape (2, len(x)) """ @@ -281,18 +289,18 @@ def validate_measurement_inputs( z: Optional[np.ndarray] = None, expected_x_dim: Optional[int] = None, expected_z_dim: Optional[int] = None, - model_name: str = "measurement model" + model_name: str = "measurement model", ) -> None: """ Validate inputs to measurement models. - + Args: x: State vector z: Measurement vector (optional) expected_x_dim: Expected state dimension (if known) expected_z_dim: Expected measurement dimension (if known) model_name: Name of model for error messages - + Raises: ValueError: If validation fails TypeError: If wrong types provided @@ -310,10 +318,14 @@ def validate_measurement_inputs( if z is not None: if not isinstance(z, np.ndarray): - raise TypeError(f"{model_name}: measurement must be numpy array, got {type(z)}") + raise TypeError( + f"{model_name}: measurement must be numpy array, got {type(z)}" + ) if z.ndim != 1: - raise ValueError(f"{model_name}: measurement must be 1D, got shape {z.shape}") + raise ValueError( + f"{model_name}: measurement must be 1D, got shape {z.shape}" + ) if expected_z_dim is not None and z.shape[0] != expected_z_dim: raise ValueError( @@ -322,26 +334,25 @@ def validate_measurement_inputs( def create_measurement_noise_covariance( - noise_std: np.ndarray, - correlation: Optional[np.ndarray] = None + noise_std: np.ndarray, correlation: Optional[np.ndarray] = None ) -> np.ndarray: """ Create measurement noise covariance matrix. - + Args: noise_std: Standard deviations for each measurement, shape (m,) correlation: Optional correlation matrix, shape (m, m) If None, assumes uncorrelated measurements - + Returns: Measurement noise covariance R, shape (m, m) - + Example: >>> # Independent measurements >>> R = create_measurement_noise_covariance(np.array([0.5, 0.5, 0.05, 0.05])) >>> np.diag(R) array([0.25, 0.25, 0.0025, 0.0025]) - + >>> # Correlated measurements >>> corr = np.array([[1, 0.5], [0.5, 1]]) >>> R = create_measurement_noise_covariance(np.array([1.0, 1.0]), corr) @@ -372,15 +383,10 @@ def create_measurement_noise_covariance( raise ValueError("Correlation matrix must be symmetric") if not np.allclose(np.diag(correlation), 1.0): - warnings.warn( - "Correlation matrix diagonal should be 1.0", - RuntimeWarning - ) + warnings.warn("Correlation matrix diagonal should be 1.0", RuntimeWarning) # Construct covariance Sigma = np.diag(noise_std) R = Sigma @ correlation @ Sigma return R - - diff --git a/core/models/motion_models.py b/core/models/motion_models.py index 821afcf..d4582f1 100644 --- a/core/models/motion_models.py +++ b/core/models/motion_models.py @@ -16,14 +16,14 @@ class ConstantVelocity1D: """ 1D Constant Velocity Motion Model. - + State: x = [position, velocity] Dynamics: x_{k+1} = F(dt) * x_k + w_k - + Used in: - ch3_estimators/example_kalman_1d.py - Simple 1D tracking problems - + Example: >>> model = ConstantVelocity1D() >>> x = np.array([0.0, 1.0]) # position=0, velocity=1 m/s @@ -37,12 +37,12 @@ class ConstantVelocity1D: def f(x: np.ndarray, u: Optional[np.ndarray] = None, dt: float = 1.0) -> np.ndarray: """ Process model: x_{k+1} = f(x_k, dt). - + Args: x: State [position, velocity] u: Control input (unused) dt: Time step in seconds - + Returns: Next state [position', velocity'] """ @@ -50,57 +50,48 @@ def f(x: np.ndarray, u: Optional[np.ndarray] = None, dt: float = 1.0) -> np.ndar raise ValueError(f"State must be 2D [pos, vel], got shape {x.shape}") position, velocity = x - return np.array([ - position + velocity * dt, - velocity - ]) + return np.array([position + velocity * dt, velocity]) @staticmethod def F(dt: float) -> np.ndarray: """ State transition matrix (Jacobian of f). - + Args: dt: Time step in seconds - + Returns: 2x2 state transition matrix """ - return np.array([ - [1.0, dt], - [0.0, 1.0] - ]) + return np.array([[1.0, dt], [0.0, 1.0]]) @staticmethod def Q(dt: float, q: float = 1.0) -> np.ndarray: """ Process noise covariance (continuous white noise acceleration). - + Args: dt: Time step in seconds q: Process noise intensity (acceleration variance) - + Returns: 2x2 process noise covariance matrix """ - return q * np.array([ - [dt**3 / 3, dt**2 / 2], - [dt**2 / 2, dt] - ]) + return q * np.array([[dt**3 / 3, dt**2 / 2], [dt**2 / 2, dt]]) class ConstantVelocity2D: """ 2D Constant Velocity Motion Model. - + State: x = [px, py, vx, vy] Dynamics: Constant velocity in x and y independently - + Used in: - ch3_estimators/example_ekf_range_bearing.py - ch8_sensor_fusion (fusion examples) - 2D tracking and positioning - + Example: >>> model = ConstantVelocity2D() >>> x = np.array([0.0, 0.0, 1.0, 0.5]) # At origin, moving at [1, 0.5] m/s @@ -114,12 +105,12 @@ class ConstantVelocity2D: def f(x: np.ndarray, u: Optional[np.ndarray] = None, dt: float = 1.0) -> np.ndarray: """ Process model: x_{k+1} = f(x_k, dt). - + Args: x: State [px, py, vx, vy] u: Control input (unused) dt: Time step in seconds - + Returns: Next state [px', py', vx', vy'] """ @@ -127,65 +118,64 @@ def f(x: np.ndarray, u: Optional[np.ndarray] = None, dt: float = 1.0) -> np.ndar raise ValueError(f"State must be 4D [px,py,vx,vy], got shape {x.shape}") px, py, vx, vy = x - return np.array([ - px + vx * dt, - py + vy * dt, - vx, - vy - ]) + return np.array([px + vx * dt, py + vy * dt, vx, vy]) @staticmethod def F(dt: float) -> np.ndarray: """ State transition matrix (Jacobian of f). - + Args: dt: Time step in seconds - + Returns: 4x4 state transition matrix """ - return np.array([ - [1.0, 0.0, dt, 0.0], - [0.0, 1.0, 0.0, dt], - [0.0, 0.0, 1.0, 0.0], - [0.0, 0.0, 0.0, 1.0] - ]) + return np.array( + [ + [1.0, 0.0, dt, 0.0], + [0.0, 1.0, 0.0, dt], + [0.0, 0.0, 1.0, 0.0], + [0.0, 0.0, 0.0, 1.0], + ] + ) @staticmethod def Q(dt: float, q: float = 1.0) -> np.ndarray: """ Process noise covariance (continuous white noise acceleration). - + Assumes independent noise in x and y directions. - + Args: dt: Time step in seconds q: Process noise intensity (acceleration variance) - + Returns: 4x4 process noise covariance matrix """ - return q * np.array([ - [dt**3/3, 0, dt**2/2, 0 ], - [0, dt**3/3, 0, dt**2/2], - [dt**2/2, 0, dt, 0 ], - [0, dt**2/2, 0, dt ] - ]) + return q * np.array( + [ + [dt**3 / 3, 0, dt**2 / 2, 0], + [0, dt**3 / 3, 0, dt**2 / 2], + [dt**2 / 2, 0, dt, 0], + [0, dt**2 / 2, 0, dt], + ] + ) class ConstantAcceleration2D: """ 2D Constant Acceleration Motion Model. - + State: x = [px, py, vx, vy, ax, ay] Dynamics: Constant acceleration in x and y - + Used for: - Maneuvering targets - Vehicle tracking - High-dynamics scenarios - + Example: >>> model = ConstantAcceleration2D() >>> x = np.array([0, 0, 0, 0, 1, 0.5]) # At rest, accelerating @@ -198,57 +188,63 @@ class ConstantAcceleration2D: def f(x: np.ndarray, u: Optional[np.ndarray] = None, dt: float = 1.0) -> np.ndarray: """ Process model with constant acceleration. - + Args: x: State [px, py, vx, vy, ax, ay] u: Control input (unused) dt: Time step in seconds - + Returns: Next state [px', py', vx', vy', ax', ay'] """ if x.shape != (6,): - raise ValueError(f"State must be 6D [px,py,vx,vy,ax,ay], got shape {x.shape}") + raise ValueError( + f"State must be 6D [px,py,vx,vy,ax,ay], got shape {x.shape}" + ) px, py, vx, vy, ax, ay = x - return np.array([ - px + vx * dt + 0.5 * ax * dt**2, - py + vy * dt + 0.5 * ay * dt**2, - vx + ax * dt, - vy + ay * dt, - ax, - ay - ]) + return np.array( + [ + px + vx * dt + 0.5 * ax * dt**2, + py + vy * dt + 0.5 * ay * dt**2, + vx + ax * dt, + vy + ay * dt, + ax, + ay, + ] + ) @staticmethod def F(dt: float) -> np.ndarray: """ State transition matrix. - + Args: dt: Time step in seconds - + Returns: 6x6 state transition matrix """ - return np.array([ - [1, 0, dt, 0, 0.5*dt**2, 0 ], - [0, 1, 0, dt, 0, 0.5*dt**2], - [0, 0, 1, 0, dt, 0 ], - [0, 0, 0, 1, 0, dt ], - [0, 0, 0, 0, 1, 0 ], - [0, 0, 0, 0, 0, 1 ] - ]) + return np.array( + [ + [1, 0, dt, 0, 0.5 * dt**2, 0], + [0, 1, 0, dt, 0, 0.5 * dt**2], + [0, 0, 1, 0, dt, 0], + [0, 0, 0, 1, 0, dt], + [0, 0, 0, 0, 1, 0], + [0, 0, 0, 0, 0, 1], + ] + ) @staticmethod def Q(dt: float, q: float = 1.0) -> np.ndarray: """ Process noise covariance (continuous white noise jerk). - + Args: dt: Time step in seconds q: Process noise intensity (jerk variance) - + Returns: 6x6 process noise covariance matrix """ @@ -258,11 +254,13 @@ def Q(dt: float, q: float = 1.0) -> np.ndarray: dt4 = dt**4 dt5 = dt**5 - Q_1d = q * np.array([ - [dt5/20, dt4/8, dt3/6], - [dt4/8, dt3/3, dt2/2], - [dt3/6, dt2/2, dt ] - ]) + Q_1d = q * np.array( + [ + [dt5 / 20, dt4 / 8, dt3 / 6], + [dt4 / 8, dt3 / 3, dt2 / 2], + [dt3 / 6, dt2 / 2, dt], + ] + ) # Block diagonal for x and y Q = np.zeros((6, 6)) @@ -273,31 +271,29 @@ def Q(dt: float, q: float = 1.0) -> np.ndarray: def create_process_noise_continuous_white_acceleration( - dt: float, - q: float, - dim: int = 2 + dt: float, q: float, dim: int = 2 ) -> np.ndarray: """ Create process noise covariance for continuous white acceleration model. - + This is the standard process noise model for constant velocity systems. - + Args: dt: Time step in seconds q: Process noise intensity (acceleration variance, m²/s⁴) dim: Spatial dimension (1, 2, or 3) - + Returns: Process noise covariance matrix - dim=1: 2x2 matrix for [pos, vel] - dim=2: 4x4 matrix for [px, py, vx, vy] - dim=3: 6x6 matrix for [px, py, pz, vx, vy, vz] - + Example: >>> Q = create_process_noise_continuous_white_acceleration(dt=0.1, q=0.5, dim=2) >>> Q.shape (4, 4) - + References: - Bar-Shalom et al., "Estimation with Applications to Tracking and Navigation" - Chapter 3: Process noise modeling @@ -306,10 +302,7 @@ def create_process_noise_continuous_white_acceleration( raise ValueError(f"Dimension must be 1, 2, or 3, got {dim}") # 1D block - Q_1d = q * np.array([ - [dt**3 / 3, dt**2 / 2], - [dt**2 / 2, dt] - ]) + Q_1d = q * np.array([[dt**3 / 3, dt**2 / 2], [dt**2 / 2, dt]]) if dim == 1: return Q_1d @@ -319,7 +312,7 @@ def create_process_noise_continuous_white_acceleration( Q = np.zeros((n, n)) for i in range(dim): - Q[2*i:2*i+2, 2*i:2*i+2] = Q_1d + Q[2 * i : 2 * i + 2, 2 * i : 2 * i + 2] = Q_1d return Q @@ -328,17 +321,17 @@ def validate_motion_model_inputs( x: np.ndarray, expected_dim: int, dt: Optional[float] = None, - model_name: str = "motion model" + model_name: str = "motion model", ) -> None: """ Validate inputs to motion models. - + Args: x: State vector expected_dim: Expected state dimension dt: Time step (if provided, must be positive) model_name: Name of model for error messages - + Raises: ValueError: If validation fails TypeError: If wrong types provided @@ -361,10 +354,9 @@ def validate_motion_model_inputs( raise ValueError(f"{model_name}: dt must be positive, got {dt}") if dt > 10.0: import warnings + warnings.warn( f"{model_name}: dt={dt}s is unusually large. " "Check units (should be seconds).", - RuntimeWarning + RuntimeWarning, ) - - diff --git a/core/rf/__init__.py b/core/rf/__init__.py index 33c2459..afa7066 100644 --- a/core/rf/__init__.py +++ b/core/rf/__init__.py @@ -104,4 +104,3 @@ "compute_dop_map", "position_error_from_dop", ] - diff --git a/core/rf/measurement_models.py b/core/rf/measurement_models.py index 61e1b94..12ae63a 100644 --- a/core/rf/measurement_models.py +++ b/core/rf/measurement_models.py @@ -341,7 +341,9 @@ def simulate_rtt_measurement( # Add noise to processing time if std > 0 if processing_time_std > 0: - processing_time_actual = processing_time + _rng(rng).standard_normal() * processing_time_std + processing_time_actual = ( + processing_time + _rng(rng).standard_normal() * processing_time_std + ) else: processing_time_actual = processing_time @@ -363,11 +365,11 @@ def simulate_rtt_measurement( ) info = { - 'true_range': true_range, - 'true_travel_time': true_travel_time, - 'processing_time_actual': processing_time_actual, - 'clock_drift_actual': clock_drift_actual, - 'range_estimate': range_estimate, + "true_range": true_range, + "true_travel_time": true_travel_time, + "processing_time_actual": processing_time_actual, + "clock_drift_actual": clock_drift_actual, + "range_estimate": range_estimate, } return rtt, info @@ -680,14 +682,14 @@ def simulate_rss_measurement( distance_error_factor = 10 ** (-total_fading_db / (10 * path_loss_exp)) info = { - 'true_distance': true_distance, - 'rss_true': rss_true, - 'omega_long_db': omega_long_db, - 'omega_short_db': omega_short_db, - 'omega_short_samples': omega_short_samples, - 'distance_estimate': distance_estimate, - 'distance_error_factor': distance_error_factor, - 'short_fading_model': short_fading_model, + "true_distance": true_distance, + "rss_true": rss_true, + "omega_long_db": omega_long_db, + "omega_short_db": omega_short_db, + "omega_short_samples": omega_short_samples, + "distance_estimate": distance_estimate, + "distance_error_factor": distance_error_factor, + "short_fading_model": short_fading_model, } return rss_measured, info @@ -841,9 +843,7 @@ def tdoa_measurement_vector( for i in range(n_anchors): if i == reference_anchor_idx: continue - range_diff = tdoa_range_difference( - anchors[i], reference_anchor, agent_pos - ) + range_diff = tdoa_range_difference(anchors[i], reference_anchor, agent_pos) tdoa_measurements.append(range_diff) return np.array(tdoa_measurements) @@ -1177,6 +1177,3 @@ def aoa_angle_vector( angles.append(azimuth) return np.array(angles) - - - diff --git a/core/rf/positioning.py b/core/rf/positioning.py index b1d0327..c5aa2df 100644 --- a/core/rf/positioning.py +++ b/core/rf/positioning.py @@ -106,9 +106,7 @@ def mean_solved_m(self) -> float: def max_solved_m(self) -> float: """Worst error among the fixes that actually solved.""" return ( - float(self.errors[self.solved].max()) - if self.solved.any() - else float("nan") + float(self.errors[self.solved].max()) if self.solved.any() else float("nan") ) def summary(self) -> dict: @@ -233,9 +231,7 @@ def build_tdoa_covariance( n_anchors = len(sigmas) if ref_idx < 0 or ref_idx >= n_anchors: - raise ValueError( - f"ref_idx must be in [0, {n_anchors-1}], got {ref_idx}" - ) + raise ValueError(f"ref_idx must be in [0, {n_anchors-1}], got {ref_idx}") # Reference anchor variance sigma_ref_sq = sigmas[ref_idx] ** 2 @@ -396,9 +392,7 @@ def solve( position = np.asarray(initial_guess, dtype=float).copy() if len(ranges) != self.n_anchors: - raise ValueError( - f"Expected {self.n_anchors} ranges, got {len(ranges)}" - ) + raise ValueError(f"Expected {self.n_anchors} ranges, got {len(ranges)}") # Initialize weight matrix based on method if self.method == "iterative_ls": @@ -407,9 +401,7 @@ def solve( elif self.method == "iterative_wls": # Eq. 4.23: W = Σ^{-1} if covariance is None: - raise ValueError( - "method='iterative_wls' requires covariance parameter" - ) + raise ValueError("method='iterative_wls' requires covariance parameter") W = np.linalg.inv(covariance) elif self.method == "range_weighted": # Heuristic: will be updated each iteration @@ -423,9 +415,7 @@ def solve( for iteration in range(max_iters): # Compute predicted ranges and residuals (Eq. 4.16) - predicted_ranges = np.linalg.norm( - self.anchors - position, axis=1 - ) + predicted_ranges = np.linalg.norm(self.anchors - position, axis=1) residuals = ranges - predicted_ranges # Check convergence @@ -994,7 +984,9 @@ def _compute_angle_weight_matrix( def _fill(slot, sigma): if sigma is None: return - sigma_arr = np.broadcast_to(np.asarray(sigma, dtype=float), (self.n_anchors,)) + sigma_arr = np.broadcast_to( + np.asarray(sigma, dtype=float), (self.n_anchors,) + ) variances[slot] = np.maximum(sigma_arr**2, 1e-12) if self.is_3d: @@ -1317,9 +1309,7 @@ def solve( ) if residual not in ("angle", "tan"): - raise ValueError( - f"residual must be 'angle' or 'tan', got {residual!r}" - ) + raise ValueError(f"residual must be 'angle' or 'tan', got {residual!r}") use_angle_residual = residual == "angle" # sigma_sin_theta and sigma_tan_psi describe noise in the *transformed* @@ -1343,10 +1333,11 @@ def solve( # Determine weight matrix source n_meas = len(z_measured) - use_sigma_weights = ( - weights is None and - (sigma_theta is not None or sigma_psi is not None or - sigma_sin_theta is not None or sigma_tan_psi is not None) + use_sigma_weights = weights is None and ( + sigma_theta is not None + or sigma_psi is not None + or sigma_sin_theta is not None + or sigma_tan_psi is not None ) # Initialize weight matrix @@ -1553,9 +1544,7 @@ def aoa_ove_solve( if anchors.shape[1] != 3: raise ValueError("OVE requires 3D anchors with shape (N, 3)") if len(elevation_angles) != n_anchors or len(azimuth_angles) != n_anchors: - raise ValueError( - f"Expected {n_anchors} elevation and azimuth angles each" - ) + raise ValueError(f"Expected {n_anchors} elevation and azimuth angles each") # Build the S_a matrix and z_a vector (Eq. 4.84) S_a = np.zeros((n_anchors, 3)) @@ -1774,9 +1763,7 @@ def aoa_ple_solve_3d( if anchors.shape[1] != 3: raise ValueError("PLE_3D requires 3D anchors with shape (N, 3)") if len(elevation_angles) != n_anchors or len(azimuth_angles) != n_anchors: - raise ValueError( - f"Expected {n_anchors} elevation and azimuth angles each" - ) + raise ValueError(f"Expected {n_anchors} elevation and azimuth angles each") # Step 1: Solve 2D position using azimuth only anchors_2d = anchors[:, :2] @@ -1877,21 +1864,15 @@ def toa_fang_solver( # Validate inputs if dim != 2: - raise ValueError( - f"Fang's algorithm currently supports 2D only, got dim={dim}" - ) + raise ValueError(f"Fang's algorithm currently supports 2D only, got dim={dim}") if n_anchors < 3: raise ValueError( f"Fang's algorithm requires at least 3 anchors, got {n_anchors}" ) if len(ranges) != n_anchors: - raise ValueError( - f"Expected {n_anchors} ranges, got {len(ranges)}" - ) + raise ValueError(f"Expected {n_anchors} ranges, got {len(ranges)}") if ref_idx < 0 or ref_idx >= n_anchors: - raise ValueError( - f"ref_idx must be in [0, {n_anchors-1}], got {ref_idx}" - ) + raise ValueError(f"ref_idx must be in [0, {n_anchors-1}], got {ref_idx}") # Reference anchor position and range x_ref = anchors[ref_idx] # [x_e^ref, x_n^ref] @@ -1916,9 +1897,10 @@ def toa_fang_solver( # Eq. 4.47: y^i = d_i² - d_ref² - (x_e^i² - x_e^ref²) - (x_n^i² - x_n^ref²) y_a[row] = ( - d_i**2 - d_ref**2 - - (x_i[0]**2 - x_ref[0]**2) - - (x_i[1]**2 - x_ref[1]**2) + d_i**2 + - d_ref**2 + - (x_i[0] ** 2 - x_ref[0] ** 2) + - (x_i[1] ** 2 - x_ref[1] ** 2) ) # Solve: x_a = (H_a^T H_a)^{-1} H_a^T y_a (Eq. 4.49) @@ -2030,9 +2012,7 @@ def tdoa_chan_solver( # Validate inputs if dim != 2: - raise ValueError( - f"Chan's algorithm currently supports 2D only, got dim={dim}" - ) + raise ValueError(f"Chan's algorithm currently supports 2D only, got dim={dim}") if n_anchors < 4: raise ValueError( f"Chan's algorithm requires at least 4 anchors, got {n_anchors}" @@ -2042,9 +2022,7 @@ def tdoa_chan_solver( f"Expected {n_anchors-1} TDOA measurements, got {len(tdoa_measurements)}" ) if ref_idx < 0 or ref_idx >= n_anchors: - raise ValueError( - f"ref_idx must be in [0, {n_anchors-1}], got {ref_idx}" - ) + raise ValueError(f"ref_idx must be in [0, {n_anchors-1}], got {ref_idx}") # Reference anchor position x_ref = anchors[ref_idx] # [x_e^ref, x_n^ref] @@ -2072,9 +2050,7 @@ def tdoa_chan_solver( # Eq. 4.60: y^i = (Δd^i)² - [(x_e^i² + x_n^i²) - (x_e^ref² + x_n^ref²)] y_a[row] = ( - delta_d**2 - - (x_i[0]**2 + x_i[1]**2) - + (x_ref[0]**2 + x_ref[1]**2) + delta_d**2 - (x_i[0] ** 2 + x_i[1] ** 2) + (x_ref[0] ** 2 + x_ref[1] ** 2) ) # Step 1: Initial LS solution (Eq. 4.49 applied to Chan's formulation) diff --git a/core/sensors/__init__.py b/core/sensors/__init__.py index bb39f4e..bc4d500 100644 --- a/core/sensors/__init__.py +++ b/core/sensors/__init__.py @@ -93,23 +93,23 @@ ... strapdown_update ... ) >>> import numpy as np - >>> + >>> >>> # Create IMU data packet >>> t = np.linspace(0, 1, 100) >>> accel = np.random.randn(100, 3) * 0.1 + [0, 0, -9.81] >>> gyro = np.random.randn(100, 3) * 0.01 >>> imu = ImuSeries(t=t, accel=accel, gyro=gyro, meta={'sample_rate_hz': 100}) - >>> + >>> >>> # Create navigation state >>> q0 = np.array([1.0, 0.0, 0.0, 0.0]) # identity quaternion >>> state = NavStateQPVP(q=q0, v=np.zeros(3), p=np.zeros(3)) - >>> + >>> >>> # Correct IMU measurements >>> bias_g = np.array([0.001, -0.0005, 0.0002]) >>> bias_a = np.array([0.01, -0.005, 0.02]) >>> omega = correct_gyro(gyro[0, :], bias_g) >>> f_b = correct_accel(accel[0, :], bias_a) - >>> + >>> >>> # Strapdown integration step >>> dt = t[1] - t[0] >>> q1, v1, p1 = strapdown_update(state.q, state.v, state.p, omega, f_b, dt) @@ -276,4 +276,3 @@ ] __version__ = "1.0.0" - diff --git a/core/sensors/calibration.py b/core/sensors/calibration.py index 4b73528..2fcdcee 100644 --- a/core/sensors/calibration.py +++ b/core/sensors/calibration.py @@ -94,14 +94,14 @@ def allan_variance( >>> duration = 3600 # 1 hour >>> N = int(fs * duration) >>> t = np.arange(N) / fs - >>> + >>> >>> # White noise + bias drift >>> gyro = 0.001 * np.random.randn(N) # angle random walk >>> gyro += 0.01 * np.cumsum(np.random.randn(N)) / fs # bias drift - >>> + >>> >>> # Compute Allan deviation >>> taus, adev = allan_variance(gyro, fs) - >>> + >>> >>> # Plot (log-log) to identify noise sources >>> # import matplotlib.pyplot as plt >>> # plt.loglog(taus, adev) @@ -467,12 +467,12 @@ def arw_to_noise_std(arw: float, dt: float) -> float: >>> taus, adev = allan_variance(gyro_data, fs=100.0) >>> arw = identify_random_walk(taus, adev, tau_target=1.0) >>> print(f"ARW: {np.rad2deg(arw):.4f} deg/√s") - >>> + >>> >>> # Convert to per-sample noise (Eq. 6.58) >>> dt = 1.0 / 100.0 # 100 Hz sampling >>> sigma_omega = arw_to_noise_std(arw, dt) >>> print(f"Per-sample noise: {np.rad2deg(sigma_omega):.4f} deg/s") - >>> + >>> >>> # Use in simulation >>> gyro_noise = np.random.randn(N) * sigma_omega @@ -523,7 +523,7 @@ def noise_std_to_arw(sigma: float, dt: float) -> float: >>> # Known noise level from sensor datasheet >>> sigma_omega = np.deg2rad(0.1) # 0.1 deg/s per sample >>> dt = 1.0 / 100.0 # 100 Hz - >>> + >>> >>> # Compute equivalent ARW >>> arw = noise_std_to_arw(sigma_omega, dt) >>> print(f"ARW: {np.rad2deg(arw):.4f} deg/√s") @@ -670,5 +670,3 @@ def characterize_imu_noise( } return results - - diff --git a/core/sensors/constraints.py b/core/sensors/constraints.py index 1069735..dcb5d1f 100644 --- a/core/sensors/constraints.py +++ b/core/sensors/constraints.py @@ -48,13 +48,13 @@ def zupt_test_statistic( ) -> float: """ Compute ZUPT test statistic over a window (Eq. 6.44). - + Implements the SHOE-style (Zero-velocity update aided Inertial Navigation) windowed test statistic from Equation (6.44): - + T_k = (1/N) * Σ_(l∈W_k) [ (1/σ_A) * ||ã_l - g*(ā_k)/||ā_k|| || + (1/σ_G) * ||ω̃_l||² ] - + where: - N: window length (number of samples) - W_k: time window of measurements @@ -64,11 +64,11 @@ def zupt_test_statistic( - g: gravity magnitude - ã_l: accelerometer measurement at sample l - ω̃_l: gyroscope measurement at sample l - + The test statistic measures deviation from the expected stationary condition (constant gravity, zero angular rate). Lower values indicate more likely stationary behavior. - + Args: accel_window: Accelerometer measurements in window. Shape: (N, 3). Units: m/s². @@ -85,35 +85,35 @@ def zupt_test_statistic( lat_rad: Geodetic latitude in radians (optional). If provided, uses Eq. (6.8) for gravity magnitude. If None, uses g parameter (backward compatible). - + Returns: Test statistic T_k. Units: dimensionless. Lower values indicate more stationary behavior. Typical threshold γ: 1e5 to 1e7 (depends on noise parameters). - + Notes: - Window length N typically 5-20 samples (50-200ms at 100Hz). - Longer windows: more robust, but slower response to stance transitions. - The term ||ã_l - g*(ā_k)/||ā_k|| || measures deviation from constant gravity. - The term ||ω̃_l||² measures rotation (should be near zero when stationary). - Noise parameters (σ_A, σ_G) should match IMU specifications. - + Example: >>> # Window of stationary measurements >>> accel = np.array([[0, 0, 9.8], [0.1, 0, 9.9], [-0.1, 0, 9.7]]) >>> gyro = np.array([[0.01, 0, 0], [0, 0.01, 0], [0, 0, -0.01]]) >>> T_k = zupt_test_statistic(accel, gyro, sigma_a=0.05, sigma_g=1e-3) >>> print(f"Test statistic: {T_k:.2e}") - >>> + >>> >>> # Check against threshold >>> gamma = 1e6 >>> is_stationary = (T_k < gamma) >>> print(f"Stationary: {is_stationary}") - + Related Equations: - Eq. (6.44): ZUPT test statistic (THIS FUNCTION) - Eq. (6.45): ZUPT pseudo-measurement (velocity = 0) - + References: [19] Foxlin, E. (2005). "Pedestrian tracking with shoe-mounted inertial sensors." IEEE Computer Graphics and Applications, 25(6), 38-46. @@ -124,7 +124,9 @@ def zupt_test_statistic( f"got {accel_window.shape[0]} and {gyro_window.shape[0]}" ) if accel_window.shape[1] != 3: - raise ValueError(f"accel_window must have shape (N, 3), got {accel_window.shape}") + raise ValueError( + f"accel_window must have shape (N, 3), got {accel_window.shape}" + ) if gyro_window.shape[1] != 3: raise ValueError(f"gyro_window must have shape (N, 3), got {gyro_window.shape}") if sigma_a <= 0: @@ -156,7 +158,7 @@ def zupt_test_statistic( # Gyroscope term: (1/σ_G) * ||ω̃_l||² # Note: Eq. 6.44 has (1/σ_G) * ||ω||², not (1/σ_G²) * ||ω||² - gyro_norm_sq = np.sum(gyro_window[i]**2) # ||ω||² + gyro_norm_sq = np.sum(gyro_window[i] ** 2) # ||ω||² gyro_term = gyro_norm_sq / sigma_g # Sum both terms @@ -179,15 +181,15 @@ def detect_zupt_windowed( ) -> bool: """ Detect zero velocity (ZUPT) using windowed test statistic (Eq. 6.44). - + This is the proper SHOE-style ZUPT detector that uses a window of measurements to compute a test statistic and compares it to a threshold. - + The detector returns True if: T_k < γ (test statistic below threshold) - + where T_k is computed by zupt_test_statistic(). - + Args: accel_window: Accelerometer measurements in window. Shape: (N, 3). Units: m/s². @@ -204,50 +206,52 @@ def detect_zupt_windowed( lat_rad: Geodetic latitude in radians (optional). If provided, uses Eq. (6.8) for gravity magnitude. If None, uses g parameter (backward compatible). - + Returns: True if ZUPT detected (stationary), False otherwise. - + Notes: - This is the recommended ZUPT detector (more robust than instantaneous). - Window length: typically 5-20 samples (50-200ms at 100Hz). - Tune γ based on IMU noise and application requirements. - For foot-mounted INS: γ ≈ 1e6 works well for consumer IMUs. - + Example: >>> # Collect window of measurements (e.g., last 10 samples) >>> accel_window = accel_buffer[-10:] # Shape: (10, 3) >>> gyro_window = gyro_buffer[-10:] # Shape: (10, 3) - >>> + >>> >>> # Detect ZUPT using windowed test >>> from core.sensors import units, IMUNoiseParams >>> params = IMUNoiseParams.consumer_grade() - >>> + >>> >>> is_stationary = detect_zupt_windowed( ... accel_window, gyro_window, ... sigma_a=params.accel_vrw_mps_sqrt_s * np.sqrt(100), # Scale for sample rate ... sigma_g=params.gyro_arw_rad_sqrt_s * np.sqrt(100), ... gamma=1e6 ... ) - >>> + >>> >>> if is_stationary: ... # Apply ZUPT correction ... pass - + Related Functions: - zupt_test_statistic(): Computes T_k - detect_zupt(): Simple instantaneous detector (deprecated) - + Related Equations: - Eq. (6.44): ZUPT test statistic (THIS FUNCTION) - Eq. (6.45): ZUPT pseudo-measurement - Eq. (6.8): Gravity magnitude (used when latitude provided) """ # Compute test statistic - T_k = zupt_test_statistic(accel_window, gyro_window, sigma_a, sigma_g, g, lat_rad=lat_rad) + T_k = zupt_test_statistic( + accel_window, gyro_window, sigma_a, sigma_g, g, lat_rad=lat_rad + ) # Compare to threshold - is_stationary = (T_k < gamma) + is_stationary = T_k < gamma return is_stationary @@ -260,7 +264,7 @@ def detect_zupt( ) -> bool: """ Detect zero velocity (stationary) condition for ZUPT (DEPRECATED). - + **DEPRECATED**: Use `detect_zupt_windowed()` instead for proper Eq. (6.44) implementation with windowed test statistic. @@ -307,7 +311,7 @@ def detect_zupt( >>> accel = np.array([0.0, 0.0, -9.81]) # gravity only >>> is_stationary = detect_zupt(gyro, accel, delta_omega=0.05, delta_f=0.5) >>> print(is_stationary) # True - >>> + >>> >>> # Moving sensor >>> gyro_moving = np.array([0.5, 0.2, -0.1]) # rotating >>> accel_moving = np.array([2.0, 0.5, -9.0]) # accelerating @@ -375,10 +379,10 @@ class ZuptMeasurementModel: Example: >>> import numpy as np >>> from core.sensors.constraints import ZuptMeasurementModel, detect_zupt - >>> + >>> >>> # Assume we have an EKF with state [p, v, q, b_g, b_a] (Eq. 6.16) >>> zupt_model = ZuptMeasurementModel(sigma_zupt=0.05) - >>> + >>> >>> # Check if stationary >>> if detect_zupt(gyro, accel, delta_omega=0.05, delta_f=0.5): >>> z_zupt = np.zeros(3) # zero velocity measurement @@ -772,5 +776,3 @@ def R(self, x: Optional[np.ndarray] = None) -> np.ndarray: """ R = np.diag([self.sigma_lateral**2, self.sigma_vertical**2]) return R - - diff --git a/core/sensors/environment.py b/core/sensors/environment.py index 570d7e5..530540b 100644 --- a/core/sensors/environment.py +++ b/core/sensors/environment.py @@ -36,23 +36,23 @@ def wrap_angle_diff(angle1: float, angle2: float) -> float: """ Compute the smallest signed difference between two angles. - + Returns angle1 - angle2 wrapped to [-π, π]. This ensures the result is always the shortest angular distance. - + Args: angle1: First angle (radians). angle2: Second angle (radians). - + Returns: Signed difference angle1 - angle2 in range [-π, π]. Positive means angle1 is counter-clockwise from angle2. - + Example: >>> # 350° - 10° should give -20° (not +340°) >>> diff = wrap_angle_diff(np.deg2rad(350), np.deg2rad(10)) >>> print(f"{np.rad2deg(diff):.1f}°") # -20.0° - + >>> # 10° - 350° should give +20° (not -340°) >>> diff = wrap_angle_diff(np.deg2rad(10), np.deg2rad(350)) >>> print(f"{np.rad2deg(diff):.1f}°") # 20.0° @@ -74,7 +74,7 @@ def mag_tilt_compensate( Implements Eq. (6.52) in Chapter 6: M_x = m̃_x cos(θ) + m̃_z sin(θ) M_y = m̃_y cos(ϕ) + m̃_x sin(θ)sin(ϕ) - m̃_z cos(θ)sin(ϕ) - + where θ = pitch, ϕ = roll, and [m̃_x, m̃_y, m̃_z] = mag_b. The third (vertical) component is Mz = m̃_y sin(ϕ) - m̃_x sin(θ)cos(ϕ) + m̃_z cos(θ)cos(ϕ). @@ -118,7 +118,7 @@ def mag_tilt_compensate( >>> pitch = 0.0 >>> mag_comp = mag_tilt_compensate(mag, roll, pitch) >>> print(mag_comp) # [20, 0, -40] (unchanged when level) - >>> + >>> >>> # Tilted device: 30° pitch >>> pitch_30 = np.deg2rad(30) >>> mag_comp = mag_tilt_compensate(mag, 0.0, pitch_30) @@ -320,7 +320,7 @@ def pressure_to_altitude( >>> # At sea level (p = p0) >>> h = pressure_to_altitude(p=101325, p0=101325) >>> print(f"{h:.1f} m") # 0.0 m - >>> + >>> >>> # One floor up (~3m, pressure drops ~36 Pa) >>> p_floor1 = 101325 - 36 >>> h = pressure_to_altitude(p=p_floor1, p0=101325) @@ -388,7 +388,7 @@ def detect_floor_change( >>> # No significant change >>> change = detect_floor_change(10.0, 10.2, floor_height=3.0) >>> print(change) # 0 - >>> + >>> >>> # Went up one floor >>> change = detect_floor_change(10.0, 13.5, floor_height=3.0) >>> print(change) # +1 @@ -556,5 +556,3 @@ def compensate_hard_iron( mag_corrected = mag_raw - offset return mag_corrected - - diff --git a/core/sensors/gravity.py b/core/sensors/gravity.py index 5ff3c30..a70c6dd 100644 --- a/core/sensors/gravity.py +++ b/core/sensors/gravity.py @@ -31,23 +31,23 @@ def gravity_magnitude_eq6_8(lat_rad: float) -> float: """ Compute gravity magnitude using WGS-84 model (Book Eq. 6.8). - + Implements Eq. (6.8) from Chapter 6: g(φ) = 9.7803 * (1 + 0.0053024·sin²(φ) - 0.000005·sin²(2φ)) - + where φ is geodetic latitude in radians. - + This formula models gravity variation due to: 1. Earth's oblate spheroid shape (flattening at poles) 2. Centrifugal force from Earth's rotation (stronger at equator) 3. Mass distribution in Earth's interior - + Physical Interpretation: - Equator (φ=0°): g ≈ 9.780 m/s² (minimum, strongest centrifugal effect) - 45° latitude: g ≈ 9.806 m/s² (mid-range) - Poles (φ=±90°): g ≈ 9.832 m/s² (maximum, no centrifugal effect) - Total variation: ≈ 0.052 m/s² (≈0.5% of g) - + Args: lat_rad: Geodetic latitude in radians. Range: [-π/2, +π/2] (South Pole to North Pole). @@ -55,38 +55,38 @@ def gravity_magnitude_eq6_8(lat_rad: float) -> float: 0.0 rad = Equator (0°) π/4 rad = 45° North π/2 rad = North Pole (90°) - + Returns: Gravity magnitude g in m/s². Range: approximately [9.78, 9.84] m/s². - + Notes: - This is WGS-84 gravity formula (standard for GPS/GNSS) - Assumes sea level; altitude correction not included (Eq. 6.8 scope) - For indoor positioning: latitude variation is more significant than altitude variation (buildings rarely exceed ±100m → ±0.03 m/s² effect) - More accurate than constant g=9.81 m/s² for high-precision navigation - + Example: >>> import numpy as np >>> # Equator >>> g_equator = gravity_magnitude_eq6_8(0.0) >>> print(f"Equator: {g_equator:.4f} m/s²") # 9.7803 - >>> + >>> >>> # 45° North (typical mid-latitude) >>> g_45n = gravity_magnitude_eq6_8(np.deg2rad(45.0)) >>> print(f"45°N: {g_45n:.4f} m/s²") # ~9.8062 - >>> + >>> >>> # North Pole >>> g_pole = gravity_magnitude_eq6_8(np.pi / 2) >>> print(f"Pole: {g_pole:.4f} m/s²") # ~9.8322 - + Related Equations: - Eq. (6.8): Gravity magnitude (THIS FUNCTION) - Eq. (6.7): Velocity update (uses g from this function) - Eq. (6.47): PDR gravity removal (uses g from this function) - Eq. (6.44): ZUPT test statistic (uses g from this function) - + References: Chapter 6, Eq. (6.8): Gravity vector definition WGS-84 Earth Gravitational Model @@ -108,14 +108,14 @@ def gravity_magnitude( ) -> float: """ Compute gravity magnitude with automatic fallback. - + This is the recommended interface for all Chapter 6 algorithms. Provides backward compatibility while enabling book-accurate Eq. (6.8). - + Behavior: - If lat_rad is provided: Use Eq. (6.8) for latitude-dependent gravity - If lat_rad is None: Return default_g (backward compatible) - + Args: lat_rad: Geodetic latitude in radians (optional). If None, returns default_g. @@ -123,45 +123,45 @@ def gravity_magnitude( default_g: Fallback gravity magnitude when lat_rad is None. Default: 9.81 m/s² (standard gravity). Typical range: 9.78-9.84 m/s². - + Returns: Gravity magnitude in m/s². Either from Eq. (6.8) or default_g. - + Notes: - Use this function in production code for flexibility - Use gravity_magnitude_eq6_8() directly if latitude is always known - default_g allows matching legacy behavior or custom gravity values - + Example: >>> import numpy as np >>> # Without latitude (backward compatible) >>> g = gravity_magnitude() >>> print(g) # 9.81 (default) - >>> + >>> >>> # With latitude (book-accurate) >>> lat_deg = 40.0 # New York City latitude >>> lat_rad = np.deg2rad(lat_deg) >>> g = gravity_magnitude(lat_rad=lat_rad) >>> print(f"{g:.4f}") # ~9.8018 (from Eq. 6.8) - >>> + >>> >>> # Custom default (e.g., for specific location) >>> g_tokyo = gravity_magnitude(default_g=9.798) >>> print(g_tokyo) # 9.798 (custom) - + Usage in Chapter 6: >>> # Strapdown propagation >>> g_mag = gravity_magnitude(lat_rad, default_g=9.81) >>> g_vec = frame.gravity_vector(g_mag) - >>> + >>> >>> # IMU forward model >>> g_mag = gravity_magnitude(lat_rad, default_g=9.81) >>> accel_body, gyro_body = generate_imu_from_trajectory(..., g=g_mag) - >>> + >>> >>> # PDR step detection >>> g_mag = gravity_magnitude(lat_rad, default_g=9.81) >>> a_dynamic = a_mag - g_mag - + Related Functions: - gravity_magnitude_eq6_8(): Direct Eq. (6.8) implementation """ @@ -176,31 +176,30 @@ def gravity_magnitude( def gravity_magnitude_from_lat_deg(lat_deg: float) -> float: """ Convenience wrapper: Compute gravity from latitude in degrees. - + Automatically converts degrees to radians before calling Eq. (6.8). Useful for user-facing APIs where degrees are more intuitive. - + Args: lat_deg: Geodetic latitude in degrees. Range: [-90, +90] (South Pole to North Pole). Examples: 0 (Equator), 45 (mid-latitude), 90 (North Pole). - + Returns: Gravity magnitude in m/s² from Eq. (6.8). - + Example: >>> # Tokyo, Japan (35.6762° N) >>> g_tokyo = gravity_magnitude_from_lat_deg(35.6762) >>> print(f"Tokyo: {g_tokyo:.4f} m/s²") # ~9.7976 - >>> + >>> >>> # Singapore (1.3521° N, near equator) >>> g_singapore = gravity_magnitude_from_lat_deg(1.3521) >>> print(f"Singapore: {g_singapore:.4f} m/s²") # ~9.7804 - + Related Functions: - gravity_magnitude_eq6_8(): Takes radians (more explicit) - gravity_magnitude(): Main interface with fallback """ lat_rad = np.deg2rad(lat_deg) return gravity_magnitude_eq6_8(lat_rad) - diff --git a/core/sensors/imu_models.py b/core/sensors/imu_models.py index aa85edc..47d8f6f 100644 --- a/core/sensors/imu_models.py +++ b/core/sensors/imu_models.py @@ -313,8 +313,8 @@ def remove_gravity_component( # Broadcast gravity_body across batch dimension a_true = accel_body - gravity_body[np.newaxis, :] else: - raise ValueError(f"accel_body must have shape (3,) or (N, 3), got {accel_body.shape}") + raise ValueError( + f"accel_body must have shape (3,) or (N, 3), got {accel_body.shape}" + ) return a_true - - diff --git a/core/sensors/ins_ekf.py b/core/sensors/ins_ekf.py index 557be24..eabf852 100644 --- a/core/sensors/ins_ekf.py +++ b/core/sensors/ins_ekf.py @@ -7,7 +7,7 @@ State Vector (Eq. 6.16): x = [p (3), v (3), q (4), b_g (3), b_a (3)]^T Total: 16 states - + Where: p: Position in map frame [px, py, pz] (m) v: Velocity in map frame [vx, vy, vz] (m/s) @@ -19,7 +19,7 @@ Prediction: x_k|k-1 = f(x_k-1, u_k-1) # Strapdown mechanization P_k|k-1 = F * P_k-1 * F^T + Q # Covariance propagation - + Update: S_k = H * P_k|k-1 * H^T + R # Innovation covariance (Eq. 6.40) K_k = P_k|k-1 * H^T * S_k^(-1) # Kalman gain (Eq. 6.41) @@ -52,9 +52,9 @@ def quat_normalize(q: np.ndarray) -> np.ndarray: class INSState: """ INS state vector for EKF-based navigation. - + State ordering follows Eq. (6.16): x = [p, v, q, b_g, b_a]^T - + Attributes: p: Position in map frame (m), shape (3,). v: Velocity in map frame (m/s), shape (3,). @@ -62,7 +62,7 @@ class INSState: b_g: Gyroscope bias in body frame (rad/s), shape (3,). b_a: Accelerometer bias in body frame (m/s²), shape (3,). P: State covariance matrix, shape (16, 16). - + Note: The state vector is 16-dimensional (not 13 or 15): - Position: 3 elements (indices 0:3) @@ -71,6 +71,7 @@ class INSState: - Gyro bias: 3 elements (indices 10:13) - Accel bias: 3 elements (indices 13:16) """ + p: np.ndarray # (3,) v: np.ndarray # (3,) q: np.ndarray # (4,) @@ -81,7 +82,7 @@ class INSState: def to_vector(self) -> np.ndarray: """ Convert state to vector form following Eq. (6.16). - + Returns: State vector x = [p, v, q, b_g, b_a]^T, shape (16,). """ @@ -91,11 +92,11 @@ def to_vector(self) -> np.ndarray: def from_vector(cls, x: np.ndarray, P: np.ndarray) -> "INSState": """ Create state from vector form following Eq. (6.16). - + Args: x: State vector [p, v, q, b_g, b_a]^T, shape (16,). P: Covariance matrix, shape (16, 16). - + Returns: INSState instance. """ @@ -104,22 +105,15 @@ def from_vector(cls, x: np.ndarray, P: np.ndarray) -> "INSState": if P.shape != (16, 16): raise ValueError(f"Covariance must be (16, 16), got {P.shape}") - return cls( - p=x[0:3], - v=x[3:6], - q=x[6:10], - b_g=x[10:13], - b_a=x[13:16], - P=P - ) + return cls(p=x[0:3], v=x[3:6], q=x[6:10], b_g=x[10:13], b_a=x[13:16], P=P) class ZUPT_EKF: """ Extended Kalman Filter for ZUPT-aided INS. - + Implements Equations 6.40-6.43 for Kalman update and Eq. 6.45 for ZUPT. - + Args: frame: Frame convention (ENU or NED). imu_params: IMU noise parameters. @@ -153,17 +147,17 @@ def initialize( p0: np.ndarray, v0: np.ndarray, q0: np.ndarray, - P0: Optional[np.ndarray] = None + P0: Optional[np.ndarray] = None, ) -> INSState: """ Initialize EKF state following Eq. (6.16) ordering. - + Args: p0: Initial position, shape (3,). v0: Initial velocity, shape (3,). q0: Initial quaternion, shape (4,). P0: Initial covariance, shape (16, 16). If None, uses default. - + Returns: Initial INSState. """ @@ -188,21 +182,17 @@ def initialize( return INSState(p=p0, v=v0, q=q0, b_g=b_g0, b_a=b_a0, P=P0) def predict( - self, - state: INSState, - gyro_meas: np.ndarray, - accel_meas: np.ndarray, - dt: float + self, state: INSState, gyro_meas: np.ndarray, accel_meas: np.ndarray, dt: float ) -> INSState: """ EKF prediction step (strapdown mechanization + covariance propagation). - + Args: state: Current INSState. gyro_meas: Measured angular velocity (rad/s), shape (3,). accel_meas: Measured specific force (m/s²), shape (3,). dt: Time step (s). - + Returns: Predicted INSState. """ @@ -212,9 +202,15 @@ def predict( # Strapdown propagation (nominal state) q_new, v_new, p_new = strapdown_update( - state.q, state.v, state.p, - gyro_corrected, accel_corrected, - dt, g=self.g, frame=self.frame, lat_rad=self.lat_rad + state.q, + state.v, + state.p, + gyro_corrected, + accel_corrected, + dt, + g=self.g, + frame=self.frame, + lat_rad=self.lat_rad, ) # Normalize quaternion @@ -234,22 +230,22 @@ def predict( def compute_process_noise(self, dt: float) -> np.ndarray: """ Compute process noise covariance Q for discrete time step. - + This is a simplified model. A full implementation would compute Q from continuous-time noise and integrate over dt. - + State ordering: [p, v, q, b_g, b_a] - + Args: dt: Time step (s). - + Returns: Process noise covariance Q, shape (16, 16). """ Q = np.zeros((16, 16)) # Position noise (indices 0:3, integrated from velocity noise) - v_noise = (self.imu_params.accel_vrw_mps_sqrt_s * np.sqrt(dt))**2 + v_noise = (self.imu_params.accel_vrw_mps_sqrt_s * np.sqrt(dt)) ** 2 p_noise = v_noise * dt**2 Q[0:3, 0:3] = np.eye(3) * p_noise @@ -257,15 +253,15 @@ def compute_process_noise(self, dt: float) -> np.ndarray: Q[3:6, 3:6] = np.eye(3) * v_noise * dt # Attitude noise (indices 6:10, from gyro ARW) - q_noise = (self.imu_params.gyro_arw_rad_sqrt_s * np.sqrt(dt))**2 + q_noise = (self.imu_params.gyro_arw_rad_sqrt_s * np.sqrt(dt)) ** 2 Q[6:10, 6:10] = np.eye(4) * q_noise * 0.1 # Scale down for quaternion # Gyro bias random walk (indices 10:13, very slow drift) - bg_noise = (self.imu_params.gyro_bias_rad_s * 0.01 * np.sqrt(dt))**2 + bg_noise = (self.imu_params.gyro_bias_rad_s * 0.01 * np.sqrt(dt)) ** 2 Q[10:13, 10:13] = np.eye(3) * bg_noise # Accel bias random walk (indices 13:16, very slow drift) - ba_noise = (self.imu_params.accel_bias_mps2 * 0.01 * np.sqrt(dt))**2 + ba_noise = (self.imu_params.accel_bias_mps2 * 0.01 * np.sqrt(dt)) ** 2 Q[13:16, 13:16] = np.eye(3) * ba_noise return Q @@ -273,17 +269,17 @@ def compute_process_noise(self, dt: float) -> np.ndarray: def update_zupt(self, state: INSState) -> INSState: """ EKF update step with ZUPT measurement (Eqs. 6.40-6.43, 6.45). - + Applies zero-velocity pseudo-measurement: z_k = [0, 0, 0]^T (measured velocity) h(x) = v (predicted velocity from state) H = [0_3x3, I_3, 0_3x4, 0_3x3, 0_3x3] (Jacobian, Eq. 6.45) - + State ordering (Eq. 6.16): x = [p (3), v (3), q (4), b_g (3), b_a (3)] - + Args: state: Predicted INSState. - + Returns: Updated INSState. """ @@ -328,4 +324,3 @@ def update_zupt(self, state: INSState) -> INSState: state_new.q = quat_normalize(state_new.q) return state_new - diff --git a/core/sensors/pdr.py b/core/sensors/pdr.py index 3a0ba6f..31bbdea 100644 --- a/core/sensors/pdr.py +++ b/core/sensors/pdr.py @@ -73,7 +73,7 @@ def total_accel_magnitude(accel_b: np.ndarray) -> float: >>> accel = np.array([0.0, 0.0, -9.81]) >>> mag = total_accel_magnitude(accel) >>> print(f"{mag:.2f}") # 9.81 - >>> + >>> >>> # Walking: includes motion + gravity >>> accel = np.array([1.5, 0.5, -10.2]) >>> mag = total_accel_magnitude(accel) @@ -137,7 +137,7 @@ def remove_gravity_from_magnitude( >>> a_mag_stationary = 9.81 >>> a_dyn = remove_gravity_from_magnitude(a_mag_stationary) >>> print(f"{a_dyn:.2f}") # 0.00 - >>> + >>> >>> # Walking (peak acceleration) >>> a_mag_walking = 12.0 >>> a_dyn = remove_gravity_from_magnitude(a_mag_walking) @@ -168,15 +168,15 @@ def detect_steps_peak_detector( ) -> Tuple[np.ndarray, np.ndarray]: """ Detect steps using peak detection on gravity-removed acceleration magnitude. - + Implements the book's approach (Eqs. 6.46-6.47): 1. Compute total acceleration magnitude (Eq. 6.46) 2. Remove gravity (Eq. 6.47) 3. Optionally apply low-pass filter 4. Find peaks with minimum height and distance constraints - + This is the proper peak detection method described in Chapter 6, Section 6.3.2. - + Args: accel_series: Accelerometer time series in body frame. Shape: (N, 3). Units: m/s². @@ -196,7 +196,7 @@ def detect_steps_peak_detector( lat_rad: Geodetic latitude in radians (optional). If provided, uses Eq. (6.8) for gravity magnitude. If None, uses g parameter (backward compatible). - + Returns: Tuple of (step_indices, accel_mag_filtered): step_indices: Indices of detected steps in accel_series. @@ -204,7 +204,7 @@ def detect_steps_peak_detector( accel_mag_filtered: Processed acceleration magnitude time series. Shape: (N,). Units: m/s². After gravity removal and optional filtering. - + Notes: - Follows Eq. (6.46): Compute ||a|| = sqrt(ax² + ay² + az²) - Follows Eq. (6.47): Remove gravity: a_dynamic = ||a|| - g @@ -212,7 +212,7 @@ def detect_steps_peak_detector( - min_peak_distance prevents detecting same step multiple times - Low-pass filter reduces high-frequency noise from sensors - Peaks correspond to foot strikes or hand swings (device-dependent) - + Example: >>> import numpy as np >>> # Synthetic walking: 60s at 2 Hz step frequency @@ -220,18 +220,18 @@ def detect_steps_peak_detector( >>> # Simulate vertical acceleration with steps >>> accel_z = -9.81 + 2.0 * np.sin(2 * np.pi * 2.0 * t) # 2 Hz steps >>> accel = np.column_stack([np.zeros_like(t), np.zeros_like(t), accel_z]) - >>> + >>> >>> step_indices, accel_processed = detect_steps_peak_detector( ... accel, dt=0.01, min_peak_height=0.5, min_peak_distance=0.4 ... ) >>> print(f"Detected {len(step_indices)} steps in 60s") >>> print(f"Expected ~120 steps (2 steps/s * 60s)") - + Related Equations: - Eq. (6.46): Total acceleration magnitude - Eq. (6.47): Gravity removal - Eq. (6.48): Step frequency (computed from detected peaks) - + References: Chapter 6, Section 6.3.2: Pedestrian Dead Reckoning Figure 6.12: Total accelerations during walking (shows peak pattern) @@ -245,7 +245,6 @@ def detect_steps_peak_detector( if min_peak_distance <= 0: raise ValueError(f"min_peak_distance must be positive, got {min_peak_distance}") - # Step 1: Compute total acceleration magnitude (Eq. 6.46) # a_mag[k] = ||a_k|| = sqrt(ax² + ay² + az²) accel_mag = np.linalg.norm(accel_series, axis=1) # Shape: (N,) @@ -269,7 +268,7 @@ def detect_steps_peak_detector( accel_filtered = accel_dynamic else: # Apply 4th order Butterworth filter - b, a = signal.butter(4, normalized_cutoff, btype='low') + b, a = signal.butter(4, normalized_cutoff, btype="low") accel_filtered = signal.filtfilt(b, a, accel_dynamic) else: accel_filtered = accel_dynamic @@ -323,7 +322,7 @@ def step_frequency(delta_t: float) -> float: >>> dt = 0.5 >>> freq = step_frequency(dt) >>> print(f"{freq:.2f} Hz") # 2.00 Hz (120 steps/min) - >>> + >>> >>> # Slow walking: 0.7 s between steps >>> dt = 0.7 >>> freq = step_frequency(dt) @@ -480,7 +479,7 @@ def step_length_book_eq6_49( - Eq. (6.48): Step frequency (SF) - Eq. (6.49): Step length (THIS FUNCTION) - Eq. (6.50): Position update (uses L) - + Author: Li-Ta Hsu Date: December 2025 """ @@ -551,14 +550,14 @@ def step_length_weinberg( Example: >>> # Get step segments from detector >>> step_indices, accel_filtered = detect_steps_peak_detector(accel, dt=0.01) - >>> + >>> >>> # Calibrate gain (assuming known distance) >>> ptp_per_step = [] >>> for i in range(len(step_indices)-1): >>> seg = accel_filtered[step_indices[i]:step_indices[i+1]] >>> ptp_per_step.append(np.ptp(seg)) >>> G_w = calibrate_weinberg_gain(np.array(ptp_per_step), distance_m=50.0) - >>> + >>> >>> # Compute step length per step >>> for i in range(len(step_indices)-1): >>> seg = accel_filtered[step_indices[i]:step_indices[i+1]] @@ -568,11 +567,11 @@ def step_length_weinberg( Related Functions: - `calibrate_weinberg_gain()`: Calibrate G_w from known distance - `detect_steps_peak_detector()`: Provides step windows and accel_filtered - + References: Weinberg, H. (2002). "Using the ADXL202 in Pedometer and Personal Navigation Applications." Analog Devices AN-602 Application Note. - + Author: Li-Ta Hsu Date: December 2025 """ @@ -586,7 +585,7 @@ def step_length_weinberg( ptp = max(ptp, eps) # Apply floor for numerical stability # Weinberg formula: SL = G_w * ptp^(1/4) - SL = G_w * (ptp ** power) + SL = G_w * (ptp**power) return SL @@ -639,7 +638,7 @@ def calibrate_weinberg_gain( Related Functions: - `step_length_weinberg()`: Uses calibrated G_w - `detect_steps_peak_detector()`: Provides accel_filtered for ptp computation - + Author: Li-Ta Hsu Date: December 2025 """ @@ -650,7 +649,7 @@ def calibrate_weinberg_gain( ptp = np.maximum(ptp_per_step.astype(float), eps) # Compute denominator: sum of ptp^power - denom = float(np.sum(ptp ** power)) + denom = float(np.sum(ptp**power)) if denom <= 0: raise ValueError( @@ -713,13 +712,13 @@ def pdr_step_update( >>> import numpy as np >>> # Start at origin >>> p0 = np.array([0.0, 0.0]) - >>> + >>> >>> # Take a step north (heading = π/2 = 90°) >>> L = 0.7 # m >>> heading = np.pi / 2 # north >>> p1 = pdr_step_update(p0, L, heading) >>> print(p1) # [0.0, 0.7] (moved 0.7m north) - >>> + >>> >>> # Take another step northeast (heading = π/4 = 45°) >>> heading_ne = np.pi / 4 >>> p2 = pdr_step_update(p1, L, heading_ne) @@ -784,7 +783,7 @@ def detect_step_simple( >>> mag_stationary = np.ones(20) * 9.81 >>> step = detect_step_simple(mag_stationary, threshold=11.0) >>> print(step) # False - >>> + >>> >>> # Motion with step (peak at 12 m/s²) >>> mag_with_step = np.concatenate([ >>> np.ones(10) * 9.8, @@ -884,5 +883,3 @@ def wrap_heading(heading_rad: float) -> float: # Wrap to [-π, π] wrapped = np.arctan2(np.sin(heading_rad), np.cos(heading_rad)) return wrapped - - diff --git a/core/sensors/strapdown.py b/core/sensors/strapdown.py index ceecd88..e6b94bd 100644 --- a/core/sensors/strapdown.py +++ b/core/sensors/strapdown.py @@ -272,16 +272,16 @@ def gravity_vector( Computes gravity direction from frame convention and magnitude from either: - Eq. (6.8) when latitude provided (latitude-dependent WGS-84 model) - Default g parameter when latitude not provided (backward compatible) - + NOTATION NOTE: - The book writes g^M = [0, 0, g]^T in Eq. (6.7) context, which can be + The book writes g^M = [0, 0, g]^T in Eq. (6.7) context, which can be ambiguous. We interpret this as the MAGNITUDE in the z-direction. - + Book's convention: g^M = [0, 0, +g] (upward) used with SUBTRACTION Code's convention: g_M = [0, 0, -g] (downward) used with ADDITION - + These are equivalent: (a_B - g_book) = (f_B + g_code) where g_book = -g_code - + PHYSICAL MEANING: This function returns the actual gravitational acceleration vector: - ENU: [0, 0, -g_mag] m/s² (gravity pulls downward = negative z) @@ -319,7 +319,7 @@ def gravity_vector( >>> frame_enu = FrameConvention.create_enu() >>> g_enu = gravity_vector(g=9.81, frame=frame_enu) >>> print(g_enu) # [0, 0, -9.81] - >>> + >>> >>> # ENU frame, latitude-dependent gravity (book-accurate) >>> lat_rad = np.deg2rad(45.0) # 45° North >>> g_enu_lat = gravity_vector(g=9.81, frame=frame_enu, lat_rad=lat_rad) @@ -353,24 +353,24 @@ def vel_update( Velocity update with gravity compensation (Eq. 6.7). Implements Eq. (6.7) in Chapter 6 using standard specific force convention. - + CODE FORMULATION (what this function implements): v_k^M = v_{k-1}^M + (C_B^M(q) @ f_b + g_M) * Δt - + BOOK'S EQ. (6.7) FORMULATION: v_k^M = v_{k-1}^M + (C_B^M(q) @ a_B - g_M_book) * Δt - + ALGEBRAIC EQUIVALENCE: These are identical! The difference is notation: - f_b (code) = a_B (book) = specific force (accelerometer reading) - g_M (code) = -g_M_book - + For ENU: g_M (code) = [0, 0, -9.81] (physical gravity vector, downward) g_M (book) = [0, 0, +9.81] (magnitude to subtract, upward) - + Proof: C @ a_B - [0,0,+g] = C @ a_B + [0,0,-g] = C @ f_b + g_M ✓ - + PHYSICAL MEANING: - Accelerometer measures specific force f_b (reaction force, NOT gravity) - For stationary in ENU: f_b = [0, 0, +9.81] (upward reaction from ground) @@ -611,5 +611,3 @@ def strapdown_update( p_next = pos_update(p, v_next, dt) return q_next, v_next, p_next - - diff --git a/core/sensors/types.py b/core/sensors/types.py index b20fef7..c57dd1e 100644 --- a/core/sensors/types.py +++ b/core/sensors/types.py @@ -98,32 +98,32 @@ class FrameConvention: - Eq. (6.52)-(6.53): Magnetometer heading (must match frame) """ - map_frame: Literal['ENU', 'NED'] = 'ENU' - map_axes: tuple[str, str, str] = ('x=East', 'y=North', 'z=Up') + map_frame: Literal["ENU", "NED"] = "ENU" + map_axes: tuple[str, str, str] = ("x=East", "y=North", "z=Up") gravity_direction: Literal[-1, +1] = -1 - heading_zero_direction: Literal['East', 'North'] = 'East' - heading_increases_towards: Literal['North', 'East'] = 'North' - quaternion_convention: Literal['scalar_first', 'scalar_last'] = 'scalar_first' - quaternion_meaning: Literal['body_to_map', 'map_to_body'] = 'body_to_map' + heading_zero_direction: Literal["East", "North"] = "East" + heading_increases_towards: Literal["North", "East"] = "North" + quaternion_convention: Literal["scalar_first", "scalar_last"] = "scalar_first" + quaternion_meaning: Literal["body_to_map", "map_to_body"] = "body_to_map" body_frame_axes: tuple[str, str, str] = ( - 'x=forward', - 'y=left', - 'z=up', + "x=forward", + "y=left", + "z=up", ) def __post_init__(self) -> None: """Validate frame convention consistency.""" # Validate map frame matches axes and gravity - if self.map_frame == 'ENU': - expected_axes = ('x=East', 'y=North', 'z=Up') + if self.map_frame == "ENU": + expected_axes = ("x=East", "y=North", "z=Up") expected_gravity = -1 - expected_heading_zero = 'East' - expected_heading_increases = 'North' - elif self.map_frame == 'NED': - expected_axes = ('x=North', 'y=East', 'z=Down') + expected_heading_zero = "East" + expected_heading_increases = "North" + elif self.map_frame == "NED": + expected_axes = ("x=North", "y=East", "z=Down") expected_gravity = +1 - expected_heading_zero = 'North' - expected_heading_increases = 'East' + expected_heading_zero = "North" + expected_heading_increases = "East" else: raise ValueError( f"map_frame must be 'ENU' or 'NED', got '{self.map_frame}'" @@ -171,11 +171,11 @@ def create_enu(cls) -> "FrameConvention": >>> print(frame.heading_zero_direction) # 'East' """ return cls( - map_frame='ENU', - map_axes=('x=East', 'y=North', 'z=Up'), + map_frame="ENU", + map_axes=("x=East", "y=North", "z=Up"), gravity_direction=-1, - heading_zero_direction='East', - heading_increases_towards='North', + heading_zero_direction="East", + heading_increases_towards="North", ) @classmethod @@ -195,11 +195,11 @@ def create_ned(cls) -> "FrameConvention": >>> print(frame.gravity_direction) # +1 """ return cls( - map_frame='NED', - map_axes=('x=North', 'y=East', 'z=Down'), + map_frame="NED", + map_axes=("x=North", "y=East", "z=Down"), gravity_direction=+1, - heading_zero_direction='North', - heading_increases_towards='East', + heading_zero_direction="North", + heading_increases_towards="East", ) def gravity_vector(self, g_mag: float = 9.81) -> np.ndarray: @@ -300,13 +300,13 @@ def unit_vector_to_heading(self, v: np.ndarray) -> float: class IMUNoiseParams: """ IMU noise and bias parameters with explicit units in field names. - + This dataclass eliminates ambiguity in IMU specification by making units explicit in every field name. All values are stored in SI units (rad/s, m/s²) with the original specification units documented. - + Key principle: ALWAYS use explicit unit names to prevent deg/hr vs deg/s bugs! - + Attributes: gyro_bias_rad_s: Gyroscope bias instability (rad/s). Spec sheet unit: deg/hr. @@ -328,7 +328,7 @@ class IMUNoiseParams: Typical values: 0.001-0.1 m/s/√hr. grade: IMU grade ('consumer', 'tactical', 'navigation'). For documentation purposes only. - + Example: >>> from core.sensors.units import ( ... deg_per_hour_to_rad_per_sec, @@ -336,7 +336,7 @@ class IMUNoiseParams: ... mg_to_mps2, ... mps_per_sqrt_hour_to_mps_per_sqrt_sec ... ) - >>> + >>> >>> # Consumer-grade IMU (explicit unit conversions) >>> params = IMUNoiseParams( ... gyro_bias_rad_s=deg_per_hour_to_rad_per_sec(10.0), @@ -348,13 +348,13 @@ class IMUNoiseParams: ... ) >>> print(f"Gyro bias: {params.gyro_bias_rad_s:.6e} rad/s") Gyro bias: 4.848137e-05 rad/s - + Related Equations: - Eq. (6.5): Gyro measurement model (b_G term) - Eq. (6.6): Gyro bias correction - Eq. (6.9): Accelerometer measurement model (b_A term) - Eqs. (6.56)-(6.58): Allan variance analysis - + Notes: - Use core.sensors.units module for ALL conversions - Print diagnostics should use units.format_* functions @@ -366,18 +366,18 @@ class IMUNoiseParams: gyro_rrw_rad_s_sqrt_s: float accel_bias_mps2: float accel_vrw_mps_sqrt_s: float - grade: str = 'unknown' + grade: str = "unknown" @classmethod def consumer_grade(cls) -> "IMUNoiseParams": """ Create typical consumer-grade IMU noise parameters. - + Based on typical smartphone/tablet IMU specifications. - + Returns: Consumer-grade IMU parameters. - + Example: >>> params = IMUNoiseParams.consumer_grade() >>> print(params.grade) # 'consumer' @@ -391,20 +391,24 @@ def consumer_grade(cls) -> "IMUNoiseParams": return cls( gyro_bias_rad_s=deg_per_hour_to_rad_per_sec(10.0), # 10 deg/hr - gyro_arw_rad_sqrt_s=deg_per_sqrt_hour_to_rad_per_sqrt_sec(0.1), # 0.1 deg/√hr + gyro_arw_rad_sqrt_s=deg_per_sqrt_hour_to_rad_per_sqrt_sec( + 0.1 + ), # 0.1 deg/√hr gyro_rrw_rad_s_sqrt_s=0.0, accel_bias_mps2=mg_to_mps2(10.0), # 10 mg - accel_vrw_mps_sqrt_s=mps_per_sqrt_hour_to_mps_per_sqrt_sec(0.01), # 0.01 m/s/√hr - grade='consumer', + accel_vrw_mps_sqrt_s=mps_per_sqrt_hour_to_mps_per_sqrt_sec( + 0.01 + ), # 0.01 m/s/√hr + grade="consumer", ) @classmethod def tactical_grade(cls) -> "IMUNoiseParams": """ Create typical tactical-grade IMU noise parameters. - + Based on mid-range fiber optic gyro (FOG) or MEMS specifications. - + Returns: Tactical-grade IMU parameters. """ @@ -417,20 +421,24 @@ def tactical_grade(cls) -> "IMUNoiseParams": return cls( gyro_bias_rad_s=deg_per_hour_to_rad_per_sec(1.0), # 1 deg/hr - gyro_arw_rad_sqrt_s=deg_per_sqrt_hour_to_rad_per_sqrt_sec(0.01), # 0.01 deg/√hr + gyro_arw_rad_sqrt_s=deg_per_sqrt_hour_to_rad_per_sqrt_sec( + 0.01 + ), # 0.01 deg/√hr gyro_rrw_rad_s_sqrt_s=0.0, accel_bias_mps2=mg_to_mps2(1.0), # 1 mg - accel_vrw_mps_sqrt_s=mps_per_sqrt_hour_to_mps_per_sqrt_sec(0.001), # 0.001 m/s/√hr - grade='tactical', + accel_vrw_mps_sqrt_s=mps_per_sqrt_hour_to_mps_per_sqrt_sec( + 0.001 + ), # 0.001 m/s/√hr + grade="tactical", ) @classmethod def navigation_grade(cls) -> "IMUNoiseParams": """ Create typical navigation-grade IMU noise parameters. - + Based on ring laser gyro (RLG) specifications. - + Returns: Navigation-grade IMU parameters. """ @@ -443,20 +451,24 @@ def navigation_grade(cls) -> "IMUNoiseParams": return cls( gyro_bias_rad_s=deg_per_hour_to_rad_per_sec(0.01), # 0.01 deg/hr - gyro_arw_rad_sqrt_s=deg_per_sqrt_hour_to_rad_per_sqrt_sec(0.001), # 0.001 deg/√hr + gyro_arw_rad_sqrt_s=deg_per_sqrt_hour_to_rad_per_sqrt_sec( + 0.001 + ), # 0.001 deg/√hr gyro_rrw_rad_s_sqrt_s=0.0, accel_bias_mps2=mg_to_mps2(0.1), # 0.1 mg - accel_vrw_mps_sqrt_s=mps_per_sqrt_hour_to_mps_per_sqrt_sec(0.0001), # 0.0001 m/s/√hr - grade='navigation', + accel_vrw_mps_sqrt_s=mps_per_sqrt_hour_to_mps_per_sqrt_sec( + 0.0001 + ), # 0.0001 m/s/√hr + grade="navigation", ) def format_specs(self) -> str: """ Format IMU specifications for human-readable display. - + Returns: Multi-line formatted string with all parameters. - + Example: >>> params = IMUNoiseParams.consumer_grade() >>> print(params.format_specs()) @@ -524,9 +536,7 @@ def __post_init__(self) -> None: """Validate shape consistency of IMU data.""" # Validate t is 1D if self.t.ndim != 1: - raise ValueError( - f"ImuSeries.t must be 1D array, got shape {self.t.shape}" - ) + raise ValueError(f"ImuSeries.t must be 1D array, got shape {self.t.shape}") n_samples = self.t.shape[0] @@ -747,19 +757,13 @@ class NavStateQPVP: def __post_init__(self) -> None: """Validate shape and basic consistency of navigation state.""" if self.q.shape != (4,): - raise ValueError( - f"NavStateQPVP.q must have shape (4,), got {self.q.shape}" - ) + raise ValueError(f"NavStateQPVP.q must have shape (4,), got {self.q.shape}") if self.v.shape != (3,): - raise ValueError( - f"NavStateQPVP.v must have shape (3,), got {self.v.shape}" - ) + raise ValueError(f"NavStateQPVP.v must have shape (3,), got {self.v.shape}") if self.p.shape != (3,): - raise ValueError( - f"NavStateQPVP.p must have shape (3,), got {self.p.shape}" - ) + raise ValueError(f"NavStateQPVP.p must have shape (3,), got {self.p.shape}") # Warn if quaternion is not normalized (tolerance 1e-3) q_norm = np.linalg.norm(self.q) @@ -857,5 +861,3 @@ def __post_init__(self) -> None: f"(||q|| = {q_norm:.6f}). Consider normalizing.", UserWarning, ) - - diff --git a/core/sensors/units.py b/core/sensors/units.py index 9989a03..486b230 100644 --- a/core/sensors/units.py +++ b/core/sensors/units.py @@ -28,18 +28,19 @@ # Gyroscope Unit Conversions # ============================================================================ + def deg_per_hour_to_rad_per_sec(deg_per_hr: Numeric) -> Numeric: """ Convert gyroscope bias from deg/hr to rad/s. - + This is the standard conversion for gyro bias instability. - + Args: deg_per_hr: Bias in degrees per hour. - + Returns: Bias in radians per second. - + Example: >>> bias_deg_hr = 10.0 # 10 deg/hr (consumer grade) >>> bias_rad_s = deg_per_hour_to_rad_per_sec(bias_deg_hr) @@ -54,10 +55,10 @@ def deg_per_hour_to_rad_per_sec(deg_per_hr: Numeric) -> Numeric: def deg_per_sec_to_rad_per_sec(deg_per_s: Numeric) -> Numeric: """ Convert angular velocity from deg/s to rad/s. - + Args: deg_per_s: Angular velocity in degrees per second. - + Returns: Angular velocity in radians per second. """ @@ -67,16 +68,16 @@ def deg_per_sec_to_rad_per_sec(deg_per_s: Numeric) -> Numeric: def deg_per_sqrt_hour_to_rad_per_sqrt_sec(deg_per_sqrt_hr: Numeric) -> Numeric: """ Convert gyroscope Angular Random Walk (ARW) from deg/√hr to rad/√s. - + ARW is the angle random walk coefficient, measured in deg/√hr in datasheets. To use it in simulation/analysis, convert to rad/√s. - + Args: deg_per_sqrt_hr: ARW in degrees per square root hour. - + Returns: ARW in radians per square root second. - + Example: >>> arw_deg_sqrt_hr = 0.1 # 0.1 deg/√hr (consumer grade) >>> arw_rad_sqrt_s = deg_per_sqrt_hour_to_rad_per_sqrt_sec(arw_deg_sqrt_hr) @@ -89,10 +90,10 @@ def deg_per_sqrt_hour_to_rad_per_sqrt_sec(deg_per_sqrt_hr: Numeric) -> Numeric: def rad_per_hour_to_rad_per_sec(rad_per_hr: Numeric) -> Numeric: """ Convert angular rate from rad/hr to rad/s. - + Args: rad_per_hr: Angular rate in radians per hour. - + Returns: Angular rate in radians per second. """ @@ -103,18 +104,19 @@ def rad_per_hour_to_rad_per_sec(rad_per_hr: Numeric) -> Numeric: # Accelerometer Unit Conversions # ============================================================================ + def mg_to_mps2(mg: Numeric) -> Numeric: """ Convert acceleration from milligravity (mg) to m/s². - + 1 mg = 0.001 * 9.80665 m/s² (standard gravity). - + Args: mg: Acceleration in milligravity. - + Returns: Acceleration in m/s². - + Example: >>> bias_mg = 10.0 # 10 mg (consumer grade) >>> bias_mps2 = mg_to_mps2(bias_mg) @@ -128,12 +130,12 @@ def mg_to_mps2(mg: Numeric) -> Numeric: def ug_to_mps2(ug: Numeric) -> Numeric: """ Convert acceleration from microgravity (µg) to m/s². - + 1 µg = 0.000001 * 9.80665 m/s². - + Args: ug: Acceleration in microgravity. - + Returns: Acceleration in m/s². """ @@ -144,15 +146,15 @@ def ug_to_mps2(ug: Numeric) -> Numeric: def mps_per_sqrt_hour_to_mps_per_sqrt_sec(mps_per_sqrt_hr: Numeric) -> Numeric: """ Convert accelerometer Velocity Random Walk (VRW) from m/s/√hr to m/s/√s. - + VRW is the velocity random walk coefficient. - + Args: mps_per_sqrt_hr: VRW in (m/s) per square root hour. - + Returns: VRW in (m/s) per square root second. - + Example: >>> vrw_mps_sqrt_hr = 0.01 # 0.01 m/s/√hr >>> vrw_mps_sqrt_s = mps_per_sqrt_hour_to_mps_per_sqrt_sec(vrw_mps_sqrt_hr) @@ -166,15 +168,16 @@ def mps_per_sqrt_hour_to_mps_per_sqrt_sec(mps_per_sqrt_hr: Numeric) -> Numeric: # Reverse Conversions (for display/diagnostics) # ============================================================================ + def rad_per_sec_to_deg_per_hour(rad_per_s: Numeric) -> Numeric: """ Convert gyroscope bias from rad/s to deg/hr. - + Reverse of deg_per_hour_to_rad_per_sec, useful for display. - + Args: rad_per_s: Bias in radians per second. - + Returns: Bias in degrees per hour. """ @@ -184,10 +187,10 @@ def rad_per_sec_to_deg_per_hour(rad_per_s: Numeric) -> Numeric: def rad_per_sec_to_deg_per_sec(rad_per_s: Numeric) -> Numeric: """ Convert angular velocity from rad/s to deg/s. - + Args: rad_per_s: Angular velocity in radians per second. - + Returns: Angular velocity in degrees per second. """ @@ -197,12 +200,12 @@ def rad_per_sec_to_deg_per_sec(rad_per_s: Numeric) -> Numeric: def rad_per_sqrt_sec_to_deg_per_sqrt_hour(rad_per_sqrt_s: Numeric) -> Numeric: """ Convert gyroscope ARW from rad/√s to deg/√hr. - + Reverse of deg_per_sqrt_hour_to_rad_per_sqrt_sec, useful for display. - + Args: rad_per_sqrt_s: ARW in radians per square root second. - + Returns: ARW in degrees per square root hour. """ @@ -212,12 +215,12 @@ def rad_per_sqrt_sec_to_deg_per_sqrt_hour(rad_per_sqrt_s: Numeric) -> Numeric: def mps2_to_mg(mps2: Numeric) -> Numeric: """ Convert acceleration from m/s² to milligravity (mg). - + Reverse of mg_to_mps2, useful for display. - + Args: mps2: Acceleration in m/s². - + Returns: Acceleration in milligravity. """ @@ -228,12 +231,12 @@ def mps2_to_mg(mps2: Numeric) -> Numeric: def mps_per_sqrt_sec_to_mps_per_sqrt_hour(mps_per_sqrt_s: Numeric) -> Numeric: """ Convert accelerometer VRW from m/s/√s to m/s/√hr. - + Reverse of mps_per_sqrt_hour_to_mps_per_sqrt_sec, useful for display. - + Args: mps_per_sqrt_s: VRW in (m/s) per square root second. - + Returns: VRW in (m/s) per square root hour. """ @@ -244,15 +247,16 @@ def mps_per_sqrt_sec_to_mps_per_sqrt_hour(mps_per_sqrt_s: Numeric) -> Numeric: # Noise PSD Conversions (for Allan Variance analysis) # ============================================================================ + def arw_to_gyro_noise_psd(arw_rad_sqrt_s: Numeric) -> Numeric: """ Convert ARW to gyro noise power spectral density (PSD). - + PSD_gyro = ARW² (rad²/s) - + Args: arw_rad_sqrt_s: Angular Random Walk in rad/√s. - + Returns: Noise PSD in rad²/s. """ @@ -262,12 +266,12 @@ def arw_to_gyro_noise_psd(arw_rad_sqrt_s: Numeric) -> Numeric: def vrw_to_accel_noise_psd(vrw_mps_sqrt_s: Numeric) -> Numeric: """ Convert VRW to accelerometer noise power spectral density (PSD). - + PSD_accel = VRW² ((m/s)²/s) = m²/s³ - + Args: vrw_mps_sqrt_s: Velocity Random Walk in m/s/√s. - + Returns: Noise PSD in m²/s³. """ @@ -278,16 +282,17 @@ def vrw_to_accel_noise_psd(vrw_mps_sqrt_s: Numeric) -> Numeric: # Utility Functions # ============================================================================ + def format_gyro_bias(bias_rad_s: float) -> str: """ Format gyro bias for human-readable display. - + Args: bias_rad_s: Bias in rad/s. - + Returns: Formatted string with both deg/hr and deg/s. - + Example: >>> bias = deg_per_hour_to_rad_per_sec(10.0) >>> print(format_gyro_bias(bias)) @@ -301,13 +306,13 @@ def format_gyro_bias(bias_rad_s: float) -> str: def format_accel_bias(bias_mps2: float) -> str: """ Format accelerometer bias for human-readable display. - + Args: bias_mps2: Bias in m/s². - + Returns: Formatted string with both mg and m/s². - + Example: >>> bias = mg_to_mps2(10.0) >>> print(format_accel_bias(bias)) @@ -320,13 +325,13 @@ def format_accel_bias(bias_mps2: float) -> str: def format_arw(arw_rad_sqrt_s: float) -> str: """ Format gyro ARW for human-readable display. - + Args: arw_rad_sqrt_s: ARW in rad/sqrt(s). - + Returns: Formatted string with deg/sqrt(hr). - + Example: >>> arw = deg_per_sqrt_hour_to_rad_per_sqrt_sec(0.1) >>> print(format_arw(arw)) @@ -339,13 +344,13 @@ def format_arw(arw_rad_sqrt_s: float) -> str: def format_vrw(vrw_mps_sqrt_s: float) -> str: """ Format accelerometer VRW for human-readable display. - + Args: vrw_mps_sqrt_s: VRW in m/s/sqrt(s). - + Returns: Formatted string with m/s/sqrt(hr). - + Example: >>> vrw = mps_per_sqrt_hour_to_mps_per_sqrt_sec(0.01) >>> print(format_vrw(vrw)) @@ -353,4 +358,3 @@ def format_vrw(vrw_mps_sqrt_s: float) -> str: """ mps_sqrt_hr = mps_per_sqrt_sec_to_mps_per_sqrt_hour(vrw_mps_sqrt_s) return f"{mps_sqrt_hr:.4f} m/s/sqrt(hr)" - diff --git a/core/sensors/wheel_odometry.py b/core/sensors/wheel_odometry.py index 41bb772..bd512cd 100644 --- a/core/sensors/wheel_odometry.py +++ b/core/sensors/wheel_odometry.py @@ -388,5 +388,3 @@ def wheel_odom_update( p_next = odom_pos_update(p, v_m, dt) return p_next - - diff --git a/core/sim/__init__.py b/core/sim/__init__.py index 4f21c8b..7aa7551 100644 --- a/core/sim/__init__.py +++ b/core/sim/__init__.py @@ -7,7 +7,7 @@ Modules: imu_from_trajectory: Generate IMU measurements (accel, gyro) from trajectory noise_pink: Generate 1/f (pink) noise for bias instability simulation - + The forward models implement the correct physics: - Accelerometers measure specific force (reaction force), not acceleration - Gyroscopes measure angular velocity in body frame @@ -36,15 +36,3 @@ ] __version__ = "1.0.0" - - - - - - - - - - - - diff --git a/core/sim/imu_from_trajectory.py b/core/sim/imu_from_trajectory.py index 2015d2d..8daf328 100644 --- a/core/sim/imu_from_trajectory.py +++ b/core/sim/imu_from_trajectory.py @@ -318,21 +318,11 @@ def generate_imu_from_trajectory( accel_map[0] = accel_map[1] if N > 1 else np.zeros(3) # Step 2: Compute specific force in body frame - accel_body = compute_specific_force_body(accel_map, quat_b_to_m, frame, g, lat_rad=lat_rad) + accel_body = compute_specific_force_body( + accel_map, quat_b_to_m, frame, g, lat_rad=lat_rad + ) # Step 3: Compute gyro rates in body frame gyro_body = compute_gyro_body(quat_b_to_m, dt) return accel_body, gyro_body - - - - - - - - - - - - diff --git a/core/sim/noise_pink.py b/core/sim/noise_pink.py index 39e4d29..d1f7da5 100644 --- a/core/sim/noise_pink.py +++ b/core/sim/noise_pink.py @@ -60,15 +60,15 @@ def pink_noise_1f_fft( Example: >>> import numpy as np >>> from core.sim.noise_pink import pink_noise_1f_fft - >>> + >>> >>> # Generate 1 hour of pink noise at 100 Hz >>> fs = 100.0 # Hz >>> duration = 3600.0 # seconds >>> N = int(fs * duration) - >>> + >>> >>> rng = np.random.default_rng(42) >>> pink = pink_noise_1f_fft(N, fs, rng=rng) - >>> + >>> >>> # Verify properties >>> assert np.abs(np.mean(pink)) < 0.01 # zero-mean >>> assert np.abs(np.std(pink) - 1.0) < 0.01 # unit-std @@ -186,24 +186,24 @@ def scale_to_bias_instability( >>> import numpy as np >>> from core.sim.noise_pink import pink_noise_1f_fft, scale_to_bias_instability >>> from core.sensors import allan_variance - >>> + >>> >>> # Generate unit pink noise >>> fs = 100.0 # Hz >>> N = 360000 # 1 hour at 100 Hz >>> pink_unit = pink_noise_1f_fft(N, fs) - >>> + >>> >>> # Target BI: 10 deg/hr >>> bi_deg_hr = 10.0 >>> target_bi_rad_s = np.deg2rad(bi_deg_hr) / 3600.0 - >>> + >>> >>> # Create tau grid for Allan deviation >>> tau_grid = np.logspace(0, 3, 50) # 1s to 1000s - >>> + >>> >>> # Scale pink noise to match target BI >>> pink_scaled = scale_to_bias_instability( ... pink_unit, target_bi_rad_s, allan_variance, tau_grid, fs ... ) - >>> + >>> >>> # Verify: compute Allan deviation and check minimum >>> taus, sigma = allan_variance(pink_scaled, fs, tau_grid) >>> sigma_min = np.min(sigma) @@ -222,13 +222,10 @@ def scale_to_bias_instability( sigma_min = np.min(sigma) if sigma_min <= 0: - raise ValueError( - "Allan sigma_min <= 0; check allan_sigma_func implementation." - ) + raise ValueError("Allan sigma_min <= 0; check allan_sigma_func implementation.") # Scale so that: target_bi_rad_s ≈ sigma_min_scaled / bi_factor # => sigma_min_scaled ≈ target_bi_rad_s * bi_factor scale = (target_bi_rad_s * bi_factor) / sigma_min return pink_unit * scale - diff --git a/core/slam/__init__.py b/core/slam/__init__.py index 5f21de0..d945fc2 100644 --- a/core/slam/__init__.py +++ b/core/slam/__init__.py @@ -25,14 +25,14 @@ Example usage: >>> from core.slam import Pose2, se2_compose, se2_inverse, se2_apply >>> import numpy as np - >>> + >>> >>> # Create poses >>> p1 = Pose2(x=0.0, y=0.0, yaw=0.0) >>> p2 = Pose2(x=1.0, y=0.0, yaw=np.pi/2) - >>> + >>> >>> # Compose poses >>> p_composed = se2_compose(p1.to_array(), p2.to_array()) - >>> + >>> >>> # Transform points >>> points = np.array([[1, 0], [0, 1]]) >>> points_transformed = se2_apply(p2.to_array(), points) @@ -163,4 +163,3 @@ ] __version__ = "0.1.0" - diff --git a/core/slam/camera.py b/core/slam/camera.py index a69857a..99a4aff 100644 --- a/core/slam/camera.py +++ b/core/slam/camera.py @@ -311,7 +311,9 @@ def unproject_pixel( else: depth_array = np.asarray(depth) if depth_array.shape != (N,): - raise ValueError(f"Depth shape {depth_array.shape} doesn't match pixels {N}") + raise ValueError( + f"Depth shape {depth_array.shape} doesn't match pixels {N}" + ) # Scale directions by depth directions = directions * depth_array[:, np.newaxis] @@ -387,11 +389,7 @@ def essential_matrix_from_pose(R: np.ndarray, t: np.ndarray) -> np.ndarray: raise ValueError(f"t must be (3,), got {t.shape}") # Skew-symmetric matrix of translation - t_skew = np.array([ - [0, -t[2], t[1]], - [t[2], 0, -t[0]], - [-t[1], t[0], 0] - ]) + t_skew = np.array([[0, -t[2], t[1]], [t[2], 0, -t[0]], [-t[1], t[0], 0]]) # Essential matrix E = t_skew @ R @@ -463,5 +461,3 @@ def triangulate_point( point2 = origin2_in_cam1 + u * ray2_in_cam1 return (point1 + point2) / 2.0 - - diff --git a/core/slam/factors.py b/core/slam/factors.py index fb99d2d..3f28c16 100644 --- a/core/slam/factors.py +++ b/core/slam/factors.py @@ -154,7 +154,7 @@ def create_loop_closure_factor( The close-loop constraint from Eq. (7.22): residual = ln((ΔT_ij')^{-1} T_i^{-1} T_j)^∨ - + where: - T_i = pose at earlier time i (pose_id_from) - T_j = pose at later time j (pose_id_to) @@ -500,7 +500,7 @@ def create_reprojection_factor( Implements the reprojection residual from Section 7.4.2 (Bundle Adjustment): - Eq. (7.70): Bundle adjustment objective function {R_i, t_i, p_k} = argmin Σ ||p_pixel - π(R_i p_k + t_i)||² - + Note: This implementation uses SE(2) planar poses [x, y, yaw] instead of full SE(3) poses (R_i, t_i) from Eq. (7.70). This is a pedagogical simplification for 2D SLAM examples. The reprojection error principle @@ -530,7 +530,9 @@ def create_reprojection_factor( information = np.eye(2) / (pixel_std**2) if observed_pixel.shape != (2,): - raise ValueError(f"observed_pixel must be shape (2,), got {observed_pixel.shape}") + raise ValueError( + f"observed_pixel must be shape (2,), got {observed_pixel.shape}" + ) def residual_func(variables: list[np.ndarray]) -> np.ndarray: """ @@ -561,7 +563,9 @@ def residual_func(variables: list[np.ndarray]) -> np.ndarray: elif landmark.shape[0] == 3: lx, ly, lz = landmark else: - raise ValueError(f"Landmark must be 2D or 3D, got shape {landmark.shape}") + raise ValueError( + f"Landmark must be 2D or 3D, got shape {landmark.shape}" + ) # Relative position in map frame dx_map = lx - x_cam @@ -653,4 +657,3 @@ def jacobian_func(variables: list[np.ndarray]) -> list[np.ndarray]: ) return factor - diff --git a/core/slam/frontend_2d.py b/core/slam/frontend_2d.py index cfda986..d8da738 100644 --- a/core/slam/frontend_2d.py +++ b/core/slam/frontend_2d.py @@ -24,13 +24,14 @@ @dataclass class MatchQuality: """Quality metrics for scan-to-map alignment. - + Attributes: residual: ICP alignment residual (lower is better). converged: Whether ICP converged. n_correspondences: Number of point correspondences found. iters: Number of ICP iterations performed. """ + residual: float converged: bool n_correspondences: int @@ -39,15 +40,15 @@ class MatchQuality: class SlamFrontend2D: """Online SLAM front-end with scan-to-map alignment. - + Implements the core SLAM loop: 1. Predict pose from odometry delta 2. Refine pose via scan-to-map ICP alignment 3. Update local submap with refined pose - + This front-end maintains a local submap (sliding window of recent scans) and uses it to correct odometry drift through scan-to-map matching. - + Attributes: submap: Local submap (Submap2D) storing accumulated scans. pose_est: Current pose estimate in map frame [x, y, yaw]. @@ -58,20 +59,20 @@ class SlamFrontend2D: per correspondence in meters. max_correspondence_distance: ICP correspondence gate in meters (d_threshold in Eq. 7.11), or None to disable gating. - + Example: >>> frontend = SlamFrontend2D(submap_voxel_size=0.1) - >>> + >>> >>> # First step (initialization) >>> odom_delta = np.array([0.0, 0.0, 0.0]) >>> scan = np.array([[1.0, 0.0], [2.0, 0.0]]) >>> result = frontend.step(0, odom_delta, scan) - >>> + >>> >>> # Subsequent steps >>> odom_delta = np.array([0.1, 0.0, 0.0]) # Move 0.1m forward >>> scan = np.array([[1.0, 0.0], [2.0, 0.0]]) >>> result = frontend.step(1, odom_delta, scan) - >>> + >>> >>> print(f"Predicted: {result['pose_pred']}") >>> print(f"Estimated: {result['pose_est']}") >>> print(f"Converged: {result['match_quality'].converged}") @@ -127,29 +128,29 @@ def step( scan: np.ndarray, ) -> Dict: """Execute one step of SLAM front-end. - + This is the core SLAM loop: 1. Predict pose from odometry 2. Refine pose via scan-to-map ICP 3. Update submap with refined pose - + Args: step_index: Current time step (for logging/debugging). odom_delta: Odometry delta (relative pose) [dx, dy, dyaw], shape (3,). This is the motion estimate from wheel encoders or previous scan-to-scan matching. scan: LiDAR scan points in robot frame, shape (N, 2). - + Returns: Dictionary with: - 'pose_pred': Predicted pose (odometry only) [x, y, yaw] - 'pose_est': Estimated pose (after scan matching) [x, y, yaw] - 'match_quality': MatchQuality dataclass with ICP metrics - 'correction_magnitude': Euclidean distance between pred and est - + Raises: ValueError: If odom_delta or scan have invalid shapes. - + Example: >>> frontend = SlamFrontend2D() >>> odom = np.array([0.1, 0.0, 0.0]) @@ -171,9 +172,7 @@ def step( pose_pred = se2_compose(self.pose_est, odom_delta) # 2. CORRECTION: Scan-to-map alignment via ICP - pose_est, match_quality = self._scan_to_map_alignment( - scan, pose_pred - ) + pose_est, match_quality = self._scan_to_map_alignment(scan, pose_pred) # 3. MAP UPDATE: Add scan to submap with estimated pose self.submap.add_scan(pose_est, scan) @@ -185,10 +184,10 @@ def step( correction_magnitude = np.linalg.norm(pose_est[:2] - pose_pred[:2]) return { - 'pose_pred': pose_pred, - 'pose_est': pose_est, - 'match_quality': match_quality, - 'correction_magnitude': correction_magnitude, + "pose_pred": pose_pred, + "pose_est": pose_est, + "match_quality": match_quality, + "correction_magnitude": correction_magnitude, } def _initialize_first_step( @@ -197,11 +196,11 @@ def _initialize_first_step( scan: np.ndarray, ) -> Dict: """Initialize front-end with first scan. - + Args: step_index: Step index (should be 0). scan: First scan in robot frame. - + Returns: Result dictionary with initialization values. """ @@ -225,10 +224,10 @@ def _initialize_first_step( ) return { - 'pose_pred': self.pose_est.copy(), - 'pose_est': self.pose_est.copy(), - 'match_quality': match_quality, - 'correction_magnitude': 0.0, + "pose_pred": self.pose_est.copy(), + "pose_est": self.pose_est.copy(), + "match_quality": match_quality, + "correction_magnitude": 0.0, } def _scan_to_map_alignment( @@ -237,16 +236,16 @@ def _scan_to_map_alignment( pose_pred: np.ndarray, ) -> Tuple[np.ndarray, MatchQuality]: """Align current scan to submap via ICP. - + Args: scan: Current scan in robot frame, shape (N, 2). pose_pred: Predicted pose in map frame [x, y, yaw]. - + Returns: Tuple of (pose_est, match_quality): - pose_est: Refined pose after ICP [x, y, yaw] - match_quality: ICP alignment metrics - + Notes: - If submap has too few points, returns prediction (no correction) - If ICP fails to converge, returns prediction (fallback) @@ -291,7 +290,7 @@ def _scan_to_map_alignment( except Exception: # ICP failed (e.g., numerical issues) match_quality = MatchQuality( - residual=float('inf'), + residual=float("inf"), converged=False, n_correspondences=0, iters=0, @@ -320,21 +319,18 @@ def _scan_to_map_alignment( def get_current_pose(self) -> Optional[np.ndarray]: """Get current pose estimate. - + Returns: Current pose [x, y, yaw] or None if not initialized. """ return self.pose_est.copy() if self.initialized else None - def get_submap_points( - self, - voxel_size: Optional[float] = None - ) -> np.ndarray: + def get_submap_points(self, voxel_size: Optional[float] = None) -> np.ndarray: """Get current submap points. - + Args: voxel_size: Optional voxel size for downsampling. - + Returns: Submap points in map frame, shape (M, 2). """ diff --git a/core/slam/loop_closure_2d.py b/core/slam/loop_closure_2d.py index 4e66059..884c386 100644 --- a/core/slam/loop_closure_2d.py +++ b/core/slam/loop_closure_2d.py @@ -28,13 +28,14 @@ @dataclass class LoopClosureCandidate: """Loop closure candidate with similarity score. - + Attributes: i: Query scan index. j: Match scan index (j < i). descriptor_similarity: Descriptor similarity score. distance: Optional position distance (if poses provided). """ + i: int j: int descriptor_similarity: float @@ -44,7 +45,7 @@ class LoopClosureCandidate: @dataclass class LoopClosure: """Verified loop closure with geometric transformation. - + Attributes: i: Query scan index. j: Match scan index (j < i). @@ -54,6 +55,7 @@ class LoopClosure: icp_residual: ICP alignment residual. icp_iterations: Number of ICP iterations. """ + i: int j: int rel_pose: np.ndarray @@ -65,12 +67,12 @@ class LoopClosure: class LoopClosureDetector2D: """Observation-based loop closure detector for 2D LiDAR SLAM. - + This detector finds loop closures using scan descriptor similarity as the primary criterion, with optional distance-based filtering as a secondary check. Geometric verification via ICP ensures only valid loop closures are returned. - + Attributes: n_bins: Number of bins for range histogram descriptor. max_range: Maximum range for descriptor (meters). @@ -81,16 +83,16 @@ class LoopClosureDetector2D: max_icp_residual: Maximum ICP residual to accept loop closure. icp_max_iterations: Maximum ICP iterations. icp_tolerance: ICP convergence tolerance. - + Example: >>> detector = LoopClosureDetector2D(min_descriptor_similarity=0.7) - >>> + >>> >>> # Detect loop closures >>> loop_closures = detector.detect( ... scans=scans, ... poses=poses, # Optional, for distance gating ... ) - >>> + >>> >>> print(f"Found {len(loop_closures)} loop closures") """ @@ -107,7 +109,7 @@ def __init__( icp_tolerance: float = 1e-4, ): """Initialize loop closure detector. - + Args: n_bins: Number of histogram bins for descriptor. max_range: Maximum range for descriptor histogram. @@ -142,7 +144,7 @@ def detect( poses: Optional[List[np.ndarray]] = None, ) -> List[LoopClosure]: """Detect loop closures in a sequence of scans. - + Pipeline: 1. Compute descriptors for all scans 2. For each query scan i: @@ -150,21 +152,21 @@ def detect( b. Optionally filter by position distance (if poses provided) c. Verify with ICP geometric alignment d. Accept if ICP converges with low residual - + Args: scans: List of N scans, each with shape (M_i, 2) in robot frame. poses: Optional list of N poses [x, y, yaw] for distance gating. - + Returns: List of verified loop closures, sorted by query index i. - + Example: >>> scans = [scan0, scan1, ..., scanN] >>> poses = [pose0, pose1, ..., poseN] # Optional - >>> + >>> >>> detector = LoopClosureDetector2D() >>> loop_closures = detector.detect(scans, poses) - >>> + >>> >>> for lc in loop_closures: ... print(f"Loop: {lc.j} -> {lc.i}, sim={lc.descriptor_similarity:.3f}") """ @@ -184,9 +186,7 @@ def detect( # 2. For each query scan (starting after min_time_separation) for i in range(self.min_time_separation, n_scans): # Find candidates using descriptor similarity - candidates = self._find_candidates( - i, descriptors, poses - ) + candidates = self._find_candidates(i, descriptors, poses) if len(candidates) == 0: continue @@ -197,7 +197,10 @@ def detect( # Run ICP to verify geometric consistency verified = self._verify_candidate( - scans[i], scans[j], poses[i] if poses else None, poses[j] if poses else None + scans[i], + scans[j], + poses[i] if poses else None, + poses[j] if poses else None, ) if verified is not None: @@ -224,15 +227,15 @@ def _find_candidates( poses: Optional[List[np.ndarray]], ) -> List[LoopClosureCandidate]: """Find loop closure candidates for a query scan. - + Primary filter: Descriptor similarity Secondary filter (optional): Position distance - + Args: query_idx: Query scan index i. descriptors: Array of descriptors, shape (N, n_bins). poses: Optional list of poses for distance gating. - + Returns: List of candidates, sorted by descriptor similarity (descending). """ @@ -282,13 +285,13 @@ def _verify_candidate( pose_j: Optional[np.ndarray], ) -> Optional[Tuple[np.ndarray, np.ndarray, float, int]]: """Verify loop closure candidate with ICP. - + Args: scan_i: Query scan (robot frame). scan_j: Match scan (robot frame). pose_i: Optional query pose [x, y, yaw] for initial guess. pose_j: Optional match pose [x, y, yaw] for initial guess. - + Returns: Tuple of (rel_pose, covariance, residual, iterations) if verified, None if ICP fails or residual too high. diff --git a/core/slam/ndt.py b/core/slam/ndt.py index 102ce45..029d59e 100644 --- a/core/slam/ndt.py +++ b/core/slam/ndt.py @@ -429,13 +429,9 @@ def ndt_align( """ # Validate inputs if source_scan.ndim != 2 or source_scan.shape[1] != 2: - raise ValueError( - f"source_scan must have shape (N, 2), got {source_scan.shape}" - ) + raise ValueError(f"source_scan must have shape (N, 2), got {source_scan.shape}") if target_scan.ndim != 2 or target_scan.shape[1] != 2: - raise ValueError( - f"target_scan must have shape (M, 2), got {target_scan.shape}" - ) + raise ValueError(f"target_scan must have shape (M, 2), got {target_scan.shape}") if source_scan.shape[0] == 0: raise ValueError("source_scan is empty") @@ -534,8 +530,11 @@ def _wrap_yaw(p: np.ndarray) -> np.ndarray: return current_pose, iteration + 1, current_score, False if alpha * float(np.linalg.norm(delta)) < tolerance: - return current_pose, iteration + 1, current_score, ( - current_score < NO_MATCH_SCORE + return ( + current_pose, + iteration + 1, + current_score, + (current_score < NO_MATCH_SCORE), ) return current_pose, max_iterations, current_score, False @@ -578,5 +577,3 @@ def ndt_covariance( cov = np.diag([sigma_xy**2, sigma_xy**2, sigma_yaw**2]) return cov - - diff --git a/core/slam/scan_descriptor_2d.py b/core/slam/scan_descriptor_2d.py index 7736e1a..58e9da8 100644 --- a/core/slam/scan_descriptor_2d.py +++ b/core/slam/scan_descriptor_2d.py @@ -23,16 +23,16 @@ def compute_scan_descriptor( max_range: float = 10.0, ) -> np.ndarray: """Compute range histogram descriptor for a 2D LiDAR scan. - + This descriptor represents the distribution of ranges in a scan as a normalized histogram. It is rotation-invariant and provides a simple yet effective signature for place recognition. - + Algorithm: 1. Compute range r = sqrt(x^2 + y^2) for each point 2. Create histogram of ranges with n_bins over [0, max_range] 3. Normalize to sum = 1 (probability distribution) - + Args: scan_xy: Scan points in robot frame, shape (N, 2). n_bins: Number of histogram bins. More bins = more discriminative @@ -40,14 +40,14 @@ def compute_scan_descriptor( max_range: Maximum range for histogram (meters). Points beyond this range are placed in the last bin. Should match LiDAR max range. - + Returns: Descriptor vector of shape (n_bins,), normalized to sum = 1. Returns zero vector if scan is empty. - + Raises: ValueError: If scan_xy has invalid shape or n_bins < 1. - + Example: >>> scan = np.array([[1.0, 0.0], [2.0, 0.0], [3.0, 0.0]]) >>> desc = compute_scan_descriptor(scan, n_bins=8, max_range=5.0) @@ -55,7 +55,7 @@ def compute_scan_descriptor( (8,) >>> print(np.sum(desc)) # Should be 1.0 1.0 - + Notes: - Rotation-invariant: descriptor(scan) = descriptor(rotate(scan)) - Not translation-invariant: assumes scan in robot frame @@ -104,7 +104,7 @@ def compute_descriptor_similarity( method: str = "cosine", ) -> float: """Compute similarity between two scan descriptors. - + Args: desc1: First descriptor, shape (n_bins,). desc2: Second descriptor, shape (n_bins,). @@ -112,15 +112,15 @@ def compute_descriptor_similarity( - "cosine": Cosine similarity (range: [-1, 1], higher = more similar) - "correlation": Pearson correlation (range: [-1, 1]) - "l2": Negative L2 distance (range: (-inf, 0], higher = more similar) - + Returns: Similarity score. Higher values indicate more similar descriptors. For "cosine" and "correlation": range is [-1, 1], with 1 = identical. For "l2": range is (-inf, 0], with 0 = identical. - + Raises: ValueError: If descriptors have different shapes or invalid method. - + Example: >>> desc1 = np.array([0.5, 0.3, 0.2]) >>> desc2 = np.array([0.5, 0.3, 0.2]) @@ -172,15 +172,15 @@ def batch_compute_descriptors( max_range: float = 10.0, ) -> np.ndarray: """Compute descriptors for a batch of scans. - + Args: scans: List of N scans, each with shape (M_i, 2). n_bins: Number of histogram bins. max_range: Maximum range for histogram. - + Returns: Array of descriptors with shape (N, n_bins). - + Example: >>> scans = [ ... np.array([[1.0, 0.0], [2.0, 0.0]]), diff --git a/core/slam/scan_generation.py b/core/slam/scan_generation.py index cc62dd2..8e6d699 100644 --- a/core/slam/scan_generation.py +++ b/core/slam/scan_generation.py @@ -35,22 +35,22 @@ def ray_segment_intersection( segment_end: np.ndarray, ) -> Tuple[Optional[np.ndarray], float]: """Compute intersection between a ray and a line segment. - + Uses parametric line equations to find intersection: Ray: P = origin + t * direction (t >= 0) Segment: Q = start + s * (end - start) (0 <= s <= 1) - + Args: ray_origin: Ray starting point [x, y]. ray_direction: Ray direction vector [dx, dy] (should be normalized). segment_start: Segment start point [x, y]. segment_end: Segment end point [x, y]. - + Returns: Tuple of (intersection_point, distance): - intersection_point: [x, y] if intersection exists, None otherwise - distance: Distance along ray to intersection (inf if no intersection) - + Notes: - Ray direction should be normalized for distance to be meaningful - Returns None if ray doesn't intersect segment or intersection is behind ray @@ -67,7 +67,7 @@ def ray_segment_intersection( # Check for degenerate segment (zero length) if s_len_sq < 1e-10: - return None, float('inf') + return None, float("inf") # Solve for intersection using Cramer's rule # Ray: o + t*d, Segment: s_start + u*s_dir @@ -83,7 +83,7 @@ def ray_segment_intersection( # Check if ray and segment are parallel if abs(det) < 1e-10: - return None, float('inf') + return None, float("inf") # Solve for t and u t = (diff[0] * s_dir[1] - diff[1] * s_dir[0]) / det @@ -91,7 +91,7 @@ def ray_segment_intersection( # Check validity: t >= 0 (ray goes forward), 0 <= u <= 1 (point on segment) if t < 0 or u < 0 or u > 1: - return None, float('inf') + return None, float("inf") # Compute intersection point intersection = o + t * d @@ -110,14 +110,14 @@ def generate_scan_with_occlusion( rng=None, ) -> np.ndarray: """Generate 2D LiDAR scan with proper occlusion handling using ray-casting. - + Simulates a 2D LiDAR sensor that casts rays in 360 directions around the robot. For each ray, finds the CLOSEST wall intersection, properly handling occlusions where near objects block far objects. - + This corrects the bug in `generate_dense_wall_scan` which included all walls within range regardless of occlusion. - + Args: pose: Robot pose [x, y, yaw] in global frame. walls: List of (start_point, end_point) tuples defining wall segments. @@ -127,11 +127,11 @@ def generate_scan_with_occlusion( noise_std: Standard deviation of range measurement noise (meters). min_range: Minimum sensor range (meters). Points closer than this are filtered out to simulate sensor blind zone. - + Returns: Point cloud in robot's local frame, shape (M, 2) where M <= num_rays. Each point is [x_local, y_local]. - + Example: >>> walls = [ ... (np.array([0, 0]), np.array([10, 0])), # Bottom wall @@ -140,7 +140,7 @@ def generate_scan_with_occlusion( >>> pose = np.array([5.0, 5.0, 0.0]) >>> scan = generate_scan_with_occlusion(pose, walls, num_rays=360) >>> print(f"Generated {len(scan)} points") - + Notes: - Ray-casting ensures only the CLOSEST hit is recorded per ray direction - Properly handles occlusions: near obstacles block far walls @@ -209,24 +209,24 @@ def generate_dense_wall_scan( rng=None, ) -> np.ndarray: """Generate dense LiDAR scan from walls (legacy, without occlusion handling). - + [DEPRECATED] This function has a known occlusion bug: it includes points from ALL walls within range, even if they are blocked by closer obstacles. - + Use `generate_scan_with_occlusion()` instead for physically accurate scans. - + This function is kept for backward compatibility with existing code. - + Args: pose: Robot pose [x, y, yaw]. walls: List of (start_point, end_point) tuples defining wall segments. max_range: Maximum sensor range in meters. noise_std: Standard deviation of measurement noise. points_per_wall: Number of points to sample per wall segment. - + Returns: Point cloud in robot local frame, shape (M, 2). - + Warning: This function does NOT handle occlusions properly. Near objects do not block far objects, resulting in non-physical scan data. diff --git a/core/slam/scan_matching.py b/core/slam/scan_matching.py index 90bd554..4c1fa9f 100644 --- a/core/slam/scan_matching.py +++ b/core/slam/scan_matching.py @@ -43,10 +43,10 @@ def find_correspondences( For each point in the source cloud, finds the closest point in the target cloud using a KD-tree for efficient nearest-neighbor search. Correspondences are gated by the distance threshold d_threshold as described in Eq. (7.11): - + b_{i,j} = { 0 if ||p_{i,t-1} - T p_{j,t}|| > d_threshold { 1 otherwise - + This implements the binary selector for valid point pairs in ICP. Args: @@ -189,7 +189,7 @@ def align_svd( Given corresponding point sets, computes the rigid transformation (rotation + translation) that minimizes the point-to-point error in Eq. (7.10). The SVD-based solution is described in Section 7.3.1 text after Eq. (7.11). - + The book mentions that to solve Eq. (7.10), a nonlinear optimizer can be used, or the rotation matrix can be solved first by SVD, then the translation can be computed as: Δx = p̄_{t-1} - (Ĉ p̄_t), where p̄ denotes the geometric center. @@ -245,9 +245,7 @@ def align_svd( N = source_points.shape[0] if N < 2: - raise ValueError( - f"Need at least 2 correspondences for SVD alignment, got {N}" - ) + raise ValueError(f"Need at least 2 correspondences for SVD alignment, got {N}") # Step 1: Compute centroids centroid_source = np.mean(source_points, axis=0) # shape (2,) @@ -358,13 +356,9 @@ def icp_point_to_point( """ # Validate inputs if source_scan.ndim != 2 or source_scan.shape[1] != 2: - raise ValueError( - f"source_scan must have shape (N, 2), got {source_scan.shape}" - ) + raise ValueError(f"source_scan must have shape (N, 2), got {source_scan.shape}") if target_scan.ndim != 2 or target_scan.shape[1] != 2: - raise ValueError( - f"target_scan must have shape (M, 2), got {target_scan.shape}" - ) + raise ValueError(f"target_scan must have shape (M, 2), got {target_scan.shape}") if source_scan.shape[0] == 0: raise ValueError("source_scan is empty") @@ -488,4 +482,3 @@ def compute_icp_covariance( cov = np.diag([sigma_xy**2, sigma_xy**2, sigma_yaw**2]) return cov - diff --git a/core/slam/se2.py b/core/slam/se2.py index a1d6523..f5ac8ab 100644 --- a/core/slam/se2.py +++ b/core/slam/se2.py @@ -208,9 +208,7 @@ def se2_inverse(p: Union[np.ndarray, Pose2]) -> np.ndarray: return np.array([x_inv, y_inv, yaw_inv], dtype=np.float64) -def se2_apply( - p: Union[np.ndarray, Pose2], points: np.ndarray -) -> np.ndarray: +def se2_apply(p: Union[np.ndarray, Pose2], points: np.ndarray) -> np.ndarray: """ Transform 2D points by an SE(2) pose. @@ -271,9 +269,7 @@ def se2_apply( # Validate points shape if points.ndim != 2 or points.shape[1] != 2: - raise ValueError( - f"points must have shape (N, 2), got {points.shape}" - ) + raise ValueError(f"points must have shape (N, 2), got {points.shape}") x, y, yaw = p @@ -426,5 +422,3 @@ def se2_from_matrix(T: np.ndarray) -> np.ndarray: yaw = np.arctan2(T[1, 0], T[0, 0]) return np.array([x, y, yaw], dtype=np.float64) - - diff --git a/core/slam/submap_2d.py b/core/slam/submap_2d.py index dd187bc..4b6eb8e 100644 --- a/core/slam/submap_2d.py +++ b/core/slam/submap_2d.py @@ -15,15 +15,15 @@ class Submap2D: """Lightweight 2D submap for storing accumulated scan points. - + A submap maintains a collection of 2D points in a map frame, built by transforming individual scans from their robot frames into the map frame. Used in SLAM front-end for scan-to-map alignment. - + Attributes: points: Accumulated map points in map frame, shape (N, 2). n_scans: Number of scans added to this submap. - + Example: >>> submap = Submap2D() >>> pose = np.array([1.0, 2.0, 0.5]) # [x, y, yaw] @@ -32,7 +32,7 @@ class Submap2D: >>> map_points = submap.get_points() >>> print(map_points.shape) (2, 2) - + Notes: - Points are stored in map frame (global coordinates) - No automatic downsampling unless explicitly requested @@ -46,14 +46,14 @@ def __init__(self) -> None: def add_scan(self, pose_se2: np.ndarray, scan_xy: np.ndarray) -> None: """Add a scan to the submap by transforming it into map frame. - + Args: pose_se2: Robot pose in map frame [x, y, yaw], shape (3,). scan_xy: Scan points in robot local frame, shape (N, 2). - + Raises: ValueError: If pose_se2 is not shape (3,) or scan_xy is not shape (N, 2). - + Example: >>> submap = Submap2D() >>> pose = np.array([0.0, 0.0, 0.0]) @@ -85,15 +85,15 @@ def add_scan(self, pose_se2: np.ndarray, scan_xy: np.ndarray) -> None: def get_points(self, voxel_size: Optional[float] = None) -> np.ndarray: """Get all map points, optionally downsampled. - + Args: voxel_size: If provided, downsample points using voxel grid filter. Points within the same voxel are replaced by their centroid. - + Returns: Map points in map frame, shape (M, 2). If voxel_size is provided, M <= original point count. If no points exist, returns empty array. - + Example: >>> submap = Submap2D() >>> pose = np.array([0.0, 0.0, 0.0]) @@ -117,11 +117,11 @@ def get_points(self, voxel_size: Optional[float] = None) -> np.ndarray: def downsample(self, voxel_size: float) -> None: """Downsample map points in-place using voxel grid filter. - + Args: voxel_size: Voxel grid size in meters. Points within the same voxel are replaced by their centroid. - + Example: >>> submap = Submap2D() >>> pose = np.array([0.0, 0.0, 0.0]) @@ -140,7 +140,7 @@ def downsample(self, voxel_size: float) -> None: def clear(self) -> None: """Clear all points and reset scan count. - + Example: >>> submap = Submap2D() >>> pose = np.array([0.0, 0.0, 0.0]) @@ -157,7 +157,7 @@ def clear(self) -> None: def __len__(self) -> int: """Return number of points in the submap. - + Returns: Number of points currently stored. """ @@ -166,14 +166,14 @@ def __len__(self) -> int: @staticmethod def _voxel_downsample(points: np.ndarray, voxel_size: float) -> np.ndarray: """Voxel grid downsampling using quantization and centroid computation. - + Args: points: Input points, shape (N, 2). voxel_size: Voxel grid size in meters. - + Returns: Downsampled points, shape (M, 2) where M <= N. - + Notes: - Points are quantized to voxel grid coordinates - Points in the same voxel are averaged to compute centroid diff --git a/core/slam/types.py b/core/slam/types.py index c050720..f0e529e 100644 --- a/core/slam/types.py +++ b/core/slam/types.py @@ -200,13 +200,9 @@ def __post_init__(self) -> None: if self.height <= 0: raise ValueError(f"height must be positive, got {self.height}") if not (0 <= self.cx < self.width): - raise ValueError( - f"cx must be in [0, {self.width}), got {self.cx}" - ) + raise ValueError(f"cx must be in [0, {self.width}), got {self.cx}") if not (0 <= self.cy < self.height): - raise ValueError( - f"cy must be in [0, {self.height}), got {self.cy}" - ) + raise ValueError(f"cy must be in [0, {self.height}), got {self.cy}") def to_matrix(self) -> np.ndarray: """ @@ -257,5 +253,3 @@ def __repr__(self) -> str: f"distortion=[k1={self.k1:.4f}, k2={self.k2:.4f}, k3={self.k3:.4f}, " f"p1={self.p1:.4f}, p2={self.p2:.4f}])" ) - - diff --git a/core/utils/__init__.py b/core/utils/__init__.py index 2c776cf..ecdbd04 100644 --- a/core/utils/__init__.py +++ b/core/utils/__init__.py @@ -7,26 +7,29 @@ from .angles import wrap_angle, wrap_angle_array, angle_diff from .paths import resolve_data_path, repo_root -from .geometry import normalize_jacobian_singularities, check_anchor_geometry, compute_gdop_2d +from .geometry import ( + normalize_jacobian_singularities, + check_anchor_geometry, + compute_gdop_2d, +) from .observability import ( check_observability, compute_observability_matrix, check_range_only_observability_2d, - estimate_observability_time_constant + estimate_observability_time_constant, ) __all__ = [ - 'resolve_data_path', - 'repo_root', - 'wrap_angle', - 'wrap_angle_array', - 'angle_diff', - 'normalize_jacobian_singularities', - 'check_anchor_geometry', - 'compute_gdop_2d', - 'check_observability', - 'compute_observability_matrix', - 'check_range_only_observability_2d', - 'estimate_observability_time_constant', + "resolve_data_path", + "repo_root", + "wrap_angle", + "wrap_angle_array", + "angle_diff", + "normalize_jacobian_singularities", + "check_anchor_geometry", + "compute_gdop_2d", + "check_observability", + "compute_observability_matrix", + "check_range_only_observability_2d", + "estimate_observability_time_constant", ] - diff --git a/core/utils/angles.py b/core/utils/angles.py index 01de77a..810db41 100644 --- a/core/utils/angles.py +++ b/core/utils/angles.py @@ -17,24 +17,24 @@ def wrap_angle(angle: float) -> float: """ Wrap angle to [-π, π] range. - + This is critical for bearing measurements and angular innovations in Kalman filters. Without wrapping, angles near ±180° can cause large incorrect innovations (e.g., -179° vs +179° = 358° error instead of 2° error). - + Args: angle: Angle in radians (can be any value) - + Returns: Wrapped angle in range [-π, π] - + Example: >>> wrap_angle(3.5 * np.pi) # 630° -> -90° -1.5707963267948966 >>> wrap_angle(-3.5 * np.pi) # -630° -> 90° 1.5707963267948966 - + References: Used in Extended Kalman Filter bearing measurement updates """ @@ -45,15 +45,15 @@ def wrap_angle(angle: float) -> float: def wrap_angle_array(angles: np.ndarray) -> np.ndarray: """ Wrap array of angles to [-π, π] range. - + Vectorized version of wrap_angle() for efficiency. - + Args: angles: Array of angles in radians - + Returns: Array of wrapped angles in range [-π, π] - + Example: >>> angles = np.array([0, np.pi/2, np.pi, -np.pi, 3*np.pi]) >>> wrap_angle_array(angles) @@ -62,35 +62,36 @@ def wrap_angle_array(angles: np.ndarray) -> np.ndarray: return np.arctan2(np.sin(angles), np.cos(angles)) -def angle_diff(angle1: Union[float, np.ndarray], - angle2: Union[float, np.ndarray]) -> Union[float, np.ndarray]: +def angle_diff( + angle1: Union[float, np.ndarray], angle2: Union[float, np.ndarray] +) -> Union[float, np.ndarray]: """ Compute the shortest angular difference between two angles. - + Returns angle1 - angle2, wrapped to [-π, π]. This is the innovation for bearing measurements in EKF/UKF. - + Args: angle1: First angle in radians (measured) angle2: Second angle in radians (predicted) - + Returns: Shortest signed difference angle1 - angle2 in [-π, π] - + Example: >>> angle_diff(np.pi - 0.1, -np.pi + 0.1) # Nearly opposite -0.2 >>> angle_diff(0.1, -0.1) # Small difference 0.2 - + Notes: This is critical for EKF bearing updates: innovation = angle_diff(measured_bearing, predicted_bearing) - + Without this, bearings near ±180° would have huge innovations: measured = +179°, predicted = -179° -> innovation = 358° (WRONG!) with angle_diff: innovation = 2° (CORRECT!) - + References: EKF bearing measurement update (Chapter 3) """ @@ -108,6 +109,3 @@ def degrees_to_radians(degrees: Union[float, np.ndarray]) -> Union[float, np.nda def radians_to_degrees(radians: Union[float, np.ndarray]) -> Union[float, np.ndarray]: """Convert radians to degrees.""" return np.rad2deg(radians) - - - diff --git a/core/utils/geometry.py b/core/utils/geometry.py index 6acc1be..62cd8f4 100644 --- a/core/utils/geometry.py +++ b/core/utils/geometry.py @@ -18,32 +18,30 @@ def normalize_jacobian_singularities( - diff: np.ndarray, - ranges: np.ndarray, - epsilon: float = EPSILON_RANGE + diff: np.ndarray, ranges: np.ndarray, epsilon: float = EPSILON_RANGE ) -> np.ndarray: """ Safely compute normalized Jacobian, avoiding singularities. - + Computes H[i] = diff[i] / range[i] with protection against division by zero when range → 0 (receiver at anchor position). - + Args: diff: Difference vectors (receiver - anchor), shape (N, d) ranges: Range values, shape (N,) or (N, 1) epsilon: Minimum range threshold (default: 1e-10 meters = 10 pm) - + Returns: Normalized Jacobian H = diff / range, shape (N, d) At singularities (range < epsilon), returns zero vector - + Example: >>> diff = np.array([[1.0, 0.0], [1e-12, 1e-12], [3.0, 4.0]]) >>> ranges = np.array([1.0, 1e-12, 5.0]) >>> H = normalize_jacobian_singularities(diff, ranges) >>> H[1] # Singularity -> zero vector array([0., 0.]) - + References: Used in range-bearing EKF measurement Jacobians (Chapter 3) """ @@ -63,7 +61,7 @@ def normalize_jacobian_singularities( warnings.warn( f"{np.sum(singular_mask)} measurement(s) at singularity (range < {epsilon}m). " "Setting Jacobian rows to zero. Check anchor-receiver geometry.", - RuntimeWarning + RuntimeWarning, ) return H @@ -74,35 +72,35 @@ def check_anchor_geometry( position: Optional[np.ndarray] = None, min_anchors_2d: int = 3, min_anchors_3d: int = 4, - warn_degenerate: bool = True + warn_degenerate: bool = True, ) -> Tuple[bool, str]: """ Check if anchor geometry is suitable for positioning. - + Performs geometric checks: 1. Sufficient number of anchors 2. Anchors are not colinear (2D) / coplanar (3D) 3. Position (if given) has reasonable geometry with anchors - + Args: anchors: Anchor positions, shape (N, d) where d=2 or 3 position: Optional receiver position to check geometry, shape (d,) min_anchors_2d: Minimum anchors for 2D positioning (default: 3) min_anchors_3d: Minimum anchors for 3D positioning (default: 4) warn_degenerate: If True, issue warnings for degenerate cases - + Returns: Tuple of (is_valid, message): - is_valid: True if geometry is acceptable - message: Description of geometry issue (empty if valid) - + Example: >>> # Good 2D geometry (triangle) >>> anchors = np.array([[0, 0], [10, 0], [5, 10]]) >>> is_valid, msg = check_anchor_geometry(anchors) >>> is_valid True - + >>> # Bad 2D geometry (colinear) >>> anchors = np.array([[0, 0], [5, 0], [10, 0]]) >>> is_valid, msg = check_anchor_geometry(anchors) @@ -110,7 +108,7 @@ def check_anchor_geometry( False >>> 'colinear' in msg.lower() True - + References: DOP analysis (Chapter 4), Observability (Chapter 8) """ @@ -154,7 +152,10 @@ def check_anchor_geometry( if position is not None: position = np.asarray(position) if position.shape != (dim,): - return False, f"Position dimension mismatch: expected {dim}, got {position.shape}" + return ( + False, + f"Position dimension mismatch: expected {dim}, got {position.shape}", + ) # Check if position is within convex hull (rough approximation) # For 2D: check if position is within bounding box extended by 20% @@ -163,7 +164,9 @@ def check_anchor_geometry( max_bounds = anchors.max(axis=0) margin = 0.2 * (max_bounds - min_bounds) - if np.any(position < min_bounds - margin) or np.any(position > max_bounds + margin): + if np.any(position < min_bounds - margin) or np.any( + position > max_bounds + margin + ): msg = ( f"Position {position} is far outside anchor region " f"[{min_bounds}, {max_bounds}]. This may cause poor DOP." @@ -178,20 +181,20 @@ def check_anchor_geometry( def compute_gdop_2d(anchors: np.ndarray, position: np.ndarray) -> float: """ Compute Geometric Dilution of Precision for 2D positioning. - + GDOP quantifies how anchor geometry affects positioning accuracy: - GDOP < 2: Excellent - GDOP 2-5: Good - GDOP 5-10: Moderate - GDOP > 10: Poor - + Args: anchors: Anchor positions, shape (N, 2) position: Receiver position, shape (2,) - + Returns: GDOP value (dimensionless) - + Example: >>> # Square geometry >>> anchors = np.array([[0, 0], [10, 0], [10, 10], [0, 10]]) @@ -199,7 +202,7 @@ def compute_gdop_2d(anchors: np.ndarray, position: np.ndarray) -> float: >>> gdop = compute_gdop_2d(anchors, position) >>> gdop < 2.0 # Should be excellent True - + References: DOP analysis (core/rf/dop.py), Chapter 4 """ @@ -209,6 +212,3 @@ def compute_gdop_2d(anchors: np.ndarray, position: np.ndarray) -> float: dop = compute_dop(H) return dop.get("GDOP", np.inf) - - - diff --git a/core/utils/observability.py b/core/utils/observability.py index f7b0fc6..03c7ad9 100644 --- a/core/utils/observability.py +++ b/core/utils/observability.py @@ -19,34 +19,32 @@ def compute_observability_matrix( - F: np.ndarray, - H: np.ndarray, - n_steps: Optional[int] = None + F: np.ndarray, H: np.ndarray, n_steps: Optional[int] = None ) -> np.ndarray: """ Compute the observability matrix for a linear system. - + For a linear system: x_{k+1} = F x_k z_k = H x_k - + The observability matrix is: O = [H] [H*F] [H*F^2] - [...] + [...] [H*F^(n-1)] - + where n is the state dimension. - + Args: F: State transition matrix, shape (n, n) H: Measurement matrix, shape (m, n) n_steps: Number of steps to use (default: n, the state dimension) - + Returns: Observability matrix O, shape (m*n_steps, n) - + Example: >>> # Position-only observable system >>> F = np.array([[1, 1], [0, 1]]) # [pos, vel] @@ -54,14 +52,14 @@ def compute_observability_matrix( >>> O = compute_observability_matrix(F, H) >>> np.linalg.matrix_rank(O) # Should be 2 (full rank) 2 - + >>> # Unobservable system (constant offset) >>> F = np.array([[1, 0], [0, 1]]) # No dynamics >>> H = np.array([[1, 0]]) # Observe x only >>> O = compute_observability_matrix(F, H) >>> np.linalg.matrix_rank(O) # Should be 1 (y unobservable) 1 - + References: Linear system theory, Chapter 8 """ @@ -78,9 +76,7 @@ def compute_observability_matrix( m_meas = H.shape[0] if H.shape[1] != n_states: - raise ValueError( - f"H shape {H.shape} incompatible with F shape {F.shape}" - ) + raise ValueError(f"H shape {H.shape} incompatible with F shape {F.shape}") if n_steps is None: n_steps = n_states @@ -90,7 +86,7 @@ def compute_observability_matrix( H_Fi = H.copy() for i in range(n_steps): - O[i * m_meas:(i + 1) * m_meas, :] = H_Fi + O[i * m_meas : (i + 1) * m_meas, :] = H_Fi H_Fi = H_Fi @ F # H * F^i return O @@ -100,49 +96,49 @@ def check_observability( F: np.ndarray, H: np.ndarray, n_steps: Optional[int] = None, - tolerance: float = 1e-10 + tolerance: float = 1e-10, ) -> Tuple[bool, int, np.ndarray]: """ - Check if a linear system is observable. - - A system is observable if all states can be determined from measurements. - This is checked by computing the rank of the observability matrix. - - Args: - F: State transition matrix, shape (n, n) - H: Measurement matrix, shape (m, n) - n_steps: Number of steps for observability matrix (default: n) - tolerance: Numerical tolerance for rank computation - - Returns: - Tuple of (is_observable, rank, singular_values): - - is_observable: True if system is fully observable - - rank: Rank of observability matrix - - singular_values: Singular values of O (for diagnost - -ics) - - Example: - >>> # Observable system - >>> F = np.eye(2) - >>> H = np.eye(2) - >>> is_obs, rank, _ = check_observability(F, H) - >>> is_obs - True - >>> rank - 2 - - >>> # Unobservable system (position bias) - >>> F = np.eye(2) - >>> H = np.array([[1, -1]]) # Observe difference only - >>> is_obs, rank, _ = check_observability(F, H) - >>> is_obs - False - >>> rank - 1 - - References: - Chapter 8, Observability Analysis + Check if a linear system is observable. + + A system is observable if all states can be determined from measurements. + This is checked by computing the rank of the observability matrix. + + Args: + F: State transition matrix, shape (n, n) + H: Measurement matrix, shape (m, n) + n_steps: Number of steps for observability matrix (default: n) + tolerance: Numerical tolerance for rank computation + + Returns: + Tuple of (is_observable, rank, singular_values): + - is_observable: True if system is fully observable + - rank: Rank of observability matrix + - singular_values: Singular values of O (for diagnost + + ics) + + Example: + >>> # Observable system + >>> F = np.eye(2) + >>> H = np.eye(2) + >>> is_obs, rank, _ = check_observability(F, H) + >>> is_obs + True + >>> rank + 2 + + >>> # Unobservable system (position bias) + >>> F = np.eye(2) + >>> H = np.array([[1, -1]]) # Observe difference only + >>> is_obs, rank, _ = check_observability(F, H) + >>> is_obs + False + >>> rank + 1 + + References: + Chapter 8, Observability Analysis """ O = compute_observability_matrix(F, H, n_steps) @@ -156,34 +152,32 @@ def check_observability( else: rank = np.sum(singular_values > tolerance * singular_values[0]) - is_observable = (rank == n_states) + is_observable = rank == n_states return is_observable, rank, singular_values def check_range_only_observability_2d( - anchors: np.ndarray, - position: np.ndarray, - warn: bool = True + anchors: np.ndarray, position: np.ndarray, warn: bool = True ) -> Tuple[bool, str]: """ Check observability for 2D range-only positioning. - + For range-only measurements in 2D: - Need at least 3 non-colinear anchors - Position must not be at an anchor (singularity) - Better geometry = better observability - + Args: anchors: Anchor positions, shape (N, 2) position: Receiver position, shape (2,) warn: If True, issue warnings for poor observability - + Returns: Tuple of (is_observable, message): - is_observable: True if position is observable - message: Description of observability issue - + Example: >>> # Good configuration >>> anchors = np.array([[0, 0], [10, 0], [5, 10]]) @@ -191,7 +185,7 @@ def check_range_only_observability_2d( >>> is_obs, msg = check_range_only_observability_2d(anchors, position) >>> is_obs True - + >>> # Bad: colinear anchors >>> anchors = np.array([[0, 0], [5, 0], [10, 0]]) >>> position = np.array([5.0, 1.0]) @@ -200,7 +194,7 @@ def check_range_only_observability_2d( False >>> 'colinear' in msg.lower() True - + References: Range-based positioning (Chapter 4), Observability (Chapter 8) """ @@ -228,26 +222,24 @@ def check_range_only_observability_2d( def estimate_observability_time_constant( - F: np.ndarray, - H: np.ndarray, - dt: float = 1.0 + F: np.ndarray, H: np.ndarray, dt: float = 1.0 ) -> float: """ Estimate time constant for observability to manifest. - + For systems where observability depends on dynamics (e.g., velocity observable through position changes), estimates how long it takes for the state to become observable. - + Args: F: State transition matrix (discrete time) H: Measurement matrix dt: Time step (seconds) - + Returns: Estimated time constant (seconds) for observability Returns np.inf if unobservable - + Example: >>> # Constant velocity: velocity observable after some motion >>> F = np.array([[1, 0.1], [0, 1]]) # dt=0.1s @@ -277,6 +269,3 @@ def estimate_observability_time_constant( tau = n_states * dt / min_sv return tau - - - diff --git a/scripts/generate_ch2_coordinate_transforms_dataset.py b/scripts/generate_ch2_coordinate_transforms_dataset.py index 4c52780..1eba2b1 100644 --- a/scripts/generate_ch2_coordinate_transforms_dataset.py +++ b/scripts/generate_ch2_coordinate_transforms_dataset.py @@ -84,9 +84,9 @@ def generate_building_trajectory_llh( half = building_size_m / 2.0 north_m = rng.uniform(-half, half, n_points) east_m = rng.uniform(-half, half, n_points) - offsets = np.array([ - enu_to_llh_offset(e, n, lat_center) for e, n in zip(east_m, north_m) - ]) + offsets = np.array( + [enu_to_llh_offset(e, n, lat_center) for e, n in zip(east_m, north_m)] + ) # Generate random positions within building footprint lats = lat_center + offsets[:, 0] @@ -116,9 +116,9 @@ def generate_rotation_sequence( # Random rotations (typical for handheld device) # Roll/Pitch: ±30°, Yaw: full 360° - roll = rng.uniform(-np.pi/6, np.pi/6, n_points) - pitch = rng.uniform(-np.pi/6, np.pi/6, n_points) - yaw = rng.uniform(0, 2*np.pi, n_points) + roll = rng.uniform(-np.pi / 6, np.pi / 6, n_points) + pitch = rng.uniform(-np.pi / 6, np.pi / 6, n_points) + yaw = rng.uniform(0, 2 * np.pi, n_points) return np.column_stack([roll, pitch, yaw]) @@ -273,16 +273,27 @@ def generate_dataset( # Convert to ECEF print("\nStep 2: Converting LLH -> ECEF...") - ecef = np.array([llh_to_ecef(lat, lon, h) for lat, lon, h in zip(lats, lons, heights)]) - print(f" ECEF X range: {ecef[:, 0].min()/1e3:.1f}km to {ecef[:, 0].max()/1e3:.1f}km") - print(f" ECEF Y range: {ecef[:, 1].min()/1e3:.1f}km to {ecef[:, 1].max()/1e3:.1f}km") - print(f" ECEF Z range: {ecef[:, 2].min()/1e3:.1f}km to {ecef[:, 2].max()/1e3:.1f}km") + ecef = np.array( + [llh_to_ecef(lat, lon, h) for lat, lon, h in zip(lats, lons, heights)] + ) + print( + f" ECEF X range: {ecef[:, 0].min()/1e3:.1f}km to {ecef[:, 0].max()/1e3:.1f}km" + ) + print( + f" ECEF Y range: {ecef[:, 1].min()/1e3:.1f}km to {ecef[:, 1].max()/1e3:.1f}km" + ) + print( + f" ECEF Z range: {ecef[:, 2].min()/1e3:.1f}km to {ecef[:, 2].max()/1e3:.1f}km" + ) # Convert to ENU print("\nStep 3: Converting ECEF -> ENU (local frame)...") - enu = np.array([ecef_to_enu(pt[0], pt[1], pt[2], - lat_center, lon_center, height_ground) - for pt in ecef]) + enu = np.array( + [ + ecef_to_enu(pt[0], pt[1], pt[2], lat_center, lon_center, height_ground) + for pt in ecef + ] + ) print(f" ENU East range: {enu[:, 0].min():.1f}m to {enu[:, 0].max():.1f}m") print(f" ENU North range: {enu[:, 1].min():.1f}m to {enu[:, 1].max():.1f}m") print(f" ENU Up range: {enu[:, 2].min():.1f}m to {enu[:, 2].max():.1f}m") @@ -301,15 +312,20 @@ def generate_dataset( print("\nStep 5: Generating rotation representations...") euler = generate_rotation_sequence(n_points, seed) quaternions = np.array([euler_to_quat(e[0], e[1], e[2]) for e in euler]) - rotation_matrices = np.array([euler_to_rotation_matrix(e[0], e[1], e[2]) for e in euler]) - print(f" Euler angles: roll ±{np.rad2deg(np.abs(euler[:, 0]).max()):.1f}°, " - f"pitch ±{np.rad2deg(np.abs(euler[:, 1]).max()):.1f}°, " - f"yaw 0-{np.rad2deg(euler[:, 2].max()):.1f}°") + rotation_matrices = np.array( + [euler_to_rotation_matrix(e[0], e[1], e[2]) for e in euler] + ) + print( + f" Euler angles: roll ±{np.rad2deg(np.abs(euler[:, 0]).max()):.1f}°, " + f"pitch ±{np.rad2deg(np.abs(euler[:, 1]).max()):.1f}°, " + f"yaw 0-{np.rad2deg(euler[:, 2].max()):.1f}°" + ) # Verify rotation round-trip print("\nStep 6: Verifying rotation round-trips...") - euler_from_quat = np.array([rotation_matrix_to_euler(quat_to_rotation_matrix(q)) - for q in quaternions]) + euler_from_quat = np.array( + [rotation_matrix_to_euler(quat_to_rotation_matrix(q)) for q in quaternions] + ) # Wrap the difference to [-pi, pi] before taking its size. Yaw is sampled on # [0, 2pi) but recovered on (-pi, pi], so an exact round-trip of 4.4307 rad # comes back as -1.8525 rad and a raw subtraction calls that 2pi of error. @@ -343,7 +359,7 @@ def generate_dataset( "2.1 (LLH->ECEF)", "2.2 (ECEF->LLH)", "2.3 (ECEF->ENU)", - "2.5-2.10 (Rotations)" + "2.5-2.10 (Rotations)", ], "seed": seed, } @@ -419,23 +435,34 @@ def main(): # Location parameters loc_group = parser.add_argument_group("Location Parameters") loc_group.add_argument( - "--latitude", type=float, default=37.7749, help="Center latitude in degrees (default: 37.7749)" + "--latitude", + type=float, + default=37.7749, + help="Center latitude in degrees (default: 37.7749)", ) loc_group.add_argument( - "--longitude", type=float, default=-122.4194, help="Center longitude in degrees (default: -122.4194)" + "--longitude", + type=float, + default=-122.4194, + help="Center longitude in degrees (default: -122.4194)", ) # Building parameters building_group = parser.add_argument_group("Building Parameters") building_group.add_argument( - "--building-size", type=float, default=50.0, help="Building footprint size in meters (default: 50.0)" + "--building-size", + type=float, + default=50.0, + help="Building footprint size in meters (default: 50.0)", ) building_group.add_argument( "--n-points", type=int, default=20, help="Number of sample points (default: 20)" ) # Other - parser.add_argument("--seed", type=int, default=42, help="Random seed (default: 42)") + parser.add_argument( + "--seed", type=int, default=42, help="Random seed (default: 42)" + ) args = parser.parse_args() @@ -453,4 +480,3 @@ def main(): if __name__ == "__main__": main() - diff --git a/scripts/generate_ch3_estimator_comparison_dataset.py b/scripts/generate_ch3_estimator_comparison_dataset.py index 92eff0c..33a0142 100644 --- a/scripts/generate_ch3_estimator_comparison_dataset.py +++ b/scripts/generate_ch3_estimator_comparison_dataset.py @@ -312,15 +312,10 @@ def generate_dataset( print("\nStep 2: Placing beacons...") if n_beacons == 4: # Square arrangement - beacons = np.array([ - [-15, -15], - [15, -15], - [15, 15], - [-15, 15] - ], dtype=float) + beacons = np.array([[-15, -15], [15, -15], [15, 15], [-15, 15]], dtype=float) elif n_beacons == 8: # Octagon arrangement - angles = np.linspace(0, 2*np.pi, n_beacons, endpoint=False) + angles = np.linspace(0, 2 * np.pi, n_beacons, endpoint=False) radius = 20.0 beacons = radius * np.column_stack([np.cos(angles), np.sin(angles)]) else: @@ -329,7 +324,9 @@ def generate_dataset( beacons = rng.uniform(-20, 20, (n_beacons, 2)) print(f" Beacons: {n_beacons}") - print(f" Configuration: {'square' if n_beacons == 4 else 'octagon' if n_beacons == 8 else 'random'}") + print( + f" Configuration: {'square' if n_beacons == 4 else 'octagon' if n_beacons == 8 else 'random'}" + ) # Generate measurements print("\nStep 3: Generating measurements...") @@ -345,9 +342,12 @@ def generate_dataset( print(f" Outlier rate: {outlier_rate*100:.1f}%") # Compute measurement statistics - true_ranges = np.array([[np.linalg.norm(states[i, :2] - beacons[j]) - for j in range(len(beacons))] - for i in range(len(states))]) + true_ranges = np.array( + [ + [np.linalg.norm(states[i, :2] - beacons[j]) for j in range(len(beacons))] + for i in range(len(states)) + ] + ) range_errors = ranges - true_ranges range_rmse = np.sqrt(np.mean(range_errors**2)) @@ -382,7 +382,7 @@ def generate_dataset( "3.11-3.19 (KF)", "3.21 (EKF)", "3.24-3.30 (UKF)", - "3.32-3.34 (PF)" + "3.32-3.34 (PF)", ], "seed": seed, } @@ -461,7 +461,10 @@ def main(): help="Trajectory type (default: circular)", ) traj_group.add_argument( - "--duration", type=float, default=30.0, help="Duration in seconds (default: 30.0)" + "--duration", + type=float, + default=30.0, + help="Duration in seconds (default: 30.0)", ) traj_group.add_argument( "--dt", type=float, default=0.1, help="Time step in seconds (default: 0.1)" @@ -476,17 +479,28 @@ def main(): # Noise parameters noise_group = parser.add_argument_group("Noise Parameters") noise_group.add_argument( - "--range-noise", type=float, default=0.5, help="Range noise std (m) (default: 0.5)" + "--range-noise", + type=float, + default=0.5, + help="Range noise std (m) (default: 0.5)", ) noise_group.add_argument( - "--bearing-noise", type=float, default=5.0, help="Bearing noise std (deg) (default: 5.0)" + "--bearing-noise", + type=float, + default=5.0, + help="Bearing noise std (deg) (default: 5.0)", ) noise_group.add_argument( - "--outlier-rate", type=float, default=0.0, help="Outlier rate 0-1 (default: 0.0)" + "--outlier-rate", + type=float, + default=0.0, + help="Outlier rate 0-1 (default: 0.0)", ) # Other - parser.add_argument("--seed", type=int, default=42, help="Random seed (default: 42)") + parser.add_argument( + "--seed", type=int, default=42, help="Random seed (default: 42)" + ) args = parser.parse_args() @@ -507,4 +521,3 @@ def main(): if __name__ == "__main__": main() - diff --git a/scripts/generate_ch4_rf_2d_positioning_dataset.py b/scripts/generate_ch4_rf_2d_positioning_dataset.py index 15b1dec..30a8dc9 100644 --- a/scripts/generate_ch4_rf_2d_positioning_dataset.py +++ b/scripts/generate_ch4_rf_2d_positioning_dataset.py @@ -115,12 +115,10 @@ def create_beacon_geometry( """ if geometry_type == "square": # Beacons at corners (good GDOP in center) - beacons = np.array([ - [0, 0], - [area_size, 0], - [area_size, area_size], - [0, area_size] - ], dtype=float) + beacons = np.array( + [[0, 0], [area_size, 0], [area_size, area_size], [0, area_size]], + dtype=float, + ) elif geometry_type == "optimal": # Beacons optimally placed (tetrahedral-like in 2D) @@ -131,21 +129,22 @@ def create_beacon_geometry( elif geometry_type == "linear": # Linear array (poor GDOP perpendicular to line) - beacons = np.array([ - [area_size * 0.2, area_size / 2], - [area_size * 0.4, area_size / 2], - [area_size * 0.6, area_size / 2], - [area_size * 0.8, area_size / 2] - ], dtype=float) + beacons = np.array( + [ + [area_size * 0.2, area_size / 2], + [area_size * 0.4, area_size / 2], + [area_size * 0.6, area_size / 2], + [area_size * 0.8, area_size / 2], + ], + dtype=float, + ) elif geometry_type == "lshape": # L-shaped array (poor GDOP in some regions) - beacons = np.array([ - [0, 0], - [area_size / 2, 0], - [area_size, 0], - [0, area_size / 2] - ], dtype=float) + beacons = np.array( + [[0, 0], [area_size / 2, 0], [area_size, 0], [0, area_size / 2]], + dtype=float, + ) elif geometry_type == "poor": # Clustered beacons (very poor GDOP) @@ -310,8 +309,11 @@ def run_positioning( # four ch4 datasets. def solve_all(solver, measurements): outcome = solve_batch( - solver, measurements, initial_guess, true_positions, - divergence_m=np.inf, # the magnitude check is applied downstream + solver, + measurements, + initial_guess, + true_positions, + divergence_m=np.inf, # the magnitude check is applied downstream ) return outcome.estimates, outcome.solved @@ -514,8 +516,14 @@ def generate_dataset( start = time.time() toa_ranges, tdoa_diffs, aoa_angles = generate_measurements( - beacons, positions, toa_noise, tdoa_noise, aoa_noise_deg, - nlos_beacons, nlos_bias, seed + beacons, + positions, + toa_noise, + tdoa_noise, + aoa_noise_deg, + nlos_beacons, + nlos_bias, + seed, ) elapsed = time.time() - start print(f" Generation time: {elapsed:.3f} s") @@ -529,9 +537,15 @@ def generate_dataset( elapsed = time.time() - start print(f" Computation time: {elapsed:.3f} s") - print(f" TOA GDOP: mean={gdop_toa.mean():.2f}, min={gdop_toa.min():.2f}, max={gdop_toa.max():.2f}") - print(f" TDOA GDOP: mean={gdop_tdoa.mean():.2f}, min={gdop_tdoa.min():.2f}, max={gdop_tdoa.max():.2f}") - print(f" AOA GDOP: mean={gdop_aoa.mean():.2f}, min={gdop_aoa.min():.2f}, max={gdop_aoa.max():.2f}") + print( + f" TOA GDOP: mean={gdop_toa.mean():.2f}, min={gdop_toa.min():.2f}, max={gdop_toa.max():.2f}" + ) + print( + f" TDOA GDOP: mean={gdop_tdoa.mean():.2f}, min={gdop_tdoa.min():.2f}, max={gdop_tdoa.max():.2f}" + ) + print( + f" AOA GDOP: mean={gdop_aoa.mean():.2f}, min={gdop_aoa.min():.2f}, max={gdop_aoa.max():.2f}" + ) # Run positioning print("\nStep 5: Running positioning algorithms...") @@ -713,7 +727,10 @@ def main(): help="Beacon geometry type (default: square)", ) geom_group.add_argument( - "--area-size", type=float, default=20.0, help="Area size in meters (default: 20.0)" + "--area-size", + type=float, + default=20.0, + help="Area size in meters (default: 20.0)", ) # Trajectory parameters @@ -726,19 +743,31 @@ def main(): help="Trajectory type (default: grid)", ) traj_group.add_argument( - "--num-points", type=int, default=100, help="Number of evaluation points (default: 100)" + "--num-points", + type=int, + default=100, + help="Number of evaluation points (default: 100)", ) # Measurement noise parameters noise_group = parser.add_argument_group("Measurement Noise Parameters") noise_group.add_argument( - "--toa-noise", type=float, default=0.1, help="TOA noise std dev in meters (default: 0.1)" + "--toa-noise", + type=float, + default=0.1, + help="TOA noise std dev in meters (default: 0.1)", ) noise_group.add_argument( - "--tdoa-noise", type=float, default=0.1, help="TDOA noise std dev in meters (default: 0.1)" + "--tdoa-noise", + type=float, + default=0.1, + help="TDOA noise std dev in meters (default: 0.1)", ) noise_group.add_argument( - "--aoa-noise", type=float, default=2.0, help="AOA noise std dev in degrees (default: 2.0)" + "--aoa-noise", + type=float, + default=2.0, + help="AOA noise std dev in degrees (default: 2.0)", ) # NLOS parameters @@ -747,11 +776,16 @@ def main(): "--add-nlos", action="store_true", help="Add NLOS bias to beacons 1 and 2" ) nlos_group.add_argument( - "--nlos-bias", type=float, default=0.5, help="NLOS bias in meters (default: 0.5)" + "--nlos-bias", + type=float, + default=0.5, + help="NLOS bias in meters (default: 0.5)", ) # Other - parser.add_argument("--seed", type=int, default=42, help="Random seed (default: 42)") + parser.add_argument( + "--seed", type=int, default=42, help="Random seed (default: 42)" + ) args = parser.parse_args() @@ -774,4 +808,3 @@ def main(): if __name__ == "__main__": main() - diff --git a/scripts/generate_ch5_wifi_fingerprint_dataset.py b/scripts/generate_ch5_wifi_fingerprint_dataset.py index 593c9d5..0e15418 100644 --- a/scripts/generate_ch5_wifi_fingerprint_dataset.py +++ b/scripts/generate_ch5_wifi_fingerprint_dataset.py @@ -33,16 +33,16 @@ def log_distance_path_loss( ) -> float: """ Compute RSS using log-distance path-loss model. - + Model: P(d) = P0 - 10*n*log10(d/d0) + X_sigma - + Args: d: Distance from AP to reference point (meters). P0: Reference power at distance d0 (dBm). d0: Reference distance (meters). n: Path-loss exponent (2.0 = free space, 2-4 = indoor). sigma: Shadow fading standard deviation (dBm). - + Returns: RSS in dBm. """ @@ -72,7 +72,7 @@ def generate_wifi_fingerprint_database( ) -> FingerprintDatabase: """ Generate synthetic Wi-Fi fingerprint database. - + Args: area_size: (width, height) in meters. grid_spacing: Distance between reference points (meters). @@ -84,7 +84,7 @@ def generate_wifi_fingerprint_database( If > 1, creates multi-sample DB (M, S, N) for proper μ and σ estimation per Eq. 5.6. seed: Random seed for reproducibility. - + Returns: FingerprintDatabase with multi-floor RSS fingerprints. """ @@ -101,24 +101,30 @@ def generate_wifi_fingerprint_database( print(f"{'='*60}") print(f"Area size: {width}m × {height}m") print(f"Grid spacing: {grid_spacing}m") - print(f"Grid dimensions: {len(x_coords)} × {len(y_coords)} = {len(x_coords) * len(y_coords)} RPs per floor") + print( + f"Grid dimensions: {len(x_coords)} × {len(y_coords)} = {len(x_coords) * len(y_coords)} RPs per floor" + ) print(f"Floors: {n_floors}") print(f"Total reference points: {len(x_coords) * len(y_coords) * n_floors}") print(f"Access points: {n_aps}") - print(f"Samples per RP: {n_samples_per_rp} {'(multi-sample DB)' if n_samples_per_rp > 1 else '(single-sample DB)'}") + print( + f"Samples per RP: {n_samples_per_rp} {'(multi-sample DB)' if n_samples_per_rp > 1 else '(single-sample DB)'}" + ) # Generate AP positions (strategic placement on walls/ceiling) # APs at corners, mid-walls, and center ceiling of first floor - ap_positions = np.array([ - [0, 0, 2.5], # Corner 1 (wall) - [width, 0, 2.5], # Corner 2 (wall) - [width, height, 2.5], # Corner 3 (wall) - [0, height, 2.5], # Corner 4 (wall) - [width/2, 0, 2.5], # Mid-wall 1 - [width/2, height, 2.5],# Mid-wall 2 - [0, height/2, 2.5], # Mid-wall 3 - [width, height/2, 2.5],# Mid-wall 4 - ])[:n_aps] + ap_positions = np.array( + [ + [0, 0, 2.5], # Corner 1 (wall) + [width, 0, 2.5], # Corner 2 (wall) + [width, height, 2.5], # Corner 3 (wall) + [0, height, 2.5], # Corner 4 (wall) + [width / 2, 0, 2.5], # Mid-wall 1 + [width / 2, height, 2.5], # Mid-wall 2 + [0, height / 2, 2.5], # Mid-wall 3 + [width, height / 2, 2.5], # Mid-wall 4 + ] + )[:n_aps] ap_ids = [f"AP{i+1}" for i in range(n_aps)] @@ -296,20 +302,29 @@ def main(): type=str, default=None, help="Output directory. Defaults to the preset's own directory, or " - "data/sim/ch5_wifi_fingerprint_grid without a preset. Given " - "explicitly it always wins -- a preset does not override it.", + "data/sim/ch5_wifi_fingerprint_grid without a preset. Given " + "explicitly it always wins -- a preset does not override it.", ) # Area parameters area_group = parser.add_argument_group("Area Parameters") area_group.add_argument( - "--area-width", type=float, default=50.0, help="Area width in meters (default: 50.0)" + "--area-width", + type=float, + default=50.0, + help="Area width in meters (default: 50.0)", ) area_group.add_argument( - "--area-height", type=float, default=50.0, help="Area height in meters (default: 50.0)" + "--area-height", + type=float, + default=50.0, + help="Area height in meters (default: 50.0)", ) area_group.add_argument( - "--grid-spacing", type=float, default=5.0, help="Grid spacing in meters (default: 5.0)" + "--grid-spacing", + type=float, + default=5.0, + help="Grid spacing in meters (default: 5.0)", ) # Building parameters @@ -318,7 +333,10 @@ def main(): "--n-floors", type=int, default=3, help="Number of floors (default: 3)" ) building_group.add_argument( - "--floor-height", type=float, default=3.0, help="Floor height in meters (default: 3.0)" + "--floor-height", + type=float, + default=3.0, + help="Floor height in meters (default: 3.0)", ) # AP parameters @@ -330,13 +348,17 @@ def main(): # Survey parameters survey_group = parser.add_argument_group("Survey Parameters") survey_group.add_argument( - "--n-samples", type=int, default=1, + "--n-samples", + type=int, + default=1, help="Number of RSS samples per RP (default: 1). " - "Use >1 for multi-sample DB to estimate μ and σ per Eq. 5.6" + "Use >1 for multi-sample DB to estimate μ and σ per Eq. 5.6", ) # Other - parser.add_argument("--seed", type=int, default=42, help="Random seed (default: 42)") + parser.add_argument( + "--seed", type=int, default=42, help="Random seed (default: 42)" + ) args = parser.parse_args() @@ -415,10 +437,12 @@ def main(): print("\nValidation Results:") print(" OK Database loaded successfully") print(" OK All validation checks passed") - if 'floor_coverage' in stats: + if "floor_coverage" in stats: print(f" Floor coverage: {stats['floor_coverage']}") - if 'feature_variance_min' in stats and 'feature_variance_max' in stats: - print(f" Feature variance: min={stats['feature_variance_min']:.2f}, max={stats['feature_variance_max']:.2f}") + if "feature_variance_min" in stats and "feature_variance_max" in stats: + print( + f" Feature variance: min={stats['feature_variance_min']:.2f}, max={stats['feature_variance_max']:.2f}" + ) # Per-floor statistics print("\nPer-Floor Statistics:") @@ -427,7 +451,9 @@ def main(): n_rps = np.sum(mask) rss_mean = db.features[mask].mean() rss_std = db.features[mask].std() - print(f" Floor {floor_id}: {n_rps} RPs, RSS mean={rss_mean:.1f} dBm, std={rss_std:.1f} dBm") + print( + f" Floor {floor_id}: {n_rps} RPs, RSS mean={rss_mean:.1f} dBm, std={rss_std:.1f} dBm" + ) print(f"\n{'='*60}") print("SUCCESS: Dataset generation complete!") @@ -436,4 +462,3 @@ def main(): if __name__ == "__main__": main() - diff --git a/scripts/generate_ch6_env_sensors_dataset.py b/scripts/generate_ch6_env_sensors_dataset.py index 2cd05ea..655b842 100644 --- a/scripts/generate_ch6_env_sensors_dataset.py +++ b/scripts/generate_ch6_env_sensors_dataset.py @@ -86,8 +86,8 @@ def generate_building_walk( # Constants MAG_NORTH = 20.0 # microTesla (horizontal component) - MAG_DOWN = 40.0 # microTesla (vertical component) - P0 = 101325.0 # Sea level pressure (Pa) + MAG_DOWN = 40.0 # microTesla (vertical component) + P0 = 101325.0 # Sea level pressure (Pa) GRAVITY = 9.81 # Initialize arrays @@ -225,11 +225,13 @@ def generate_building_walk( # made mag_heading return exactly minus the true heading, for a 65.7 deg # mean error that config.json then recorded as if it were a property of # the sensor. - mag_level = np.array([ - MAG_NORTH * np.cos(yaw), - MAG_NORTH * np.sin(yaw), - -MAG_DOWN, - ]) + mag_level = np.array( + [ + MAG_NORTH * np.cos(yaw), + MAG_NORTH * np.sin(yaw), + -MAG_DOWN, + ] + ) # Tilt the level-frame field into the body frame. This is the forward # rotation that mag_tilt_compensate (Eq. 6.52) inverts, so the reading @@ -247,7 +249,7 @@ def generate_building_walk( # Barometric pressure (decreases with altitude) # International barometric formula (Eq. 6.54) T0 = 288.15 # K (15°C at sea level) - L = 0.0065 # K/m (temperature lapse rate) + L = 0.0065 # K/m (temperature lapse rate) M = 0.0289644 # kg/mol (molar mass of air) R = 8.31447 # J/(mol·K) (gas constant) g = GRAVITY @@ -585,7 +587,9 @@ def generate_dataset( altitude_smooth = np.zeros_like(altitude_est) altitude_smooth[0] = altitude_est[0] for k in range(1, len(altitude_est)): - altitude_smooth[k] = smooth_measurement_simple(altitude_smooth[k - 1], altitude_est[k], alpha=0.1) + altitude_smooth[k] = smooth_measurement_simple( + altitude_smooth[k - 1], altitude_est[k], alpha=0.1 + ) # Compute altitude error altitude_true = pos_true[:, 2] @@ -598,7 +602,10 @@ def generate_dataset( current_floor = 0 for k in range(1, len(t)): delta_floor = detect_floor_change( - altitude_smooth[k - 1], altitude_smooth[k], floor_height=floor_height, threshold=1.5 + altitude_smooth[k - 1], + altitude_smooth[k], + floor_height=floor_height, + threshold=1.5, ) current_floor += delta_floor floor_detected[k] = max(0, min(2, current_floor)) @@ -628,7 +635,9 @@ def generate_dataset( "magnetometer": { "noise_std_uT": mag_noise, "disturbances_enabled": mag_disturbance, - "num_disturbance_events": len(disturbance_locations) if disturbance_locations else 0, + "num_disturbance_events": ( + len(disturbance_locations) if disturbance_locations else 0 + ), }, "barometer": { "noise_std_Pa": pressure_noise, @@ -725,10 +734,16 @@ def main(): # Trajectory parameters traj_group = parser.add_argument_group("Trajectory Parameters") traj_group.add_argument( - "--duration", type=float, default=180.0, help="Total duration in seconds (default: 180.0)" + "--duration", + type=float, + default=180.0, + help="Total duration in seconds (default: 180.0)", ) traj_group.add_argument( - "--floor-height", type=float, default=3.5, help="Height of each floor in meters (default: 3.5)" + "--floor-height", + type=float, + default=3.5, + help="Height of each floor in meters (default: 3.5)", ) traj_group.add_argument( "--dt", type=float, default=0.1, help="Time step in seconds (default: 0.1)" @@ -737,20 +752,33 @@ def main(): # Sensor noise parameters noise_group = parser.add_argument_group("Sensor Noise Parameters") noise_group.add_argument( - "--mag-noise", type=float, default=2.0, help="Magnetometer noise in microTesla (default: 2.0)" + "--mag-noise", + type=float, + default=2.0, + help="Magnetometer noise in microTesla (default: 2.0)", ) noise_group.add_argument( - "--add-disturbances", action="store_true", help="Add indoor magnetic disturbances" + "--add-disturbances", + action="store_true", + help="Add indoor magnetic disturbances", ) noise_group.add_argument( - "--pressure-noise", type=float, default=10.0, help="Pressure noise in Pa (default: 10.0)" + "--pressure-noise", + type=float, + default=10.0, + help="Pressure noise in Pa (default: 10.0)", ) noise_group.add_argument( - "--weather-drift", type=float, default=50.0, help="Weather pressure drift in Pa (default: 50.0)" + "--weather-drift", + type=float, + default=50.0, + help="Weather pressure drift in Pa (default: 50.0)", ) # Other - parser.add_argument("--seed", type=int, default=42, help="Random seed (default: 42)") + parser.add_argument( + "--seed", type=int, default=42, help="Random seed (default: 42)" + ) args = parser.parse_args() @@ -771,4 +799,3 @@ def main(): if __name__ == "__main__": main() - diff --git a/scripts/generate_ch6_pdr_dataset.py b/scripts/generate_ch6_pdr_dataset.py index 5f76e9c..c46081f 100644 --- a/scripts/generate_ch6_pdr_dataset.py +++ b/scripts/generate_ch6_pdr_dataset.py @@ -49,7 +49,9 @@ def generate_corridor_walk( leg_length: float = 30.0, step_freq: float = 2.0, dt: float = 0.01, -) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray]: +) -> Tuple[ + np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray +]: """ Generate corridor walk trajectory with turns. @@ -232,7 +234,7 @@ def generate_corridor_walk( for idx, yaw_at_step, forward, vertical in step_samples: lo, hi = idx - pulse_half, idx + pulse_half + 1 window = slice(max(lo, 0), min(hi, N)) - clipped = shape[max(0, -lo):len(shape) - max(0, hi - N)] + clipped = shape[max(0, -lo) : len(shape) - max(0, hi - N)] accel[window, 0] += forward * np.cos(yaw_at_step) * clipped accel[window, 1] += forward * np.sin(yaw_at_step) * clipped accel[window, 2] += vertical * clipped @@ -243,10 +245,15 @@ def generate_corridor_walk( # changes -- smaller than the teleport this replaced, but the same defect. window = max(int(round(0.5 / dt)), 1) speed_smooth = np.convolve(speed_profile, np.ones(window) / window, mode="same") - step_xy = np.column_stack([ - speed_smooth * np.cos(heading), - speed_smooth * np.sin(heading), - ]) * dt + step_xy = ( + np.column_stack( + [ + speed_smooth * np.cos(heading), + speed_smooth * np.sin(heading), + ] + ) + * dt + ) pos = np.cumsum(step_xy, axis=0) - step_xy[0] return t, pos, accel, gyro, mag, heading, step_times @@ -322,7 +329,7 @@ def run_pdr_gyro( for k in range(1, N): # Step detection: simple peak crossing at 11 m/s^2 a_mag = total_accel_magnitude(accel_meas[k]) - is_step = (last_a_mag < 11.0 and a_mag >= 11.0) + is_step = last_a_mag < 11.0 and a_mag >= 11.0 last_a_mag = a_mag if is_step and (t[k] - last_step_time) > 0.3: # Min 0.3s between steps @@ -378,7 +385,7 @@ def run_pdr_mag( for k in range(1, N): # Step detection a_mag = total_accel_magnitude(accel_meas[k]) - is_step = (last_a_mag < 11.0 and a_mag >= 11.0) + is_step = last_a_mag < 11.0 and a_mag >= 11.0 last_a_mag = a_mag if is_step and (t[k] - last_step_time) > 0.3: @@ -567,11 +574,13 @@ def generate_dataset( # Generate trajectory print("\nStep 1: Generating corridor walk...") - t, pos_true, accel_true, gyro_true, mag_true, heading_true, step_times = generate_corridor_walk( - num_legs=num_legs, - leg_length=leg_length, - step_freq=step_freq, - dt=dt, + t, pos_true, accel_true, gyro_true, mag_true, heading_true, step_times = ( + generate_corridor_walk( + num_legs=num_legs, + leg_length=leg_length, + step_freq=step_freq, + dt=dt, + ) ) total_distance = np.sum(np.linalg.norm(np.diff(pos_true, axis=0), axis=1)) @@ -633,7 +642,9 @@ def generate_dataset( print("\nHeading Source Comparison:") print(f" Gyro: {final_error_gyro:.2f}m error (drifts over time)") print(f" Magnetometer: {final_error_mag:.2f}m error (absolute but noisy)") - print(f" Improvement: {final_error_gyro / final_error_mag:.1f}x better with magnetometer") + print( + f" Improvement: {final_error_gyro / final_error_mag:.1f}x better with magnetometer" + ) # Save dataset config = { @@ -757,13 +768,22 @@ def main(): "--num-legs", type=int, default=4, help="Number of corridor legs (default: 4)" ) traj_group.add_argument( - "--leg-length", type=float, default=30.0, help="Length of each leg in meters (default: 30.0)" + "--leg-length", + type=float, + default=30.0, + help="Length of each leg in meters (default: 30.0)", ) traj_group.add_argument( - "--step-freq", type=float, default=2.0, help="Step frequency in Hz (default: 2.0)" + "--step-freq", + type=float, + default=2.0, + help="Step frequency in Hz (default: 2.0)", ) traj_group.add_argument( - "--height", type=float, default=1.75, help="Pedestrian height in meters (default: 1.75)" + "--height", + type=float, + default=1.75, + help="Pedestrian height in meters (default: 1.75)", ) traj_group.add_argument( "--dt", type=float, default=0.01, help="Time step in seconds (default: 0.01)" @@ -772,20 +792,34 @@ def main(): # Sensor noise parameters noise_group = parser.add_argument_group("Sensor Noise Parameters") noise_group.add_argument( - "--accel-noise", type=float, default=0.2, help="Accel noise std dev in m/s^2 (default: 0.2)" + "--accel-noise", + type=float, + default=0.2, + help="Accel noise std dev in m/s^2 (default: 0.2)", ) noise_group.add_argument( - "--gyro-noise", type=float, default=0.01, help="Gyro noise std dev in rad/s (default: 0.01)" + "--gyro-noise", + type=float, + default=0.01, + help="Gyro noise std dev in rad/s (default: 0.01)", ) noise_group.add_argument( - "--gyro-bias", type=float, default=0.005, help="Gyro bias in rad/s (default: 0.005)" + "--gyro-bias", + type=float, + default=0.005, + help="Gyro bias in rad/s (default: 0.005)", ) noise_group.add_argument( - "--mag-noise", type=float, default=0.1, help="Mag noise std dev normalized (default: 0.1)" + "--mag-noise", + type=float, + default=0.1, + help="Mag noise std dev normalized (default: 0.1)", ) # Other - parser.add_argument("--seed", type=int, default=42, help="Random seed (default: 42)") + parser.add_argument( + "--seed", type=int, default=42, help="Random seed (default: 42)" + ) args = parser.parse_args() @@ -808,4 +842,3 @@ def main(): if __name__ == "__main__": main() - diff --git a/scripts/generate_ch6_strapdown_dataset.py b/scripts/generate_ch6_strapdown_dataset.py index 8e157b3..ae52861 100644 --- a/scripts/generate_ch6_strapdown_dataset.py +++ b/scripts/generate_ch6_strapdown_dataset.py @@ -27,37 +27,37 @@ # ============================================================================ PRESETS = { - 'tactical': { - 'description': 'Tactical-grade IMU (low noise, minimal drift)', - 'accel_noise_std': 0.01, - 'gyro_noise_std': 0.001, - 'accel_bias_x': 0.0, - 'accel_bias_y': 0.0, - 'gyro_bias': 0.0, + "tactical": { + "description": "Tactical-grade IMU (low noise, minimal drift)", + "accel_noise_std": 0.01, + "gyro_noise_std": 0.001, + "accel_bias_x": 0.0, + "accel_bias_y": 0.0, + "gyro_bias": 0.0, }, - 'consumer': { - 'description': 'Consumer-grade IMU (smartphone-like, moderate drift)', - 'accel_noise_std': 0.1, - 'gyro_noise_std': 0.01, - 'accel_bias_x': 0.0, - 'accel_bias_y': 0.0, - 'gyro_bias': 0.0, + "consumer": { + "description": "Consumer-grade IMU (smartphone-like, moderate drift)", + "accel_noise_std": 0.1, + "gyro_noise_std": 0.01, + "accel_bias_x": 0.0, + "accel_bias_y": 0.0, + "gyro_bias": 0.0, }, - 'mems': { - 'description': 'MEMS-grade IMU (high noise, significant drift)', - 'accel_noise_std': 0.5, - 'gyro_noise_std': 0.05, - 'accel_bias_x': 0.0, - 'accel_bias_y': 0.0, - 'gyro_bias': 0.0, + "mems": { + "description": "MEMS-grade IMU (high noise, significant drift)", + "accel_noise_std": 0.5, + "gyro_noise_std": 0.05, + "accel_bias_x": 0.0, + "accel_bias_y": 0.0, + "gyro_bias": 0.0, }, - 'biased_consumer': { - 'description': 'Consumer IMU with constant bias (systematic drift)', - 'accel_noise_std': 0.1, - 'gyro_noise_std': 0.01, - 'accel_bias_x': 0.05, - 'accel_bias_y': 0.03, - 'gyro_bias': 0.002, + "biased_consumer": { + "description": "Consumer IMU with constant bias (systematic drift)", + "accel_noise_std": 0.1, + "gyro_noise_std": 0.01, + "accel_bias_x": 0.05, + "accel_bias_y": 0.03, + "gyro_bias": 0.002, }, } @@ -66,6 +66,7 @@ # TRAJECTORY GENERATION # ============================================================================ + def generate_circular_trajectory( radius: float = 10.0, speed: float = 1.0, @@ -73,20 +74,20 @@ def generate_circular_trajectory( duration: float = 60.0, ) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]: """Generate 2D circular trajectory at constant speed. - + Args: radius: Circle radius (meters). speed: Constant speed (m/s). dt: Time step (seconds). duration: Total duration (seconds). - + Returns: Tuple of (t, p_xy, v_xy, yaw): t: timestamps (N,) p_xy: positions (N, 2) in meters v_xy: velocities (N, 2) in m/s yaw: heading angles (N,) in radians - + References: Circular motion for Ch6 drift demonstration. """ @@ -105,7 +106,7 @@ def generate_circular_trajectory( # Velocity (tangent to circle) v_xy = np.zeros((N, 2)) v_xy[:, 0] = -radius * omega * np.sin(theta) # vx = -rω sin(θ) - v_xy[:, 1] = radius * omega * np.cos(theta) # vy = rω cos(θ) + v_xy[:, 1] = radius * omega * np.cos(theta) # vy = rω cos(θ) # Heading (tangent direction) yaw = theta + np.pi / 2 # perpendicular to radius @@ -117,6 +118,7 @@ def generate_circular_trajectory( # IMU MEASUREMENT GENERATION # ============================================================================ + def generate_imu_measurements( t: np.ndarray, v_xy: np.ndarray, @@ -128,7 +130,7 @@ def generate_imu_measurements( gyro_bias: float = 0.0, ) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: """Generate synthetic IMU measurements from ground truth. - + Args: t: timestamps (N,) v_xy: velocities (N, 2) in m/s @@ -138,13 +140,13 @@ def generate_imu_measurements( accel_bias_x: X-axis accelerometer bias (m/s²) accel_bias_y: Y-axis accelerometer bias (m/s²) gyro_bias: Z-axis gyroscope bias (rad/s) - + Returns: Tuple of (t_imu, accel_xy, gyro_z): t_imu: IMU timestamps (N,) accel_xy: 2D accelerations (N, 2) in m/s² gyro_z: Yaw rate (N,) in rad/s - + References: IMU error model from Ch6, Eqs. (6.5), (6.9). """ @@ -163,10 +165,12 @@ def generate_imu_measurements( # noise -- which is how the frame was identified. accel_map_true = np.gradient(v_xy, axis=0) / dt[:, None] cos_y, sin_y = np.cos(yaw), np.sin(yaw) - accel_xy_true = np.column_stack([ - cos_y * accel_map_true[:, 0] + sin_y * accel_map_true[:, 1], - -sin_y * accel_map_true[:, 0] + cos_y * accel_map_true[:, 1], - ]) + accel_xy_true = np.column_stack( + [ + cos_y * accel_map_true[:, 0] + sin_y * accel_map_true[:, 1], + -sin_y * accel_map_true[:, 0] + cos_y * accel_map_true[:, 1], + ] + ) # Compute true yaw rate (derivative of yaw) yaw_unwrapped = np.unwrap(yaw) @@ -174,17 +178,9 @@ def generate_imu_measurements( # Add noise and bias accel_bias = np.array([accel_bias_x, accel_bias_y]) - accel_xy = ( - accel_xy_true - + accel_bias - + np.random.randn(N, 2) * accel_noise_std - ) + accel_xy = accel_xy_true + accel_bias + np.random.randn(N, 2) * accel_noise_std - gyro_z = ( - gyro_z_true - + gyro_bias - + np.random.randn(N) * gyro_noise_std - ) + gyro_z = gyro_z_true + gyro_bias + np.random.randn(N) * gyro_noise_std return t, accel_xy, gyro_z @@ -193,6 +189,7 @@ def generate_imu_measurements( # DATASET GENERATION # ============================================================================ + def generate_ch6_strapdown_dataset( output_dir: str = "data/sim/ch6_strapdown_basic", seed: int = 42, @@ -209,7 +206,7 @@ def generate_ch6_strapdown_dataset( gyro_bias: float = 0.0, ) -> None: """Generate and save IMU strapdown dataset. - + Args: output_dir: Output directory path. seed: Random seed for reproducibility. @@ -241,10 +238,7 @@ def generate_ch6_strapdown_dataset( print(f" IMU rate: {1/dt:.0f} Hz") t, p_xy, v_xy, yaw = generate_circular_trajectory( - radius=radius, - speed=speed, - dt=dt, - duration=duration + radius=radius, speed=speed, dt=dt, duration=duration ) print(f" Generated {len(t)} samples") @@ -253,13 +247,7 @@ def generate_ch6_strapdown_dataset( print(f" Circular motion: omega={omega:.3f} rad/s, period={period:.1f}s") # Save ground truth - np.savez( - output_path / "truth.npz", - t=t, - p_xy=p_xy, - v_xy=v_xy, - yaw=yaw - ) + np.savez(output_path / "truth.npz", t=t, p_xy=p_xy, v_xy=v_xy, yaw=yaw) print(" Saved: truth.npz") # 2. Generate IMU measurements @@ -270,20 +258,17 @@ def generate_ch6_strapdown_dataset( print(f" Gyro bias: {gyro_bias} rad/s") t_imu, accel_xy, gyro_z = generate_imu_measurements( - t, v_xy, yaw, + t, + v_xy, + yaw, accel_noise_std=accel_noise_std, gyro_noise_std=gyro_noise_std, accel_bias_x=accel_bias_x, accel_bias_y=accel_bias_y, - gyro_bias=gyro_bias + gyro_bias=gyro_bias, ) - np.savez( - output_path / "imu.npz", - t=t_imu, - accel_xy=accel_xy, - gyro_z=gyro_z - ) + np.savez(output_path / "imu.npz", t=t_imu, accel_xy=accel_xy, gyro_z=gyro_z) print(f" Generated {len(t_imu)} IMU samples") print(" Saved: imu.npz") @@ -295,26 +280,22 @@ def generate_ch6_strapdown_dataset( "description": "IMU strapdown integration dataset for Ch6", "seed": seed, "duration_sec": duration, - "num_samples": int(len(t)) - }, - "trajectory": { - "type": "circular", - "radius_m": radius, - "speed_m_s": speed + "num_samples": int(len(t)), }, + "trajectory": {"type": "circular", "radius_m": radius, "speed_m_s": speed}, "imu": { "rate_hz": 1 / dt, "dt_sec": dt, "accel_noise_std_m_s2": accel_noise_std, "gyro_noise_std_rad_s": gyro_noise_std, "accel_bias_m_s2": [accel_bias_x, accel_bias_y], - "gyro_bias_rad_s": gyro_bias + "gyro_bias_rad_s": gyro_bias, }, "coordinate_frame": { "description": "ENU (East-North-Up)", "origin": "Circle center at (0, 0)", - "units": "meters" - } + "units": "meters", + }, } with open(output_path / "config.json", "w") as f: @@ -342,6 +323,7 @@ def generate_ch6_strapdown_dataset( # COMMAND-LINE INTERFACE # ============================================================================ + def main(): """Main entry point with CLI argument parsing.""" parser = argparse.ArgumentParser( @@ -364,92 +346,90 @@ def main(): # Longer trajectory python %(prog)s --duration 120 --output data/sim/ch6_strapdown_long -Available presets: """ + ", ".join(PRESETS.keys()) +Available presets: """ + + ", ".join(PRESETS.keys()), ) # Preset configuration parser.add_argument( - '--preset', + "--preset", type=str, choices=PRESETS.keys(), - help='Use preset configuration (overrides individual parameters)' + help="Use preset configuration (overrides individual parameters)", ) # Output parser.add_argument( - '--output', + "--output", type=str, - default='data/sim/ch6_strapdown_basic', - help='Output directory (default: data/sim/ch6_strapdown_basic)' + default="data/sim/ch6_strapdown_basic", + help="Output directory (default: data/sim/ch6_strapdown_basic)", ) parser.add_argument( - '--seed', + "--seed", type=int, default=42, - help='Random seed for reproducibility (default: 42)' + help="Random seed for reproducibility (default: 42)", ) # Trajectory parameters - traj_group = parser.add_argument_group('Trajectory Parameters') + traj_group = parser.add_argument_group("Trajectory Parameters") traj_group.add_argument( - '--radius', + "--radius", type=float, default=10.0, - help='Circle radius in meters (default: 10.0)' + help="Circle radius in meters (default: 10.0)", ) traj_group.add_argument( - '--speed', - type=float, - default=1.0, - help='Constant speed in m/s (default: 1.0)' + "--speed", type=float, default=1.0, help="Constant speed in m/s (default: 1.0)" ) traj_group.add_argument( - '--duration', + "--duration", type=float, default=60.0, - help='Trajectory duration in seconds (default: 60.0)' + help="Trajectory duration in seconds (default: 60.0)", ) traj_group.add_argument( - '--dt', + "--dt", type=float, default=0.01, - help='Time step in seconds (default: 0.01, i.e., 100 Hz)' + help="Time step in seconds (default: 0.01, i.e., 100 Hz)", ) # IMU parameters - imu_group = parser.add_argument_group('IMU Parameters') + imu_group = parser.add_argument_group("IMU Parameters") imu_group.add_argument( - '--accel-noise', + "--accel-noise", type=float, default=0.1, - dest='accel_noise_std', - help='Accelerometer noise std in m/s² (default: 0.1)' + dest="accel_noise_std", + help="Accelerometer noise std in m/s² (default: 0.1)", ) imu_group.add_argument( - '--gyro-noise', + "--gyro-noise", type=float, default=0.01, - dest='gyro_noise_std', - help='Gyroscope noise std in rad/s (default: 0.01)' + dest="gyro_noise_std", + help="Gyroscope noise std in rad/s (default: 0.01)", ) imu_group.add_argument( - '--accel-bias-x', + "--accel-bias-x", type=float, default=0.0, - help='Accelerometer X-axis bias in m/s² (default: 0.0)' + help="Accelerometer X-axis bias in m/s² (default: 0.0)", ) imu_group.add_argument( - '--accel-bias-y', + "--accel-bias-y", type=float, default=0.0, - help='Accelerometer Y-axis bias in m/s² (default: 0.0)' + help="Accelerometer Y-axis bias in m/s² (default: 0.0)", ) imu_group.add_argument( - '--gyro-bias', + "--gyro-bias", type=float, default=0.0, - help='Gyroscope Z-axis bias in rad/s (default: 0.0)' + help="Gyroscope Z-axis bias in rad/s (default: 0.0)", ) # Parse arguments @@ -463,7 +443,7 @@ def main(): # Override parameters with preset values for key, value in preset_config.items(): - if key != 'description' and hasattr(args, key): + if key != "description" and hasattr(args, key): setattr(args, key, value) # Validate parameters @@ -490,10 +470,9 @@ def main(): gyro_noise_std=args.gyro_noise_std, accel_bias_x=args.accel_bias_x, accel_bias_y=args.accel_bias_y, - gyro_bias=args.gyro_bias + gyro_bias=args.gyro_bias, ) if __name__ == "__main__": main() - diff --git a/scripts/generate_ch6_wheel_odom_dataset.py b/scripts/generate_ch6_wheel_odom_dataset.py index 731479e..2aa9ffc 100644 --- a/scripts/generate_ch6_wheel_odom_dataset.py +++ b/scripts/generate_ch6_wheel_odom_dataset.py @@ -107,7 +107,9 @@ def generate_square_trajectory( elif time_in_lap < 2 * straight_time + 2 * turn_time: # Turn 2 omega_z = turn_omega v_forward = speed * 0.8 - yaw_target = np.pi / 2 + (time_in_lap - 2 * straight_time - turn_time) * turn_omega + yaw_target = ( + np.pi / 2 + (time_in_lap - 2 * straight_time - turn_time) * turn_omega + ) elif time_in_lap < 3 * straight_time + 2 * turn_time: # Side 3: West omega_z = 0.0 v_forward = speed @@ -115,7 +117,9 @@ def generate_square_trajectory( elif time_in_lap < 3 * straight_time + 3 * turn_time: # Turn 3 omega_z = turn_omega v_forward = speed * 0.8 - yaw_target = np.pi + (time_in_lap - 3 * straight_time - 2 * turn_time) * turn_omega + yaw_target = ( + np.pi + (time_in_lap - 3 * straight_time - 2 * turn_time) * turn_omega + ) elif time_in_lap < 4 * straight_time + 3 * turn_time: # Side 4: South omega_z = 0.0 v_forward = speed @@ -123,7 +127,10 @@ def generate_square_trajectory( else: # Turn 4 omega_z = turn_omega v_forward = speed * 0.8 - yaw_target = 3 * np.pi / 2 + (time_in_lap - 4 * straight_time - 3 * turn_time) * turn_omega + yaw_target = ( + 3 * np.pi / 2 + + (time_in_lap - 4 * straight_time - 3 * turn_time) * turn_omega + ) # Update yaw yaw = yaw_target @@ -218,7 +225,7 @@ def add_wheel_noise( for start_t, end_t in slip_intervals: mask = (t >= start_t) & (t <= end_t) # Slip: reduce measured wheel speed (wheel spins but vehicle doesn't move) - wheel_meas[mask, 0] *= (1.0 + slip_magnitude) + wheel_meas[mask, 0] *= 1.0 + slip_magnitude return wheel_meas, gyro_meas @@ -417,12 +424,14 @@ def generate_dataset( # Generate trajectory print("\nStep 1: Generating square trajectory...") - t, pos_true, vel_true, quat_true, wheel_true, gyro_true = generate_square_trajectory( - side_length=side_length, - speed=speed, - num_laps=num_laps, - dt=dt, - lever_arm=np.array(lever_arm), + t, pos_true, vel_true, quat_true, wheel_true, gyro_true = ( + generate_square_trajectory( + side_length=side_length, + speed=speed, + num_laps=num_laps, + dt=dt, + lever_arm=np.array(lever_arm), + ) ) total_distance = np.sum(np.linalg.norm(np.diff(pos_true, axis=0), axis=1)) @@ -446,7 +455,9 @@ def generate_dataset( t_offset = lap * lap_time # 4 turns per lap for turn_idx in range(4): - t_start = t_offset + (turn_idx + 1) * straight_time + turn_idx * turn_time + t_start = ( + t_offset + (turn_idx + 1) * straight_time + turn_idx * turn_time + ) t_end = t_start + turn_time slip_intervals.append((t_start, t_end)) @@ -609,7 +620,10 @@ def main(): # Trajectory parameters traj_group = parser.add_argument_group("Trajectory Parameters") traj_group.add_argument( - "--side-length", type=float, default=20.0, help="Square side length in meters (default: 20.0)" + "--side-length", + type=float, + default=20.0, + help="Square side length in meters (default: 20.0)", ) traj_group.add_argument( "--speed", type=float, default=5.0, help="Forward speed in m/s (default: 5.0)" @@ -624,16 +638,28 @@ def main(): # Sensor noise parameters noise_group = parser.add_argument_group("Sensor Noise Parameters") noise_group.add_argument( - "--encoder-noise", type=float, default=0.05, help="Encoder noise std dev in m/s (default: 0.05)" + "--encoder-noise", + type=float, + default=0.05, + help="Encoder noise std dev in m/s (default: 0.05)", ) noise_group.add_argument( - "--gyro-noise", type=float, default=0.001, help="Gyro noise std dev in rad/s (default: 0.001)" + "--gyro-noise", + type=float, + default=0.001, + help="Gyro noise std dev in rad/s (default: 0.001)", ) noise_group.add_argument( - "--wheel-bias", type=float, default=0.01, help="Wheel speed bias in m/s (default: 0.01)" + "--wheel-bias", + type=float, + default=0.01, + help="Wheel speed bias in m/s (default: 0.01)", ) noise_group.add_argument( - "--gyro-bias", type=float, default=0.0005, help="Gyro bias in rad/s (default: 0.0005)" + "--gyro-bias", + type=float, + default=0.0005, + help="Gyro bias in rad/s (default: 0.0005)", ) # Lever arm @@ -660,7 +686,9 @@ def main(): ) # Other - parser.add_argument("--seed", type=int, default=42, help="Random seed (default: 42)") + parser.add_argument( + "--seed", type=int, default=42, help="Random seed (default: 42)" + ) args = parser.parse_args() @@ -685,4 +713,3 @@ def main(): if __name__ == "__main__": main() - diff --git a/scripts/generate_ch6_zupt_dataset.py b/scripts/generate_ch6_zupt_dataset.py index 1bd2ece..b3b5c8a 100644 --- a/scripts/generate_ch6_zupt_dataset.py +++ b/scripts/generate_ch6_zupt_dataset.py @@ -27,37 +27,37 @@ # ============================================================================ PRESETS = { - 'baseline': { - 'description': 'Standard walking with clear stance phases', - 'step_length': 0.7, - 'step_duration': 0.6, - 'stance_duration': 0.2, - 'accel_noise_std': 0.1, - 'gyro_noise_std': 0.01, + "baseline": { + "description": "Standard walking with clear stance phases", + "step_length": 0.7, + "step_duration": 0.6, + "stance_duration": 0.2, + "accel_noise_std": 0.1, + "gyro_noise_std": 0.01, }, - 'fast_walk': { - 'description': 'Fast walking with shorter stance phases', - 'step_length': 0.9, - 'step_duration': 0.5, - 'stance_duration': 0.15, - 'accel_noise_std': 0.1, - 'gyro_noise_std': 0.01, + "fast_walk": { + "description": "Fast walking with shorter stance phases", + "step_length": 0.9, + "step_duration": 0.5, + "stance_duration": 0.15, + "accel_noise_std": 0.1, + "gyro_noise_std": 0.01, }, - 'slow_walk': { - 'description': 'Slow walking with longer stance phases', - 'step_length': 0.5, - 'step_duration': 0.8, - 'stance_duration': 0.3, - 'accel_noise_std': 0.1, - 'gyro_noise_std': 0.01, + "slow_walk": { + "description": "Slow walking with longer stance phases", + "step_length": 0.5, + "step_duration": 0.8, + "stance_duration": 0.3, + "accel_noise_std": 0.1, + "gyro_noise_std": 0.01, }, - 'noisy_imu': { - 'description': 'Degraded IMU to test ZUPT robustness', - 'step_length': 0.7, - 'step_duration': 0.6, - 'stance_duration': 0.2, - 'accel_noise_std': 0.3, - 'gyro_noise_std': 0.03, + "noisy_imu": { + "description": "Degraded IMU to test ZUPT robustness", + "step_length": 0.7, + "step_duration": 0.6, + "stance_duration": 0.2, + "accel_noise_std": 0.3, + "gyro_noise_std": 0.03, }, } @@ -66,6 +66,7 @@ # TRAJECTORY GENERATION # ============================================================================ + def generate_walking_trajectory( num_steps: int = 20, step_length: float = 0.7, @@ -74,14 +75,14 @@ def generate_walking_trajectory( dt: float = 0.01, ) -> Tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, np.ndarray]: """Generate 2D walking trajectory with stance phases. - + Args: num_steps: Number of steps to take. step_length: Distance per step (meters). step_duration: Time per step including stance (seconds). stance_duration: Time foot is stationary per step (seconds). dt: Time step (seconds). - + Returns: Tuple of (t, p_xy, v_xy, yaw, is_stance): t: timestamps (N,) @@ -89,7 +90,7 @@ def generate_walking_trajectory( v_xy: velocities (N, 2) in m/s yaw: heading angles (N,) in radians (constant, walking forward) is_stance: stance phase indicator (N,) boolean - + References: Walking gait model for ZUPT demonstration (Ch6, Section 6.3). """ @@ -151,6 +152,7 @@ def generate_walking_trajectory( # IMU MEASUREMENT GENERATION # ============================================================================ + def generate_imu_measurements( t: np.ndarray, v_xy: np.ndarray, @@ -162,7 +164,7 @@ def generate_imu_measurements( gyro_bias: float = 0.0, ) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: """Generate synthetic IMU measurements from ground truth. - + Args: t: timestamps (N,) v_xy: velocities (N, 2) in m/s @@ -172,13 +174,13 @@ def generate_imu_measurements( accel_bias_x: X-axis accelerometer bias (m/s²) accel_bias_y: Y-axis accelerometer bias (m/s²) gyro_bias: Z-axis gyroscope bias (rad/s) - + Returns: Tuple of (t_imu, accel_xy, gyro_z): t_imu: IMU timestamps (N,) accel_xy: 2D accelerations (N, 2) in m/s² gyro_z: Yaw rate (N,) in rad/s - + References: IMU error model from Ch6, Eqs. (6.5), (6.9). """ @@ -197,10 +199,12 @@ def generate_imu_measurements( # noise -- which is how the frame was identified. accel_map_true = np.gradient(v_xy, axis=0) / dt[:, None] cos_y, sin_y = np.cos(yaw), np.sin(yaw) - accel_xy_true = np.column_stack([ - cos_y * accel_map_true[:, 0] + sin_y * accel_map_true[:, 1], - -sin_y * accel_map_true[:, 0] + cos_y * accel_map_true[:, 1], - ]) + accel_xy_true = np.column_stack( + [ + cos_y * accel_map_true[:, 0] + sin_y * accel_map_true[:, 1], + -sin_y * accel_map_true[:, 0] + cos_y * accel_map_true[:, 1], + ] + ) # Compute true yaw rate (derivative of yaw) yaw_unwrapped = np.unwrap(yaw) @@ -208,17 +212,9 @@ def generate_imu_measurements( # Add noise and bias accel_bias = np.array([accel_bias_x, accel_bias_y]) - accel_xy = ( - accel_xy_true - + accel_bias - + np.random.randn(N, 2) * accel_noise_std - ) + accel_xy = accel_xy_true + accel_bias + np.random.randn(N, 2) * accel_noise_std - gyro_z = ( - gyro_z_true - + gyro_bias - + np.random.randn(N) * gyro_noise_std - ) + gyro_z = gyro_z_true + gyro_bias + np.random.randn(N) * gyro_noise_std return t, accel_xy, gyro_z @@ -227,6 +223,7 @@ def generate_imu_measurements( # DATASET GENERATION # ============================================================================ + def generate_ch6_zupt_dataset( output_dir: str = "data/sim/ch6_foot_zupt_walk", seed: int = 42, @@ -244,7 +241,7 @@ def generate_ch6_zupt_dataset( gyro_bias: float = 0.0, ) -> None: """Generate and save foot-mounted IMU with ZUPT dataset. - + Args: output_dir: Output directory path. seed: Random seed for reproducibility. @@ -282,7 +279,7 @@ def generate_ch6_zupt_dataset( step_length=step_length, step_duration=step_duration, stance_duration=stance_duration, - dt=dt + dt=dt, ) print(f" Generated {len(t)} samples") @@ -300,7 +297,7 @@ def generate_ch6_zupt_dataset( p_xy=p_xy, v_xy=v_xy, yaw=yaw, - is_stance=is_stance + is_stance=is_stance, ) print(" Saved: truth.npz") @@ -312,20 +309,17 @@ def generate_ch6_zupt_dataset( print(f" Gyro bias: {gyro_bias} rad/s") t_imu, accel_xy, gyro_z = generate_imu_measurements( - t, v_xy, yaw, + t, + v_xy, + yaw, accel_noise_std=accel_noise_std, gyro_noise_std=gyro_noise_std, accel_bias_x=accel_bias_x, accel_bias_y=accel_bias_y, - gyro_bias=gyro_bias + gyro_bias=gyro_bias, ) - np.savez( - output_path / "imu.npz", - t=t_imu, - accel_xy=accel_xy, - gyro_z=gyro_z - ) + np.savez(output_path / "imu.npz", t=t_imu, accel_xy=accel_xy, gyro_z=gyro_z) print(f" Generated {len(t_imu)} IMU samples") print(" Saved: imu.npz") @@ -339,7 +333,7 @@ def generate_ch6_zupt_dataset( "seed": seed, "duration_sec": float(duration), "num_samples": int(len(t)), - "total_distance_m": float(total_distance) + "total_distance_m": float(total_distance), }, "trajectory": { "type": "walking_linear", @@ -348,7 +342,7 @@ def generate_ch6_zupt_dataset( "step_duration_sec": step_duration, "stance_duration_sec": stance_duration, "swing_duration_sec": float(swing_duration), - "stance_ratio": float(stance_ratio) + "stance_ratio": float(stance_ratio), }, "imu": { "rate_hz": 1 / dt, @@ -356,17 +350,17 @@ def generate_ch6_zupt_dataset( "accel_noise_std_m_s2": accel_noise_std, "gyro_noise_std_rad_s": gyro_noise_std, "accel_bias_m_s2": [accel_bias_x, accel_bias_y], - "gyro_bias_rad_s": gyro_bias + "gyro_bias_rad_s": gyro_bias, }, "zupt": { "stance_threshold_description": "Use is_stance from truth.npz for ideal ZUPT", - "detection_note": "In practice, detect stance from IMU statistics" + "detection_note": "In practice, detect stance from IMU statistics", }, "coordinate_frame": { "description": "ENU (East-North-Up)", "origin": "Starting position at (0, 0)", - "units": "meters" - } + "units": "meters", + }, } with open(output_path / "config.json", "w") as f: @@ -396,6 +390,7 @@ def generate_ch6_zupt_dataset( # COMMAND-LINE INTERFACE # ============================================================================ + def main(): """Main entry point with CLI argument parsing.""" parser = argparse.ArgumentParser( @@ -418,98 +413,99 @@ def main(): # Test ZUPT with noisy IMU python %(prog)s --preset noisy_imu --output data/sim/ch6_zupt_noisy -Available presets: """ + ", ".join(PRESETS.keys()) +Available presets: """ + + ", ".join(PRESETS.keys()), ) # Preset configuration parser.add_argument( - '--preset', + "--preset", type=str, choices=PRESETS.keys(), - help='Use preset configuration (overrides individual parameters)' + help="Use preset configuration (overrides individual parameters)", ) # Output parser.add_argument( - '--output', + "--output", type=str, - default='data/sim/ch6_foot_zupt_walk', - help='Output directory (default: data/sim/ch6_foot_zupt_walk)' + default="data/sim/ch6_foot_zupt_walk", + help="Output directory (default: data/sim/ch6_foot_zupt_walk)", ) parser.add_argument( - '--seed', + "--seed", type=int, default=42, - help='Random seed for reproducibility (default: 42)' + help="Random seed for reproducibility (default: 42)", ) # Trajectory parameters - traj_group = parser.add_argument_group('Trajectory Parameters') + traj_group = parser.add_argument_group("Trajectory Parameters") traj_group.add_argument( - '--num-steps', + "--num-steps", type=int, default=20, - help='Number of steps to take (default: 20)' + help="Number of steps to take (default: 20)", ) traj_group.add_argument( - '--step-length', + "--step-length", type=float, default=0.7, - help='Distance per step in meters (default: 0.7)' + help="Distance per step in meters (default: 0.7)", ) traj_group.add_argument( - '--step-duration', + "--step-duration", type=float, default=0.6, - help='Time per step in seconds (default: 0.6)' + help="Time per step in seconds (default: 0.6)", ) traj_group.add_argument( - '--stance-duration', + "--stance-duration", type=float, default=0.2, - help='Time foot is stationary per step in seconds (default: 0.2)' + help="Time foot is stationary per step in seconds (default: 0.2)", ) traj_group.add_argument( - '--dt', + "--dt", type=float, default=0.01, - help='Time step in seconds (default: 0.01, i.e., 100 Hz)' + help="Time step in seconds (default: 0.01, i.e., 100 Hz)", ) # IMU parameters - imu_group = parser.add_argument_group('IMU Parameters') + imu_group = parser.add_argument_group("IMU Parameters") imu_group.add_argument( - '--accel-noise', + "--accel-noise", type=float, default=0.1, - dest='accel_noise_std', - help='Accelerometer noise std in m/s² (default: 0.1)' + dest="accel_noise_std", + help="Accelerometer noise std in m/s² (default: 0.1)", ) imu_group.add_argument( - '--gyro-noise', + "--gyro-noise", type=float, default=0.01, - dest='gyro_noise_std', - help='Gyroscope noise std in rad/s (default: 0.01)' + dest="gyro_noise_std", + help="Gyroscope noise std in rad/s (default: 0.01)", ) imu_group.add_argument( - '--accel-bias-x', + "--accel-bias-x", type=float, default=0.0, - help='Accelerometer X-axis bias in m/s² (default: 0.0)' + help="Accelerometer X-axis bias in m/s² (default: 0.0)", ) imu_group.add_argument( - '--accel-bias-y', + "--accel-bias-y", type=float, default=0.0, - help='Accelerometer Y-axis bias in m/s² (default: 0.0)' + help="Accelerometer Y-axis bias in m/s² (default: 0.0)", ) imu_group.add_argument( - '--gyro-bias', + "--gyro-bias", type=float, default=0.0, - help='Gyroscope Z-axis bias in rad/s (default: 0.0)' + help="Gyroscope Z-axis bias in rad/s (default: 0.0)", ) # Parse arguments @@ -523,9 +519,9 @@ def main(): # Override parameters with preset values for key, value in preset_config.items(): - if key != 'description': + if key != "description": # Handle both underscore and hyphen versions - key_underscore = key.replace('-', '_') + key_underscore = key.replace("-", "_") if hasattr(args, key_underscore): setattr(args, key_underscore, value) @@ -556,11 +552,9 @@ def main(): gyro_noise_std=args.gyro_noise_std, accel_bias_x=args.accel_bias_x, accel_bias_y=args.accel_bias_y, - gyro_bias=args.gyro_bias + gyro_bias=args.gyro_bias, ) if __name__ == "__main__": main() - - diff --git a/scripts/generate_ch7_slam_2d_dataset.py b/scripts/generate_ch7_slam_2d_dataset.py index 0b5239a..5dd8efa 100644 --- a/scripts/generate_ch7_slam_2d_dataset.py +++ b/scripts/generate_ch7_slam_2d_dataset.py @@ -90,9 +90,9 @@ def generate_trajectory( n_total = n_poses_per_side * 4 for i in range(n_total): t = 2 * np.pi * i / n_total - x = size/2 * np.sin(t) - y = size/4 * np.sin(2 * t) - yaw = np.arctan2(size/2 * np.cos(2 * t), size/2 * np.cos(t)) + x = size / 2 * np.sin(t) + y = size / 4 * np.sin(2 * t) + yaw = np.arctan2(size / 2 * np.cos(2 * t), size / 2 * np.cos(t)) poses.append(np.array([x, y, yaw])) elif trajectory_type == "random_walk": @@ -103,7 +103,7 @@ def generate_trajectory( for _ in range(n_poses_per_side * 4): # Random forward/turn forward = rng.uniform(0.5, 2.0) - turn = rng.uniform(-np.pi/6, np.pi/6) + turn = rng.uniform(-np.pi / 6, np.pi / 6) # Update pose pose[2] += turn @@ -143,11 +143,7 @@ def generate_landmarks( x_max, y_max = positions.max(axis=0) + area_margin # Generate random landmarks in area - landmarks = rng.uniform( - [x_min, y_min], - [x_max, y_max], - (n_landmarks, 2) - ) + landmarks = rng.uniform([x_min, y_min], [x_max, y_max], (n_landmarks, 2)) return landmarks @@ -306,8 +302,7 @@ def save_dataset( # Save scans (as compressed numpy) np.savez_compressed( - output_dir / "scans.npz", - **{f"scan_{i}": scan for i, scan in enumerate(scans)} + output_dir / "scans.npz", **{f"scan_{i}": scan for i, scan in enumerate(scans)} ) # Save loop closures @@ -428,9 +423,7 @@ def generate_dataset( # Add odometry noise print("\nStep 4: Adding odometry drift...") - odom_poses = add_odometry_noise( - true_poses, translation_noise, rotation_noise, seed - ) + odom_poses = add_odometry_noise(true_poses, translation_noise, rotation_noise, seed) drift_error = np.linalg.norm( np.array(odom_poses[-1][:2]) - np.array(true_poses[-1][:2]) ) @@ -440,7 +433,9 @@ def generate_dataset( # Detect loop closures print("\nStep 5: Detecting loop closures...") - loop_closures = detect_loop_closures(true_poses, min_index_diff=15, max_distance=2.0) + loop_closures = detect_loop_closures( + true_poses, min_index_diff=15, max_distance=2.0 + ) print(f" Loop closures detected: {len(loop_closures)}") if loop_closures: print(f" Examples: {loop_closures[:3]}") @@ -553,10 +548,16 @@ def main(): help="Trajectory type (default: square)", ) traj_group.add_argument( - "--size", type=float, default=20.0, help="Trajectory size in meters (default: 20.0)" + "--size", + type=float, + default=20.0, + help="Trajectory size in meters (default: 20.0)", ) traj_group.add_argument( - "--n-poses-per-side", type=int, default=10, help="Poses per segment (default: 10)" + "--n-poses-per-side", + type=int, + default=10, + help="Poses per segment (default: 10)", ) # Environment parameters @@ -565,23 +566,37 @@ def main(): "--n-landmarks", type=int, default=50, help="Number of landmarks (default: 50)" ) env_group.add_argument( - "--max-range", type=float, default=15.0, help="Sensor max range in meters (default: 15.0)" + "--max-range", + type=float, + default=15.0, + help="Sensor max range in meters (default: 15.0)", ) # Noise parameters noise_group = parser.add_argument_group("Noise Parameters") noise_group.add_argument( - "--translation-noise", type=float, default=0.1, help="Odometry translation noise std (m) (default: 0.1)" + "--translation-noise", + type=float, + default=0.1, + help="Odometry translation noise std (m) (default: 0.1)", ) noise_group.add_argument( - "--rotation-noise", type=float, default=0.02, help="Odometry rotation noise std (rad) (default: 0.02)" + "--rotation-noise", + type=float, + default=0.02, + help="Odometry rotation noise std (rad) (default: 0.02)", ) noise_group.add_argument( - "--scan-noise", type=float, default=0.05, help="Scan range noise std (m) (default: 0.05)" + "--scan-noise", + type=float, + default=0.05, + help="Scan range noise std (m) (default: 0.05)", ) # Other - parser.add_argument("--seed", type=int, default=42, help="Random seed (default: 42)") + parser.add_argument( + "--seed", type=int, default=42, help="Random seed (default: 42)" + ) args = parser.parse_args() @@ -603,4 +618,3 @@ def main(): if __name__ == "__main__": main() - diff --git a/scripts/generate_ch8_fusion_2d_imu_uwb_dataset.py b/scripts/generate_ch8_fusion_2d_imu_uwb_dataset.py index 6fec486..844a191 100644 --- a/scripts/generate_ch8_fusion_2d_imu_uwb_dataset.py +++ b/scripts/generate_ch8_fusion_2d_imu_uwb_dataset.py @@ -28,79 +28,79 @@ #: Where each preset writes when the caller does not pass --output. PRESET_DIRS = { - 'baseline': 'data/sim/ch8_fusion_2d_imu_uwb', + "baseline": "data/sim/ch8_fusion_2d_imu_uwb", # NOT ch8_fusion_2d_imu_uwb_nlos. That shipped dataset uses a 0.8 m bias -- # see its README and --all-variants -- while this preset is the more severe # 1.5 m case, so pointing it there would overwrite the shipped data with a # different scenario under the same name. - 'nlos_severe': 'data/sim/ch8_fusion_2d_imu_uwb_nlos_severe', - 'time_offset_50ms': 'data/sim/ch8_fusion_2d_imu_uwb_timeoffset', - 'high_dropout': 'data/sim/ch8_fusion_2d_imu_uwb_high_dropout', - 'degraded_imu': 'data/sim/ch8_fusion_2d_imu_uwb_degraded_imu', - 'tactical_imu': 'data/sim/ch8_fusion_2d_imu_uwb_tactical_imu', + "nlos_severe": "data/sim/ch8_fusion_2d_imu_uwb_nlos_severe", + "time_offset_50ms": "data/sim/ch8_fusion_2d_imu_uwb_timeoffset", + "high_dropout": "data/sim/ch8_fusion_2d_imu_uwb_high_dropout", + "degraded_imu": "data/sim/ch8_fusion_2d_imu_uwb_degraded_imu", + "tactical_imu": "data/sim/ch8_fusion_2d_imu_uwb_tactical_imu", } PRESETS = { - 'baseline': { - 'description': 'Standard configuration with nominal parameters', - 'accel_noise_std': 0.1, - 'gyro_noise_std': 0.01, - 'range_noise_std': 0.05, - 'nlos_anchors': [], - 'nlos_bias': 0.0, - 'dropout_rate': 0.05, - 'time_offset_sec': 0.0, + "baseline": { + "description": "Standard configuration with nominal parameters", + "accel_noise_std": 0.1, + "gyro_noise_std": 0.01, + "range_noise_std": 0.05, + "nlos_anchors": [], + "nlos_bias": 0.0, + "dropout_rate": 0.05, + "time_offset_sec": 0.0, }, - 'nlos_severe': { - 'description': 'Severe NLOS on 2 anchors to test robust loss functions', - 'accel_noise_std': 0.1, - 'gyro_noise_std': 0.01, - 'range_noise_std': 0.05, - 'nlos_anchors': [1, 2], - 'nlos_bias': 1.5, - 'dropout_rate': 0.05, - 'time_offset_sec': 0.0, + "nlos_severe": { + "description": "Severe NLOS on 2 anchors to test robust loss functions", + "accel_noise_std": 0.1, + "gyro_noise_std": 0.01, + "range_noise_std": 0.05, + "nlos_anchors": [1, 2], + "nlos_bias": 1.5, + "dropout_rate": 0.05, + "time_offset_sec": 0.0, }, - 'high_dropout': { - 'description': 'High dropout rate to test multi-rate fusion', - 'accel_noise_std': 0.1, - 'gyro_noise_std': 0.01, - 'range_noise_std': 0.05, - 'nlos_anchors': [], - 'nlos_bias': 0.0, - 'dropout_rate': 0.3, - 'time_offset_sec': 0.0, + "high_dropout": { + "description": "High dropout rate to test multi-rate fusion", + "accel_noise_std": 0.1, + "gyro_noise_std": 0.01, + "range_noise_std": 0.05, + "nlos_anchors": [], + "nlos_bias": 0.0, + "dropout_rate": 0.3, + "time_offset_sec": 0.0, }, - 'degraded_imu': { - 'description': 'Poor IMU quality (MEMS-grade) to test IMU drift', - 'accel_noise_std': 0.5, - 'gyro_noise_std': 0.05, - 'range_noise_std': 0.05, - 'nlos_anchors': [], - 'nlos_bias': 0.0, - 'dropout_rate': 0.05, - 'time_offset_sec': 0.0, + "degraded_imu": { + "description": "Poor IMU quality (MEMS-grade) to test IMU drift", + "accel_noise_std": 0.5, + "gyro_noise_std": 0.05, + "range_noise_std": 0.05, + "nlos_anchors": [], + "nlos_bias": 0.0, + "dropout_rate": 0.05, + "time_offset_sec": 0.0, }, - 'time_offset_50ms': { - 'description': 'UWB 50ms behind IMU with clock drift', - 'accel_noise_std': 0.1, - 'gyro_noise_std': 0.01, - 'range_noise_std': 0.05, - 'nlos_anchors': [], - 'nlos_bias': 0.0, - 'dropout_rate': 0.05, - 'time_offset_sec': -0.05, - 'clock_drift': 0.0001, + "time_offset_50ms": { + "description": "UWB 50ms behind IMU with clock drift", + "accel_noise_std": 0.1, + "gyro_noise_std": 0.01, + "range_noise_std": 0.05, + "nlos_anchors": [], + "nlos_bias": 0.0, + "dropout_rate": 0.05, + "time_offset_sec": -0.05, + "clock_drift": 0.0001, }, - 'tactical_imu': { - 'description': 'Tactical-grade IMU (low noise)', - 'accel_noise_std': 0.01, - 'gyro_noise_std': 0.001, - 'range_noise_std': 0.05, - 'nlos_anchors': [], - 'nlos_bias': 0.0, - 'dropout_rate': 0.05, - 'time_offset_sec': 0.0, + "tactical_imu": { + "description": "Tactical-grade IMU (low noise)", + "accel_noise_std": 0.01, + "gyro_noise_std": 0.001, + "range_noise_std": 0.05, + "nlos_anchors": [], + "nlos_bias": 0.0, + "dropout_rate": 0.05, + "time_offset_sec": 0.0, }, } @@ -138,7 +138,7 @@ def generate_rectangular_trajectory( duration: Total duration (seconds). corner_radius: Turn radius at each corner (meters). Must be positive and no more than half the shorter side. - + Returns: Tuple of (t, p_xy, v_xy, yaw): t: timestamps (N,) @@ -161,8 +161,8 @@ def generate_rectangular_trajectory( # r at each end, joined by four quarter-circle arcs. Walking it at a # constant speed makes position, velocity and yaw all continuous, and the # yaw rate piecewise constant at 0 or speed / r. - straight_x = width - 2 * r # length of the sides parallel to x - straight_y = height - 2 * r # length of the sides parallel to y + straight_x = width - 2 * r # length of the sides parallel to x + straight_y = height - 2 * r # length of the sides parallel to y arc = 0.5 * np.pi * r perimeter = 2 * (straight_x + straight_y) + 4 * arc @@ -200,9 +200,7 @@ def generate_rectangular_trajectory( # sits at radius r from the centre, 90 degrees to its right. heading = heading0 + travelled / r outward = heading - 0.5 * np.pi - position = anchor + r * np.array( - [np.cos(outward), np.sin(outward)] - ) + position = anchor + r * np.array([np.cos(outward), np.sin(outward)]) p_xy[i] = position v_xy[i] = speed * np.array([np.cos(heading), np.sin(heading)]) yaw[i] = heading % (2 * np.pi) @@ -221,7 +219,7 @@ def generate_imu_measurements( gyro_bias: float = 0.0, ) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: """Generate synthetic IMU measurements from ground truth. - + Args: t: timestamps (N,) v_xy: velocities (N, 2) in m/s @@ -230,7 +228,7 @@ def generate_imu_measurements( gyro_noise_std: Gyroscope noise std (rad/s) accel_bias: Accelerometer bias (2,) in m/s² (default [0, 0]) gyro_bias: Gyroscope bias in rad/s - + Returns: Tuple of (t_imu, accel_xy, gyro_z): t_imu: IMU timestamps (N,) @@ -251,17 +249,9 @@ def generate_imu_measurements( gyro_z_true = np.gradient(yaw_unwrapped) / dt # Add noise and bias - accel_xy = ( - accel_xy_true - + accel_bias - + np.random.randn(N, 2) * accel_noise_std - ) + accel_xy = accel_xy_true + accel_bias + np.random.randn(N, 2) * accel_noise_std - gyro_z = ( - gyro_z_true - + gyro_bias - + np.random.randn(N) * gyro_noise_std - ) + gyro_z = gyro_z_true + gyro_bias + np.random.randn(N) * gyro_noise_std return t, accel_xy, gyro_z @@ -279,17 +269,17 @@ def generate_uwb_measurements( clock_drift: float = 0.0, ) -> Tuple[np.ndarray, np.ndarray]: """Generate synthetic UWB range measurements with temporal calibration. - + Time Model (matches TimeSyncModel from core/fusion/types.py): - Fusion time (truth/IMU): t_fusion - UWB sensor time: t_uwb_sensor = (t_fusion - offset) / (1 + drift) - + Where: - offset < 0: UWB sensor clock is behind fusion time (typical case) - offset > 0: UWB sensor clock is ahead of fusion time - drift > 0: UWB clock runs faster than fusion clock - drift < 0: UWB clock runs slower than fusion clock - + Args: t: Ground truth timestamps in FUSION time (N,) p_xy: Ground truth positions (N, 2) @@ -301,7 +291,7 @@ def generate_uwb_measurements( dropout_rate: Probability of measurement dropout per anchor time_offset_sec: Time offset in seconds (negative = UWB behind) clock_drift: Relative clock drift (e.g., 0.0001 = 100 ppm) - + Returns: Tuple of (t_uwb_sensor, ranges): t_uwb_sensor: UWB timestamps in SENSOR time (M,) @@ -321,10 +311,9 @@ def generate_uwb_measurements( t_uwb_sensor = (t_uwb_fusion - time_offset_sec) / (1.0 + clock_drift) # 3. Interpolate positions at FUSION timestamps (ranges are measured in fusion time) - p_xy_uwb = np.column_stack([ - np.interp(t_uwb_fusion, t, p_xy[:, 0]), - np.interp(t_uwb_fusion, t, p_xy[:, 1]) - ]) + p_xy_uwb = np.column_stack( + [np.interp(t_uwb_fusion, t, p_xy[:, 0]), np.interp(t_uwb_fusion, t, p_xy[:, 1])] + ) # 4. Compute true ranges A = anchor_positions.shape[0] @@ -375,7 +364,7 @@ def generate_fusion_2d_imu_uwb_dataset( clock_drift: float = 0.0, ) -> None: """Generate and save fusion dataset. - + Args: output_dir: Output directory path seed: Random seed for reproducibility @@ -399,24 +388,16 @@ def generate_fusion_2d_imu_uwb_dataset( print(f" IMU rate: {1/dt_imu:.0f} Hz") t, p_xy, v_xy, yaw = generate_rectangular_trajectory( - width=width, - height=height, - speed=speed, - dt=dt_imu, - duration=duration + width=width, height=height, speed=speed, dt=dt_imu, duration=duration ) print(f" Generated {len(t)} samples") - print(f" Perimeter: {2*(width+height):.1f}m, {(2*(width+height)/speed):.1f}s per lap") + print( + f" Perimeter: {2*(width+height):.1f}m, {(2*(width+height)/speed):.1f}s per lap" + ) # Save ground truth - np.savez( - output_path / "truth.npz", - t=t, - p_xy=p_xy, - v_xy=v_xy, - yaw=yaw - ) + np.savez(output_path / "truth.npz", t=t, p_xy=p_xy, v_xy=v_xy, yaw=yaw) print(" Saved: truth.npz") # 2. Generate IMU measurements @@ -428,30 +409,29 @@ def generate_fusion_2d_imu_uwb_dataset( accel_bias = np.zeros(2) t_imu, accel_xy, gyro_z = generate_imu_measurements( - t, v_xy, yaw, + t, + v_xy, + yaw, accel_noise_std=accel_noise_std, gyro_noise_std=gyro_noise_std, accel_bias=accel_bias, - gyro_bias=gyro_bias + gyro_bias=gyro_bias, ) - np.savez( - output_path / "imu.npz", - t=t_imu, - accel_xy=accel_xy, - gyro_z=gyro_z - ) + np.savez(output_path / "imu.npz", t=t_imu, accel_xy=accel_xy, gyro_z=gyro_z) print(f" Generated {len(t_imu)} IMU samples") print(" Saved: imu.npz") # 3. Place UWB anchors at corners (plus center offset) print("\n3. Generating UWB measurements...") - anchor_positions = np.array([ - [0.0, 0.0], # Bottom-left - [width, 0.0], # Bottom-right - [width, height], # Top-right - [0.0, height] # Top-left - ]) + anchor_positions = np.array( + [ + [0.0, 0.0], # Bottom-left + [width, 0.0], # Bottom-right + [width, height], # Top-right + [0.0, height], # Top-left + ] + ) np.save(output_path / "uwb_anchors.npy", anchor_positions) print(f" Anchors: {anchor_positions.shape[0]} at corners") @@ -469,21 +449,19 @@ def generate_fusion_2d_imu_uwb_dataset( print(" NOTE: UWB timestamps are in SENSOR time, not fusion time") t_uwb, ranges = generate_uwb_measurements( - t, p_xy, anchor_positions, + t, + p_xy, + anchor_positions, uwb_rate=uwb_rate, range_noise_std=range_noise_std, nlos_anchors=nlos_anchors, nlos_bias=nlos_bias, dropout_rate=dropout_rate, time_offset_sec=time_offset_sec, - clock_drift=clock_drift + clock_drift=clock_drift, ) - np.savez( - output_path / "uwb_ranges.npz", - t=t_uwb, - ranges=ranges - ) + np.savez(output_path / "uwb_ranges.npz", t=t_uwb, ranges=ranges) # Count dropouts n_dropouts = np.sum(np.isnan(ranges)) @@ -501,13 +479,13 @@ def generate_fusion_2d_imu_uwb_dataset( "seed": seed, "duration_sec": duration, "imu_samples": int(len(t_imu)), - "uwb_samples": int(len(t_uwb)) + "uwb_samples": int(len(t_uwb)), }, "trajectory": { "type": "rectangular_walk", "width_m": width, "height_m": height, - "speed_m_s": speed + "speed_m_s": speed, }, "imu": { "rate_hz": 1 / dt_imu, @@ -515,7 +493,7 @@ def generate_fusion_2d_imu_uwb_dataset( "accel_noise_std_m_s2": accel_noise_std, "gyro_noise_std_rad_s": gyro_noise_std, "accel_bias_m_s2": accel_bias.tolist(), - "gyro_bias_rad_s": gyro_bias + "gyro_bias_rad_s": gyro_bias, }, "uwb": { "rate_hz": uwb_rate, @@ -523,18 +501,18 @@ def generate_fusion_2d_imu_uwb_dataset( "range_noise_std_m": range_noise_std, "nlos_anchors": nlos_anchors if nlos_anchors else [], "nlos_bias_m": nlos_bias, - "dropout_rate": dropout_rate + "dropout_rate": dropout_rate, }, "temporal_calibration": { "time_offset_sec": time_offset_sec, "clock_drift": clock_drift, - "note": "UWB timestamps are in SENSOR time. Use TimeSyncModel.to_fusion_time() to convert to fusion time (truth/IMU time). Formula: t_fusion = (1 + drift) * t_sensor + offset" + "note": "UWB timestamps are in SENSOR time. Use TimeSyncModel.to_fusion_time() to convert to fusion time (truth/IMU time). Formula: t_fusion = (1 + drift) * t_sensor + offset", }, "coordinate_frame": { "description": "ENU (East-North-Up)", "origin": "Bottom-left corner (0, 0)", - "units": "meters" - } + "units": "meters", + }, } with open(output_path / "config.json", "w") as f: @@ -566,6 +544,7 @@ def generate_fusion_2d_imu_uwb_dataset( # COMMAND-LINE INTERFACE # ============================================================================ + def main(): """Main entry point with CLI argument parsing.""" parser = argparse.ArgumentParser( @@ -591,159 +570,157 @@ def main(): # High dropout test python %(prog)s --dropout-rate 0.3 --output data/sim/fusion_high_dropout -Available presets: """ + ", ".join(PRESETS.keys()) +Available presets: """ + + ", ".join(PRESETS.keys()), ) # Preset configuration parser.add_argument( - '--preset', + "--preset", type=str, choices=PRESETS.keys(), - help='Use preset configuration (overrides individual parameters)' + help="Use preset configuration (overrides individual parameters)", ) parser.add_argument( - '--all-variants', - action='store_true', - help='Generate all 3 standard variants (baseline, nlos, timeoffset)' + "--all-variants", + action="store_true", + help="Generate all 3 standard variants (baseline, nlos, timeoffset)", ) # Output parser.add_argument( - '--output', + "--output", type=str, default=None, help=( "Output directory. Defaults to the preset's own directory, or " - 'data/sim/ch8_fusion_2d_imu_uwb without a preset. Given ' - 'explicitly it always wins.' - ) + "data/sim/ch8_fusion_2d_imu_uwb without a preset. Given " + "explicitly it always wins." + ), ) parser.add_argument( - '--seed', + "--seed", type=int, default=42, - help='Random seed for reproducibility (default: 42)' + help="Random seed for reproducibility (default: 42)", ) # Trajectory parameters - traj_group = parser.add_argument_group('Trajectory Parameters') + traj_group = parser.add_argument_group("Trajectory Parameters") traj_group.add_argument( - '--width', + "--width", type=float, default=20.0, - help='Rectangle width in meters (default: 20.0)' + help="Rectangle width in meters (default: 20.0)", ) traj_group.add_argument( - '--height', + "--height", type=float, default=15.0, - help='Rectangle height in meters (default: 15.0)' + help="Rectangle height in meters (default: 15.0)", ) traj_group.add_argument( - '--speed', - type=float, - default=1.0, - help='Walking speed in m/s (default: 1.0)' + "--speed", type=float, default=1.0, help="Walking speed in m/s (default: 1.0)" ) traj_group.add_argument( - '--duration', + "--duration", type=float, default=60.0, - help='Trajectory duration in seconds (default: 60.0)' + help="Trajectory duration in seconds (default: 60.0)", ) traj_group.add_argument( - '--dt-imu', + "--dt-imu", type=float, default=0.01, - help='IMU time step in seconds (default: 0.01, i.e., 100 Hz)' + help="IMU time step in seconds (default: 0.01, i.e., 100 Hz)", ) # IMU parameters - imu_group = parser.add_argument_group('IMU Parameters') + imu_group = parser.add_argument_group("IMU Parameters") imu_group.add_argument( - '--accel-noise', + "--accel-noise", type=float, default=0.1, - dest='accel_noise_std', - help='Accelerometer noise std in m/s² (default: 0.1)' + dest="accel_noise_std", + help="Accelerometer noise std in m/s² (default: 0.1)", ) imu_group.add_argument( - '--gyro-noise', + "--gyro-noise", type=float, default=0.01, - dest='gyro_noise_std', - help='Gyroscope noise std in rad/s (default: 0.01)' + dest="gyro_noise_std", + help="Gyroscope noise std in rad/s (default: 0.01)", ) imu_group.add_argument( - '--accel-bias-x', + "--accel-bias-x", type=float, default=0.0, - help='Accelerometer X-axis bias in m/s² (default: 0.0)' + help="Accelerometer X-axis bias in m/s² (default: 0.0)", ) imu_group.add_argument( - '--accel-bias-y', + "--accel-bias-y", type=float, default=0.0, - help='Accelerometer Y-axis bias in m/s² (default: 0.0)' + help="Accelerometer Y-axis bias in m/s² (default: 0.0)", ) imu_group.add_argument( - '--gyro-bias', + "--gyro-bias", type=float, default=0.0, - help='Gyroscope Z-axis bias in rad/s (default: 0.0)' + help="Gyroscope Z-axis bias in rad/s (default: 0.0)", ) # UWB parameters - uwb_group = parser.add_argument_group('UWB Parameters') + uwb_group = parser.add_argument_group("UWB Parameters") uwb_group.add_argument( - '--uwb-rate', + "--uwb-rate", type=float, default=10.0, - help='UWB measurement rate in Hz (default: 10.0)' + help="UWB measurement rate in Hz (default: 10.0)", ) uwb_group.add_argument( - '--range-noise', + "--range-noise", type=float, default=0.05, - dest='range_noise_std', - help='UWB range noise std in meters (default: 0.05)' + dest="range_noise_std", + help="UWB range noise std in meters (default: 0.05)", ) uwb_group.add_argument( - '--nlos-anchors', + "--nlos-anchors", type=int, - nargs='+', + nargs="+", default=[], - help='List of NLOS anchor indices (e.g., --nlos-anchors 1 2)' + help="List of NLOS anchor indices (e.g., --nlos-anchors 1 2)", ) uwb_group.add_argument( - '--nlos-bias', + "--nlos-bias", type=float, default=0.5, - help='NLOS positive bias in meters (default: 0.5)' + help="NLOS positive bias in meters (default: 0.5)", ) uwb_group.add_argument( - '--dropout-rate', + "--dropout-rate", type=float, default=0.05, - help='Measurement dropout probability per anchor (default: 0.05)' + help="Measurement dropout probability per anchor (default: 0.05)", ) # Temporal calibration - temporal_group = parser.add_argument_group('Temporal Calibration Parameters') + temporal_group = parser.add_argument_group("Temporal Calibration Parameters") temporal_group.add_argument( - '--time-offset', + "--time-offset", type=float, default=0.0, - dest='time_offset_sec', - help='UWB time offset in seconds (negative = UWB behind) (default: 0.0)' + dest="time_offset_sec", + help="UWB time offset in seconds (negative = UWB behind) (default: 0.0)", ) temporal_group.add_argument( - '--clock-drift', + "--clock-drift", type=float, default=0.0, - help='Relative clock drift (e.g., 0.0001 = 100 ppm) (default: 0.0)' + help="Relative clock drift (e.g., 0.0001 = 100 ppm) (default: 0.0)", ) # Parse arguments @@ -771,7 +748,7 @@ def main(): range_noise_std=args.range_noise_std, nlos_anchors=[], time_offset_sec=0.0, - clock_drift=0.0 + clock_drift=0.0, ) # NLOS variant @@ -792,7 +769,7 @@ def main(): nlos_bias=0.8, dropout_rate=args.dropout_rate, time_offset_sec=0.0, - clock_drift=0.0 + clock_drift=0.0, ) # Time offset variant @@ -812,7 +789,7 @@ def main(): nlos_anchors=[], dropout_rate=args.dropout_rate, time_offset_sec=-0.05, - clock_drift=0.0001 + clock_drift=0.0001, ) print(f"\n{'='*70}") @@ -828,7 +805,7 @@ def main(): # Override parameters with preset values for key, value in preset_config.items(): - if key != 'description' and hasattr(args, key): + if key != "description" and hasattr(args, key): setattr(args, key, value) # A preset picks its own directory unless the caller named one. Without @@ -838,7 +815,7 @@ def main(): # shipped dataset -- both reproduce theirs exactly -- and the rest get a # directory of their own rather than borrowing one. args.output = args.output or PRESET_DIRS.get( - args.preset, 'data/sim/ch8_fusion_2d_imu_uwb' + args.preset, "data/sim/ch8_fusion_2d_imu_uwb" ) # Validate parameters @@ -880,10 +857,9 @@ def main(): nlos_bias=args.nlos_bias, dropout_rate=args.dropout_rate, time_offset_sec=args.time_offset_sec, - clock_drift=args.clock_drift + clock_drift=args.clock_drift, ) if __name__ == "__main__": main() - diff --git a/scripts/verify_temporal_calibration.py b/scripts/verify_temporal_calibration.py index ae3d10c..0502692 100644 --- a/scripts/verify_temporal_calibration.py +++ b/scripts/verify_temporal_calibration.py @@ -18,9 +18,11 @@ from core.fusion import TimeSyncModel -def verify_temporal_calibration(dataset_path: str = "data/sim/ch8_fusion_2d_imu_uwb_timeoffset"): +def verify_temporal_calibration( + dataset_path: str = "data/sim/ch8_fusion_2d_imu_uwb_timeoffset", +): """Verify that temporal calibration is real, not cosmetic. - + Args: dataset_path: Path to the timeoffset dataset """ @@ -35,11 +37,12 @@ def verify_temporal_calibration(dataset_path: str = "data/sim/ch8_fusion_2d_imu_ # Load config import json + with open(dataset_path / "config.json") as f: config = json.load(f) - time_offset = config['temporal_calibration']['time_offset_sec'] - clock_drift = config['temporal_calibration']['clock_drift'] + time_offset = config["temporal_calibration"]["time_offset_sec"] + clock_drift = config["temporal_calibration"]["clock_drift"] print("\nConfig parameters:") print(f" Time offset: {time_offset*1000:.1f} ms") @@ -50,9 +53,9 @@ def verify_temporal_calibration(dataset_path: str = "data/sim/ch8_fusion_2d_imu_ imu = np.load(dataset_path / "imu.npz") uwb = np.load(dataset_path / "uwb_ranges.npz") - t_truth = truth['t'] - t_imu = imu['t'] - t_uwb_sensor = uwb['t'] + t_truth = truth["t"] + t_imu = imu["t"] + t_uwb_sensor = uwb["t"] print("\nTimestamp ranges:") print(f" Truth: [{t_truth[0]:.3f}, {t_truth[-1]:.3f}] s") @@ -85,12 +88,16 @@ def verify_temporal_calibration(dataset_path: str = "data/sim/ch8_fusion_2d_imu_ print("\nAt fusion time t=0.0s:") print(f" Expected sensor time: {t_sensor_expected_0:.6f} s") print(f" Actual sensor time: {t_sensor_actual_0:.6f} s") - print(f" Difference: {abs(t_sensor_actual_0 - t_sensor_expected_0)*1000:.3f} ms") + print( + f" Difference: {abs(t_sensor_actual_0 - t_sensor_expected_0)*1000:.3f} ms" + ) print("\nAt fusion time t~10.0s:") print(f" Expected sensor time: {t_sensor_expected_10:.6f} s") print(f" Actual sensor time: {t_sensor_actual_10:.6f} s") - print(f" Difference: {abs(t_sensor_actual_10 - t_sensor_expected_10)*1000:.3f} ms") + print( + f" Difference: {abs(t_sensor_actual_10 - t_sensor_expected_10)*1000:.3f} ms" + ) # Check that UWB timestamps ARE different from fusion time offset_at_start = t_uwb_sensor[0] - t_truth[0] @@ -101,12 +108,14 @@ def verify_temporal_calibration(dataset_path: str = "data/sim/ch8_fusion_2d_imu_ # At t_fusion=0: t_sensor ≈ 0.05 / 1.0001 ≈ 0.04999 # So sensor is ahead by ~50ms (because offset is negative = sensor behind fusion) - test1_pass = abs(offset_at_start*1000 - (-time_offset*1000)) < 1.0 # Within 1ms + test1_pass = abs(offset_at_start * 1000 - (-time_offset * 1000)) < 1.0 # Within 1ms if test1_pass: print("[PASS] TEST 1 PASSED: UWB timestamps are in sensor time") else: - print("[FAIL] TEST 1 FAILED: UWB timestamps appear to be in fusion time (cosmetic offset)") + print( + "[FAIL] TEST 1 FAILED: UWB timestamps appear to be in fusion time (cosmetic offset)" + ) # ======================================================================== # TEST 2: Verify that drift accumulates over time @@ -137,9 +146,11 @@ def verify_temporal_calibration(dataset_path: str = "data/sim/ch8_fusion_2d_imu_ error = abs(t_sens_actual - t_sens_expected) * 1000 drift_errors.append(error) - print(f" t_fusion={t_fus:5.1f}s: sensor={t_sens_actual:7.4f}s, " - f"expected={t_sens_expected:7.4f}s, error={error:.3f}ms, " - f"drift_contrib={drift_contribution*1000:.2f}ms") + print( + f" t_fusion={t_fus:5.1f}s: sensor={t_sens_actual:7.4f}s, " + f"expected={t_sens_expected:7.4f}s, error={error:.3f}ms, " + f"drift_contrib={drift_contribution*1000:.2f}ms" + ) # Drift errors should be small and not grow significantly # (errors from nearest-neighbor sampling should dominate, not systematic drift error) @@ -147,9 +158,13 @@ def verify_temporal_calibration(dataset_path: str = "data/sim/ch8_fusion_2d_imu_ test2_pass = max_drift_error < 2.0 # Within 2ms (generous for UWB rate) if test2_pass: - print(f"[PASS] TEST 2 PASSED: Drift correctly applied (max error {max_drift_error:.3f}ms)") + print( + f"[PASS] TEST 2 PASSED: Drift correctly applied (max error {max_drift_error:.3f}ms)" + ) else: - print(f"[FAIL] TEST 2 FAILED: Drift not correctly applied (max error {max_drift_error:.3f}ms)") + print( + f"[FAIL] TEST 2 FAILED: Drift not correctly applied (max error {max_drift_error:.3f}ms)" + ) # ======================================================================== # TEST 3: TimeSyncModel can recover fusion time @@ -168,7 +183,7 @@ def verify_temporal_calibration(dataset_path: str = "data/sim/ch8_fusion_2d_imu_ # Expected: The recovered fusion times should be close to the original fusion time grid # UWB was sampled at 10 Hz starting at fusion t=0, so fusion grid is [0, 0.1, 0.2, 0.3, 0.4, ...] - uwb_rate = config['uwb']['rate_hz'] + uwb_rate = config["uwb"]["rate_hz"] dt_uwb = 1.0 / uwb_rate # The generation code samples at np.arange(t[0], t[-1], dt_uwb) @@ -176,25 +191,33 @@ def verify_temporal_calibration(dataset_path: str = "data/sim/ch8_fusion_2d_imu_ t_fusion_grid = np.arange(0.0, 0.5, dt_uwb) print("\nFirst 5 UWB measurements:") - print(f"{'Sensor Time':>12} {'Recovered Fusion':>18} {'Expected Fusion':>18} {'Error (ms)':>12}") + print( + f"{'Sensor Time':>12} {'Recovered Fusion':>18} {'Expected Fusion':>18} {'Error (ms)':>12}" + ) print(f"{'-'*70}") recovery_errors = [] for i in range(min(5, len(t_uwb_recovered))): error = (t_uwb_recovered[i] - t_fusion_grid[i]) * 1000 recovery_errors.append(abs(error)) - print(f"{t_uwb_sensor[i]:12.6f} {t_uwb_recovered[i]:18.6f} " - f"{t_fusion_grid[i]:18.6f} {error:12.3f}") + print( + f"{t_uwb_sensor[i]:12.6f} {t_uwb_recovered[i]:18.6f} " + f"{t_fusion_grid[i]:18.6f} {error:12.3f}" + ) max_recovery_error = max(recovery_errors) test3_pass = max_recovery_error < 0.5 # Within 0.5ms if test3_pass: - print(f"[PASS] TEST 3 PASSED: TimeSyncModel correctly recovers fusion time " - f"(max error {max_recovery_error:.3f}ms)") + print( + f"[PASS] TEST 3 PASSED: TimeSyncModel correctly recovers fusion time " + f"(max error {max_recovery_error:.3f}ms)" + ) else: - print(f"[FAIL] TEST 3 FAILED: TimeSyncModel does not recover fusion time " - f"(max error {max_recovery_error:.3f}ms)") + print( + f"[FAIL] TEST 3 FAILED: TimeSyncModel does not recover fusion time " + f"(max error {max_recovery_error:.3f}ms)" + ) # ======================================================================== # FINAL VERDICT @@ -230,11 +253,10 @@ def verify_temporal_calibration(dataset_path: str = "data/sim/ch8_fusion_2d_imu_ "--dataset", type=str, default="data/sim/ch8_fusion_2d_imu_uwb_timeoffset", - help="Path to timeoffset dataset (default: data/sim/ch8_fusion_2d_imu_uwb_timeoffset)" + help="Path to timeoffset dataset (default: data/sim/ch8_fusion_2d_imu_uwb_timeoffset)", ) args = parser.parse_args() exit_code = verify_temporal_calibration(args.dataset) sys.exit(exit_code) - diff --git a/tests/__init__.py b/tests/__init__.py index 662d1fa..51e7895 100644 --- a/tests/__init__.py +++ b/tests/__init__.py @@ -1,2 +1 @@ """Unit tests for IPIN book examples.""" - diff --git a/tests/ch2_coords/test_coordinate_files_agree.py b/tests/ch2_coords/test_coordinate_files_agree.py index 1503505..096428d 100644 --- a/tests/ch2_coords/test_coordinate_files_agree.py +++ b/tests/ch2_coords/test_coordinate_files_agree.py @@ -34,7 +34,9 @@ from core.coords import ecef_to_enu, llh_to_ecef -DATA_DIR = Path(__file__).resolve().parents[2] / "data" / "sim" / "ch2_coords_san_francisco" +DATA_DIR = ( + Path(__file__).resolve().parents[2] / "data" / "sim" / "ch2_coords_san_francisco" +) #: Coordinates are stored at 3 decimals, so 1 mm is the quantisation floor. STORAGE_TOL_M = 2e-3 @@ -80,7 +82,8 @@ def test_the_reference_height_is_the_enu_origin(self): offset = float(np.mean(up_predicted - enu[:, 2])) self.assertLess( - abs(offset), STORAGE_TOL_M, + abs(offset), + STORAGE_TOL_M, f"Up is offset by {offset:.3f} m, so reference_llh.txt's height " f"({ref[2]:.3f} m) is not the origin the ENU was built about", ) diff --git a/tests/ch2_coords/test_dataset_matches_its_config.py b/tests/ch2_coords/test_dataset_matches_its_config.py index 673021a..cfb96b4 100644 --- a/tests/ch2_coords/test_dataset_matches_its_config.py +++ b/tests/ch2_coords/test_dataset_matches_its_config.py @@ -37,8 +37,7 @@ import numpy as np DATA_DIR = ( - Path(__file__).resolve().parents[2] - / "data" / "sim" / "ch2_coords_san_francisco" + Path(__file__).resolve().parents[2] / "data" / "sim" / "ch2_coords_san_francisco" ) #: ENU is stored at ``fmt="%.3f"``, so 1 mm is the quantisation floor. The diff --git a/tests/ch2_coords/test_enu_offsets_are_metres.py b/tests/ch2_coords/test_enu_offsets_are_metres.py index cfe5c72..db7f774 100644 --- a/tests/ch2_coords/test_enu_offsets_are_metres.py +++ b/tests/ch2_coords/test_enu_offsets_are_metres.py @@ -70,9 +70,11 @@ def setUpClass(cls): cls.example = run_example(MODULE) cls.reported = { match.group("name"): np.array( - [float(match.group("e")), - float(match.group("n")), - float(match.group("u"))] + [ + float(match.group("e")), + float(match.group("n")), + float(match.group("u")), + ] ) for match in TARGET_BLOCK.finditer(cls.example.process.stdout) } @@ -102,7 +104,7 @@ def test_every_named_target_was_found(self) -> None: ) def test_each_offset_returns_the_metres_it_is_named_for(self) -> None: - """"100m East" is 100 m east, not 100 units of whatever the code did.""" + """ "100m East" is 100 m east, not 100 units of whatever the code did.""" for name, expected in NAMED_OFFSETS.items(): with self.subTest(target=name): actual = self.reported.get(name) diff --git a/tests/ch2_coords/test_example_attitude_visualization.py b/tests/ch2_coords/test_example_attitude_visualization.py index a4a9aa5..ddee52e 100644 --- a/tests/ch2_coords/test_example_attitude_visualization.py +++ b/tests/ch2_coords/test_example_attitude_visualization.py @@ -82,9 +82,7 @@ def test_every_figure_is_written_in_every_format(self): path = self.figs_dir / f"{name}.{suffix}" with self.subTest(figure=path.name): self.assertTrue(path.exists(), f"missing {path}") - self.assertGreater( - path.stat().st_size, 0, f"empty {path}" - ) + self.assertGreater(path.stat().st_size, 0, f"empty {path}") if __name__ == "__main__": diff --git a/tests/ch2_coords/test_rotation_files_agree.py b/tests/ch2_coords/test_rotation_files_agree.py index a24dc14..1e4bbc2 100644 --- a/tests/ch2_coords/test_rotation_files_agree.py +++ b/tests/ch2_coords/test_rotation_files_agree.py @@ -37,7 +37,9 @@ quat_to_rotation_matrix, ) -DATA_DIR = Path(__file__).resolve().parents[2] / "data" / "sim" / "ch2_coords_san_francisco" +DATA_DIR = ( + Path(__file__).resolve().parents[2] / "data" / "sim" / "ch2_coords_san_francisco" +) #: Files store 6 decimal places, so ~2e-6 is the floor for any comparison. STORAGE_TOL = 1e-5 @@ -61,11 +63,14 @@ def test_the_three_files_describe_the_same_rotations(self): for i, (roll, pitch, yaw) in enumerate(euler): with self.subTest(point=i): np.testing.assert_allclose( - quats[i], euler_to_quat(roll, pitch, yaw), atol=STORAGE_TOL, + quats[i], + euler_to_quat(roll, pitch, yaw), + atol=STORAGE_TOL, err_msg="quaternion does not match its Euler angles", ) np.testing.assert_allclose( - mats[i], euler_to_rotation_matrix(roll, pitch, yaw), + mats[i], + euler_to_rotation_matrix(roll, pitch, yaw), atol=STORAGE_TOL, err_msg="rotation matrix does not match its Euler angles", ) @@ -82,7 +87,9 @@ def test_the_quaternion_and_matrix_files_agree_with_each_other(self): for i in range(len(quats)): with self.subTest(point=i): np.testing.assert_allclose( - quat_to_rotation_matrix(quats[i]), mats[i], atol=STORAGE_TOL, + quat_to_rotation_matrix(quats[i]), + mats[i], + atol=STORAGE_TOL, err_msg="quaternion and matrix describe different rotations", ) diff --git a/tests/ch3_estimators/test_figure_cost_model.py b/tests/ch3_estimators/test_figure_cost_model.py index 0639e5a..06aeca6 100644 --- a/tests/ch3_estimators/test_figure_cost_model.py +++ b/tests/ch3_estimators/test_figure_cost_model.py @@ -74,9 +74,7 @@ def test_particle_filter_costs_its_particles(self): This is why the panel needs a log axis: 300 particles put the PF two and a half decades above everything else. """ - self.assertEqual( - self.counts["PF"], self.counts["EKF"] * N_PARTICLES - ) + self.assertEqual(self.counts["PF"], self.counts["EKF"] * N_PARTICLES) def test_fgo_costs_the_iterations_it_actually_took(self): """FGO scales with real iterations, not the limit it was given. @@ -85,9 +83,7 @@ def test_fgo_costs_the_iterations_it_actually_took(self): returns, so an early-converging solve is reported as cheaper rather than being charged for all ten. """ - self.assertEqual( - self.counts["FGO"], self.counts["EKF"] * FGO_ITERATIONS - ) + self.assertEqual(self.counts["FGO"], self.counts["EKF"] * FGO_ITERATIONS) half = model_evaluation_counts( n_steps=N_STEPS, diff --git a/tests/ch3_estimators/test_particle_bimodal.py b/tests/ch3_estimators/test_particle_bimodal.py index b9b2d4f..dc92ddd 100644 --- a/tests/ch3_estimators/test_particle_bimodal.py +++ b/tests/ch3_estimators/test_particle_bimodal.py @@ -60,8 +60,9 @@ def test_cloud_collapses_when_resolved(self): [np.sqrt(np.linalg.det(np.cov(c.T))) for c in self.scenario["clouds"]] ) - self.assertGreater(spread[self.bimodal].mean(), - 10.0 * spread[self.resolved].mean()) + self.assertGreater( + spread[self.bimodal].mean(), 10.0 * spread[self.resolved].mean() + ) def test_mean_is_misleading_while_bimodal(self): """The headline lesson: the mean sits between the modes, in nothing. diff --git a/tests/ch3_estimators/test_reported_improvements_are_real.py b/tests/ch3_estimators/test_reported_improvements_are_real.py index 9d79ce0..d2d62f0 100644 --- a/tests/ch3_estimators/test_reported_improvements_are_real.py +++ b/tests/ch3_estimators/test_reported_improvements_are_real.py @@ -76,12 +76,14 @@ def _q_func(dt): - return Q_SCALE * np.array([ - [dt ** 3 / 3, 0, dt ** 2 / 2, 0], - [0, dt ** 3 / 3, 0, dt ** 2 / 2], - [dt ** 2 / 2, 0, dt, 0], - [0, dt ** 2 / 2, 0, dt], - ]) + return Q_SCALE * np.array( + [ + [dt**3 / 3, 0, dt**2 / 2, 0], + [0, dt**3 / 3, 0, dt**2 / 2], + [dt**2 / 2, 0, dt, 0], + [0, dt**2 / 2, 0, dt], + ] + ) def _iekf_errors(seed, range_std=RANGE_STD, bearing_std=BEARING_STD): @@ -101,29 +103,37 @@ def _iekf_errors(seed, range_std=RANGE_STD, bearing_std=BEARING_STD): def r_func(): diag = [] for _ in landmarks: - diag.extend([range_std ** 2, bearing_std ** 2]) + diag.extend([range_std**2, bearing_std**2]) return np.diag(diag) innovation = create_range_bearing_innovation_func(len(landmarks)) common = (process_model, process_jac, meas_model, meas_jac, _q_func, r_func) ekf = ExtendedKalmanFilter( - *common, X0_EST.copy(), P0.copy(), innovation_func=innovation) + *common, X0_EST.copy(), P0.copy(), innovation_func=innovation + ) iekf = IteratedExtendedKalmanFilter( - *common, X0_EST.copy(), P0.copy(), max_iterations=5, - convergence_tol=1e-6, innovation_func=innovation) + *common, + X0_EST.copy(), + P0.copy(), + max_iterations=5, + convergence_tol=1e-6, + innovation_func=innovation, + ) np.random.seed(seed) # legacy stream, as the example uses true_states, state = [true_x0.copy()], true_x0.copy() for _ in range(N_STEPS): state = process_model(state, None, DT) + np.random.multivariate_normal( - np.zeros(4), _q_func(DT)) + np.zeros(4), _q_func(DT) + ) true_states.append(state.copy()) measurements = [] for state in true_states[1:]: truth = meas_model(state) - measurements.append(truth + np.random.multivariate_normal( - np.zeros(len(truth)), r_func())) + measurements.append( + truth + np.random.multivariate_normal(np.zeros(len(truth)), r_func()) + ) ekf_est, iekf_est = [X0_EST.copy()], [X0_EST.copy()] for z in measurements: @@ -161,32 +171,36 @@ def _errors(trials=2000, seed=0): anchors, truth = setup_positioning_scenario() h, jacobian = create_range_model(anchors) stds = np.array([0.05, 0.3, 0.3, 0.3]) - W = np.diag(1.0 / stds ** 2) + W = np.diag(1.0 / stds**2) x0 = np.array([5.0, 5.0]) A = jacobian(x0) rng = np.random.default_rng(seed) e_wls, e_ls = [], [] for _ in range(trials): - y = np.array([ - np.linalg.norm(truth - anchors[i]) + rng.normal(0, stds[i]) - for i in range(len(anchors)) - ]) + y = np.array( + [ + np.linalg.norm(truth - anchors[i]) + rng.normal(0, stds[i]) + for i in range(len(anchors)) + ] + ) r = y - h(x0) - e_wls.append(np.linalg.norm(x0 + weighted_least_squares(A, r, W)[0] - truth)) + e_wls.append( + np.linalg.norm(x0 + weighted_least_squares(A, r, W)[0] - truth) + ) e_ls.append(np.linalg.norm(x0 + linear_least_squares(A, r)[0] - truth)) return np.asarray(e_wls), np.asarray(e_ls) def test_weighting_helps_on_average(self): e_wls, e_ls = self._errors() - rms_gain = 1.0 - np.sqrt((e_wls ** 2).mean()) / np.sqrt((e_ls ** 2).mean()) + rms_gain = 1.0 - np.sqrt((e_wls**2).mean()) / np.sqrt((e_ls**2).mean()) self.assertGreater(rms_gain, 0.05) def test_the_gain_is_far_below_the_single_draw_that_was_reported(self): """36.7% was a lucky realisation, not the method's accuracy.""" e_wls, e_ls = self._errors() - rms_gain = 1.0 - np.sqrt((e_wls ** 2).mean()) / np.sqrt((e_ls ** 2).mean()) + rms_gain = 1.0 - np.sqrt((e_wls**2).mean()) / np.sqrt((e_ls**2).mean()) self.assertLess(rms_gain, 0.25) @@ -205,10 +219,12 @@ def test_the_example_still_generates_ranges_the_way_this_test_assumes(self): """ anchors, truth = setup_positioning_scenario() np.random.seed(7) - drawn = np.array([ - compute_ranges(truth, anchors[i:i + 1], noise_std=0.0)[0] - for i in range(len(anchors)) - ]) + drawn = np.array( + [ + compute_ranges(truth, anchors[i : i + 1], noise_std=0.0)[0] + for i in range(len(anchors)) + ] + ) exact = np.array([np.linalg.norm(truth - a) for a in anchors]) np.testing.assert_allclose(drawn, exact, atol=1e-12) diff --git a/tests/ch4_rf_point_positioning/test_aoa_bearing_points_at_the_agent.py b/tests/ch4_rf_point_positioning/test_aoa_bearing_points_at_the_agent.py index 07f0174..198bee7 100644 --- a/tests/ch4_rf_point_positioning/test_aoa_bearing_points_at_the_agent.py +++ b/tests/ch4_rf_point_positioning/test_aoa_bearing_points_at_the_agent.py @@ -40,7 +40,9 @@ def test_psi_is_the_bearing_from_the_agent_to_the_anchor(self): with self.subTest(anchor=tuple(anchor)): np.testing.assert_allclose( - direction, span / np.linalg.norm(span), atol=1e-9, + direction, + span / np.linalg.norm(span), + atol=1e-9, err_msg="psi does not point from the agent toward the anchor", ) @@ -57,12 +59,15 @@ def test_the_ray_drawn_from_an_anchor_must_be_negated(self): with self.subTest(anchor=tuple(anchor)): np.testing.assert_allclose( - anchor + distance * toward, AGENT, atol=1e-9, + anchor + distance * toward, + AGENT, + atol=1e-9, err_msg="the anchor-to-agent ray does not reach the agent", ) wrong = anchor + distance * (-toward) self.assertGreater( - float(np.linalg.norm(wrong - AGENT)), distance, + float(np.linalg.norm(wrong - AGENT)), + distance, "the un-negated ray should move away from the agent", ) diff --git a/tests/ch4_rf_point_positioning/test_aoa_initialisation_basin.py b/tests/ch4_rf_point_positioning/test_aoa_initialisation_basin.py index f4802ca..66a5d52 100644 --- a/tests/ch4_rf_point_positioning/test_aoa_initialisation_basin.py +++ b/tests/ch4_rf_point_positioning/test_aoa_initialisation_basin.py @@ -93,9 +93,7 @@ def test_the_tan_parameterisation_still_fails_from_the_centroid(self): If this ever stops failing, the tan form has been repaired too and the `residual` switch has lost its reason to exist. """ - errors, converged = _solve_from( - lambda p: ANCHORS.mean(axis=0), residual="tan" - ) + errors, converged = _solve_from(lambda p: ANCHORS.mean(axis=0), residual="tan") gross = errors[converged] > 1.0 self.assertGreater(int(np.sum(gross)), 0) diff --git a/tests/ch4_rf_point_positioning/test_initial_guess_basin.py b/tests/ch4_rf_point_positioning/test_initial_guess_basin.py index d78473c..f6abb71 100644 --- a/tests/ch4_rf_point_positioning/test_initial_guess_basin.py +++ b/tests/ch4_rf_point_positioning/test_initial_guess_basin.py @@ -114,8 +114,10 @@ def test_seeds_inside_the_room_all_solve_with_the_wrapped_form(self): """The practically relevant statement: a seed anywhere in the room is fine.""" _, angle = sweeps() inside = ( - (angle["xx"] >= ANCHORS[:, 0].min()) & (angle["xx"] <= ANCHORS[:, 0].max()) - & (angle["yy"] >= ANCHORS[:, 1].min()) & (angle["yy"] <= ANCHORS[:, 1].max()) + (angle["xx"] >= ANCHORS[:, 0].min()) + & (angle["xx"] <= ANCHORS[:, 0].max()) + & (angle["yy"] >= ANCHORS[:, 1].min()) + & (angle["yy"] <= ANCHORS[:, 1].max()) ) self.assertGreater(int(np.sum(inside)), 100) diff --git a/tests/ch4_rf_point_positioning/test_shipped_measurements_match_the_solver_convention.py b/tests/ch4_rf_point_positioning/test_shipped_measurements_match_the_solver_convention.py index 3770aa9..d417d53 100644 --- a/tests/ch4_rf_point_positioning/test_shipped_measurements_match_the_solver_convention.py +++ b/tests/ch4_rf_point_positioning/test_shipped_measurements_match_the_solver_convention.py @@ -129,8 +129,10 @@ def _predicted_tdoa(beacons, positions): """ return np.array( [ - [tdoa_range_difference(beacons[j], beacons[0], p) - for j in range(1, len(beacons))] + [ + tdoa_range_difference(beacons[j], beacons[0], p) + for j in range(1, len(beacons)) + ] for p in positions ] ) @@ -191,9 +193,9 @@ def test_aoa_angles_are_measured_from_the_agent_toward_the_anchor(dataset): """Wrapped, because a raw bearing difference straddles the branch cut.""" data = _load(dataset) sigma_deg = data["config"]["measurements"]["aoa_noise_std_deg"] - residual = angle_diff(data["aoa"], _predicted_aoa( - data["beacons"], data["positions"] - )) + residual = angle_diff( + data["aoa"], _predicted_aoa(data["beacons"], data["positions"]) + ) worst = _worst_column(residual) assert worst < TOLERANCE_SIGMA * np.deg2rad(sigma_deg), ( f"{dataset}/aoa_angles.txt disagrees with `aoa_azimuth` by " @@ -224,27 +226,33 @@ def test_the_bound_is_justified_against_both_the_noise_and_the_defect(): corruptions = { # (arm, what a defect of that kind looks like in the stored file) "TOA: +1.1 m undeclared on one beacon": ( - "toa", "toa_noise_std_m", + "toa", + "toa_noise_std_m", lambda a: _bump_column(a, 0, 1.1), ), "TDOA: sign convention reversed": ( - "tdoa", "tdoa_noise_std_m", + "tdoa", + "tdoa_noise_std_m", lambda a: -a, ), "TDOA: +0.6 m on one column": ( - "tdoa", "tdoa_noise_std_m", + "tdoa", + "tdoa_noise_std_m", lambda a: _bump_column(a, 0, 0.6), ), "AOA: azimuth sign flipped": ( - "aoa", "aoa_noise_std_deg", + "aoa", + "aoa_noise_std_deg", lambda a: -a, ), "AOA: reverse bearing (anchor to agent)": ( - "aoa", "aoa_noise_std_deg", + "aoa", + "aoa_noise_std_deg", lambda a: np.arctan2(-np.sin(a), -np.cos(a)), ), "AOA: atan2 arguments swapped": ( - "aoa", "aoa_noise_std_deg", + "aoa", + "aoa_noise_std_deg", lambda a: np.pi / 2 - a, ), } @@ -257,9 +265,7 @@ def test_the_bound_is_justified_against_both_the_noise_and_the_defect(): data = _load(dataset) for arm in ("toa", "tdoa", "aoa"): sigma = _sigma_for(data["config"], arm) - worst_honest = max( - worst_honest, _residual(data, arm, data[arm]) / sigma - ) + worst_honest = max(worst_honest, _residual(data, arm, data[arm]) / sigma) for name, (arm, _key, corrupt) in corruptions.items(): sigma = _sigma_for(data["config"], arm) @@ -298,9 +304,7 @@ def _sigma_for(config, arm): def _residual(data, arm, observed): """Worst-column mean |residual| of `observed` against the forward model.""" if arm == "toa": - predicted = _predicted_toa( - data["beacons"], data["positions"], data["config"] - ) + predicted = _predicted_toa(data["beacons"], data["positions"], data["config"]) return _worst_column(observed - predicted) if arm == "tdoa": predicted = _predicted_tdoa(data["beacons"], data["positions"]) diff --git a/tests/ch5_fingerprinting/test_figure_cost_model.py b/tests/ch5_fingerprinting/test_figure_cost_model.py index 707cebf..c630aed 100644 --- a/tests/ch5_fingerprinting/test_figure_cost_model.py +++ b/tests/ch5_fingerprinting/test_figure_cost_model.py @@ -46,9 +46,7 @@ def test_counts_are_plain_integers(self): def test_repeated_calls_agree(self): """The whole point: the number cannot move between runs.""" - self.assertEqual( - per_query_operation_counts(self.db, floor_id=0), self.on_floor - ) + self.assertEqual(per_query_operation_counts(self.db, floor_id=0), self.on_floor) def test_linear_model_cost_ignores_the_database(self): """The learned model is O(N_features), not O(N_RPs). @@ -76,9 +74,7 @@ def test_floor_constraint_helps_distances_but_not_likelihoods(self): self.assertLess( self.on_floor["NN (Euclidean)"], self.all_floors["NN (Euclidean)"] ) - self.assertLess( - self.on_floor["k-NN (k=3)"], self.all_floors["k-NN (k=3)"] - ) + self.assertLess(self.on_floor["k-NN (k=3)"], self.all_floors["k-NN (k=3)"]) self.assertEqual(self.on_floor["MAP"], self.all_floors["MAP"]) self.assertEqual( @@ -103,9 +99,7 @@ def test_top_k_trims_the_cheap_term_only(self): self.assertGreater(top_k, likelihood_terms) self.assertLess(top_k, full) saving = (full - top_k) / full - self.assertLess( - saving, 0.25, f"top-k saves {saving:.0%}, more than expected" - ) + self.assertLess(saving, 0.25, f"top-k saves {saving:.0%}, more than expected") class TestDeterministicCostModel(unittest.TestCase): diff --git a/tests/ch5_fingerprinting/test_floor_classification_is_real.py b/tests/ch5_fingerprinting/test_floor_classification_is_real.py index 5f72761..bb1586f 100644 --- a/tests/ch5_fingerprinting/test_floor_classification_is_real.py +++ b/tests/ch5_fingerprinting/test_floor_classification_is_real.py @@ -39,7 +39,9 @@ load_fingerprint_database, ) -DB_PATH = Path(__file__).resolve().parents[2] / "data" / "sim" / "ch5_wifi_fingerprint_grid" +DB_PATH = ( + Path(__file__).resolve().parents[2] / "data" / "sim" / "ch5_wifi_fingerprint_grid" +) QUERY_NOISE_DBM = 3.0 N_QUERIES = 120 @@ -56,8 +58,9 @@ def _queries(db, n=N_QUERIES, noise=QUERY_NOISE_DBM, seed=42): out = [] for _ in range(n): idx = int(rng.integers(0, db.n_reference_points)) - out.append((feats[idx] + rng.standard_normal(db.n_features) * noise, - int(floors[idx]))) + out.append( + (feats[idx] + rng.standard_normal(db.n_features) * noise, int(floors[idx])) + ) return out @@ -98,7 +101,9 @@ def test_coarse_floor_is_not_constant(self): """ db = _load() predicted = { - hierarchical_localize(z, db, coarse_method="floor", fine_method="knn", k=5)[1]["coarse_floor"] + hierarchical_localize(z, db, coarse_method="floor", fine_method="knn", k=5)[ + 1 + ]["coarse_floor"] for z, _ in _queries(db) } @@ -107,7 +112,10 @@ def test_coarse_floor_is_not_constant(self): def test_coarse_floor_is_accurate_well_above_chance(self): db = _load() hits = [ - hierarchical_localize(z, db, coarse_method="floor", fine_method="knn", k=5)[1]["coarse_floor"] == floor + hierarchical_localize(z, db, coarse_method="floor", fine_method="knn", k=5)[ + 1 + ]["coarse_floor"] + == floor for z, floor in _queries(db) ] @@ -122,7 +130,8 @@ def test_coarse_floor_equals_the_nearest_fingerprint_floor(self): for z, _ in _queries(db, n=25): expected = int(floors[int(np.argmin(np.linalg.norm(feats - z, axis=1)))]) _, info = hierarchical_localize( - z, db, coarse_method="floor", fine_method="knn", k=5) + z, db, coarse_method="floor", fine_method="knn", k=5 + ) self.assertEqual(info["coarse_floor"], expected) @@ -151,9 +160,11 @@ def test_map_scores_exact_zeros_and_knn_never_does(self): z = feats[idx] + rng.standard_normal(db.n_features) * QUERY_NOISE_DBM truth = locations[idx] pos_map, _ = hierarchical_localize( - z, db, coarse_method="floor", fine_method="map") + z, db, coarse_method="floor", fine_method="map" + ) pos_knn, _ = hierarchical_localize( - z, db, coarse_method="floor", fine_method="knn", k=5) + z, db, coarse_method="floor", fine_method="knn", k=5 + ) map_zero += np.linalg.norm(pos_map - truth) < 1e-9 knn_zero += np.linalg.norm(pos_knn - truth) < 1e-9 @@ -169,12 +180,15 @@ def setUpClass(cls): cls.db = _load() cls.feats = cls.db.get_mean_features() cls.rf = fit_classifier( - cls.db, classifier_type="random_forest", zone_type="rp", n_estimators=100) + cls.db, classifier_type="random_forest", zone_type="rp", n_estimators=100 + ) def test_recall_on_training_vectors_is_perfect_by_construction(self): """One sample per class: fitting them is guaranteed, not informative.""" hits = [ - np.allclose(self.rf.predict(self.feats[i])[0], self.db.locations[i], atol=0.1) + np.allclose( + self.rf.predict(self.feats[i])[0], self.db.locations[i], atol=0.1 + ) for i in range(self.db.n_reference_points) ] @@ -188,7 +202,8 @@ def test_held_out_accuracy_is_strictly_lower(self): idx = int(rng.integers(0, self.db.n_reference_points)) z = self.feats[idx] + rng.standard_normal(self.db.n_features) * 2.0 hits.append( - np.allclose(self.rf.predict(z)[0], self.db.locations[idx], atol=0.1)) + np.allclose(self.rf.predict(z)[0], self.db.locations[idx], atol=0.1) + ) self.assertLess(float(np.mean(hits)), 1.0) self.assertGreater(float(np.mean(hits)), 0.5) diff --git a/tests/ch6_dead_reckoning/test_allan_variance_recovers_injected_noise.py b/tests/ch6_dead_reckoning/test_allan_variance_recovers_injected_noise.py index 3610d87..3749a0f 100644 --- a/tests/ch6_dead_reckoning/test_allan_variance_recovers_injected_noise.py +++ b/tests/ch6_dead_reckoning/test_allan_variance_recovers_injected_noise.py @@ -77,7 +77,9 @@ def test_velocity_random_walk_within_20_percent(self, recovered): recovered["accel"]["velocity_random_walk"] / injected_si(GRADE)["accel_vrw"] ) - assert 0.8 < ratio < 1.2, f"accel VRW recovered {ratio:.2f}x the injected value." + assert ( + 0.8 < ratio < 1.2 + ), f"accel VRW recovered {ratio:.2f}x the injected value." def test_gyro_bias_instability_within_a_factor_of_two(self, recovered): """Looser on purpose: the shoulder is broad and the minimum is noisy.""" @@ -86,9 +88,9 @@ def test_gyro_bias_instability_within_a_factor_of_two(self, recovered): / injected_si(GRADE)["gyro_bias_instability"] ) - assert 0.5 < ratio < 2.0, ( - f"gyro bias instability recovered {ratio:.2f}x the injected value." - ) + assert ( + 0.5 < ratio < 2.0 + ), f"gyro bias instability recovered {ratio:.2f}x the injected value." class TestTheReportedNumbersAgreeWithTheReferenceTable: diff --git a/tests/ch6_dead_reckoning/test_comparison_methods.py b/tests/ch6_dead_reckoning/test_comparison_methods.py index 2177776..25f1892 100644 --- a/tests/ch6_dead_reckoning/test_comparison_methods.py +++ b/tests/ch6_dead_reckoning/test_comparison_methods.py @@ -117,9 +117,7 @@ def test_wrap_to_pi(self): """Turn deltas have to take the short way round.""" self.assertAlmostEqual(_wrap_to_pi(0.5), 0.5) # pi -> -pi/2 is a left turn of +pi/2, not a right turn of -3pi/2. - self.assertAlmostEqual( - _wrap_to_pi(-np.pi / 2 - np.pi), np.pi / 2, places=12 - ) + self.assertAlmostEqual(_wrap_to_pi(-np.pi / 2 - np.pi), np.pi / 2, places=12) self.assertAlmostEqual(_wrap_to_pi(3 * np.pi), np.pi, places=12) self.assertLessEqual(abs(_wrap_to_pi(100.0)), np.pi) @@ -140,8 +138,11 @@ def setUpClass(cls): cls.stance, cls.wheel_true, ) = generate_mixed_trajectory( - DURATION, DT, FrameConvention.create_enu(), - step_freq=STEP_FREQ, lever_arm_a=LEVER_ARM_A, + DURATION, + DT, + FrameConvention.create_enu(), + step_freq=STEP_FREQ, + lever_arm_a=LEVER_ARM_A, ) def test_walk_follows_the_waypoints(self): @@ -151,9 +152,7 @@ def test_walk_follows_the_waypoints(self): ) for waypoint in WAYPOINTS: distance = np.linalg.norm(self.pos_true[:, :2] - waypoint, axis=1) - self.assertLess( - distance.min(), 0.05, f"never reached waypoint {waypoint}" - ) + self.assertLess(distance.min(), 0.05, f"never reached waypoint {waypoint}") np.testing.assert_allclose(self.pos_true[-1, :2], 0.0, atol=0.05) def test_walking_carries_gait_dynamics(self): @@ -192,9 +191,7 @@ def test_heading_is_continuous(self): """ self.assertLess(np.abs(np.diff(self.heading_true)).max(), 0.05) # Three left turns: the walk ends 270 deg from where it started. - self.assertAlmostEqual( - np.rad2deg(self.heading_true[-1]), 270.0, delta=0.5 - ) + self.assertAlmostEqual(np.rad2deg(self.heading_true[-1]), 270.0, delta=0.5) def test_wheel_speed_is_forward_while_driving(self): """Straight driving reduces to the book's v^S = [0, v, 0] convention. @@ -225,9 +222,7 @@ def test_noise_free_wheel_odometry_reproduces_the_truth(self): estimate = run_wheel_odom( self.t, self.wheel_true, self.gyro_body, initial, LEVER_ARM_A ) - error = np.linalg.norm( - estimate[:, :2] - self.pos_true[:, :2], axis=1 - ) + error = np.linalg.norm(estimate[:, :2] - self.pos_true[:, :2], axis=1) # The residual is the first-order quaternion integrator and the # one-sample lag between measurement and update, nothing else. self.assertLess(error.max(), 0.05) @@ -240,14 +235,29 @@ class TestDetectors(unittest.TestCase): def setUpClass(cls): cls.imu_params = IMUNoiseParams.consumer_grade() ( - cls.t, _, _, accel_body, gyro_body, _, mag_body, cls.stance, + cls.t, + _, + _, + accel_body, + gyro_body, + _, + mag_body, + cls.stance, wheel_true, ) = generate_mixed_trajectory( - DURATION, DT, FrameConvention.create_enu(), - step_freq=STEP_FREQ, lever_arm_a=LEVER_ARM_A, + DURATION, + DT, + FrameConvention.create_enu(), + step_freq=STEP_FREQ, + lever_arm_a=LEVER_ARM_A, ) cls.accel_meas, cls.gyro_meas, cls.mag_meas, _ = add_sensor_noise( - accel_body, gyro_body, mag_body, wheel_true, DT, cls.imu_params, + accel_body, + gyro_body, + mag_body, + wheel_true, + DT, + cls.imu_params, seed=DEFAULT_SEED, ) @@ -266,14 +276,17 @@ def test_zupt_statistic_separates_standing_from_walking(self): for k in range(1, len(self.t), 20): # every 20th sample is plenty start, end = max(0, k - 5), min(len(self.t), k + 6) statistic[k] = zupt_test_statistic( - self.accel_meas[start:end], self.gyro_meas[start:end], - sigma_a, sigma_g, + self.accel_meas[start:end], + self.gyro_meas[start:end], + sigma_a, + sigma_g, ) standing = np.nanmedian(statistic[self.stance]) walking = np.nanmedian(statistic[~self.stance]) self.assertGreater( - walking, 10 * standing, + walking, + 10 * standing, f"no separation: {standing:.1f} standing vs {walking:.1f} walking", ) # The example's default threshold has to land inside that gap. @@ -287,13 +300,18 @@ def test_step_detector_counts_the_simulated_gait(self): 9.81 and found three "steps" in a 100 m walk. """ step_indices, _ = detect_steps_peak_detector( - self.accel_meas, dt=DT, g=9.81, min_peak_height=1.0, - min_peak_distance=0.3, lowpass_cutoff=5.0, + self.accel_meas, + dt=DT, + g=9.81, + min_peak_height=1.0, + min_peak_distance=0.3, + lowpass_cutoff=5.0, ) expected = np.sum(~self.stance) * DT * STEP_FREQ self.assertAlmostEqual(len(step_indices), expected, delta=0.1 * expected) self.assertEqual( - np.sum(self.stance[step_indices]), 0, + np.sum(self.stance[step_indices]), + 0, "steps detected while the walker was standing still", ) @@ -307,13 +325,29 @@ def setUpClass(cls): imu_params = IMUNoiseParams.consumer_grade() ( - cls.t, cls.pos_true, vel_true, accel_body, gyro_body, _, mag_body, - _, wheel_true, + cls.t, + cls.pos_true, + vel_true, + accel_body, + gyro_body, + _, + mag_body, + _, + wheel_true, ) = generate_mixed_trajectory( - DURATION, DT, frame, step_freq=STEP_FREQ, lever_arm_a=LEVER_ARM_A, + DURATION, + DT, + frame, + step_freq=STEP_FREQ, + lever_arm_a=LEVER_ARM_A, ) accel_meas, gyro_meas, mag_meas, wheel_meas = add_sensor_noise( - accel_body, gyro_body, mag_body, wheel_true, DT, imu_params, + accel_body, + gyro_body, + mag_body, + wheel_true, + DT, + imu_params, seed=DEFAULT_SEED, ) initial = NavStateQPVP( @@ -325,9 +359,7 @@ def setUpClass(cls): ) pdr_pos, cls.step_count = run_pdr(cls.t, accel_meas, mag_meas, 1.75) cls.results = { - "IMU Only": run_imu_only( - cls.t, accel_meas, gyro_meas, initial, frame - ), + "IMU Only": run_imu_only(cls.t, accel_meas, gyro_meas, initial, frame), "IMU + ZUPT": zupt_pos, "Wheel Odom": run_wheel_odom( cls.t, wheel_meas, gyro_meas, initial, LEVER_ARM_A @@ -349,7 +381,8 @@ def test_every_method_traces_a_comparable_path(self): with self.subTest(method=name): path = path_length(pos[:, :2]) self.assertGreater( - path, 0.5 * TRUTH_PATH_M, + path, + 0.5 * TRUTH_PATH_M, f"{name} traced only {path:.2f} m of a " f"{TRUTH_PATH_M:.0f} m walk", ) @@ -368,7 +401,8 @@ def test_every_method_covers_the_extent_of_the_walk(self): with self.subTest(method=name): reach = np.linalg.norm(pos[:, :2], axis=1).max() self.assertGreater( - reach, 0.5 * truth_reach, + reach, + 0.5 * truth_reach, f"{name} never got further than {reach:.2f} m from the " f"origin ({truth_reach:.1f} m expected)", ) @@ -397,9 +431,7 @@ def test_corrections_beat_unaided_strapdown(self): rmse = { name: float( np.sqrt( - np.mean( - np.sum((pos[:, :2] - self.pos_true[:, :2]) ** 2, axis=1) - ) + np.mean(np.sum((pos[:, :2] - self.pos_true[:, :2]) ** 2, axis=1)) ) ) for name, pos in self.results.items() @@ -414,9 +446,7 @@ def test_corrections_beat_unaided_strapdown(self): def test_plot_comparison_reports_path_and_writes_every_figure(self): """The path metric is what makes a frozen method visible in the table.""" with tempfile.TemporaryDirectory() as tmp: - metrics = plot_comparison( - self.t, self.pos_true, self.results, Path(tmp) - ) + metrics = plot_comparison(self.t, self.pos_true, self.results, Path(tmp)) for name in [ "comparison_trajectories", "comparison_error_time", diff --git a/tests/ch6_dead_reckoning/test_heading_error_is_wrapped.py b/tests/ch6_dead_reckoning/test_heading_error_is_wrapped.py index 62fe849..67da326 100644 --- a/tests/ch6_dead_reckoning/test_heading_error_is_wrapped.py +++ b/tests/ch6_dead_reckoning/test_heading_error_is_wrapped.py @@ -43,7 +43,9 @@ DATA_DIR = ( Path(__file__).resolve().parents[2] - / "data" / "sim" / "ch6_env_sensors_heading_altitude" + / "data" + / "sim" + / "ch6_env_sensors_heading_altitude" ) @@ -57,13 +59,17 @@ def setUpClass(cls): att = np.loadtxt(DATA_DIR / "ground_truth_attitude.txt") mag = np.loadtxt(DATA_DIR / "magnetometer.txt") cls.yaw_true = att[:, 2] - cls.heading_est = np.array([ - mag_heading(mag[k], att[k, 0], att[k, 1], declination=0.0) - for k in range(len(att)) - ]) - cls.error = np.abs(np.array([ - wrap_angle_diff(e, y) for e, y in zip(cls.heading_est, cls.yaw_true) - ])) + cls.heading_est = np.array( + [ + mag_heading(mag[k], att[k, 0], att[k, 1], declination=0.0) + for k in range(len(att)) + ] + ) + cls.error = np.abs( + np.array( + [wrap_angle_diff(e, y) for e, y in zip(cls.heading_est, cls.yaw_true)] + ) + ) def test_the_trajectory_still_exercises_the_wrap(self) -> None: """Guard the guard: the true yaw must leave (-pi, pi]. @@ -73,7 +79,8 @@ def test_the_trajectory_still_exercises_the_wrap(self) -> None: file would quietly stop testing anything. """ self.assertGreater( - self.yaw_true.max(), np.pi, + self.yaw_true.max(), + np.pi, "the building walk no longer drives yaw past pi, so the wrap this " "file exists to check is no longer exercised.", ) @@ -94,7 +101,8 @@ def test_the_naive_reduction_really_does_break_here(self) -> None: """ naive = self._naive_reduction() self.assertGreater( - int((naive < 0).sum()), 0, + int((naive < 0).sum()), + 0, "the naive reduction no longer produces negative errors here, so " "this dataset no longer demonstrates the defect and the check " "below has stopped discriminating.", @@ -109,7 +117,8 @@ def test_config_does_not_match_the_naive_reduction(self) -> None: naive_mean = float(np.rad2deg(self._naive_reduction()).mean()) reported = self.config["performance"]["magnetometer_heading"]["mean_error_deg"] self.assertGreater( - abs(reported - naive_mean), 0.05, + abs(reported - naive_mean), + 0.05, f"config.json's {reported:.4f} deg matches the naive reduction " f"({naive_mean:.4f}), which counts negative errors toward the mean.", ) @@ -128,9 +137,11 @@ def test_config_matches_the_shipped_measurements(self) -> None: ): with self.subTest(metric=name): self.assertAlmostEqual( - reported[name], actual, delta=0.05, + reported[name], + actual, + delta=0.05, msg=f"config.json says {name}={reported[name]:.4f}, the " - f"shipped data gives {actual:.4f}.", + f"shipped data gives {actual:.4f}.", ) diff --git a/tests/ch6_dead_reckoning/test_imu_is_body_frame.py b/tests/ch6_dead_reckoning/test_imu_is_body_frame.py index c42aaeb..957b296 100644 --- a/tests/ch6_dead_reckoning/test_imu_is_body_frame.py +++ b/tests/ch6_dead_reckoning/test_imu_is_body_frame.py @@ -50,10 +50,12 @@ def _accel_in_body_frame(truth): dt = np.diff(t, prepend=t[0] - (t[1] - t[0])) a_map = np.gradient(vel, axis=0) / dt[:, None] cos_y, sin_y = np.cos(yaw), np.sin(yaw) - return np.column_stack([ - cos_y * a_map[:, 0] + sin_y * a_map[:, 1], - -sin_y * a_map[:, 0] + cos_y * a_map[:, 1], - ]) + return np.column_stack( + [ + cos_y * a_map[:, 0] + sin_y * a_map[:, 1], + -sin_y * a_map[:, 0] + cos_y * a_map[:, 1], + ] + ) class TestImuIsBodyFrame(unittest.TestCase): @@ -67,12 +69,14 @@ def test_accel_matches_the_truth_rotated_into_body(self): with self.subTest(dataset=name): self.assertLess( - float(residual.std()), 1.5 * sigma, + float(residual.std()), + 1.5 * sigma, "accelerometer does not match the body-frame truth", ) # A frame error shows up here rather than in the spread. self.assertLess( - float(np.abs(residual.mean(axis=0)).max()), 0.25 * sigma, + float(np.abs(residual.mean(axis=0)).max()), + 0.25 * sigma, "residual has a systematic component, which is what a " "wrong frame looks like", ) @@ -95,22 +99,30 @@ def test_integrating_the_imu_reproduces_the_trajectory(self): for k in range(1, len(t)): theta += gyro[k - 1] * dt c, s = np.cos(theta), np.sin(theta) - a_map = np.array([ - c * accel[k - 1, 0] - s * accel[k - 1, 1], - s * accel[k - 1, 0] + c * accel[k - 1, 1], - ]) + a_map = np.array( + [ + c * accel[k - 1, 0] - s * accel[k - 1, 1], + s * accel[k - 1, 0] + c * accel[k - 1, 1], + ] + ) v = v + a_map * dt x = x + v * dt track.append(x.copy()) track = np.asarray(track) with self.subTest(dataset=name): - true_extent = float(np.linalg.norm(pos - pos.mean(axis=0), axis=1).mean()) - got_extent = float(np.linalg.norm(track - track.mean(axis=0), axis=1).mean()) + true_extent = float( + np.linalg.norm(pos - pos.mean(axis=0), axis=1).mean() + ) + got_extent = float( + np.linalg.norm(track - track.mean(axis=0), axis=1).mean() + ) self.assertAlmostEqual( - got_extent, true_extent, delta=0.25 * true_extent, + got_extent, + true_extent, + delta=0.25 * true_extent, msg=f"{name}: integrated path spans {got_extent:.2f} m about " - f"its centre against {true_extent:.2f} m for the truth", + f"its centre against {true_extent:.2f} m for the truth", ) diff --git a/tests/ch6_dead_reckoning/test_methods_actually_move.py b/tests/ch6_dead_reckoning/test_methods_actually_move.py index 24323ba..d24eda5 100644 --- a/tests/ch6_dead_reckoning/test_methods_actually_move.py +++ b/tests/ch6_dead_reckoning/test_methods_actually_move.py @@ -90,9 +90,7 @@ def setUpClass(cls): cls.results = { "IMU Only": run_imu_only(t, accel, gyro, initial, frame)[:, :2], "IMU + ZUPT": zupt_pos[:, :2], - "Wheel Odom": run_wheel_odom( - t, wheel, gyro, initial, LEVER_ARM_A - )[:, :2], + "Wheel Odom": run_wheel_odom(t, wheel, gyro, initial, LEVER_ARM_A)[:, :2], "PDR (Mag)": pdr_pos[:, :2], } diff --git a/tests/ch6_dead_reckoning/test_pdr_error_budget.py b/tests/ch6_dead_reckoning/test_pdr_error_budget.py index 6c580a5..98758c8 100644 --- a/tests/ch6_dead_reckoning/test_pdr_error_budget.py +++ b/tests/ch6_dead_reckoning/test_pdr_error_budget.py @@ -55,7 +55,7 @@ def pdr_run(): gyro_rrw_rad_s_sqrt_s=0.0, accel_bias_mps2=units.mg_to_mps2(10.0), accel_vrw_mps_sqrt_s=units.mps_per_sqrt_hour_to_mps_per_sqrt_sec(0.01), - grade='consumer (high gyro drift)', + grade="consumer (high gyro drift)", ) t, pos_true, heading_true, accel, gyro, mag, expected_steps = ( diff --git a/tests/ch6_dead_reckoning/test_zupt_animation.py b/tests/ch6_dead_reckoning/test_zupt_animation.py index 80ae452..aead7b5 100644 --- a/tests/ch6_dead_reckoning/test_zupt_animation.py +++ b/tests/ch6_dead_reckoning/test_zupt_animation.py @@ -40,13 +40,18 @@ def setUpClass(cls): frame = FrameConvention.create_enu() imu_params = IMUNoiseParams.consumer_grade() - (cls.t, cls.pos_true, vel_true, quat_true, - accel_body, gyro_body, cls.stance_mask) = generate_walking_trajectory( + ( + cls.t, + cls.pos_true, + vel_true, + quat_true, + accel_body, + gyro_body, + cls.stance_mask, + ) = generate_walking_trajectory( duration=21.0, dt=0.01, step_freq=2.0, step_length=0.7, frame=frame ) - accel_meas, gyro_meas = add_imu_noise( - accel_body, gyro_body, 0.01, imu_params - ) + accel_meas, gyro_meas = add_imu_noise(accel_body, gyro_body, 0.01, imu_params) initial_state = NavStateQPVP( q=quat_true[0].copy(), v=vel_true[0].copy(), p=cls.pos_true[0].copy() ) @@ -55,17 +60,19 @@ def setUpClass(cls): cls.t, accel_meas, gyro_meas, initial_state, frame ) cls.pos_zupt, _, cls.detections = run_imu_with_zupt_ekf( - cls.t, accel_meas, gyro_meas, initial_state, frame, imu_params, - window_size=10, gamma=1000.0, + cls.t, + accel_meas, + gyro_meas, + initial_state, + frame, + imu_params, + window_size=10, + gamma=1000.0, ) def _errors(self): - error_imu = np.linalg.norm( - self.pos_imu[:, :2] - self.pos_true[:, :2], axis=1 - ) - error_zupt = np.linalg.norm( - self.pos_zupt[:, :2] - self.pos_true[:, :2], axis=1 - ) + error_imu = np.linalg.norm(self.pos_imu[:, :2] - self.pos_true[:, :2], axis=1) + error_zupt = np.linalg.norm(self.pos_zupt[:, :2] - self.pos_true[:, :2], axis=1) return error_imu, error_zupt def test_zupt_bounds_the_drift(self): @@ -74,8 +81,8 @@ def test_zupt_bounds_the_drift(self): self.assertLess(error_zupt[-1], error_imu[-1]) self.assertLess( - np.sqrt(np.mean(error_zupt ** 2)), - 0.5 * np.sqrt(np.mean(error_imu ** 2)), + np.sqrt(np.mean(error_zupt**2)), + 0.5 * np.sqrt(np.mean(error_imu**2)), ) def test_trajectory_contains_stance_and_swing(self): @@ -86,8 +93,13 @@ def test_trajectory_contains_stance_and_swing(self): def test_animation_renders_every_frame(self): """Frame count matches the request and each frame draws.""" fig, update, n_frames = animate_zupt_drift( - self.t, self.pos_true, self.pos_imu, self.pos_zupt, - self.stance_mask, self.detections, n_frames=6, + self.t, + self.pos_true, + self.pos_imu, + self.pos_zupt, + self.stance_mask, + self.detections, + n_frames=6, ) try: self.assertEqual(n_frames, 6) @@ -100,8 +112,13 @@ def test_animation_renders_every_frame(self): def test_animation_stays_small(self): """Committed binaries live in git history forever.""" fig, update, n_frames = animate_zupt_drift( - self.t, self.pos_true, self.pos_imu, self.pos_zupt, - self.stance_mask, self.detections, n_frames=8, + self.t, + self.pos_true, + self.pos_imu, + self.pos_zupt, + self.stance_mask, + self.detections, + n_frames=8, ) try: with tempfile.TemporaryDirectory() as tmp: diff --git a/tests/ch7_slam/test_bundle_adjustment_reports_pixels.py b/tests/ch7_slam/test_bundle_adjustment_reports_pixels.py index 0540b80..b616cff 100644 --- a/tests/ch7_slam/test_bundle_adjustment_reports_pixels.py +++ b/tests/ch7_slam/test_bundle_adjustment_reports_pixels.py @@ -54,19 +54,23 @@ def _graph_with_known_residuals(reprojection, prior): graph = FactorGraph() graph.add_variable(0, np.zeros(2)) for r in reprojection: - graph.add_factor(Factor( + graph.add_factor( + Factor( + variable_ids=[0], + residual_func=lambda _x, r=np.asarray(r, dtype=float): r, + jacobian_func=lambda _x: [np.zeros((2, 2))], + information=np.eye(2) / (PIXEL_NOISE_STD**2), + ) + ) + # The gauge prior is appended last and its residual is in metres. + graph.add_factor( + Factor( variable_ids=[0], - residual_func=lambda _x, r=np.asarray(r, dtype=float): r, + residual_func=lambda _x, r=np.asarray(prior, dtype=float): r, jacobian_func=lambda _x: [np.zeros((2, 2))], - information=np.eye(2) / (PIXEL_NOISE_STD ** 2), - )) - # The gauge prior is appended last and its residual is in metres. - graph.add_factor(Factor( - variable_ids=[0], - residual_func=lambda _x, r=np.asarray(prior, dtype=float): r, - jacobian_func=lambda _x: [np.zeros((2, 2))], - information=np.eye(2), - )) + information=np.eye(2), + ) + ) return graph @@ -105,7 +109,7 @@ def test_the_cost_is_not_the_pixel_error(self): pixels = rms(reprojection_residuals_px(graph, 8)) self.assertAlmostEqual(pixels, 5.0) - self.assertAlmostEqual(cost, 8 * 25.0 / PIXEL_NOISE_STD ** 2) + self.assertAlmostEqual(cost, 8 * 25.0 / PIXEL_NOISE_STD**2) self.assertGreater(cost / pixels, 100.0) def test_the_cost_grows_with_observation_count_and_the_rms_does_not(self): diff --git a/tests/ch7_slam/test_example_scan_matching_visualization.py b/tests/ch7_slam/test_example_scan_matching_visualization.py index 327fa34..ac8e263 100644 --- a/tests/ch7_slam/test_example_scan_matching_visualization.py +++ b/tests/ch7_slam/test_example_scan_matching_visualization.py @@ -98,9 +98,7 @@ def test_icp_recovers_the_true_motion(self): ) self.assertTrue(icp_ok) - np.testing.assert_allclose( - icp_pose[:2], self.true_motion[:2], atol=0.05 - ) + np.testing.assert_allclose(icp_pose[:2], self.true_motion[:2], atol=0.05) def test_ndt_outcome_does_not_depend_on_step_size(self): """Pins the claim the score-surface figure makes. @@ -120,14 +118,15 @@ def test_ndt_outcome_does_not_depend_on_step_size(self): poses = {} for step_size in (0.05, 0.1, 0.3, 0.5, 1.0): pose, iters, _, converged = ndt_align( - self.source, self.target, voxel_size=1.0, - step_size=step_size, max_iterations=200, + self.source, + self.target, + voxel_size=1.0, + step_size=step_size, + max_iterations=200, ) poses[step_size] = pose with self.subTest(step_size=step_size): - np.testing.assert_allclose( - pose[:2], self.true_motion[:2], atol=0.05 - ) + np.testing.assert_allclose(pose[:2], self.true_motion[:2], atol=0.05) self.assertTrue(converged) # Not merely all-correct: they agree with each other to within the @@ -137,7 +136,8 @@ def test_ndt_outcome_does_not_depend_on_step_size(self): # it lands inside the tolerance.) spread = max( np.linalg.norm(a[:2] - b[:2]) - for a in poses.values() for b in poses.values() + for a in poses.values() + for b in poses.values() ) self.assertLess(spread, 0.05, f"step size still steers the answer: {poses}") @@ -179,9 +179,7 @@ def test_icp_animation_renders_small_and_monotone(self): try: self.assertGreater(n_frames, 5) with tempfile.TemporaryDirectory() as tmp: - path = save_animation( - fig, update, n_frames, tmp, "icp_anim", fps=4 - ) + path = save_animation(fig, update, n_frames, tmp, "icp_anim", fps=4) size_mb = path.stat().st_size / (1024 * 1024) finally: plt.close(fig) diff --git a/tests/ch7_slam/test_icp_recovers_the_delta_pose.py b/tests/ch7_slam/test_icp_recovers_the_delta_pose.py index 6592893..093b034 100644 --- a/tests/ch7_slam/test_icp_recovers_the_delta_pose.py +++ b/tests/ch7_slam/test_icp_recovers_the_delta_pose.py @@ -70,10 +70,13 @@ def _arc_scan(n=400, seed=0): radius = rng.uniform(4.0, 8.0, size=n) arc = np.stack([radius * np.cos(angles), radius * np.sin(angles)], axis=1) - wall = np.stack([ - rng.uniform(-2.0, 2.0, size=n // 5), - rng.uniform(6.0, 7.0, size=n // 5), - ], axis=1) + wall = np.stack( + [ + rng.uniform(-2.0, 2.0, size=n // 5), + rng.uniform(6.0, 7.0, size=n // 5), + ], + axis=1, + ) return np.concatenate([arc, wall], axis=0).astype(np.float64) @@ -187,8 +190,12 @@ def test_se2_relative_matches_compose_of_the_inverse(): rng = np.random.default_rng(0) for _ in range(100): - a = np.array([rng.uniform(-5, 5), rng.uniform(-5, 5), rng.uniform(-np.pi, np.pi)]) - b = np.array([rng.uniform(-5, 5), rng.uniform(-5, 5), rng.uniform(-np.pi, np.pi)]) + a = np.array( + [rng.uniform(-5, 5), rng.uniform(-5, 5), rng.uniform(-np.pi, np.pi)] + ) + b = np.array( + [rng.uniform(-5, 5), rng.uniform(-5, 5), rng.uniform(-np.pi, np.pi)] + ) assert se2_relative(a, b) == pytest.approx( se2_compose(se2_inverse(a), b), abs=1e-12 diff --git a/tests/ch7_slam/test_slam_frontend_figure.py b/tests/ch7_slam/test_slam_frontend_figure.py index 993b35a..f6dc774 100644 --- a/tests/ch7_slam/test_slam_frontend_figure.py +++ b/tests/ch7_slam/test_slam_frontend_figure.py @@ -98,9 +98,7 @@ def test_odometry_drift_is_resolvable_on_the_page(self): self.panel, self.demo["odom_xy"], self.demo["frontend_xy"] ) - self.assertGreater( - gap, 100.0, f"odometry and front-end are {gap:.1f} px apart" - ) + self.assertGreater(gap, 100.0, f"odometry and front-end are {gap:.1f} px apart") def test_the_frontend_track_clears_the_ground_truth(self): """The harder half: the *good* estimate has structure worth seeing. diff --git a/tests/ch8_sensor_fusion/test_batch_update.py b/tests/ch8_sensor_fusion/test_batch_update.py index a0dda27..60abe7d 100644 --- a/tests/ch8_sensor_fusion/test_batch_update.py +++ b/tests/ch8_sensor_fusion/test_batch_update.py @@ -30,77 +30,60 @@ def setUpClass(cls): def test_sequential_mode_runs(self): """Test that sequential mode runs without errors.""" history = run_tc_fusion( - self.dataset, - use_gating=False, - batch_update=False, - verbose=False + self.dataset, use_gating=False, batch_update=False, verbose=False ) - self.assertGreater(len(history['t']), 0) - self.assertGreater(history['n_uwb_accepted'], 0) - self.assertEqual(history['n_uwb_rejected'], 0) # No gating + self.assertGreater(len(history["t"]), 0) + self.assertGreater(history["n_uwb_accepted"], 0) + self.assertEqual(history["n_uwb_rejected"], 0) # No gating def test_batch_mode_runs(self): """Test that batch mode runs without errors.""" history = run_tc_fusion( - self.dataset, - use_gating=False, - batch_update=True, - verbose=False + self.dataset, use_gating=False, batch_update=True, verbose=False ) - self.assertGreater(len(history['t']), 0) - self.assertGreater(history['n_uwb_accepted'], 0) - self.assertEqual(history['n_uwb_rejected'], 0) # No gating + self.assertGreater(len(history["t"]), 0) + self.assertGreater(history["n_uwb_accepted"], 0) + self.assertEqual(history["n_uwb_rejected"], 0) # No gating def test_batch_vs_sequential_update_count(self): """Test that batch mode has fewer updates than sequential (epochs vs ranges).""" history_seq = run_tc_fusion( - self.dataset, - use_gating=False, - batch_update=False, - verbose=False + self.dataset, use_gating=False, batch_update=False, verbose=False ) history_batch = run_tc_fusion( - self.dataset, - use_gating=False, - batch_update=True, - verbose=False + self.dataset, use_gating=False, batch_update=True, verbose=False ) # Batch mode should have ~4x fewer updates (4 anchors → 1 batch per epoch) self.assertLess( - history_batch['n_uwb_accepted'], - history_seq['n_uwb_accepted'] / 2 + history_batch["n_uwb_accepted"], history_seq["n_uwb_accepted"] / 2 ) def test_batch_accuracy_comparable(self): """Test that batch mode achieves similar accuracy to sequential mode.""" history_seq = run_tc_fusion( - self.dataset, - use_gating=False, - batch_update=False, - verbose=False + self.dataset, use_gating=False, batch_update=False, verbose=False ) history_batch = run_tc_fusion( - self.dataset, - use_gating=False, - batch_update=True, - verbose=False + self.dataset, use_gating=False, batch_update=True, verbose=False ) # Compute RMSE for both - truth = self.dataset['truth'] + truth = self.dataset["truth"] # Interpolate truth to estimated timestamps def compute_rmse(history): - p_true_interp = np.column_stack([ - np.interp(history['t'], truth['t'], truth['p_xy'][:, 0]), - np.interp(history['t'], truth['t'], truth['p_xy'][:, 1]) - ]) - errors = history['x_est'][:, :2] - p_true_interp + p_true_interp = np.column_stack( + [ + np.interp(history["t"], truth["t"], truth["p_xy"][:, 0]), + np.interp(history["t"], truth["t"], truth["p_xy"][:, 1]), + ] + ) + errors = history["x_est"][:, :2] - p_true_interp return np.sqrt(np.mean(np.sum(errors**2, axis=1))) rmse_seq = compute_rmse(history_seq) @@ -117,51 +100,44 @@ def compute_rmse(history): def test_batch_with_gating(self): """Test that batch mode works with adaptive gating.""" history = run_tc_fusion( - self.dataset, - use_gating=True, - batch_update=True, - verbose=False + self.dataset, use_gating=True, batch_update=True, verbose=False ) - self.assertGreater(len(history['t']), 0) - self.assertGreater(history['n_uwb_accepted'], 0) + self.assertGreater(len(history["t"]), 0) + self.assertGreater(history["n_uwb_accepted"], 0) # Should have some rejections with gating - total_measurements = history['n_uwb_accepted'] + history['n_uwb_rejected'] - acceptance_rate = history['n_uwb_accepted'] / total_measurements + total_measurements = history["n_uwb_accepted"] + history["n_uwb_rejected"] + acceptance_rate = history["n_uwb_accepted"] / total_measurements # Acceptance rate should be reasonable (> 50%) self.assertGreater(acceptance_rate, 0.5) def test_batch_improves_gating_performance(self): """Test that batch mode improves accuracy when using gating. - + This is the key advantage of batch mode: applying chi-square test to the full measurement vector is more statistically sound. """ history_seq = run_tc_fusion( - self.dataset, - use_gating=True, - batch_update=False, - verbose=False + self.dataset, use_gating=True, batch_update=False, verbose=False ) history_batch = run_tc_fusion( - self.dataset, - use_gating=True, - batch_update=True, - verbose=False + self.dataset, use_gating=True, batch_update=True, verbose=False ) # Compute RMSE - truth = self.dataset['truth'] + truth = self.dataset["truth"] def compute_rmse(history): - p_true_interp = np.column_stack([ - np.interp(history['t'], truth['t'], truth['p_xy'][:, 0]), - np.interp(history['t'], truth['t'], truth['p_xy'][:, 1]) - ]) - errors = history['x_est'][:, :2] - p_true_interp + p_true_interp = np.column_stack( + [ + np.interp(history["t"], truth["t"], truth["p_xy"][:, 0]), + np.interp(history["t"], truth["t"], truth["p_xy"][:, 1]), + ] + ) + errors = history["x_est"][:, :2] - p_true_interp return np.sqrt(np.mean(np.sum(errors**2, axis=1))) rmse_seq = compute_rmse(history_seq) @@ -179,10 +155,12 @@ def test_batch_measurement_structure(self): """Test that batch measurements have correct structure.""" # Create dummy data uwb_t = np.array([1.0, 2.0]) - uwb_ranges = np.array([ - [5.0, np.nan, 7.0, 6.0], # Epoch 1: anchor 1 dropout - [5.1, 7.2, np.nan, 6.1], # Epoch 2: anchor 2 dropout - ]) + uwb_ranges = np.array( + [ + [5.0, np.nan, 7.0, 6.0], # Epoch 1: anchor 1 dropout + [5.1, 7.2, np.nan, 6.1], # Epoch 2: anchor 2 dropout + ] + ) # Simulate what batch mode does for i in range(len(uwb_t)): @@ -206,6 +184,5 @@ def test_batch_measurement_structure(self): np.testing.assert_array_almost_equal(valid_ranges, [5.1, 7.2, 6.1]) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() - diff --git a/tests/ch8_sensor_fusion/test_calibration.py b/tests/ch8_sensor_fusion/test_calibration.py index 63b8bad..95870f8 100644 --- a/tests/ch8_sensor_fusion/test_calibration.py +++ b/tests/ch8_sensor_fusion/test_calibration.py @@ -39,14 +39,12 @@ def test_perfect_stationary_data(self): # Should recover exact biases np.testing.assert_allclose( - calibration['accel_bias'], true_accel_bias, atol=1e-10 - ) - np.testing.assert_allclose( - calibration['gyro_bias'], true_gyro_bias, atol=1e-10 + calibration["accel_bias"], true_accel_bias, atol=1e-10 ) + np.testing.assert_allclose(calibration["gyro_bias"], true_gyro_bias, atol=1e-10) # Gravity axis should be Z (index 2) - self.assertEqual(calibration['gravity_axis'], 2) + self.assertEqual(calibration["gravity_axis"], 2) def test_noisy_stationary_data(self): """Test bias estimation with noisy data.""" @@ -58,12 +56,12 @@ def test_noisy_stationary_data(self): # Add noise gravity = np.array([0, 0, -9.81]) accel_samples = ( - np.tile(gravity + true_accel_bias, (n_samples, 1)) + - np.random.randn(n_samples, 3) * 0.01 + np.tile(gravity + true_accel_bias, (n_samples, 1)) + + np.random.randn(n_samples, 3) * 0.01 ) gyro_samples = ( - np.tile(true_gyro_bias, (n_samples, 1)) + - np.random.randn(n_samples, 3) * 0.001 + np.tile(true_gyro_bias, (n_samples, 1)) + + np.random.randn(n_samples, 3) * 0.001 ) calibration = estimate_imu_bias_stationary(accel_samples, gyro_samples) @@ -71,10 +69,10 @@ def test_noisy_stationary_data(self): # Should recover biases within noise tolerance # With 1000 samples, std_error = 0.01 / sqrt(1000) ≈ 0.0003 np.testing.assert_allclose( - calibration['accel_bias'], true_accel_bias, atol=0.001 + calibration["accel_bias"], true_accel_bias, atol=0.001 ) np.testing.assert_allclose( - calibration['gyro_bias'], true_gyro_bias, atol=0.0001 + calibration["gyro_bias"], true_gyro_bias, atol=0.0001 ) def test_different_gravity_orientations(self): @@ -88,14 +86,14 @@ def test_different_gravity_orientations(self): gyro_samples = np.zeros((n_samples, 3)) calibration = estimate_imu_bias_stationary(accel_samples, gyro_samples) - self.assertEqual(calibration['gravity_axis'], 0) # X-axis + self.assertEqual(calibration["gravity_axis"], 0) # X-axis # Test Y-axis gravity gravity_y = np.array([0, -9.81, 0]) accel_samples = np.tile(gravity_y + accel_bias, (n_samples, 1)) calibration = estimate_imu_bias_stationary(accel_samples, gyro_samples) - self.assertEqual(calibration['gravity_axis'], 1) # Y-axis + self.assertEqual(calibration["gravity_axis"], 1) # Y-axis def test_synthetic_data_generation(self): """Test synthetic IMU data generation.""" @@ -105,28 +103,28 @@ def test_synthetic_data_generation(self): duration=5.0, rate=100.0, accel_bias=np.array([0.1, -0.05, 0.03]), - gyro_bias=np.array([0.02, -0.01, 0.015]) + gyro_bias=np.array([0.02, -0.01, 0.015]), ) # Check data structure - self.assertIn('t', data) - self.assertIn('accel', data) - self.assertIn('gyro', data) - self.assertIn('true_accel_bias', data) - self.assertIn('true_gyro_bias', data) + self.assertIn("t", data) + self.assertIn("accel", data) + self.assertIn("gyro", data) + self.assertIn("true_accel_bias", data) + self.assertIn("true_gyro_bias", data) # Check dimensions expected_samples = int(5.0 * 100.0) - self.assertEqual(len(data['t']), expected_samples) - self.assertEqual(data['accel'].shape, (expected_samples, 3)) - self.assertEqual(data['gyro'].shape, (expected_samples, 3)) + self.assertEqual(len(data["t"]), expected_samples) + self.assertEqual(data["accel"].shape, (expected_samples, 3)) + self.assertEqual(data["gyro"].shape, (expected_samples, 3)) # Verify biases match np.testing.assert_allclose( - data['true_accel_bias'], np.array([0.1, -0.05, 0.03]) + data["true_accel_bias"], np.array([0.1, -0.05, 0.03]) ) np.testing.assert_allclose( - data['true_gyro_bias'], np.array([0.02, -0.01, 0.015]) + data["true_gyro_bias"], np.array([0.02, -0.01, 0.015]) ) @@ -181,10 +179,9 @@ def test_pure_rotation(self): # Apply rotation only angle = np.pi / 4 # 45 degrees - R_true = np.array([ - [np.cos(angle), -np.sin(angle)], - [np.sin(angle), np.cos(angle)] - ]) + R_true = np.array( + [[np.cos(angle), -np.sin(angle)], [np.sin(angle), np.cos(angle)]] + ) p_sensor2 = (R_true @ p_sensor1.T).T R_est, t_est = calibrate_extrinsic_2d_least_squares(p_sensor1, p_sensor2) @@ -204,10 +201,9 @@ def test_combined_rotation_and_translation(self): # Apply rotation and translation angle = np.pi / 6 # 30 degrees - R_true = np.array([ - [np.cos(angle), -np.sin(angle)], - [np.sin(angle), np.cos(angle)] - ]) + R_true = np.array( + [[np.cos(angle), -np.sin(angle)], [np.sin(angle), np.cos(angle)]] + ) t_true = np.array([1.0, 2.0]) p_sensor2 = (R_true @ p_sensor1.T).T + t_true @@ -228,10 +224,9 @@ def test_with_measurement_noise(self): # Apply known transformation angle = np.pi / 4 - R_true = np.array([ - [np.cos(angle), -np.sin(angle)], - [np.sin(angle), np.cos(angle)] - ]) + R_true = np.array( + [[np.cos(angle), -np.sin(angle)], [np.sin(angle), np.cos(angle)]] + ) t_true = np.array([0.5, -0.3]) p_sensor2 = (R_true @ p_sensor1.T).T + t_true @@ -269,9 +264,7 @@ def test_rotation_matrix_properties(self): # 3. Preserves norm: ||R @ v|| = ||v|| v = np.array([1.0, 2.0]) np.testing.assert_allclose( - np.linalg.norm(R_est @ v), - np.linalg.norm(v), - atol=1e-10 + np.linalg.norm(R_est @ v), np.linalg.norm(v), atol=1e-10 ) def test_synthetic_data_generation(self): @@ -282,25 +275,25 @@ def test_synthetic_data_generation(self): duration=20.0, rate=10.0, lever_arm=np.array([1.0, 0.5]), - rotation_angle=np.pi / 3 # 60 degrees + rotation_angle=np.pi / 3, # 60 degrees ) # Check data structure - self.assertIn('t', data) - self.assertIn('p_sensor1', data) - self.assertIn('p_sensor2', data) - self.assertIn('true_R', data) - self.assertIn('true_t', data) - self.assertIn('true_rotation_angle', data) + self.assertIn("t", data) + self.assertIn("p_sensor1", data) + self.assertIn("p_sensor2", data) + self.assertIn("true_R", data) + self.assertIn("true_t", data) + self.assertIn("true_rotation_angle", data) # Check dimensions expected_samples = int(20.0 * 10.0) - self.assertEqual(len(data['t']), expected_samples) - self.assertEqual(data['p_sensor1'].shape, (expected_samples, 2)) - self.assertEqual(data['p_sensor2'].shape, (expected_samples, 2)) + self.assertEqual(len(data["t"]), expected_samples) + self.assertEqual(data["p_sensor1"].shape, (expected_samples, 2)) + self.assertEqual(data["p_sensor2"].shape, (expected_samples, 2)) # Check rotation matrix properties - R = data['true_R'] + R = data["true_R"] np.testing.assert_allclose(R @ R.T, np.eye(2), atol=1e-10) self.assertAlmostEqual(np.linalg.det(R), 1.0, places=10) @@ -320,15 +313,13 @@ def test_imu_calibration_workflow(self): data = generate_synthetic_imu_stationary(duration=5.0, rate=100.0) # Estimate biases - calibration = estimate_imu_bias_stationary(data['accel'], data['gyro']) + calibration = estimate_imu_bias_stationary(data["accel"], data["gyro"]) # Verify estimates are close to truth accel_error = np.linalg.norm( - calibration['accel_bias'] - data['true_accel_bias'] - ) - gyro_error = np.linalg.norm( - calibration['gyro_bias'] - data['true_gyro_bias'] + calibration["accel_bias"] - data["true_accel_bias"] ) + gyro_error = np.linalg.norm(calibration["gyro_bias"] - data["true_gyro_bias"]) # With 500 samples and reasonable noise, errors should be small self.assertLess(accel_error, 0.005) # < 5 mm/s² @@ -346,28 +337,26 @@ def test_extrinsic_calibration_workflow(self): duration=30.0, rate=10.0, lever_arm=true_lever_arm, - rotation_angle=true_angle + rotation_angle=true_angle, ) # Estimate calibration R_est, t_est = calibrate_extrinsic_2d_least_squares( - data['p_sensor1'], - data['p_sensor2'] + data["p_sensor1"], data["p_sensor2"] ) # Verify estimates - np.testing.assert_allclose(R_est, data['true_R'], atol=0.01) - np.testing.assert_allclose(t_est, data['true_t'], atol=0.01) + np.testing.assert_allclose(R_est, data["true_R"], atol=0.01) + np.testing.assert_allclose(t_est, data["true_t"], atol=0.01) # Verify alignment quality - p1_transformed = (R_est @ data['p_sensor1'].T).T + t_est - residuals = data['p_sensor2'] - p1_transformed + p1_transformed = (R_est @ data["p_sensor1"].T).T + t_est + residuals = data["p_sensor2"] - p1_transformed rmse = np.sqrt(np.mean(np.sum(residuals**2, axis=1))) # RMSE should be close to measurement noise (~0.05m) self.assertLess(rmse, 0.15) # Allow some margin -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() - diff --git a/tests/ch8_sensor_fusion/test_lc_models.py b/tests/ch8_sensor_fusion/test_lc_models.py index 481c945..4ad9408 100644 --- a/tests/ch8_sensor_fusion/test_lc_models.py +++ b/tests/ch8_sensor_fusion/test_lc_models.py @@ -19,27 +19,18 @@ class TestSolveUWBPositionWLS(unittest.TestCase): def setUp(self): """Set up test fixtures.""" # Square anchor layout (20m x 15m) - self.anchors = np.array([ - [0.0, 0.0], - [20.0, 0.0], - [20.0, 15.0], - [0.0, 15.0] - ]) + self.anchors = np.array([[0.0, 0.0], [20.0, 0.0], [20.0, 15.0], [0.0, 15.0]]) # True position at center self.true_pos = np.array([10.0, 7.5]) # Compute true ranges - self.true_ranges = np.linalg.norm( - self.anchors - self.true_pos, axis=1 - ) + self.true_ranges = np.linalg.norm(self.anchors - self.true_pos, axis=1) def test_nominal_case_converges(self): """Test that WLS converges on noise-free measurements.""" pos, cov, converged = solve_uwb_position_wls( - ranges=self.true_ranges, - anchor_positions=self.anchors, - range_noise_std=0.05 + ranges=self.true_ranges, anchor_positions=self.anchors, range_noise_std=0.05 ) self.assertIsNotNone(pos) @@ -61,7 +52,7 @@ def test_with_measurement_noise(self): pos, cov, converged = solve_uwb_position_wls( ranges=noisy_ranges, anchor_positions=self.anchors, - range_noise_std=noise_std + range_noise_std=noise_std, ) self.assertIsNotNone(pos) @@ -79,7 +70,7 @@ def test_covariance_floor_enforced(self): ranges=self.true_ranges, anchor_positions=self.anchors, range_noise_std=0.001, # Very small noise - cov_floor_std=0.2 # Floor at 0.2m std + cov_floor_std=0.2, # Floor at 0.2m std ) self.assertIsNotNone(pos) @@ -100,7 +91,7 @@ def test_anchor_dependent_noise(self): pos, cov, converged = solve_uwb_position_wls( ranges=self.true_ranges, anchor_positions=self.anchors, - anchor_noise_std=anchor_stds + anchor_noise_std=anchor_stds, ) self.assertIsNotNone(pos) @@ -113,15 +104,13 @@ def test_anchor_dependent_noise(self): # Covariance should be larger than uniform-noise case # (degraded anchor reduces overall precision) pos_uniform, cov_uniform, _ = solve_uwb_position_wls( - ranges=self.true_ranges, - anchor_positions=self.anchors, - range_noise_std=0.05 + ranges=self.true_ranges, anchor_positions=self.anchors, range_noise_std=0.05 ) # At least one axis should have larger uncertainty self.assertTrue( cov[0, 0] > cov_uniform[0, 0] or cov[1, 1] > cov_uniform[1, 1], - "Degraded anchor should increase covariance" + "Degraded anchor should increase covariance", ) def test_dropout_handling(self): @@ -132,7 +121,7 @@ def test_dropout_handling(self): pos, cov, converged = solve_uwb_position_wls( ranges=ranges_with_dropout, anchor_positions=self.anchors, - range_noise_std=0.05 + range_noise_std=0.05, ) self.assertIsNotNone(pos) @@ -151,7 +140,7 @@ def test_insufficient_anchors_fails(self): pos, cov, converged = solve_uwb_position_wls( ranges=ranges_insufficient, anchor_positions=self.anchors, - range_noise_std=0.05 + range_noise_std=0.05, ) self.assertIsNone(pos) @@ -167,7 +156,7 @@ def test_divergence_detection(self): ranges=bad_ranges, anchor_positions=self.anchors, range_noise_std=0.05, - max_iterations=5 + max_iterations=5, ) # Should either fail or produce position within reasonable bounds @@ -178,9 +167,9 @@ def test_divergence_detection(self): margin = 50.0 self.assertTrue( - np.all(pos >= anchor_min - margin) and - np.all(pos <= anchor_max + margin), - "Divergent position not rejected" + np.all(pos >= anchor_min - margin) + and np.all(pos <= anchor_max + margin), + "Divergent position not rejected", ) def test_covariance_realism(self): @@ -190,20 +179,21 @@ def test_covariance_realism(self): ranges=self.true_ranges, anchor_positions=self.anchors, range_noise_std=0.05, - cov_floor_std=0.0 # Disable floor for this test + cov_floor_std=0.0, # Disable floor for this test ) pos_high, cov_high, _ = solve_uwb_position_wls( ranges=self.true_ranges, anchor_positions=self.anchors, range_noise_std=0.2, - cov_floor_std=0.0 + cov_floor_std=0.0, ) # Higher noise should give larger covariance self.assertGreater( - np.trace(cov_high), np.trace(cov_low), - "Covariance should increase with noise" + np.trace(cov_high), + np.trace(cov_low), + "Covariance should increase with noise", ) # Test 2: Covariance should scale approximately with σ² @@ -227,20 +217,13 @@ class TestWLSIntegrationWithGating(unittest.TestCase): def setUp(self): """Set up test fixtures.""" - self.anchors = np.array([ - [0.0, 0.0], - [20.0, 0.0], - [20.0, 15.0], - [0.0, 15.0] - ]) + self.anchors = np.array([[0.0, 0.0], [20.0, 0.0], [20.0, 15.0], [0.0, 15.0]]) self.true_pos = np.array([10.0, 7.5]) - self.true_ranges = np.linalg.norm( - self.anchors - self.true_pos, axis=1 - ) + self.true_ranges = np.linalg.norm(self.anchors - self.true_pos, axis=1) def test_realistic_covariance_for_gating(self): """Test that WLS covariance is realistic enough for chi-square gating. - + This is a regression test for the issue where overconfident WLS covariance caused gating to reject too many valid measurements. """ @@ -255,10 +238,7 @@ def test_realistic_covariance_for_gating(self): P_ekf = np.diag([0.5**2, 0.5**2, 0.2**2, 0.2**2, 0.1**2]) # Measurement Jacobian H for position measurement - H = np.array([ - [1.0, 0.0, 0.0, 0.0, 0.0], - [0.0, 1.0, 0.0, 0.0, 0.0] - ]) + H = np.array([[1.0, 0.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0, 0.0]]) n_accepted = 0 n_rejected = 0 @@ -272,7 +252,7 @@ def test_realistic_covariance_for_gating(self): ranges=noisy_ranges, anchor_positions=self.anchors, range_noise_std=noise_std, - cov_floor_std=0.2 # Realistic floor + cov_floor_std=0.2, # Realistic floor ) if not converged: @@ -300,11 +280,11 @@ def test_realistic_covariance_for_gating(self): self.assertGreater(n_accepted, 0, "No measurements accepted") self.assertLess( - rejection_rate, 0.20, - f"Rejection rate {rejection_rate:.1%} too high - covariance likely overconfident" + rejection_rate, + 0.20, + f"Rejection rate {rejection_rate:.1%} too high - covariance likely overconfident", ) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() - diff --git a/tests/ch8_sensor_fusion/test_observability.py b/tests/ch8_sensor_fusion/test_observability.py index 81193d1..f46c1af 100644 --- a/tests/ch8_sensor_fusion/test_observability.py +++ b/tests/ch8_sensor_fusion/test_observability.py @@ -51,18 +51,10 @@ def test_partially_observable_system(self): # [0, 0, 0, 1]] dt = 0.1 - F = np.array([ - [1, 0, dt, 0], - [0, 1, 0, dt], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) + F = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) # Observe velocity only - H = np.array([ - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) + H = np.array([[0, 0, 1, 0], [0, 0, 0, 1]]) F_sequence = [F] * 10 H_sequence = [H] * 10 @@ -77,28 +69,26 @@ def test_observability_improves_with_direct_measurement(self): """Test that adding direct measurements increases rank.""" # Same system as above, but with occasional position measurements dt = 0.1 - F = np.array([ - [1, 0, dt, 0], - [0, 1, 0, dt], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) - - H_velocity = np.array([ - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) - - H_position = np.array([ - [1, 0, 0, 0], - [0, 1, 0, 0] - ]) + F = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) + + H_velocity = np.array([[0, 0, 1, 0], [0, 0, 0, 1]]) + + H_position = np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) # Sequence: velocity, velocity, position, velocity, ... F_sequence = [F] * 10 - H_sequence = [H_velocity, H_velocity, H_position, H_velocity, - H_velocity, H_position, H_velocity, H_velocity, - H_position, H_velocity] + H_sequence = [ + H_velocity, + H_velocity, + H_position, + H_velocity, + H_velocity, + H_position, + H_velocity, + H_velocity, + H_position, + H_velocity, + ] O_EKF, rank, s = compute_observability_matrix(H_sequence, F_sequence) @@ -127,11 +117,7 @@ def test_state_transition_accumulation(self): # H2 * F2 * F1 = [1, 0] * [[1, dt], [0, 1]] * [[1, dt], [0, 1]] # = [1, dt] * [[1, dt], [0, 1]] = [1, 2*dt] - expected_O = np.array([ - [1, 0], - [1, dt], - [1, 2*dt] - ]) + expected_O = np.array([[1, 0], [1, dt], [1, 2 * dt]]) np.testing.assert_allclose(O_EKF, expected_O, atol=1e-10) @@ -166,27 +152,28 @@ def test_identify_unobservable_directions(self): # Only velocity is observable # Construct O that only sees velocity subspace - O_EKF = np.array([ - [0, 0, 1, 0], - [0, 0, 0, 1], - [0, 0, 1, 0], - [0, 0, 0, 1], - ]) + O_EKF = np.array( + [ + [0, 0, 1, 0], + [0, 0, 0, 1], + [0, 0, 1, 0], + [0, 0, 0, 1], + ] + ) U, s, Vt = np.linalg.svd(O_EKF, full_matrices=False) rank = np.sum(s > 1e-10) analysis = analyze_unobservable_states( - O_EKF, rank, - state_names=['px', 'py', 'vx', 'vy'] + O_EKF, rank, state_names=["px", "py", "vx", "vy"] ) - self.assertEqual(analysis['n_states'], 4) - self.assertEqual(analysis['n_observable'], 2) - self.assertEqual(analysis['n_unobservable'], 2) + self.assertEqual(analysis["n_states"], 4) + self.assertEqual(analysis["n_observable"], 2) + self.assertEqual(analysis["n_unobservable"], 2) # Null space should span position subspace - null_space = analysis['unobservable_modes'] + null_space = analysis["unobservable_modes"] self.assertEqual(null_space.shape, (4, 2)) # Check that null space vectors have zero velocity components @@ -203,8 +190,8 @@ def test_fully_observable_has_no_null_space(self): analysis = analyze_unobservable_states(O_EKF, rank) - self.assertEqual(analysis['n_unobservable'], 0) - self.assertEqual(analysis['unobservable_modes'].shape[1], 0) + self.assertEqual(analysis["n_unobservable"], 0) + self.assertEqual(analysis["unobservable_modes"].shape[1], 0) def test_singular_values_analysis(self): """Test that singular values are returned correctly.""" @@ -216,11 +203,9 @@ def test_singular_values_analysis(self): analysis = analyze_unobservable_states(O_EKF, rank) # Check singular values - self.assertGreater(len(analysis['singular_values']), 0) + self.assertGreater(len(analysis["singular_values"]), 0) np.testing.assert_allclose( - analysis['singular_values'], - [10, 5, 0.1, 0], - atol=1e-10 + analysis["singular_values"], [10, 5, 0.1, 0], atol=1e-10 ) @@ -229,26 +214,15 @@ class TestObservabilityIntegration(unittest.TestCase): def test_odometry_vs_position_fix_scenario(self): """Test odometry-only vs odometry+position scenario. - + This is the key scenario from the observability demo. """ dt = 0.1 - F = np.array([ - [1, 0, dt, 0], - [0, 1, 0, dt], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) - - H_odometry = np.array([ - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) - - H_position = np.array([ - [1, 0, 0, 0], - [0, 1, 0, 0] - ]) + F = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) + + H_odometry = np.array([[0, 0, 1, 0], [0, 0, 0, 1]]) + + H_position = np.array([[1, 0, 0, 0], [0, 1, 0, 0]]) # Scenario A: Odometry only F_seq_odom = [F] * 20 @@ -259,8 +233,13 @@ def test_odometry_vs_position_fix_scenario(self): # Scenario B: Odometry + occasional position fixes F_seq_fix = [F] * 20 - H_seq_fix = [H_odometry] * 5 + [H_position] + [H_odometry] * 5 + \ - [H_position] + [H_odometry] * 8 + H_seq_fix = ( + [H_odometry] * 5 + + [H_position] + + [H_odometry] * 5 + + [H_position] + + [H_odometry] * 8 + ) O_fix, rank_fix, _ = compute_observability_matrix(H_seq_fix, F_seq_fix) analysis_fix = analyze_unobservable_states(O_fix, rank_fix) @@ -269,8 +248,7 @@ def test_odometry_vs_position_fix_scenario(self): self.assertLess(rank_odom, 4) # Odometry-only not fully observable self.assertEqual(rank_fix, 4) # With fixes, fully observable self.assertGreater( - analysis_odom['n_unobservable'], - analysis_fix['n_unobservable'] + analysis_odom["n_unobservable"], analysis_fix["n_unobservable"] ) @@ -369,6 +347,5 @@ def test_empty_graph_raises(self): compute_fgo_observability_matrix(graph) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() - diff --git a/tests/ch8_sensor_fusion/test_temporal_interpolation.py b/tests/ch8_sensor_fusion/test_temporal_interpolation.py index 3792bcc..1cc1682 100644 --- a/tests/ch8_sensor_fusion/test_temporal_interpolation.py +++ b/tests/ch8_sensor_fusion/test_temporal_interpolation.py @@ -125,10 +125,12 @@ def test_realistic_imu_rate(self) -> None: t_imu = np.arange(0, 1.0, 0.01) # Sinusoidal accelerometer data - accel = np.column_stack([ - np.sin(2 * np.pi * 1.0 * t_imu), # 1 Hz sine - np.cos(2 * np.pi * 1.0 * t_imu) - ]) + accel = np.column_stack( + [ + np.sin(2 * np.pi * 1.0 * t_imu), # 1 Hz sine + np.cos(2 * np.pi * 1.0 * t_imu), + ] + ) gyro = 0.1 * np.sin(2 * np.pi * 0.5 * t_imu) # 0.5 Hz sine # Query at 10.5 Hz (UWB rate with slight offset) @@ -139,11 +141,9 @@ def test_realistic_imu_rate(self) -> None: # Expected: linear interpolation between samples at 0.10 and 0.11 # alpha = (0.105 - 0.10) / 0.01 = 0.5 idx = 10 # t_imu[10] = 0.10 - expected_u = 0.5 * np.array([ - accel[idx, 0], accel[idx, 1], gyro[idx] - ]) + 0.5 * np.array([ - accel[idx + 1, 0], accel[idx + 1, 1], gyro[idx + 1] - ]) + expected_u = 0.5 * np.array( + [accel[idx, 0], accel[idx, 1], gyro[idx]] + ) + 0.5 * np.array([accel[idx + 1, 0], accel[idx + 1, 1], gyro[idx + 1]]) np.testing.assert_array_almost_equal(u, expected_u, decimal=10) self.assertAlmostEqual(dt, 0.005, places=6) @@ -154,13 +154,13 @@ class TestAsynchronousTimestampShift(unittest.TestCase): def test_half_imu_dt_shift(self) -> None: """Test that fusion handles UWB timestamps shifted by half IMU dt. - + This is a regression test per the requirement: artificially shift UWB timestamps and ensure fusion still runs without collapsing. """ # Create synthetic data dt_imu = 0.01 # 100 Hz - dt_uwb = 0.1 # 10 Hz + dt_uwb = 0.1 # 10 Hz duration = 5.0 t_imu = np.arange(0, duration, dt_imu) @@ -198,22 +198,26 @@ def test_random_timestamp_offsets(self) -> None: """Test fusion robustness with random timestamp perturbations.""" # Create synthetic data dt_imu = 0.01 # 100 Hz - dt_uwb = 0.1 # 10 Hz + dt_uwb = 0.1 # 10 Hz duration = 2.0 t_imu = np.arange(0, duration, dt_imu) # Varying accelerometer data - accel = np.column_stack([ - 0.1 * np.sin(2 * np.pi * 0.5 * t_imu), - 0.1 * np.cos(2 * np.pi * 0.5 * t_imu) - ]) + accel = np.column_stack( + [ + 0.1 * np.sin(2 * np.pi * 0.5 * t_imu), + 0.1 * np.cos(2 * np.pi * 0.5 * t_imu), + ] + ) gyro = 0.05 * np.sin(2 * np.pi * 1.0 * t_imu) # UWB timestamps with random offsets (±5ms) np.random.seed(42) t_uwb_nominal = np.arange(0, duration, dt_uwb) - t_uwb_perturbed = t_uwb_nominal + np.random.uniform(-0.005, 0.005, len(t_uwb_nominal)) + t_uwb_perturbed = t_uwb_nominal + np.random.uniform( + -0.005, 0.005, len(t_uwb_nominal) + ) # Test interpolation at perturbed times for t_query in t_uwb_perturbed: @@ -235,4 +239,3 @@ def test_random_timestamp_offsets(self) -> None: if __name__ == "__main__": unittest.main() - diff --git a/tests/core/__init__.py b/tests/core/__init__.py index 7b29b01..360e152 100644 --- a/tests/core/__init__.py +++ b/tests/core/__init__.py @@ -1,2 +1 @@ """Unit tests for core modules.""" - diff --git a/tests/core/coords/test_rotations.py b/tests/core/coords/test_rotations.py index 8bedbdc..132ffc8 100644 --- a/tests/core/coords/test_rotations.py +++ b/tests/core/coords/test_rotations.py @@ -138,9 +138,7 @@ def test_gimbal_lock_positive(self) -> None: self.assertAlmostEqual(euler[2], 0.0, places=9) # Recovered angles must still reconstruct the same matrix - np.testing.assert_allclose( - euler_to_rotation_matrix(*euler), R, atol=1e-9 - ) + np.testing.assert_allclose(euler_to_rotation_matrix(*euler), R, atol=1e-9) def test_gimbal_lock_negative(self) -> None: """Test gimbal lock at roll = -90° (about Y in the book convention).""" @@ -154,9 +152,7 @@ def test_gimbal_lock_negative(self) -> None: self.assertAlmostEqual(euler[2], 0.0, places=9) # Recovered angles must still reconstruct the same matrix - np.testing.assert_allclose( - euler_to_rotation_matrix(*euler), R, atol=1e-9 - ) + np.testing.assert_allclose(euler_to_rotation_matrix(*euler), R, atol=1e-9) def test_invalid_matrix_shape(self) -> None: """Test that invalid matrix shape raises ValueError.""" @@ -362,9 +358,7 @@ def test_90_degree_yaw(self) -> None: q = rotation_matrix_to_quat(R) # Expected: [cos(45°), 0, 0, sin(45°)] or negative (quaternion double cover) - expected_magnitude = np.array( - [np.cos(np.pi / 4), 0.0, 0.0, np.sin(np.pi / 4)] - ) + expected_magnitude = np.array([np.cos(np.pi / 4), 0.0, 0.0, np.sin(np.pi / 4)]) # Check that q is either +expected or -expected is_positive = np.allclose(q, expected_magnitude, atol=1e-9) diff --git a/tests/core/estimators/__init__.py b/tests/core/estimators/__init__.py index 390fb57..333f798 100644 --- a/tests/core/estimators/__init__.py +++ b/tests/core/estimators/__init__.py @@ -1,4 +1 @@ """Unit tests for state estimators.""" - - - diff --git a/tests/core/estimators/test_angle_wrapping.py b/tests/core/estimators/test_angle_wrapping.py index 5bb26c5..beacd08 100644 --- a/tests/core/estimators/test_angle_wrapping.py +++ b/tests/core/estimators/test_angle_wrapping.py @@ -22,8 +22,10 @@ def create_bearing_only_innovation_func(): """Create innovation function that wraps all measurements as bearings.""" + def innovation_func(z: np.ndarray, z_pred: np.ndarray) -> np.ndarray: return np.array([angle_diff(z[i], z_pred[i]) for i in range(len(z))]) + return innovation_func @@ -32,6 +34,7 @@ class TestEKFAngleWrapping(unittest.TestCase): def setUp(self): """Setup bearing-only tracking scenario.""" + # Process model: constant angular velocity def process_model(x, u, dt): # State: [angle, angular_velocity] @@ -69,10 +72,15 @@ def test_ekf_without_wrapping_fails_at_pi_crossing(self): # EKF without angle wrapping ekf_no_wrap = ExtendedKalmanFilter( - self.process_model, self.process_jacobian, - self.measurement_model, self.measurement_jacobian, - self.Q_func, self.R_func, x0.copy(), P0.copy(), - innovation_func=None # No wrapping! + self.process_model, + self.process_jacobian, + self.measurement_model, + self.measurement_jacobian, + self.Q_func, + self.R_func, + x0.copy(), + P0.copy(), + innovation_func=None, # No wrapping! ) # True trajectory crosses pi -> -pi @@ -103,8 +111,9 @@ def test_ekf_without_wrapping_fails_at_pi_crossing(self): # Should have at least one large error when crossing pi max_error = max(errors_no_wrap) # Without wrapping, when we cross pi, innovation can be ~2*pi instead of small - self.assertGreater(max_error, 1.0, - f"Expected large error without wrapping, got {max_error}") + self.assertGreater( + max_error, 1.0, f"Expected large error without wrapping, got {max_error}" + ) def test_ekf_with_wrapping_handles_pi_crossing(self): """Test that EKF WITH angle wrapping correctly handles pi <-> -pi crossing.""" @@ -115,10 +124,15 @@ def test_ekf_with_wrapping_handles_pi_crossing(self): # EKF with angle wrapping innovation_func = create_bearing_only_innovation_func() ekf_wrap = ExtendedKalmanFilter( - self.process_model, self.process_jacobian, - self.measurement_model, self.measurement_jacobian, - self.Q_func, self.R_func, x0.copy(), P0.copy(), - innovation_func=innovation_func + self.process_model, + self.process_jacobian, + self.measurement_model, + self.measurement_jacobian, + self.Q_func, + self.R_func, + x0.copy(), + P0.copy(), + innovation_func=innovation_func, ) # True trajectory crosses pi -> -pi @@ -151,8 +165,9 @@ def test_ekf_with_wrapping_handles_pi_crossing(self): # With wrapping, errors should remain small throughout max_error = max(errors_wrap) - self.assertLess(max_error, 0.5, - f"Expected small error with wrapping, got {max_error}") + self.assertLess( + max_error, 0.5, f"Expected small error with wrapping, got {max_error}" + ) def test_pi_crossing_scenario_detailed(self): """Detailed test showing exactly what happens at pi crossing.""" @@ -169,16 +184,22 @@ def test_pi_crossing_scenario_detailed(self): innovation_wrap = angle_diff(z, z_pred) # ~ +0.3 (CORRECT!) # Verify - self.assertLess(abs(innovation_wrap), 0.5, - "Wrapped innovation should be small") - self.assertGreater(abs(innovation_no_wrap), 5.0, - "Unwrapped innovation should be large (~2*pi)") + self.assertLess(abs(innovation_wrap), 0.5, "Wrapped innovation should be small") + self.assertGreater( + abs(innovation_no_wrap), 5.0, "Unwrapped innovation should be large (~2*pi)" + ) print("\nPi Crossing Test:") - print(f" z_pred (predicted bearing): {np.rad2deg(z_pred):.1f} deg (+179.4 deg)") + print( + f" z_pred (predicted bearing): {np.rad2deg(z_pred):.1f} deg (+179.4 deg)" + ) print(f" z (true bearing): {np.rad2deg(z):.1f} deg (-177.1 deg)") - print(f" Innovation WITHOUT wrap: {np.rad2deg(innovation_no_wrap):.1f} deg (WRONG!)") - print(f" Innovation WITH wrap: {np.rad2deg(innovation_wrap):.1f} deg (CORRECT)") + print( + f" Innovation WITHOUT wrap: {np.rad2deg(innovation_no_wrap):.1f} deg (WRONG!)" + ) + print( + f" Innovation WITH wrap: {np.rad2deg(innovation_wrap):.1f} deg (CORRECT)" + ) class TestIEKFAngleWrapping(unittest.TestCase): @@ -186,6 +207,7 @@ class TestIEKFAngleWrapping(unittest.TestCase): def test_iekf_with_wrapping_handles_pi_crossing(self): """Test IEKF with angle wrapping at pi crossing.""" + # Process model: constant angular velocity def process_model(x, u, dt): return np.array([x[0] + x[1] * dt, x[1]]) @@ -214,11 +236,16 @@ def R_func(): # IEKF with angle wrapping innovation_func = create_bearing_only_innovation_func() iekf = IteratedExtendedKalmanFilter( - process_model, process_jacobian, - measurement_model, measurement_jacobian, - Q_func, R_func, x0.copy(), P0.copy(), + process_model, + process_jacobian, + measurement_model, + measurement_jacobian, + Q_func, + R_func, + x0.copy(), + P0.copy(), innovation_func=innovation_func, - max_iterations=3 + max_iterations=3, ) # Cross pi @@ -241,8 +268,11 @@ def R_func(): # With wrapping, errors should remain small max_error = max(errors) - self.assertLess(max_error, 0.5, - f"IEKF with wrapping should have small error, got {max_error}") + self.assertLess( + max_error, + 0.5, + f"IEKF with wrapping should have small error, got {max_error}", + ) class TestUKFAngleWrapping(unittest.TestCase): @@ -250,6 +280,7 @@ class TestUKFAngleWrapping(unittest.TestCase): def test_ukf_with_wrapping_handles_pi_crossing(self): """Test UKF with angle wrapping at pi crossing.""" + # Process model: constant angular velocity def process_model(x, u, dt): return np.array([x[0] + x[1] * dt, x[1]]) @@ -272,9 +303,13 @@ def R_func(): # UKF with angle wrapping innovation_func = create_bearing_only_innovation_func() ukf = UnscentedKalmanFilter( - process_model, measurement_model, - Q_func, R_func, x0.copy(), P0.copy(), - innovation_func=innovation_func + process_model, + measurement_model, + Q_func, + R_func, + x0.copy(), + P0.copy(), + innovation_func=innovation_func, ) # Cross pi @@ -297,23 +332,12 @@ def R_func(): # With wrapping, errors should remain small max_error = max(errors) - self.assertLess(max_error, 0.5, - f"UKF with wrapping should have small error, got {max_error}") + self.assertLess( + max_error, + 0.5, + f"UKF with wrapping should have small error, got {max_error}", + ) if __name__ == "__main__": unittest.main() - - - - - - - - - - - - - - diff --git a/tests/core/estimators/test_extended_kalman_filter.py b/tests/core/estimators/test_extended_kalman_filter.py index 54e9446..5741b61 100644 --- a/tests/core/estimators/test_extended_kalman_filter.py +++ b/tests/core/estimators/test_extended_kalman_filter.py @@ -42,10 +42,7 @@ def process_model(x, u, dt): def process_jacobian(x, u, dt): """Jacobian depends on x[0], so evaluation point matters!""" - return np.array([ - [1.0 + 0.2 * x[0] * dt, dt], - [0.0, 1.0] - ]) + return np.array([[1.0 + 0.2 * x[0] * dt, dt], [0.0, 1.0]]) def measurement_model(x): return np.array([x[0]]) @@ -73,9 +70,14 @@ def test_jacobian_evaluated_at_pre_prediction_state(self): P0 = np.diag([1.0, 0.5]) ekf = ExtendedKalmanFilter( - self.process_model, self.process_jacobian, - self.measurement_model, self.measurement_jacobian, - self.Q_func, self.R_func, x0, P0 + self.process_model, + self.process_jacobian, + self.measurement_model, + self.measurement_jacobian, + self.Q_func, + self.R_func, + x0, + P0, ) # Record pre-prediction state @@ -99,20 +101,28 @@ def test_jacobian_evaluated_at_pre_prediction_state(self): P_wrong = F_wrong @ P0 @ F_wrong.T + self.Q_func(self.dt) # Jacobians at different points should differ - self.assertFalse(np.allclose(F_correct, F_wrong), - "Jacobians at different states should differ") + self.assertFalse( + np.allclose(F_correct, F_wrong), + "Jacobians at different states should differ", + ) # ...and so must the covariances they produce. Without this the check # below is not discriminating: if P_correct and P_wrong happened to # coincide, asserting the filter matches P_correct would pass whichever # evaluation point it had used. - self.assertFalse(np.allclose(P_correct, P_wrong), - "Covariances from the two Jacobians should differ, " - "otherwise this test cannot tell them apart") + self.assertFalse( + np.allclose(P_correct, P_wrong), + "Covariances from the two Jacobians should differ, " + "otherwise this test cannot tell them apart", + ) # EKF must use CORRECT (pre-state) Jacobian - assert_allclose(ekf.covariance, P_correct, atol=1e-10, - err_msg="EKF used wrong Jacobian evaluation point!") + assert_allclose( + ekf.covariance, + P_correct, + atol=1e-10, + err_msg="EKF used wrong Jacobian evaluation point!", + ) def test_jacobian_difference_is_significant(self): """Test that the Jacobian difference is large enough to matter.""" @@ -126,8 +136,9 @@ def test_jacobian_difference_is_significant(self): diff = abs(F_pre[0, 0] - F_post[0, 0]) relative_diff = diff / F_pre[0, 0] - self.assertGreater(relative_diff, 0.01, - f"Jacobian difference too small: {relative_diff:.4f}") + self.assertGreater( + relative_diff, 0.01, f"Jacobian difference too small: {relative_diff:.4f}" + ) class TestEKFRangeOnlyTracking(unittest.TestCase): @@ -139,21 +150,11 @@ def test_range_only_tracking_converges(self): n_steps = 100 def process_model(x, u, dt): - F = np.array([ - [1, 0, dt, 0], - [0, 1, 0, dt], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) + F = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) return F @ x def process_jacobian(x, u, dt): - return np.array([ - [1, 0, dt, 0], - [0, 1, 0, dt], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) + return np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) def measurement_model(x): return np.array([np.sqrt(x[0] ** 2 + x[1] ** 2)]) @@ -167,12 +168,14 @@ def measurement_jacobian(x): q = 0.1 def Q_func(dt): - return q * np.array([ - [dt ** 3 / 3, 0, dt ** 2 / 2, 0], - [0, dt ** 3 / 3, 0, dt ** 2 / 2], - [dt ** 2 / 2, 0, dt, 0], - [0, dt ** 2 / 2, 0, dt] - ]) + return q * np.array( + [ + [dt**3 / 3, 0, dt**2 / 2, 0], + [0, dt**3 / 3, 0, dt**2 / 2], + [dt**2 / 2, 0, dt, 0], + [0, dt**2 / 2, 0, dt], + ] + ) def R_func(): return np.array([[0.5]]) @@ -181,9 +184,14 @@ def R_func(): P0 = np.diag([1.0, 1.0, 0.5, 0.5]) ekf = ExtendedKalmanFilter( - process_model, process_jacobian, - measurement_model, measurement_jacobian, - Q_func, R_func, x0, P0 + process_model, + process_jacobian, + measurement_model, + measurement_jacobian, + Q_func, + R_func, + x0, + P0, ) # Generate true trajectory @@ -206,10 +214,12 @@ def R_func(): position_error = np.linalg.norm(x_est[:2] - true_state[:2]) velocity_error = np.linalg.norm(x_est[2:] - true_state[2:]) - self.assertLess(position_error, 3.0, - f"Position error too large: {position_error}") - self.assertLess(velocity_error, 2.0, - f"Velocity error too large: {velocity_error}") + self.assertLess( + position_error, 3.0, f"Position error too large: {position_error}" + ) + self.assertLess( + velocity_error, 2.0, f"Velocity error too large: {velocity_error}" + ) class TestEKFCovarianceProperties(unittest.TestCase): @@ -217,6 +227,7 @@ class TestEKFCovarianceProperties(unittest.TestCase): def test_covariance_remains_positive_definite(self): """Test covariance stays positive definite after prediction.""" + def process_model(x, u, dt): return x + np.array([x[1] * dt, 0]) @@ -239,9 +250,14 @@ def R_func(): P0 = np.eye(2) ekf = ExtendedKalmanFilter( - process_model, process_jacobian, - measurement_model, measurement_jacobian, - Q_func, R_func, x0, P0 + process_model, + process_jacobian, + measurement_model, + measurement_jacobian, + Q_func, + R_func, + x0, + P0, ) for _ in range(10): @@ -249,23 +265,8 @@ def R_func(): # Check positive definite eigvals = np.linalg.eigvalsh(ekf.covariance) - self.assertTrue(np.all(eigvals > 0), - "Covariance not positive definite") + self.assertTrue(np.all(eigvals > 0), "Covariance not positive definite") if __name__ == "__main__": unittest.main() - - - - - - - - - - - - - - diff --git a/tests/core/estimators/test_factor_graph_optimization.py b/tests/core/estimators/test_factor_graph_optimization.py index ce462fb..f2a5dff 100644 --- a/tests/core/estimators/test_factor_graph_optimization.py +++ b/tests/core/estimators/test_factor_graph_optimization.py @@ -22,7 +22,7 @@ def create_range_positioning_graph( anchors: np.ndarray, true_pos: np.ndarray, initial_guess: np.ndarray, - noise_std: float = 0.0 + noise_std: float = 0.0, ) -> FactorGraph: """ Create a factor graph for 2D positioning from range measurements. @@ -80,16 +80,15 @@ def test_lm_converges_to_correct_solution(self): graph = create_range_positioning_graph(anchors, true_pos, initial_guess) optimized_vars, error_history = graph.optimize( - method="levenberg_marquardt", - max_iterations=50, - initial_mu=1e-2 + method="levenberg_marquardt", max_iterations=50, initial_mu=1e-2 ) estimated_pos = optimized_vars[0] position_error = np.linalg.norm(estimated_pos - true_pos) - self.assertLess(position_error, 0.01, - f"LM should converge, got error {position_error}") + self.assertLess( + position_error, 0.01, f"LM should converge, got error {position_error}" + ) def test_lm_error_decreases(self): """LM should decrease error overall (not diverge).""" @@ -100,14 +99,13 @@ def test_lm_error_decreases(self): graph = create_range_positioning_graph(anchors, true_pos, initial_guess) _, error_history = graph.optimize( - method="levenberg_marquardt", - max_iterations=30, - initial_mu=1e-2 + method="levenberg_marquardt", max_iterations=30, initial_mu=1e-2 ) # Final error should be less than initial - self.assertLess(error_history[-1], error_history[0], - "LM should reduce total error") + self.assertLess( + error_history[-1], error_history[0], "LM should reduce total error" + ) def test_lm_does_not_diverge(self): """LM should not diverge even with poor initial guess.""" @@ -118,14 +116,15 @@ def test_lm_does_not_diverge(self): graph = create_range_positioning_graph(anchors, true_pos, initial_guess) _, error_history = graph.optimize( - method="levenberg_marquardt", - max_iterations=100, - initial_mu=0.1 + method="levenberg_marquardt", max_iterations=100, initial_mu=0.1 ) # Should not diverge: final error should not be much worse than initial - self.assertLessEqual(error_history[-1], error_history[0] * 1.5, - "LM should not diverge significantly") + self.assertLessEqual( + error_history[-1], + error_history[0] * 1.5, + "LM should not diverge significantly", + ) def test_lm_alias_works(self): """The 'lm' alias should work.""" @@ -174,8 +173,9 @@ def test_line_search_monotonic_decrease(self): # Check monotonic decrease for i in range(1, len(error_history)): self.assertLessEqual( - error_history[i], error_history[i - 1] + 1e-10, - f"Line search should guarantee decrease at step {i}" + error_history[i], + error_history[i - 1] + 1e-10, + f"Line search should guarantee decrease at step {i}", ) @@ -192,24 +192,22 @@ def test_all_methods_converge(self): solutions = {} for method in methods: - graph = create_range_positioning_graph( - anchors, true_pos, initial_guess - ) + graph = create_range_positioning_graph(anchors, true_pos, initial_guess) optimized_vars, _ = graph.optimize(method=method, max_iterations=50) solutions[method] = optimized_vars[0] # All methods should find similar solutions for method in methods: error = np.linalg.norm(solutions[method] - true_pos) - self.assertLess(error, 0.01, - f"{method} should converge, got error {error}") + self.assertLess(error, 0.01, f"{method} should converge, got error {error}") # All solutions should be close to each other for m1 in methods: for m2 in methods: diff = np.linalg.norm(solutions[m1] - solutions[m2]) - self.assertLess(diff, 0.01, - f"{m1} and {m2} should find similar solutions") + self.assertLess( + diff, 0.01, f"{m1} and {m2} should find similar solutions" + ) def test_lm_handles_poor_initial_better_than_gn(self): """LM should handle poor initial guesses at least as well as GN.""" @@ -238,10 +236,12 @@ def test_lm_handles_poor_initial_better_than_gn(self): self.assertLess(error_lm, 0.1, "LM should converge") # LM should reduce error significantly from initial - self.assertLess(error_history_lm[-1], error_history_lm[0] * 0.01, - "LM should reduce error significantly") + self.assertLess( + error_history_lm[-1], + error_history_lm[0] * 0.01, + "LM should reduce error significantly", + ) if __name__ == "__main__": unittest.main() - diff --git a/tests/core/estimators/test_iterated_extended_kalman_filter.py b/tests/core/estimators/test_iterated_extended_kalman_filter.py index 8943b7e..11d7303 100644 --- a/tests/core/estimators/test_iterated_extended_kalman_filter.py +++ b/tests/core/estimators/test_iterated_extended_kalman_filter.py @@ -22,6 +22,7 @@ class TestIEKFAlgorithm(unittest.TestCase): def setUp(self): """Setup simple nonlinear system for testing.""" + # Process model: simple 2D motion def process_model(x, u, dt): return np.array([x[0] + x[1] * dt, x[1]]) @@ -56,10 +57,15 @@ def test_iekf_initialization(self): P0 = np.eye(2) iekf = IteratedExtendedKalmanFilter( - self.process_model, self.process_jacobian, - self.measurement_model, self.measurement_jacobian, - self.Q_func, self.R_func, x0, P0, - max_iterations=1 # Only one iteration + self.process_model, + self.process_jacobian, + self.measurement_model, + self.measurement_jacobian, + self.Q_func, + self.R_func, + x0, + P0, + max_iterations=1, # Only one iteration ) x_before_update, _ = iekf.get_state() @@ -80,11 +86,16 @@ def test_iekf_convergence(self): P0 = np.eye(2) iekf = IteratedExtendedKalmanFilter( - self.process_model, self.process_jacobian, - self.measurement_model, self.measurement_jacobian, - self.Q_func, self.R_func, x0, P0, + self.process_model, + self.process_jacobian, + self.measurement_model, + self.measurement_jacobian, + self.Q_func, + self.R_func, + x0, + P0, max_iterations=10, - convergence_tol=1e-8 + convergence_tol=1e-8, ) iekf.predict(dt=0.1) @@ -99,10 +110,15 @@ def test_iekf_returns_iteration_count(self): P0 = np.eye(2) iekf = IteratedExtendedKalmanFilter( - self.process_model, self.process_jacobian, - self.measurement_model, self.measurement_jacobian, - self.Q_func, self.R_func, x0, P0, - max_iterations=5 + self.process_model, + self.process_jacobian, + self.measurement_model, + self.measurement_jacobian, + self.Q_func, + self.R_func, + x0, + P0, + max_iterations=5, ) iekf.predict(dt=0.1) @@ -120,25 +136,14 @@ def setUp(self): self.landmarks = np.array([[0, 0], [10, 0], [10, 10], [0, 10]]) def process_model(x, u, dt): - F = np.array([ - [1, 0, dt, 0], - [0, 1, 0, dt], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) + F = np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) return F @ x def process_jacobian(x, u, dt): - return np.array([ - [1, 0, dt, 0], - [0, 1, 0, dt], - [0, 0, 1, 0], - [0, 0, 0, 1] - ]) + return np.array([[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]) def measurement_model(x): - return np.array([np.linalg.norm(lm - x[:2]) - for lm in self.landmarks]) + return np.array([np.linalg.norm(lm - x[:2]) for lm in self.landmarks]) def measurement_jacobian(x): H = [] @@ -153,12 +158,14 @@ def measurement_jacobian(x): def Q_func(dt): q = 0.1 - return q * np.array([ - [dt ** 3 / 3, 0, dt ** 2 / 2, 0], - [0, dt ** 3 / 3, 0, dt ** 2 / 2], - [dt ** 2 / 2, 0, dt, 0], - [0, dt ** 2 / 2, 0, dt] - ]) + return q * np.array( + [ + [dt**3 / 3, 0, dt**2 / 2, 0], + [0, dt**3 / 3, 0, dt**2 / 2], + [dt**2 / 2, 0, dt, 0], + [0, dt**2 / 2, 0, dt], + ] + ) def R_func(): return 0.5 * np.eye(4) @@ -177,16 +184,26 @@ def test_iekf_not_worse_than_ekf(self): P0 = np.diag([2.0, 2.0, 1.0, 1.0]) ekf = ExtendedKalmanFilter( - self.process_model, self.process_jacobian, - self.measurement_model, self.measurement_jacobian, - self.Q_func, self.R_func, x0_est.copy(), P0.copy() + self.process_model, + self.process_jacobian, + self.measurement_model, + self.measurement_jacobian, + self.Q_func, + self.R_func, + x0_est.copy(), + P0.copy(), ) iekf = IteratedExtendedKalmanFilter( - self.process_model, self.process_jacobian, - self.measurement_model, self.measurement_jacobian, - self.Q_func, self.R_func, x0_est.copy(), P0.copy(), - max_iterations=5 + self.process_model, + self.process_jacobian, + self.measurement_model, + self.measurement_jacobian, + self.Q_func, + self.R_func, + x0_est.copy(), + P0.copy(), + max_iterations=5, ) # Run simulation @@ -200,12 +217,11 @@ def test_iekf_not_worse_than_ekf(self): for _ in range(n_steps): true_state = self.process_model(true_state, None, dt) - true_state += np.random.multivariate_normal( - np.zeros(4), self.Q_func(dt) - ) + true_state += np.random.multivariate_normal(np.zeros(4), self.Q_func(dt)) - z = self.measurement_model(true_state) + \ - np.random.multivariate_normal(np.zeros(4), self.R_func()) + z = self.measurement_model(true_state) + np.random.multivariate_normal( + np.zeros(4), self.R_func() + ) ekf.predict(dt=dt) ekf.update(z) @@ -222,7 +238,7 @@ def test_iekf_not_worse_than_ekf(self): self.assertLessEqual( np.mean(iekf_errors), np.mean(ekf_errors) * 1.1, - "IEKF should not be significantly worse than EKF" + "IEKF should not be significantly worse than EKF", ) @@ -254,10 +270,15 @@ def R_func(): P0 = np.eye(2) iekf = IteratedExtendedKalmanFilter( - process_model, process_jacobian, - measurement_model, measurement_jacobian, - Q_func, R_func, x0, P0, - max_iterations=5 + process_model, + process_jacobian, + measurement_model, + measurement_jacobian, + Q_func, + R_func, + x0, + P0, + max_iterations=5, ) for _ in range(10): @@ -268,23 +289,9 @@ def R_func(): eigvals = np.linalg.eigvalsh(P) self.assertTrue( np.all(eigvals > 0), - f"Covariance not positive definite: eigvals={eigvals}" + f"Covariance not positive definite: eigvals={eigvals}", ) if __name__ == "__main__": unittest.main() - - - - - - - - - - - - - - diff --git a/tests/core/estimators/test_least_squares.py b/tests/core/estimators/test_least_squares.py index 33991a1..d53746b 100644 --- a/tests/core/estimators/test_least_squares.py +++ b/tests/core/estimators/test_least_squares.py @@ -380,9 +380,7 @@ def test_tukey_with_outliers(self): A = np.array([[1, 0], [1, 0], [1, 0], [0, 1], [0, 1], [0, 1]]) b = np.array([1.0, 1.1, 5.0, 2.0, 2.1, -3.0]) # Two outliers - x_robust, P, weights = robust_least_squares( - A, b, method="tukey", threshold=2.0 - ) + x_robust, P, weights = robust_least_squares(A, b, method="tukey", threshold=2.0) # Outliers should have zero or near-zero weight self.assertLess(weights[2], 0.1) @@ -442,9 +440,7 @@ def test_geman_mcclure_downweights_large_residuals(self): A = np.ones((6, 1)) # 6 measurements of same quantity b = np.array([1.0, 1.1, 0.95, 1.05, 10.0, -5.0]) # Two severe outliers - x_gm, P_gm, weights_gm = robust_least_squares( - A, b, method="gm", threshold=2.0 - ) + x_gm, P_gm, weights_gm = robust_least_squares(A, b, method="gm", threshold=2.0) # Outliers (indices 4, 5) should have very low weights self.assertLess(weights_gm[4], 0.1) # 10.0 outlier @@ -499,9 +495,7 @@ def test_gm_stronger_than_cauchy_on_outliers(self): _, _, weights_cauchy = robust_least_squares( A, b, method="cauchy", threshold=1.5 ) - _, _, weights_gm = robust_least_squares( - A, b, method="gm", threshold=1.5 - ) + _, _, weights_gm = robust_least_squares(A, b, method="gm", threshold=1.5) # G-M should downweight the outlier more than Cauchy self.assertLess(weights_gm[-1], weights_cauchy[-1]) @@ -524,4 +518,3 @@ def test_robust_methods_table_3_1_coverage(self): if __name__ == "__main__": unittest.main() - diff --git a/tests/core/estimators/test_nonlinear_least_squares.py b/tests/core/estimators/test_nonlinear_least_squares.py index 2273e2d..5f7b1c0 100644 --- a/tests/core/estimators/test_nonlinear_least_squares.py +++ b/tests/core/estimators/test_nonlinear_least_squares.py @@ -87,7 +87,7 @@ def test_different_initial_guesses(self): def test_covariance_is_positive_semidefinite(self): """Test that returned covariance is positive semi-definite. - + Note: With exact measurements (m=n), covariance may have near-zero eigenvalues due to numerical precision. We check for positive semi-definiteness with a tolerance. @@ -195,9 +195,7 @@ def test_weights_emphasize_accurate_measurements(self): y[3] += 3.0 # Add 3m error to last measurement # Without weights (uniform) - result_unweighted = gauss_newton( - self.h, self.jacobian, y, np.array([5.0, 5.0]) - ) + result_unweighted = gauss_newton(self.h, self.jacobian, y, np.array([5.0, 5.0])) # With weights: downweight the bad measurement weights = np.array([1.0, 1.0, 1.0, 0.01]) @@ -248,10 +246,10 @@ def jacobian(x): def test_huber_rejects_outlier(self): """Test Huber loss downweights outlier measurement.""" # Use more anchors to provide redundancy for robust estimation - anchors_extended = np.array([ - [0, 0], [10, 0], [0, 10], [10, 10], - [5, 0], [0, 5], [10, 5], [5, 10] - ], dtype=float) + anchors_extended = np.array( + [[0, 0], [10, 0], [0, 10], [10, 10], [5, 0], [0, 5], [10, 5], [5, 10]], + dtype=float, + ) true_pos = np.array([3.0, 4.0]) def h_ext(x): @@ -312,9 +310,7 @@ def test_gm_stronger_outlier_rejection_than_cauchy(self): result_cauchy = robust_gauss_newton( self.h, self.jacobian, y_outlier, x0, loss="cauchy" ) - result_gm = robust_gauss_newton( - self.h, self.jacobian, y_outlier, x0, loss="gm" - ) + result_gm = robust_gauss_newton(self.h, self.jacobian, y_outlier, x0, loss="gm") # G-M should downweight outlier more self.assertLess(result_gm.weights[3], result_cauchy.weights[3]) @@ -354,26 +350,21 @@ def jacobian(x): def test_method_gn(self): """Test method='gn' uses Gauss-Newton.""" x0 = np.array([5.0, 5.0]) - result = solve_nonlinear_ls( - self.h, self.jacobian, self.y, x0, method="gn" - ) + result = solve_nonlinear_ls(self.h, self.jacobian, self.y, x0, method="gn") assert_allclose(result.x, self.true_pos, atol=1e-6) def test_method_lm(self): """Test method='lm' uses Levenberg-Marquardt.""" x0 = np.array([5.0, 5.0]) - result = solve_nonlinear_ls( - self.h, self.jacobian, self.y, x0, method="lm" - ) + result = solve_nonlinear_ls(self.h, self.jacobian, self.y, x0, method="lm") assert_allclose(result.x, self.true_pos, atol=1e-6) def test_robust_loss_parameter(self): """Test robust_loss parameter activates robust estimation.""" # Use more anchors for redundancy - anchors_ext = np.array([ - [0, 0], [10, 0], [0, 10], [10, 10], - [5, 0], [0, 5] - ], dtype=float) + anchors_ext = np.array( + [[0, 0], [10, 0], [0, 10], [10, 10], [5, 0], [0, 5]], dtype=float + ) true_pos = np.array([3.0, 4.0]) def h_ext(x): @@ -402,9 +393,7 @@ def test_invalid_method_raises_error(self): """Test that invalid method raises ValueError.""" x0 = np.array([5.0, 5.0]) with self.assertRaises(ValueError): - solve_nonlinear_ls( - self.h, self.jacobian, self.y, x0, method="invalid" - ) + solve_nonlinear_ls(self.h, self.jacobian, self.y, x0, method="invalid") class TestInputValidation(unittest.TestCase): @@ -412,6 +401,7 @@ class TestInputValidation(unittest.TestCase): def setUp(self): """Setup simple measurement model.""" + def h(x): return np.array([x[0], x[1]]) @@ -425,45 +415,34 @@ def test_y_must_be_1d(self): """Test that 2D y raises error.""" with self.assertRaises(ValueError): gauss_newton( - self.h, self.jacobian, - y=np.array([[1], [2]]), # 2D - x0=np.array([0, 0]) + self.h, self.jacobian, y=np.array([[1], [2]]), x0=np.array([0, 0]) # 2D ) def test_x0_must_be_1d(self): """Test that 2D x0 raises error.""" with self.assertRaises(ValueError): gauss_newton( - self.h, self.jacobian, - y=np.array([1, 2]), - x0=np.array([[0], [0]]) # 2D + self.h, self.jacobian, y=np.array([1, 2]), x0=np.array([[0], [0]]) # 2D ) def test_jacobian_shape_validation(self): """Test that wrong Jacobian shape raises error.""" + def bad_jacobian(x): return np.eye(3) # Wrong shape with self.assertRaises(ValueError): - gauss_newton( - self.h, bad_jacobian, - y=np.array([1, 2]), - x0=np.array([0, 0]) - ) + gauss_newton(self.h, bad_jacobian, y=np.array([1, 2]), x0=np.array([0, 0])) def test_h_output_shape_validation(self): """Test that wrong h output shape raises error.""" + def bad_h(x): return np.array([x[0]]) # Wrong length with self.assertRaises(ValueError): - gauss_newton( - bad_h, self.jacobian, - y=np.array([1, 2]), - x0=np.array([0, 0]) - ) + gauss_newton(bad_h, self.jacobian, y=np.array([1, 2]), x0=np.array([0, 0])) if __name__ == "__main__": unittest.main() - diff --git a/tests/core/estimators/test_particle_filter.py b/tests/core/estimators/test_particle_filter.py index 67f6728..e8b1f0c 100644 --- a/tests/core/estimators/test_particle_filter.py +++ b/tests/core/estimators/test_particle_filter.py @@ -32,8 +32,12 @@ def lik(z, x): return _gauss_pdf(z - H @ x, R) pf = ParticleFilter( - pm, lik, n_particles=200, x0=np.array([0.0, 0.0]), - P0=np.eye(2), resample_threshold=0.0, # never resample + pm, + lik, + n_particles=200, + x0=np.array([0.0, 0.0]), + P0=np.eye(2), + resample_threshold=0.0, # never resample ) particles = pf.particles.copy() z = np.array([0.3]) @@ -48,8 +52,11 @@ def test_systematic_resampling_is_unbiased(): """Systematic resampling reproduces the weighted mean of the particles.""" np.random.seed(1) pf = ParticleFilter( - lambda x, u, dt: x, lambda z, x: 1.0, n_particles=50000, - x0=np.array([0.0]), P0=np.array([[1.0]]), + lambda x, u, dt: x, + lambda z, x: 1.0, + n_particles=50000, + x0=np.array([0.0]), + P0=np.array([[1.0]]), ) # Impose a skewed weight distribution, then resample. pf.particles = np.linspace(-3, 3, 50000).reshape(-1, 1) @@ -67,8 +74,11 @@ def test_systematic_resampling_is_unbiased(): def test_effective_sample_size(): """N_eff = 1/sum(w^2): N for uniform, 1 for a single dominant particle.""" pf = ParticleFilter( - lambda x, u, dt: x, lambda z, x: 1.0, n_particles=100, - x0=np.array([0.0]), P0=np.array([[1.0]]), + lambda x, u, dt: x, + lambda z, x: 1.0, + n_particles=100, + x0=np.array([0.0]), + P0=np.array([[1.0]]), ) assert np.isclose(pf._effective_sample_size(), 100.0) pf.weights = np.zeros(100) diff --git a/tests/core/estimators/test_unscented_kalman_filter.py b/tests/core/estimators/test_unscented_kalman_filter.py index 89de860..e6ef94d 100644 --- a/tests/core/estimators/test_unscented_kalman_filter.py +++ b/tests/core/estimators/test_unscented_kalman_filter.py @@ -17,9 +17,15 @@ def test_sigma_point_weights_match_eq_3_24(): n = 3 alpha, beta, kappa = 0.5, 2.0, 0.0 ukf = UnscentedKalmanFilter( - lambda x, u, dt: x, lambda x: x, - lambda dt: np.zeros((n, n)), lambda: np.zeros((n, n)), - np.zeros(n), np.eye(n), alpha=alpha, beta=beta, kappa=kappa, + lambda x, u, dt: x, + lambda x: x, + lambda dt: np.zeros((n, n)), + lambda: np.zeros((n, n)), + np.zeros(n), + np.eye(n), + alpha=alpha, + beta=beta, + kappa=kappa, ) lam = alpha**2 * (n + kappa) - n assert np.isclose(ukf.Wm[0], lam / (n + lam)) @@ -47,9 +53,15 @@ def test_ukf_matches_kf_on_linear_system(): kf = KalmanFilter(F, Q, H, R, x0.copy(), P0.copy()) ukf = UnscentedKalmanFilter( - lambda x, u, dt: F @ x, lambda x: H @ x, - lambda dt: Q, lambda: R, x0.copy(), P0.copy(), - alpha=1.0, beta=2.0, kappa=0.0, + lambda x, u, dt: F @ x, + lambda x: H @ x, + lambda dt: Q, + lambda: R, + x0.copy(), + P0.copy(), + alpha=1.0, + beta=2.0, + kappa=0.0, ) for z_val in [0.7, 1.9, 2.5, 3.1]: z = np.array([z_val]) diff --git a/tests/core/eval/__init__.py b/tests/core/eval/__init__.py index a814e73..08282d5 100644 --- a/tests/core/eval/__init__.py +++ b/tests/core/eval/__init__.py @@ -1,3 +1 @@ # Tests for core.fusion module (gating, tuning, types) - - diff --git a/tests/core/eval/test_plots.py b/tests/core/eval/test_plots.py index 4da3d01..6318c8e 100644 --- a/tests/core/eval/test_plots.py +++ b/tests/core/eval/test_plots.py @@ -130,9 +130,7 @@ def test_scale_and_origin_are_applied(self, ax3d): origin = np.array([1.0, -2.0, 0.5]) plot_frame_3d(ax3d, np.eye(3), origin=origin, scale=2.0) - np.testing.assert_allclose( - _drawn_axes(ax3d), 2.0 * np.eye(3), atol=1e-12 - ) + np.testing.assert_allclose(_drawn_axes(ax3d), 2.0 * np.eye(3), atol=1e-12) for line in ax3d.lines: xs, ys, zs = line.get_data_3d() np.testing.assert_allclose([xs[0], ys[0], zs[0]], origin, atol=1e-12) @@ -219,9 +217,9 @@ def test_output_is_byte_reproducible(self, tmp_path): plt.close(fig) for lhs, rhs in zip(first, second): - assert lhs.read_bytes() == rhs.read_bytes(), ( - f"{lhs.suffix} output is not reproducible" - ) + assert ( + lhs.read_bytes() == rhs.read_bytes() + ), f"{lhs.suffix} output is not reproducible" class TestPlotTrajectory2D: @@ -237,9 +235,7 @@ def test_axis_labels_are_configurable(self): truth = np.array([[0.0, 0.0], [1.0, 1.0], [2.0, 0.0]]) est = {"method": truth + 0.1} - fig = plot_trajectory_2d( - truth, est, axis_labels=("East [m]", "North [m]") - ) + fig = plot_trajectory_2d(truth, est, axis_labels=("East [m]", "North [m]")) try: axes = fig.axes[0] assert axes.get_xlabel() == "East [m]" @@ -333,9 +329,7 @@ def test_supplied_axes_and_zoom_are_mutually_exclusive(self): try: with pytest.raises(ValueError, match="zoom_to_truth"): - plot_trajectory_2d( - truth, {"m": truth}, ax=host_ax, zoom_to_truth=True - ) + plot_trajectory_2d(truth, {"m": truth}, ax=host_ax, zoom_to_truth=True) finally: plt.close(host) @@ -494,9 +488,7 @@ def test_zoom_panels_keep_equal_aspect_by_default(self): """The zoom path's existing behaviour is untouched.""" truth = np.array([[0.0, 0.0], [10.0, 0.0], [10.0, 5.0]]) - fig = plot_trajectory_2d( - truth, {"drift": truth * 50.0}, zoom_to_truth=True - ) + fig = plot_trajectory_2d(truth, {"drift": truth * 50.0}, zoom_to_truth=True) try: assert fig.axes[0].get_aspect() == 1.0 assert fig.axes[1].get_aspect() == 1.0 @@ -517,7 +509,9 @@ def test_accepts_vector_and_scalar_errors(self): """(N, 2) vectors are normed; (N,) magnitudes are taken as given.""" vector = np.array([[3.0, 4.0], [6.0, 8.0]]) - fig = plot_error_magnitude_time({"vec": vector, "scalar": np.array([5.0, 10.0])}) + fig = plot_error_magnitude_time( + {"vec": vector, "scalar": np.array([5.0, 10.0])} + ) try: lines = fig.axes[0].lines assert len(lines) == 2 @@ -625,6 +619,7 @@ def test_title_weight_is_selectable(self): finally: plt.close(fig) + class TestFigureOutputRedirect: """Test the IPIN_FIGS_DIR escape hatch that keeps pytest out of the repo. diff --git a/tests/core/fingerprinting/__init__.py b/tests/core/fingerprinting/__init__.py index 572e075..55eb103 100644 --- a/tests/core/fingerprinting/__init__.py +++ b/tests/core/fingerprinting/__init__.py @@ -1,3 +1 @@ """Unit tests for core.fingerprinting module (Chapter 5).""" - - diff --git a/tests/core/fingerprinting/test_classification.py b/tests/core/fingerprinting/test_classification.py index 04e9d5a..6c6e600 100644 --- a/tests/core/fingerprinting/test_classification.py +++ b/tests/core/fingerprinting/test_classification.py @@ -38,27 +38,27 @@ def test_fit_random_forest_rp_based(self): """Test fitting Random Forest with RP-based classes.""" # Create simple database locations = np.array([[0, 0], [10, 0], [10, 10], [0, 10]], dtype=float) - features = np.array([ - [-50, -60, -70], - [-60, -50, -80], - [-70, -80, -50], - [-80, -70, -60], - ], dtype=float) + features = np.array( + [ + [-50, -60, -70], + [-60, -50, -80], + [-70, -80, -50], + [-80, -70, -60], + ], + dtype=float, + ) floor_ids = np.array([0, 0, 0, 0]) db = FingerprintDatabase( locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"} + meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"}, ) # Fit classifier classifier = fit_classifier( - db, - classifier_type="random_forest", - zone_type="rp", - n_estimators=50 + db, classifier_type="random_forest", zone_type="rp", n_estimators=50 ) assert classifier is not None @@ -68,28 +68,27 @@ def test_fit_random_forest_rp_based(self): def test_fit_svm_rp_based(self): """Test fitting SVM with RP-based classes.""" locations = np.array([[0, 0], [10, 0], [10, 10], [0, 10]], dtype=float) - features = np.array([ - [-50, -60, -70], - [-60, -50, -80], - [-70, -80, -50], - [-80, -70, -60], - ], dtype=float) + features = np.array( + [ + [-50, -60, -70], + [-60, -50, -80], + [-70, -80, -50], + [-80, -70, -60], + ], + dtype=float, + ) floor_ids = np.array([0, 0, 0, 0]) db = FingerprintDatabase( locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"} + meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"}, ) # Fit SVM classifier classifier = fit_classifier( - db, - classifier_type="svm", - zone_type="rp", - C=1.0, - kernel="rbf" + db, classifier_type="svm", zone_type="rp", C=1.0, kernel="rbf" ) assert classifier is not None @@ -99,27 +98,27 @@ def test_fit_with_floor_constraint(self): """Test fitting classifier on single floor only.""" # Multi-floor database locations = np.array([[0, 0], [10, 0], [0, 5], [10, 5]], dtype=float) - features = np.array([ - [-50, -60, -70], - [-60, -50, -80], - [-55, -65, -75], - [-65, -55, -85], - ], dtype=float) + features = np.array( + [ + [-50, -60, -70], + [-60, -50, -80], + [-55, -65, -75], + [-65, -55, -85], + ], + dtype=float, + ) floor_ids = np.array([0, 0, 1, 1]) db = FingerprintDatabase( locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"} + meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"}, ) # Fit on floor 0 only classifier = fit_classifier( - db, - classifier_type="random_forest", - zone_type="rp", - floor_id=0 + db, classifier_type="random_forest", zone_type="rp", floor_id=0 ) # Should only have 2 classes (floor 0 RPs) @@ -132,10 +131,7 @@ def test_fit_invalid_zone_type(self): floor_ids = np.array([0, 0]) db = FingerprintDatabase( - locations=locations, - features=features, - floor_ids=floor_ids, - meta={} + locations=locations, features=features, floor_ids=floor_ids, meta={} ) with pytest.raises((ValueError, NotImplementedError)): @@ -148,10 +144,7 @@ def test_fit_invalid_classifier_type(self): floor_ids = np.array([0, 0]) db = FingerprintDatabase( - locations=locations, - features=features, - floor_ids=floor_ids, - meta={} + locations=locations, features=features, floor_ids=floor_ids, meta={} ) with pytest.raises(ValueError, match="Unknown classifier_type"): @@ -164,18 +157,21 @@ class TestClassificationLocalizer: def test_predict_exact_match(self): """Test prediction with exact feature match.""" locations = np.array([[0, 0], [10, 0], [10, 10]], dtype=float) - features = np.array([ - [-50, -60, -70], - [-60, -50, -80], - [-70, -80, -50], - ], dtype=float) + features = np.array( + [ + [-50, -60, -70], + [-60, -50, -80], + [-70, -80, -50], + ], + dtype=float, + ) floor_ids = np.array([0, 0, 0]) db = FingerprintDatabase( locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"} + meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"}, ) classifier = fit_classifier(db, classifier_type="random_forest", zone_type="rp") @@ -191,18 +187,21 @@ def test_predict_exact_match(self): def test_predict_approximate_match(self): """Test prediction with approximate feature match.""" locations = np.array([[0, 0], [10, 0], [10, 10]], dtype=float) - features = np.array([ - [-50, -60, -70], - [-60, -50, -80], - [-70, -80, -50], - ], dtype=float) + features = np.array( + [ + [-50, -60, -70], + [-60, -50, -80], + [-70, -80, -50], + ], + dtype=float, + ) floor_ids = np.array([0, 0, 0]) db = FingerprintDatabase( locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"} + meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"}, ) classifier = fit_classifier(db, classifier_type="random_forest", zone_type="rp") @@ -217,18 +216,21 @@ def test_predict_approximate_match(self): def test_predict_with_probabilities(self): """Test prediction with class probabilities.""" locations = np.array([[0, 0], [10, 0], [10, 10]], dtype=float) - features = np.array([ - [-50, -60, -70], - [-60, -50, -80], - [-70, -80, -50], - ], dtype=float) + features = np.array( + [ + [-50, -60, -70], + [-60, -50, -80], + [-70, -80, -50], + ], + dtype=float, + ) floor_ids = np.array([0, 0, 0]) db = FingerprintDatabase( locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"} + meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"}, ) classifier = fit_classifier(db, classifier_type="random_forest", zone_type="rp") @@ -242,18 +244,21 @@ def test_predict_with_probabilities(self): def test_predict_with_missing_values(self): """Test prediction with missing values (NaN) in query.""" locations = np.array([[0, 0], [10, 0], [10, 10]], dtype=float) - features = np.array([ - [-50, -60, -70], - [-60, -50, -80], - [-70, -80, -50], - ], dtype=float) + features = np.array( + [ + [-50, -60, -70], + [-60, -50, -80], + [-70, -80, -50], + ], + dtype=float, + ) floor_ids = np.array([0, 0, 0]) db = FingerprintDatabase( locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"} + meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"}, ) classifier = fit_classifier(db, classifier_type="random_forest", zone_type="rp") @@ -278,38 +283,46 @@ class TestHierarchicalLocalize: def test_hierarchical_floor_then_knn(self): """Test hierarchical: floor classification, then k-NN.""" # Multi-floor database - locations = np.array([ - [0, 0], [10, 0], [10, 10], [0, 10], # Floor 0 - [0, 0], [10, 0], [10, 10], [0, 10], # Floor 1 - ], dtype=float) - features = np.array([ - [-50, -60, -70], # Floor 0 - [-60, -50, -80], - [-70, -80, -50], - [-80, -70, -60], - [-55, -65, -75], # Floor 1 (different RSS) - [-65, -55, -85], - [-75, -85, -55], - [-85, -75, -65], - ], dtype=float) + locations = np.array( + [ + [0, 0], + [10, 0], + [10, 10], + [0, 10], # Floor 0 + [0, 0], + [10, 0], + [10, 10], + [0, 10], # Floor 1 + ], + dtype=float, + ) + features = np.array( + [ + [-50, -60, -70], # Floor 0 + [-60, -50, -80], + [-70, -80, -50], + [-80, -70, -60], + [-55, -65, -75], # Floor 1 (different RSS) + [-65, -55, -85], + [-75, -85, -55], + [-85, -75, -65], + ], + dtype=float, + ) floor_ids = np.array([0, 0, 0, 0, 1, 1, 1, 1]) db = FingerprintDatabase( locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"} + meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"}, ) # Query on floor 1 query = np.array([-60, -70, -80]) pos, info = hierarchical_localize( - query, - db, - coarse_method="floor", - fine_method="knn", - k=3 + query, db, coarse_method="floor", fine_method="knn", k=3 ) assert pos.shape == (2,) @@ -320,28 +333,28 @@ def test_hierarchical_floor_then_knn(self): def test_hierarchical_single_floor(self): """Test hierarchical on single-floor database (should skip coarse).""" locations = np.array([[0, 0], [10, 0], [10, 10], [0, 10]], dtype=float) - features = np.array([ - [-50, -60, -70], - [-60, -50, -80], - [-70, -80, -50], - [-80, -70, -60], - ], dtype=float) + features = np.array( + [ + [-50, -60, -70], + [-60, -50, -80], + [-70, -80, -50], + [-80, -70, -60], + ], + dtype=float, + ) floor_ids = np.array([0, 0, 0, 0]) db = FingerprintDatabase( locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"} + meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"}, ) query = np.array([-55, -65, -75]) pos, info = hierarchical_localize( - query, - db, - coarse_method="floor", - fine_method="nn" + query, db, coarse_method="floor", fine_method="nn" ) assert pos.shape == (2,) @@ -350,35 +363,42 @@ def test_hierarchical_single_floor(self): def test_hierarchical_with_probabilistic_fine(self): """Test hierarchical with probabilistic fine localization.""" # Multi-floor database - locations = np.array([ - [0, 0], [10, 0], [10, 10], # Floor 0 - [0, 0], [10, 0], [10, 10], # Floor 1 - ], dtype=float) - features = np.array([ - [-50, -60, -70], - [-60, -50, -80], - [-70, -80, -50], - [-55, -65, -75], - [-65, -55, -85], - [-75, -85, -55], - ], dtype=float) + locations = np.array( + [ + [0, 0], + [10, 0], + [10, 10], # Floor 0 + [0, 0], + [10, 0], + [10, 10], # Floor 1 + ], + dtype=float, + ) + features = np.array( + [ + [-50, -60, -70], + [-60, -50, -80], + [-70, -80, -50], + [-55, -65, -75], + [-65, -55, -85], + [-75, -85, -55], + ], + dtype=float, + ) floor_ids = np.array([0, 0, 0, 1, 1, 1]) db = FingerprintDatabase( locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"} + meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"}, ) query = np.array([-58, -68, -78]) # Test MAP fine method pos_map, info_map = hierarchical_localize( - query, - db, - coarse_method="floor", - fine_method="map" + query, db, coarse_method="floor", fine_method="map" ) assert pos_map.shape == (2,) @@ -386,10 +406,7 @@ def test_hierarchical_with_probabilistic_fine(self): # Test posterior mean fine method pos_mean, info_mean = hierarchical_localize( - query, - db, - coarse_method="floor", - fine_method="posterior_mean" + query, db, coarse_method="floor", fine_method="posterior_mean" ) assert pos_mean.shape == (2,) @@ -397,35 +414,41 @@ def test_hierarchical_with_probabilistic_fine(self): def test_hierarchical_random_forest_coarse(self): """Test hierarchical with Random Forest coarse classification.""" # Multi-floor database - locations = np.array([ - [0, 0], [10, 0], [10, 10], # Floor 0 - [0, 0], [10, 0], [10, 10], # Floor 1 - ], dtype=float) - features = np.array([ - [-50, -60, -70], - [-60, -50, -80], - [-70, -80, -50], - [-55, -65, -75], - [-65, -55, -85], - [-75, -85, -55], - ], dtype=float) + locations = np.array( + [ + [0, 0], + [10, 0], + [10, 10], # Floor 0 + [0, 0], + [10, 0], + [10, 10], # Floor 1 + ], + dtype=float, + ) + features = np.array( + [ + [-50, -60, -70], + [-60, -50, -80], + [-70, -80, -50], + [-55, -65, -75], + [-65, -55, -85], + [-75, -85, -55], + ], + dtype=float, + ) floor_ids = np.array([0, 0, 0, 1, 1, 1]) db = FingerprintDatabase( locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"} + meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"}, ) query = np.array([-58, -68, -78]) pos, info = hierarchical_localize( - query, - db, - coarse_method="random_forest", - fine_method="knn", - k=3 + query, db, coarse_method="random_forest", fine_method="knn", k=3 ) assert pos.shape == (2,) @@ -439,10 +462,7 @@ def test_hierarchical_invalid_coarse_method(self): floor_ids = np.array([0, 0]) db = FingerprintDatabase( - locations=locations, - features=features, - floor_ids=floor_ids, - meta={} + locations=locations, features=features, floor_ids=floor_ids, meta={} ) query = np.array([-55, -65]) @@ -457,10 +477,7 @@ def test_hierarchical_invalid_fine_method(self): floor_ids = np.array([0, 0]) db = FingerprintDatabase( - locations=locations, - features=features, - floor_ids=floor_ids, - meta={} + locations=locations, features=features, floor_ids=floor_ids, meta={} ) query = np.array([-55, -65]) @@ -474,18 +491,28 @@ class TestFitFloorClassifier: @staticmethod def _make_multi_floor_db(): - locations = np.array([ - [0, 0], [10, 0], [10, 10], - [0, 0], [10, 0], [10, 10], - ], dtype=float) - features = np.array([ - [-50, -60, -70], - [-60, -50, -80], - [-70, -80, -50], - [-55, -65, -75], - [-65, -55, -85], - [-75, -85, -55], - ], dtype=float) + locations = np.array( + [ + [0, 0], + [10, 0], + [10, 10], + [0, 0], + [10, 0], + [10, 10], + ], + dtype=float, + ) + features = np.array( + [ + [-50, -60, -70], + [-60, -50, -80], + [-70, -80, -50], + [-55, -65, -75], + [-65, -55, -85], + [-75, -85, -55], + ], + dtype=float, + ) floor_ids = np.array([0, 0, 0, 1, 1, 1]) return FingerprintDatabase( locations=locations, @@ -520,10 +547,12 @@ def test_hierarchical_with_pretrained_coarse_model(self): query = np.array([-58, -68, -78]) pos, info = hierarchical_localize( - query, db, + query, + db, coarse_method="random_forest", coarse_model=clf, - fine_method="knn", k=3, + fine_method="knn", + k=3, ) assert pos.shape == (2,) assert info["coarse_method"] == "random_forest" @@ -534,9 +563,11 @@ def test_hierarchical_without_coarse_model_still_works(self): db = self._make_multi_floor_db() query = np.array([-58, -68, -78]) pos, info = hierarchical_localize( - query, db, + query, + db, coarse_method="random_forest", - fine_method="knn", k=3, + fine_method="knn", + k=3, ) assert pos.shape == (2,) assert "coarse_floor" in info @@ -559,15 +590,12 @@ def test_classification_vs_knn_consistency(self): locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": [f"AP{i}" for i in range(5)], "unit": "dBm"} + meta={"ap_ids": [f"AP{i}" for i in range(5)], "unit": "dBm"}, ) # Fit classifier classifier = fit_classifier( - db, - classifier_type="random_forest", - zone_type="rp", - n_estimators=100 + db, classifier_type="random_forest", zone_type="rp", n_estimators=100 ) # Test queries @@ -612,23 +640,17 @@ def test_hierarchical_improves_efficiency(self): locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": ["AP1", "AP2", "AP3", "AP4"], "unit": "dBm"} + meta={"ap_ids": ["AP1", "AP2", "AP3", "AP4"], "unit": "dBm"}, ) # Query on floor 1 query = -50 - 1 * 10 - np.random.rand(4) * 20 pos, info = hierarchical_localize( - query, - db, - coarse_method="floor", - fine_method="knn", - k=5 + query, db, coarse_method="floor", fine_method="knn", k=5 ) # Should correctly identify floor 1 # (coarse step reduces search space from 75 to 25 RPs) assert pos.shape == (2,) assert 0 <= info["coarse_floor"] < 3 - - diff --git a/tests/core/fingerprinting/test_dataset.py b/tests/core/fingerprinting/test_dataset.py index 2a10b8f..ebee86b 100644 --- a/tests/core/fingerprinting/test_dataset.py +++ b/tests/core/fingerprinting/test_dataset.py @@ -299,5 +299,3 @@ def test_print_summary_multifloor_breakdown(self, sample_database, capsys): if __name__ == "__main__": pytest.main([__file__, "-v"]) - - diff --git a/tests/core/fingerprinting/test_deterministic.py b/tests/core/fingerprinting/test_deterministic.py index c909d76..e9f8df2 100644 --- a/tests/core/fingerprinting/test_deterministic.py +++ b/tests/core/fingerprinting/test_deterministic.py @@ -393,5 +393,3 @@ def test_increasing_k_smoothing_effect(self, multifloor_database): if __name__ == "__main__": pytest.main([__file__, "-v"]) - - diff --git a/tests/core/fingerprinting/test_missing_aps.py b/tests/core/fingerprinting/test_missing_aps.py index 3680f88..05b4033 100644 --- a/tests/core/fingerprinting/test_missing_aps.py +++ b/tests/core/fingerprinting/test_missing_aps.py @@ -112,11 +112,13 @@ class TestPairwiseDistancesWithMissingAPs: def test_pairwise_no_missing(self): """Test normal pairwise distances without missing values.""" z = np.array([-50.0, -60.0, -70.0]) - F = np.array([ - [-52.0, -58.0, -72.0], - [-48.0, -62.0, -68.0], - [-55.0, -55.0, -75.0], - ]) + F = np.array( + [ + [-52.0, -58.0, -72.0], + [-48.0, -62.0, -68.0], + [-55.0, -55.0, -75.0], + ] + ) distances = pairwise_distances(z, F, metric="euclidean") @@ -131,11 +133,13 @@ def test_pairwise_no_missing(self): def test_pairwise_query_missing(self): """Test pairwise distances with missing values in query.""" z = np.array([-50.0, np.nan, -70.0]) - F = np.array([ - [-52.0, -58.0, -72.0], # AP1, AP3 valid - [-48.0, -62.0, -68.0], # AP1, AP3 valid - [-55.0, -55.0, np.nan], # Only AP1 valid - ]) + F = np.array( + [ + [-52.0, -58.0, -72.0], # AP1, AP3 valid + [-48.0, -62.0, -68.0], # AP1, AP3 valid + [-55.0, -55.0, np.nan], # Only AP1 valid + ] + ) distances = pairwise_distances(z, F, metric="euclidean") @@ -150,11 +154,13 @@ def test_pairwise_query_missing(self): def test_pairwise_reference_missing(self): """Test pairwise distances with missing values in references.""" z = np.array([-50.0, -60.0, -70.0]) - F = np.array([ - [-52.0, np.nan, -72.0], # AP2 missing - [np.nan, -62.0, -68.0], # AP1 missing - [np.nan, np.nan, np.nan], # All missing -> inf - ]) + F = np.array( + [ + [-52.0, np.nan, -72.0], # AP2 missing + [np.nan, -62.0, -68.0], # AP1 missing + [np.nan, np.nan, np.nan], # All missing -> inf + ] + ) distances = pairwise_distances(z, F, metric="euclidean") @@ -169,10 +175,12 @@ def test_pairwise_reference_missing(self): def test_pairwise_manhattan_with_missing(self): """Test pairwise Manhattan distance with missing values.""" z = np.array([-50.0, np.nan, -70.0]) - F = np.array([ - [-52.0, -58.0, -72.0], - [-48.0, -62.0, -68.0], - ]) + F = np.array( + [ + [-52.0, -58.0, -72.0], + [-48.0, -62.0, -68.0], + ] + ) distances = pairwise_distances(z, F, metric="manhattan") @@ -189,19 +197,22 @@ def test_nn_with_missing_query(self): """Test NN localization when query has missing APs.""" # Create database with 4 RPs locations = np.array([[0, 0], [10, 0], [10, 10], [0, 10]], dtype=float) - features = np.array([ - [-50, -60, -70], - [-60, -50, -80], - [-70, -80, -50], - [-80, -70, -60], - ], dtype=float) + features = np.array( + [ + [-50, -60, -70], + [-60, -50, -80], + [-70, -80, -50], + [-80, -70, -60], + ], + dtype=float, + ) floor_ids = np.array([0, 0, 0, 0]) db = FingerprintDatabase( locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"} + meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"}, ) # Query with missing AP2 @@ -217,18 +228,21 @@ def test_nn_with_missing_query(self): def test_nn_with_missing_database(self): """Test NN when some RPs have missing APs.""" locations = np.array([[0, 0], [10, 0], [10, 10]], dtype=float) - features = np.array([ - [-50, -60, -70], - [-60, np.nan, -80], # RP1 missing AP2 - [np.nan, -80, -50], # RP2 missing AP1 - ], dtype=float) + features = np.array( + [ + [-50, -60, -70], + [-60, np.nan, -80], # RP1 missing AP2 + [np.nan, -80, -50], # RP2 missing AP1 + ], + dtype=float, + ) floor_ids = np.array([0, 0, 0]) db = FingerprintDatabase( locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"} + meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"}, ) query = np.array([-55.0, -65.0, -75.0]) @@ -244,19 +258,22 @@ class TestKNNLocalizationWithMissingAPs: def test_knn_with_missing_query(self): """Test k-NN localization when query has missing APs.""" locations = np.array([[0, 0], [10, 0], [10, 10], [0, 10]], dtype=float) - features = np.array([ - [-50, -60, -70], - [-60, -50, -80], - [-70, -80, -50], - [-80, -70, -60], - ], dtype=float) + features = np.array( + [ + [-50, -60, -70], + [-60, -50, -80], + [-70, -80, -50], + [-80, -70, -60], + ], + dtype=float, + ) floor_ids = np.array([0, 0, 0, 0]) db = FingerprintDatabase( locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"} + meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"}, ) # Query with missing AP2 @@ -284,7 +301,7 @@ def test_knn_with_high_dropout_rate(self): locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": [f"AP{i}" for i in range(n_aps)], "unit": "dBm"} + meta={"ap_ids": [f"AP{i}" for i in range(n_aps)], "unit": "dBm"}, ) # Generate query with 20% dropout @@ -314,7 +331,7 @@ def test_log_likelihood_no_missing(self): locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": ["AP1", "AP2"], "unit": "dBm"} + meta={"ap_ids": ["AP1", "AP2"], "unit": "dBm"}, ) model = fit_gaussian_naive_bayes(db, min_std=2.0) @@ -335,7 +352,7 @@ def test_log_likelihood_partial_missing(self): locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"} + meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"}, ) model = fit_gaussian_naive_bayes(db, min_std=2.0) @@ -358,7 +375,7 @@ def test_log_likelihood_all_missing(self): locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": ["AP1", "AP2"], "unit": "dBm"} + meta={"ap_ids": ["AP1", "AP2"], "unit": "dBm"}, ) model = fit_gaussian_naive_bayes(db, min_std=2.0) @@ -387,7 +404,7 @@ def test_log_likelihood_high_dropout_rate(self): locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": [f"AP{i}" for i in range(n_aps)], "unit": "dBm"} + meta={"ap_ids": [f"AP{i}" for i in range(n_aps)], "unit": "dBm"}, ) model = fit_gaussian_naive_bayes(db, min_std=2.0) @@ -411,19 +428,22 @@ class TestMAPLocalizationWithMissingAPs: def test_map_with_missing_query(self): """Test MAP localization when query has missing APs.""" locations = np.array([[0, 0], [10, 0], [10, 10], [0, 10]], dtype=float) - features = np.array([ - [-50, -60, -70], - [-60, -50, -80], - [-70, -80, -50], - [-80, -70, -60], - ], dtype=float) + features = np.array( + [ + [-50, -60, -70], + [-60, -50, -80], + [-70, -80, -50], + [-80, -70, -60], + ], + dtype=float, + ) floor_ids = np.array([0, 0, 0, 0]) db = FingerprintDatabase( locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"} + meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"}, ) model = fit_gaussian_naive_bayes(db, min_std=2.0) @@ -444,19 +464,22 @@ class TestPosteriorMeanLocalizationWithMissingAPs: def test_posterior_mean_with_missing_query(self): """Test posterior mean when query has missing APs.""" locations = np.array([[0, 0], [10, 0], [10, 10], [0, 10]], dtype=float) - features = np.array([ - [-50, -60, -70], - [-60, -50, -80], - [-70, -80, -50], - [-80, -70, -60], - ], dtype=float) + features = np.array( + [ + [-50, -60, -70], + [-60, -50, -80], + [-70, -80, -50], + [-80, -70, -60], + ], + dtype=float, + ) floor_ids = np.array([0, 0, 0, 0]) db = FingerprintDatabase( locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"} + meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"}, ) model = fit_gaussian_naive_bayes(db, min_std=2.0) @@ -485,7 +508,7 @@ def test_posterior_mean_with_top_k_and_missing(self): locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": [f"AP{i}" for i in range(n_aps)], "unit": "dBm"} + meta={"ap_ids": [f"AP{i}" for i in range(n_aps)], "unit": "dBm"}, ) model = fit_gaussian_naive_bayes(db, min_std=2.0) @@ -509,10 +532,20 @@ class TestDatabaseWithMissingAPsInSamples: def test_database_creation_with_nan(self): """Test that database allows NaN in features.""" locations = np.array([[0, 0], [10, 0]], dtype=float) - features = np.array([ - [[-50, -60], [-51, np.nan], [-49, -61]], # RP0: 3 samples, sample 2 missing AP2 - [[-60, -50], [np.nan, -49], [-59, -51]], # RP1: 3 samples, sample 2 missing AP1 - ]) # Shape: (2, 3, 2) + features = np.array( + [ + [ + [-50, -60], + [-51, np.nan], + [-49, -61], + ], # RP0: 3 samples, sample 2 missing AP2 + [ + [-60, -50], + [np.nan, -49], + [-59, -51], + ], # RP1: 3 samples, sample 2 missing AP1 + ] + ) # Shape: (2, 3, 2) floor_ids = np.array([0, 0]) # Should not raise error @@ -520,7 +553,7 @@ def test_database_creation_with_nan(self): locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": ["AP1", "AP2"], "unit": "dBm"} + meta={"ap_ids": ["AP1", "AP2"], "unit": "dBm"}, ) assert db.n_reference_points == 2 @@ -530,16 +563,18 @@ def test_database_creation_with_nan(self): def test_mean_std_computation_with_nan(self): """Test that mean/std computation handles NaN properly.""" locations = np.array([[0, 0]], dtype=float) - features = np.array([ - [[-50, -60], [-52, np.nan], [-48, -62], [np.nan, -58], [-51, -59]], - ]) # Shape: (1, 5, 2) + features = np.array( + [ + [[-50, -60], [-52, np.nan], [-48, -62], [np.nan, -58], [-51, -59]], + ] + ) # Shape: (1, 5, 2) floor_ids = np.array([0]) db = FingerprintDatabase( locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": ["AP1", "AP2"], "unit": "dBm"} + meta={"ap_ids": ["AP1", "AP2"], "unit": "dBm"}, ) # Compute mean (should use nanmean) @@ -560,17 +595,19 @@ def test_mean_std_computation_with_nan(self): def test_fit_model_with_nan_samples(self): """Test fitting Gaussian Naive Bayes with NaN in samples.""" locations = np.array([[0, 0], [10, 0]], dtype=float) - features = np.array([ - [[-50, -60], [-51, np.nan], [-49, -61]], - [[-60, -50], [np.nan, -49], [-59, -51]], - ]) + features = np.array( + [ + [[-50, -60], [-51, np.nan], [-49, -61]], + [[-60, -50], [np.nan, -49], [-59, -51]], + ] + ) floor_ids = np.array([0, 0]) db = FingerprintDatabase( locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": ["AP1", "AP2"], "unit": "dBm"} + meta={"ap_ids": ["AP1", "AP2"], "unit": "dBm"}, ) # Should fit successfully @@ -602,7 +639,7 @@ def test_acceptance_20_percent_dropout_no_crash(self): locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": [f"AP{i}" for i in range(n_aps)], "unit": "dBm"} + meta={"ap_ids": [f"AP{i}" for i in range(n_aps)], "unit": "dBm"}, ) # Fit probabilistic model @@ -615,7 +652,9 @@ def test_acceptance_20_percent_dropout_no_crash(self): query = -50 - np.random.rand(n_aps) * 40 # Apply 20% dropout - dropout_indices = np.random.choice(n_aps, size=int(0.2 * n_aps), replace=False) + dropout_indices = np.random.choice( + n_aps, size=int(0.2 * n_aps), replace=False + ) query[dropout_indices] = np.nan # Test deterministic methods @@ -649,7 +688,7 @@ def test_acceptance_varying_dropout_rates(self): locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": [f"AP{i}" for i in range(n_aps)], "unit": "dBm"} + meta={"ap_ids": [f"AP{i}" for i in range(n_aps)], "unit": "dBm"}, ) model = fit_gaussian_naive_bayes(db, min_std=2.0) @@ -674,4 +713,3 @@ def test_acceptance_varying_dropout_rates(self): for pos in [pos_nn, pos_knn, pos_map, pos_mean]: assert not np.any(np.isnan(pos)) assert not np.any(np.isinf(pos)) - diff --git a/tests/core/fingerprinting/test_multisamples.py b/tests/core/fingerprinting/test_multisamples.py index 8536551..a482308 100644 --- a/tests/core/fingerprinting/test_multisamples.py +++ b/tests/core/fingerprinting/test_multisamples.py @@ -28,25 +28,28 @@ def test_single_sample_db(): """Test backward compatibility with single-sample DB.""" - print("\n" + "="*70) + print("\n" + "=" * 70) print("TEST 1: Single-Sample Database (Backward Compatibility)") - print("="*70) + print("=" * 70) # Create simple single-sample DB locations = np.array([[0, 0], [10, 0], [10, 10], [0, 10]], dtype=float) - features = np.array([ - [-50, -60, -70], - [-60, -50, -80], - [-70, -80, -50], - [-80, -70, -60], - ], dtype=float) + features = np.array( + [ + [-50, -60, -70], + [-60, -50, -80], + [-70, -80, -50], + [-80, -70, -60], + ], + dtype=float, + ) floor_ids = np.array([0, 0, 0, 0]) db = FingerprintDatabase( locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"} + meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"}, ) print(f"[OK] Created single-sample DB: {db}") @@ -77,9 +80,9 @@ def test_single_sample_db(): def test_multi_sample_db(): """Test multi-sample DB with proper mu and sigma estimation.""" - print("\n" + "="*70) + print("\n" + "=" * 70) print("TEST 2: Multi-Sample Database (mu and sigma Estimation)") - print("="*70) + print("=" * 70) # Create multi-sample DB # 3 RPs, 5 samples each, 2 APs @@ -107,7 +110,7 @@ def test_multi_sample_db(): locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": ["AP1", "AP2"], "unit": "dBm", "n_samples_per_rp": 5} + meta={"ap_ids": ["AP1", "AP2"], "unit": "dBm", "n_samples_per_rp": 5}, ) print(f"[OK] Created multi-sample DB: {db}") @@ -158,9 +161,9 @@ def test_multi_sample_db(): def test_behavior_with_varying_sigma(): """Demonstrate that localization behavior changes with varying sigma.""" - print("\n" + "="*70) + print("\n" + "=" * 70) print("TEST 3: Localization Behavior with Varying sigma") - print("="*70) + print("=" * 70) # Create two identical DBs but with different variances locations = np.array([[0, 0], [10, 0]], dtype=float) @@ -183,14 +186,14 @@ def test_behavior_with_varying_sigma(): locations=locations, features=features1, floor_ids=floor_ids, - meta={"ap_ids": ["AP1", "AP2"], "unit": "dBm"} + meta={"ap_ids": ["AP1", "AP2"], "unit": "dBm"}, ) db2 = FingerprintDatabase( locations=locations, features=features2, floor_ids=floor_ids, - meta={"ap_ids": ["AP1", "AP2"], "unit": "dBm"} + meta={"ap_ids": ["AP1", "AP2"], "unit": "dBm"}, ) model1 = fit_gaussian_naive_bayes(db1, min_std=0.5) @@ -240,9 +243,9 @@ def test_behavior_with_varying_sigma(): def main(): """Run all tests.""" - print("\n" + "="*70) + print("\n" + "=" * 70) print("MULTI-SAMPLE FINGERPRINTING VALIDATION") - print("="*70) + print("=" * 70) print("\nThis script validates the implementation of Option A:") print(" - Extended database format to support multiple samples per RP") print(" - Proper mu and sigma estimation from survey samples (Eq. 5.6)") @@ -253,9 +256,9 @@ def main(): test_multi_sample_db() test_behavior_with_varying_sigma() - print("\n" + "="*70) + print("\n" + "=" * 70) print("ALL TESTS PASSED OK") - print("="*70) + print("=" * 70) print("\nKey Findings:") print(" 1. Single-sample DBs work as before (backward compatible)") print(" 2. Multi-sample DBs compute actual mu and sigma from samples") @@ -269,6 +272,7 @@ def main(): except Exception as e: print(f"\nX TEST FAILED: {e}") import traceback + traceback.print_exc() return False @@ -278,5 +282,3 @@ def main(): if __name__ == "__main__": success = main() sys.exit(0 if success else 1) - - diff --git a/tests/core/fingerprinting/test_pattern_recognition.py b/tests/core/fingerprinting/test_pattern_recognition.py index 14c3ca1..8399f3f 100644 --- a/tests/core/fingerprinting/test_pattern_recognition.py +++ b/tests/core/fingerprinting/test_pattern_recognition.py @@ -22,7 +22,9 @@ def simple_database(): # x ≈ 0.1 * z1, y ≈ 0.1 * z2 # z3 has independent variation return FingerprintDatabase( - locations=np.array([[0.0, 0.0], [1.0, 2.0], [2.0, 4.0], [3.0, 6.0], [4.0, 8.0]]), + locations=np.array( + [[0.0, 0.0], [1.0, 2.0], [2.0, 4.0], [3.0, 6.0], [4.0, 8.0]] + ), features=np.array( [ [0, 0, -70], @@ -433,4 +435,3 @@ def test_regularization_prevents_overfitting(self): if __name__ == "__main__": pytest.main([__file__, "-v"]) - diff --git a/tests/core/fingerprinting/test_preprocess.py b/tests/core/fingerprinting/test_preprocess.py index b214897..3abca8c 100644 --- a/tests/core/fingerprinting/test_preprocess.py +++ b/tests/core/fingerprinting/test_preprocess.py @@ -23,11 +23,13 @@ class TestAverageScans: def test_average_scans_mean_perfect(self): """Test mean averaging with identical scans.""" - scans = np.array([ - [-50, -60, -70], - [-50, -60, -70], - [-50, -60, -70], - ]) + scans = np.array( + [ + [-50, -60, -70], + [-50, -60, -70], + [-50, -60, -70], + ] + ) avg = average_scans(scans, method="mean") @@ -47,7 +49,9 @@ def test_average_scans_mean_with_noise(self): # Averaged should be closer to true values on average # Check that average error is smaller than median single-scan error avg_error = np.linalg.norm(avg - true_values) - scan_errors = [np.linalg.norm(scans[i] - true_values) for i in range(len(scans))] + scan_errors = [ + np.linalg.norm(scans[i] - true_values) for i in range(len(scans)) + ] median_scan_error = np.median(scan_errors) # Average should be better than median single scan @@ -55,13 +59,15 @@ def test_average_scans_mean_with_noise(self): def test_average_scans_median_robust_to_outliers(self): """Test median averaging is robust to outliers.""" - scans = np.array([ - [-50, -60, -70], - [-51, -59, -71], - [-49, -61, -69], - [-20, -65, -72], # Outlier in AP1 - [-52, -58, -68], - ]) + scans = np.array( + [ + [-50, -60, -70], + [-51, -59, -71], + [-49, -61, -69], + [-20, -65, -72], # Outlier in AP1 + [-52, -58, -68], + ] + ) avg_mean = average_scans(scans, method="mean") avg_median = average_scans(scans, method="median") @@ -72,13 +78,15 @@ def test_average_scans_median_robust_to_outliers(self): def test_average_scans_trimmed_mean(self): """Test trimmed mean averaging.""" - scans = np.array([ - [-50, -60, -70], - [-51, -59, -71], - [-49, -61, -69], - [-20, -65, -72], # Outlier - [-52, -58, -68], - ]) + scans = np.array( + [ + [-50, -60, -70], + [-51, -59, -71], + [-49, -61, -69], + [-20, -65, -72], # Outlier + [-52, -58, -68], + ] + ) avg = average_scans(scans, method="trimmed_mean", trim_percent=0.2) @@ -88,12 +96,14 @@ def test_average_scans_trimmed_mean(self): def test_average_scans_with_nan(self): """Test averaging with missing values (NaN).""" - scans = np.array([ - [-50, np.nan, -70], - [-52, -58, -72], - [-48, -62, np.nan], - [-51, -60, -69], - ]) + scans = np.array( + [ + [-50, np.nan, -70], + [-52, -58, -72], + [-48, -62, np.nan], + [-51, -60, -69], + ] + ) avg = average_scans(scans, method="mean") @@ -106,11 +116,13 @@ def test_average_scans_with_nan(self): def test_average_scans_all_nan_feature(self): """Test averaging when all scans for a feature are NaN.""" - scans = np.array([ - [-50, np.nan, -70], - [-52, np.nan, -72], - [-48, np.nan, -68], - ]) + scans = np.array( + [ + [-50, np.nan, -70], + [-52, np.nan, -72], + [-48, np.nan, -68], + ] + ) avg = average_scans(scans, method="mean") @@ -261,11 +273,13 @@ class TestPreprocessQuery: def test_preprocess_multiple_scans_no_norm(self): """Test preprocessing with multiple scans, no normalization.""" - scans = np.array([ - [-50, -60, -70], - [-52, -58, -72], - [-48, -62, -68], - ]) + scans = np.array( + [ + [-50, -60, -70], + [-52, -58, -72], + [-48, -62, -68], + ] + ) z_prep, info = preprocess_query(scans, normalization_method="none") @@ -287,16 +301,16 @@ def test_preprocess_multiple_scans_with_zscore(self): were the only things separating them. Any assertion on the output would have passed whether or not the averaging step ran at all. """ - scans = np.array([ - [-50, -60, -70], - [-52, -55, -80], - [-45, -70, -60], - ]) + scans = np.array( + [ + [-50, -60, -70], + [-52, -55, -80], + [-45, -70, -60], + ] + ) z_prep, info = preprocess_query( - scans, - averaging_method="mean", - normalization_method="zscore" + scans, averaging_method="mean", normalization_method="zscore" ) # Z-score should have mean ~0, std ~1 @@ -313,18 +327,15 @@ def test_preprocess_multiple_scans_with_zscore(self): # instead of the average gives a different answer for this data. for scan in scans: single = (scan - np.mean(scan)) / np.std(scan, ddof=1) - assert not np.allclose(z_prep, single), ( - "test data cannot distinguish averaging from not averaging" - ) + assert not np.allclose( + z_prep, single + ), "test data cannot distinguish averaging from not averaging" def test_preprocess_single_scan_with_norm(self): """Test preprocessing with single scan (just normalization).""" single_scan = np.array([-50, -60, -70, -80]) - z_prep, info = preprocess_query( - single_scan, - normalization_method="zscore" - ) + z_prep, info = preprocess_query(single_scan, normalization_method="zscore") assert info["averaging"]["n_scans"] == 1 assert info["averaging"]["method"] == "single_scan" @@ -332,10 +343,12 @@ def test_preprocess_single_scan_with_norm(self): def test_preprocess_with_reference_stats(self): """Test preprocessing with reference normalization statistics.""" - scans = np.array([ - [-55, -65, -75], - [-56, -64, -74], - ]) + scans = np.array( + [ + [-55, -65, -75], + [-56, -64, -74], + ] + ) # Use pre-computed normalization params (per-feature) ref_mean = np.array([-60.0, -70.0, -80.0]) @@ -346,7 +359,7 @@ def test_preprocess_with_reference_stats(self): averaging_method="mean", normalization_method="zscore", ref_mean=ref_mean, - ref_std=ref_std + ref_std=ref_std, ) # Check that reference stats were used (should be arrays) @@ -355,19 +368,21 @@ def test_preprocess_with_reference_stats(self): def test_preprocess_trimmed_mean_averaging(self): """Test preprocessing with trimmed mean averaging.""" - scans = np.array([ - [-50, -60, -70], - [-51, -59, -71], - [-20, -65, -72], # Outlier - [-49, -61, -69], - [-52, -58, -68], - ]) + scans = np.array( + [ + [-50, -60, -70], + [-51, -59, -71], + [-20, -65, -72], # Outlier + [-49, -61, -69], + [-52, -58, -68], + ] + ) z_prep, info = preprocess_query( scans, averaging_method="trimmed_mean", trim_percent=0.2, - normalization_method="none" + normalization_method="none", ) assert info["averaging"]["method"] == "trimmed_mean" @@ -379,13 +394,15 @@ class TestComputeNormalizationParams: def test_compute_zscore_params(self): """Test computing z-score parameters from fingerprints.""" # Simulate database features (M=5 RPs, N=3 APs) - fingerprints = np.array([ - [-50, -60, -70], - [-55, -65, -75], - [-45, -55, -65], - [-60, -70, -80], - [-50, -60, -70], - ]) + fingerprints = np.array( + [ + [-50, -60, -70], + [-55, -65, -75], + [-45, -55, -65], + [-60, -70, -80], + [-50, -60, -70], + ] + ) params = compute_normalization_params(fingerprints, method="zscore") @@ -403,12 +420,14 @@ def test_compute_zscore_params(self): def test_compute_minmax_params(self): """Test computing minmax parameters from fingerprints.""" - fingerprints = np.array([ - [-50, -60, -70], - [-55, -65, -75], - [-45, -55, -65], - [-60, -70, -80], - ]) + fingerprints = np.array( + [ + [-50, -60, -70], + [-55, -65, -75], + [-45, -55, -65], + [-60, -70, -80], + ] + ) params = compute_normalization_params(fingerprints, method="minmax") @@ -422,12 +441,14 @@ def test_compute_minmax_params(self): def test_compute_params_with_nan(self): """Test computing parameters with missing values (NaN).""" - fingerprints = np.array([ - [-50, np.nan, -70], - [-55, -65, -75], - [-45, -55, np.nan], - [-60, -70, -80], - ]) + fingerprints = np.array( + [ + [-50, np.nan, -70], + [-55, -65, -75], + [-45, -55, np.nan], + [-60, -70, -80], + ] + ) params = compute_normalization_params(fingerprints, method="zscore") @@ -440,11 +461,13 @@ def test_compute_params_with_nan(self): def test_compute_params_constant_feature(self): """Test computing parameters when feature is constant (zero variance).""" - fingerprints = np.array([ - [-50, -60, -70], - [-55, -60, -75], - [-45, -60, -65], - ]) + fingerprints = np.array( + [ + [-50, -60, -70], + [-55, -60, -75], + [-45, -60, -65], + ] + ) params = compute_normalization_params(fingerprints, method="zscore") @@ -476,12 +499,14 @@ def test_end_to_end_preprocessing(self): scans = true_fingerprint + noise # Step 2: Compute normalization params from database - db_features = np.array([ - [-50.0, -60.0, -70.0, -80.0], - [-55.0, -65.0, -75.0, -85.0], - [-45.0, -55.0, -65.0, -75.0], - [-60.0, -70.0, -80.0, -90.0], - ]) + db_features = np.array( + [ + [-50.0, -60.0, -70.0, -80.0], + [-55.0, -65.0, -75.0, -85.0], + [-45.0, -55.0, -65.0, -75.0], + [-60.0, -70.0, -80.0, -90.0], + ] + ) norm_params = compute_normalization_params(db_features, method="zscore") # Step 3: Preprocess query (note: ref_mean and ref_std are arrays, not scalars) @@ -493,7 +518,7 @@ def test_end_to_end_preprocessing(self): z_avg, method="zscore", ref_mean=norm_params["mean"][0], # Use first AP's mean as reference - ref_std=norm_params["std"][0] # Use first AP's std as reference + ref_std=norm_params["std"][0], # Use first AP's std as reference ) # Verify pipeline executed @@ -542,4 +567,3 @@ def test_normalization_handles_device_offset(self): # Normalized error should be smaller assert error_norm < error_raw - diff --git a/tests/core/fingerprinting/test_probabilistic.py b/tests/core/fingerprinting/test_probabilistic.py index a61757f..29c7cca 100644 --- a/tests/core/fingerprinting/test_probabilistic.py +++ b/tests/core/fingerprinting/test_probabilistic.py @@ -75,8 +75,12 @@ def test_model_creation_valid(self, simple_database): N = 3 model = NaiveBayesFingerprintModel( - means=np.array([[-50, -60, -70], [-60, -50, -80], [-70, -80, -50], [-55, -55, -55]]), - stds=np.array([[2.0, 2.0, 2.0], [2.0, 2.0, 2.0], [2.0, 2.0, 2.0], [2.0, 2.0, 2.0]]), + means=np.array( + [[-50, -60, -70], [-60, -50, -80], [-70, -80, -50], [-55, -55, -55]] + ), + stds=np.array( + [[2.0, 2.0, 2.0], [2.0, 2.0, 2.0], [2.0, 2.0, 2.0], [2.0, 2.0, 2.0]] + ), locations=simple_database.locations, floor_ids=simple_database.floor_ids, prior_probs=np.ones(M) / M, @@ -93,7 +97,9 @@ def test_model_stds_shape_mismatch_error(self, simple_database): with pytest.raises(ValueError, match="stds shape .* must match means shape"): NaiveBayesFingerprintModel( - means=np.array([[-50, -60, -70], [-60, -50, -80], [-70, -80, -50], [-55, -55, -55]]), + means=np.array( + [[-50, -60, -70], [-60, -50, -80], [-70, -80, -50], [-55, -55, -55]] + ), stds=np.array([[2.0, 2.0]]), # Wrong shape locations=simple_database.locations, floor_ids=simple_database.floor_ids, @@ -107,7 +113,9 @@ def test_model_locations_size_mismatch_error(self, simple_database): with pytest.raises(ValueError, match="locations has .* RPs, but means has"): NaiveBayesFingerprintModel( - means=np.array([[-50, -60, -70], [-60, -50, -80], [-70, -80, -50], [-55, -55, -55]]), + means=np.array( + [[-50, -60, -70], [-60, -50, -80], [-70, -80, -50], [-55, -55, -55]] + ), stds=np.full((4, 3), 2.0), locations=np.array([[0.0, 0.0], [10.0, 0.0]]), # Only 2 RPs floor_ids=simple_database.floor_ids, @@ -119,10 +127,16 @@ def test_model_non_positive_std_error(self, simple_database): """Test that non-positive stds raise ValueError.""" M = 4 - with pytest.raises(ValueError, match="All standard deviations must be positive"): + with pytest.raises( + ValueError, match="All standard deviations must be positive" + ): NaiveBayesFingerprintModel( - means=np.array([[-50, -60, -70], [-60, -50, -80], [-70, -80, -50], [-55, -55, -55]]), - stds=np.array([[2.0, 0.0, 2.0], [2.0, 2.0, 2.0], [2.0, 2.0, 2.0], [2.0, 2.0, 2.0]]), # Zero std + means=np.array( + [[-50, -60, -70], [-60, -50, -80], [-70, -80, -50], [-55, -55, -55]] + ), + stds=np.array( + [[2.0, 0.0, 2.0], [2.0, 2.0, 2.0], [2.0, 2.0, 2.0], [2.0, 2.0, 2.0]] + ), # Zero std locations=simple_database.locations, floor_ids=simple_database.floor_ids, prior_probs=np.ones(M) / M, @@ -134,7 +148,9 @@ def test_model_priors_not_normalized_error(self, simple_database): with pytest.raises(ValueError, match="Prior probabilities must sum to 1"): NaiveBayesFingerprintModel( - means=np.array([[-50, -60, -70], [-60, -50, -80], [-70, -80, -50], [-55, -55, -55]]), + means=np.array( + [[-50, -60, -70], [-60, -50, -80], [-70, -80, -50], [-55, -55, -55]] + ), stds=np.full((4, 3), 2.0), locations=simple_database.locations, floor_ids=simple_database.floor_ids, @@ -476,5 +492,3 @@ def test_larger_std_increases_uncertainty(self, simple_database): if __name__ == "__main__": pytest.main([__file__, "-v"]) - - diff --git a/tests/core/fingerprinting/test_topk_posterior_mean.py b/tests/core/fingerprinting/test_topk_posterior_mean.py index 5457b7e..86079e5 100644 --- a/tests/core/fingerprinting/test_topk_posterior_mean.py +++ b/tests/core/fingerprinting/test_topk_posterior_mean.py @@ -28,25 +28,28 @@ def test_topk_none_vs_full(): """Test that top_k=None reproduces current behavior.""" - print("\n" + "="*70) + print("\n" + "=" * 70) print("TEST 1: top_k=None reproduces full posterior mean") - print("="*70) + print("=" * 70) # Create simple database locations = np.array([[0, 0], [10, 0], [10, 10], [0, 10]], dtype=float) - features = np.array([ - [-50, -60, -70], - [-60, -50, -80], - [-70, -80, -50], - [-80, -70, -60], - ], dtype=float) + features = np.array( + [ + [-50, -60, -70], + [-60, -50, -80], + [-70, -80, -50], + [-80, -70, -60], + ], + dtype=float, + ) floor_ids = np.array([0, 0, 0, 0]) db = FingerprintDatabase( locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"} + meta={"ap_ids": ["AP1", "AP2", "AP3"], "unit": "dBm"}, ) model = fit_gaussian_naive_bayes(db, min_std=2.0) @@ -66,9 +69,9 @@ def test_topk_none_vs_full(): def test_topk_accuracy(): """Test that top_k yields nearly identical results.""" - print("\n" + "="*70) + print("\n" + "=" * 70) print("TEST 2: top_k yields nearly identical results to full") - print("="*70) + print("=" * 70) # Create larger database (20 RPs) np.random.seed(42) @@ -81,7 +84,7 @@ def test_topk_accuracy(): locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": ["AP1", "AP2", "AP3", "AP4"], "unit": "dBm"} + meta={"ap_ids": ["AP1", "AP2", "AP3", "AP4"], "unit": "dBm"}, ) model = fit_gaussian_naive_bayes(db, min_std=2.0) @@ -115,18 +118,22 @@ def test_topk_accuracy(): error_k10 = np.linalg.norm(pos_full - pos_k10) if error_k10 <= error_k3: - print(f" [OK] k=10 error ({error_k10:.4f}m) <= k=3 error ({error_k3:.4f}m)") + print( + f" [OK] k=10 error ({error_k10:.4f}m) <= k=3 error ({error_k3:.4f}m)" + ) else: - print(f" [WARNING] k=10 error ({error_k10:.4f}m) > k=3 error ({error_k3:.4f}m)") + print( + f" [WARNING] k=10 error ({error_k10:.4f}m) > k=3 error ({error_k3:.4f}m)" + ) return True def test_topk_speedup(): """Test that top_k provides speedup for large databases.""" - print("\n" + "="*70) + print("\n" + "=" * 70) print("TEST 3: top_k provides speedup for large databases") - print("="*70) + print("=" * 70) # Create large database (500 RPs) np.random.seed(42) @@ -139,7 +146,7 @@ def test_topk_speedup(): locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": [f"AP{i+1}" for i in range(8)], "unit": "dBm"} + meta={"ap_ids": [f"AP{i+1}" for i in range(8)], "unit": "dBm"}, ) model = fit_gaussian_naive_bayes(db, min_std=2.0) @@ -196,24 +203,27 @@ def test_topk_speedup(): def test_topk_edge_cases(): """Test edge cases for top_k.""" - print("\n" + "="*70) + print("\n" + "=" * 70) print("TEST 4: Edge cases (top_k=1, top_k=M)") - print("="*70) + print("=" * 70) # Create simple database locations = np.array([[0, 0], [10, 0], [10, 10]], dtype=float) - features = np.array([ - [-50, -60], - [-60, -50], - [-70, -70], - ], dtype=float) + features = np.array( + [ + [-50, -60], + [-60, -50], + [-70, -70], + ], + dtype=float, + ) floor_ids = np.array([0, 0, 0]) db = FingerprintDatabase( locations=locations, features=features, floor_ids=floor_ids, - meta={"ap_ids": ["AP1", "AP2"], "unit": "dBm"} + meta={"ap_ids": ["AP1", "AP2"], "unit": "dBm"}, ) model = fit_gaussian_naive_bayes(db, min_std=2.0) @@ -261,9 +271,9 @@ def test_topk_edge_cases(): def main(): """Run all tests.""" - print("\n" + "="*70) + print("\n" + "=" * 70) print("TOP-K POSTERIOR MEAN VALIDATION") - print("="*70) + print("=" * 70) print("\nThis script validates the top-k posterior mean implementation:") print(" - Book guidance: 'top k candidates typically sufficient'") print(" - Provides speedup for large databases") @@ -275,9 +285,9 @@ def main(): test_topk_speedup() test_topk_edge_cases() - print("\n" + "="*70) + print("\n" + "=" * 70) print("ALL TESTS PASSED OK") - print("="*70) + print("=" * 70) print("\nKey Findings:") print(" 1. top_k=None reproduces current behavior (backward compatible)") print(" 2. top_k=small yields nearly identical results to full sum") @@ -290,6 +300,7 @@ def main(): except Exception as e: print(f"\nX TEST FAILED: {e}") import traceback + traceback.print_exc() return False @@ -299,5 +310,3 @@ def main(): if __name__ == "__main__": success = main() sys.exit(0 if success else 1) - - diff --git a/tests/core/fingerprinting/test_types.py b/tests/core/fingerprinting/test_types.py index 8a533e7..7eb0f0b 100644 --- a/tests/core/fingerprinting/test_types.py +++ b/tests/core/fingerprinting/test_types.py @@ -20,7 +20,14 @@ def test_creation_valid_data(self): # Create a simple 2D database with 6 RPs on 2 floors db = FingerprintDatabase( locations=np.array( - [[0.0, 0.0], [5.0, 0.0], [10.0, 0.0], [0.0, 5.0], [5.0, 5.0], [10.0, 5.0]] + [ + [0.0, 0.0], + [5.0, 0.0], + [10.0, 0.0], + [0.0, 5.0], + [5.0, 5.0], + [10.0, 5.0], + ] ), features=np.array( [ @@ -256,5 +263,3 @@ def test_fingerprint_is_ndarray(self): if __name__ == "__main__": pytest.main([__file__, "-v"]) - - diff --git a/tests/core/fusion/__init__.py b/tests/core/fusion/__init__.py index a814e73..08282d5 100644 --- a/tests/core/fusion/__init__.py +++ b/tests/core/fusion/__init__.py @@ -1,3 +1 @@ # Tests for core.fusion module (gating, tuning, types) - - diff --git a/tests/core/fusion/test_adaptive_gating.py b/tests/core/fusion/test_adaptive_gating.py index 22b814f..52ce90f 100644 --- a/tests/core/fusion/test_adaptive_gating.py +++ b/tests/core/fusion/test_adaptive_gating.py @@ -66,7 +66,7 @@ def test_consecutive_reject_triggers_inflation(self): else: # Third reject: triggers inflation and forces accept self.assertTrue(accept) # Forced accept after adaptation - self.assertEqual(action, 'inflate_P') + self.assertEqual(action, "inflate_P") self.assertEqual(self.mgr.total_adaptations, 1) # Counter should reset after adaptation @@ -129,12 +129,12 @@ def test_get_stats(self): stats = self.mgr.get_stats() - self.assertEqual(stats['total_measurements'], 3) - self.assertEqual(stats['total_accepts'], 2) - self.assertEqual(stats['total_rejects'], 1) - self.assertAlmostEqual(stats['acceptance_rate'], 2.0 / 3.0) - self.assertEqual(stats['expected_nis'], 1) - self.assertAlmostEqual(stats['mean_nis'], (1.0 + 2.0 + 1.5) / 3.0) + self.assertEqual(stats["total_measurements"], 3) + self.assertEqual(stats["total_accepts"], 2) + self.assertEqual(stats["total_rejects"], 1) + self.assertAlmostEqual(stats["acceptance_rate"], 2.0 / 3.0) + self.assertEqual(stats["expected_nis"], 1) + self.assertAlmostEqual(stats["mean_nis"], (1.0 + 2.0 + 1.5) / 3.0) def test_reset(self): """Test reset functionality.""" @@ -199,9 +199,8 @@ def test_restores_consistency_with_overconfident_filter(self): # Verify stats show the issue stats = mgr.get_stats() - self.assertGreater(stats['mean_nis'], stats['expected_nis'] * 1.5) + self.assertGreater(stats["mean_nis"], stats["expected_nis"] * 1.5) -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() - diff --git a/tests/core/fusion/test_fusion_gating.py b/tests/core/fusion/test_fusion_gating.py index 2e0d62f..06bd3a6 100644 --- a/tests/core/fusion/test_fusion_gating.py +++ b/tests/core/fusion/test_fusion_gating.py @@ -71,7 +71,7 @@ def test_correlated_covariance(self) -> None: # S^{-1} = (1/3) * [[2, -1], [-1, 2]] # d^2 = [1, 1] @ (1/3) * [[2, -1], [-1, 2]] @ [1, 1] # = (1/3) * [1, 1] @ [1, 1] = (1/3) * 2 = 2/3 - self.assertAlmostEqual(d_sq, 2.0/3.0, places=10) + self.assertAlmostEqual(d_sq, 2.0 / 3.0, places=10) def test_dimension_mismatch_raises(self) -> None: """Test that dimension mismatch raises ValueError.""" @@ -120,7 +120,7 @@ class TestChiSquareGate(unittest.TestCase): def test_acceptance_criterion_95_confidence(self) -> None: """Test acceptance criterion with 95% confidence (book semantics). - + Verifies that the gate threshold correctly implements Eq. 8.9: Accept if d_k^2 < χ²(m, α), where α=0.95 means 95% confidence. """ @@ -268,7 +268,7 @@ class TestChiSquareThreshold(unittest.TestCase): def test_known_values_confidence(self) -> None: """Test against known chi-square critical values using confidence parameter. - + These are the canonical values from Chapter 8, Eq. 8.9, where α represents the confidence level (e.g., 0.95 for 95% confidence). """ @@ -294,7 +294,7 @@ def test_known_values_confidence(self) -> None: def test_deprecated_alpha_parameter(self) -> None: """Test backward compatibility with deprecated alpha parameter. - + alpha was interpreted as significance level (1 - confidence). The function should issue a deprecation warning. """ @@ -472,4 +472,3 @@ def test_consistency_with_threshold(self) -> None: if __name__ == "__main__": unittest.main() - diff --git a/tests/core/fusion/test_fusion_tuning.py b/tests/core/fusion/test_fusion_tuning.py index b84a336..25578af 100644 --- a/tests/core/fusion/test_fusion_tuning.py +++ b/tests/core/fusion/test_fusion_tuning.py @@ -145,11 +145,7 @@ def test_measurement_dimension_validation(self) -> None: def test_not_2d_raises(self) -> None: """Test that 1D inputs raise ValueError.""" with self.assertRaises(ValueError): - innovation_covariance( - np.array([1.0, 0.0]), # 1D - np.eye(2), - np.eye(2) - ) + innovation_covariance(np.array([1.0, 0.0]), np.eye(2), np.eye(2)) # 1D class TestScaleMeasurementCovariance(unittest.TestCase): @@ -249,8 +245,8 @@ def test_threshold_effect(self) -> None: """Test that higher delta is more tolerant (smaller scale for same r).""" residual = 2.0 - scale_strict = huber_R_scale(residual, delta=1.0) # strict - scale_loose = huber_R_scale(residual, delta=3.0) # loose + scale_strict = huber_R_scale(residual, delta=1.0) # strict + scale_loose = huber_R_scale(residual, delta=3.0) # loose # Strict: 2.0/1.0 = 2.0, Loose: 2.0 < 3.0 -> 1.0 self.assertAlmostEqual(scale_strict, 2.0, places=10) @@ -354,8 +350,8 @@ def test_threshold_effect(self) -> None: residual = 2.0 with self.assertWarns(DeprecationWarning): - w_strict = huber_weight(residual, threshold=1.0) # strict - w_loose = huber_weight(residual, threshold=3.0) # loose + w_strict = huber_weight(residual, threshold=1.0) # strict + w_loose = huber_weight(residual, threshold=3.0) # loose # Strict threshold treats 2.0 as outlier, loose treats as inlier self.assertLess(w_strict, 1.0) @@ -555,4 +551,3 @@ def test_literal_printed_form_would_trust_outliers_more(self): if __name__ == "__main__": unittest.main() - diff --git a/tests/core/fusion/test_fusion_types.py b/tests/core/fusion/test_fusion_types.py index f167cf5..86ba1c6 100644 --- a/tests/core/fusion/test_fusion_types.py +++ b/tests/core/fusion/test_fusion_types.py @@ -25,7 +25,7 @@ def test_valid_scalar_measurement(self) -> None: sensor="uwb_range", z=np.array([5.67]), R=np.array([[0.01]]), - meta={"anchor_id": 3} + meta={"anchor_id": 3}, ) self.assertEqual(meas.t, 1.234) @@ -41,7 +41,7 @@ def test_valid_vector_measurement(self) -> None: sensor="imu_accel", z=np.array([0.1, 0.05, 9.81]), R=np.diag([0.01, 0.01, 0.02]), - meta={"frame": "body"} + meta={"frame": "body"}, ) self.assertEqual(len(meas.z), 3) @@ -51,10 +51,7 @@ def test_valid_vector_measurement(self) -> None: def test_empty_meta_default(self) -> None: """Test that meta defaults to empty dict.""" meas = StampedMeasurement( - t=1.0, - sensor="test", - z=np.array([1.0]), - R=np.array([[1.0]]) + t=1.0, sensor="test", z=np.array([1.0]), R=np.array([[1.0]]) ) self.assertEqual(meas.meta, {}) @@ -63,10 +60,7 @@ def test_negative_timestamp_raises(self) -> None: """Test that negative timestamp raises ValueError.""" with self.assertRaises(ValueError): StampedMeasurement( - t=-1.0, - sensor="test", - z=np.array([1.0]), - R=np.array([[1.0]]) + t=-1.0, sensor="test", z=np.array([1.0]), R=np.array([[1.0]]) ) def test_invalid_timestamp_type_raises(self) -> None: @@ -76,18 +70,13 @@ def test_invalid_timestamp_type_raises(self) -> None: t="invalid", # type: ignore sensor="test", z=np.array([1.0]), - R=np.array([[1.0]]) + R=np.array([[1.0]]), ) def test_empty_sensor_name_raises(self) -> None: """Test that empty sensor name raises ValueError.""" with self.assertRaises(ValueError): - StampedMeasurement( - t=1.0, - sensor="", - z=np.array([1.0]), - R=np.array([[1.0]]) - ) + StampedMeasurement(t=1.0, sensor="", z=np.array([1.0]), R=np.array([[1.0]])) def test_measurement_not_1d_raises(self) -> None: """Test that 2D measurement array raises ValueError.""" @@ -96,17 +85,14 @@ def test_measurement_not_1d_raises(self) -> None: t=1.0, sensor="test", z=np.array([[1.0, 2.0]]), # 2D - R=np.array([[1.0]]) + R=np.array([[1.0]]), ) def test_covariance_not_2d_raises(self) -> None: """Test that 1D covariance array raises ValueError.""" with self.assertRaises(ValueError): StampedMeasurement( - t=1.0, - sensor="test", - z=np.array([1.0]), - R=np.array([1.0]) # 1D + t=1.0, sensor="test", z=np.array([1.0]), R=np.array([1.0]) # 1D ) def test_covariance_dimension_mismatch_raises(self) -> None: @@ -116,7 +102,7 @@ def test_covariance_dimension_mismatch_raises(self) -> None: t=1.0, sensor="test", z=np.array([1.0, 2.0]), # 2D measurement - R=np.array([[1.0]]) # 1x1 covariance + R=np.array([[1.0]]), # 1x1 covariance ) def test_asymmetric_covariance_raises(self) -> None: @@ -126,7 +112,7 @@ def test_asymmetric_covariance_raises(self) -> None: t=1.0, sensor="test", z=np.array([1.0, 2.0]), - R=np.array([[1.0, 0.5], [0.3, 1.0]]) # not symmetric + R=np.array([[1.0, 0.5], [0.3, 1.0]]), # not symmetric ) def test_negative_definite_covariance_raises(self) -> None: @@ -136,7 +122,7 @@ def test_negative_definite_covariance_raises(self) -> None: t=1.0, sensor="test", z=np.array([1.0, 2.0]), - R=np.array([[-1.0, 0.0], [0.0, 1.0]]) # negative eigenvalue + R=np.array([[-1.0, 0.0], [0.0, 1.0]]), # negative eigenvalue ) def test_positive_semidefinite_covariance_allowed(self) -> None: @@ -146,7 +132,7 @@ def test_positive_semidefinite_covariance_allowed(self) -> None: t=1.0, sensor="test", z=np.array([1.0, 2.0]), - R=np.array([[1.0, 1.0], [1.0, 1.0]]) # rank 1, positive semi-definite + R=np.array([[1.0, 1.0], [1.0, 1.0]]), # rank 1, positive semi-definite ) # Should not raise @@ -282,4 +268,3 @@ def test_round_trip_multiple_times(self) -> None: if __name__ == "__main__": unittest.main() - diff --git a/tests/core/rf/__init__.py b/tests/core/rf/__init__.py index d4052e9..004dabc 100644 --- a/tests/core/rf/__init__.py +++ b/tests/core/rf/__init__.py @@ -1,4 +1 @@ """Unit tests for core.rf module.""" - - - diff --git a/tests/core/rf/test_aoa_handcheck.py b/tests/core/rf/test_aoa_handcheck.py index c5db289..8a2d323 100644 --- a/tests/core/rf/test_aoa_handcheck.py +++ b/tests/core/rf/test_aoa_handcheck.py @@ -52,7 +52,9 @@ def test_azimuth_is_minus_45_degrees(): def test_elevation_is_the_arcsine_of_one_over_root_three(): """dU / range = 5 / sqrt(75) = 1/sqrt(3), so theta = 35.2644 degrees.""" assert aoa_sin_elevation(ANCHOR, AGENT) == pytest.approx(1 / np.sqrt(3)) - assert np.degrees(aoa_elevation(ANCHOR, AGENT)) == pytest.approx(35.264389, abs=1e-5) + assert np.degrees(aoa_elevation(ANCHOR, AGENT)) == pytest.approx( + 35.264389, abs=1e-5 + ) def test_the_measurement_vector_is_sin_elevation_then_tan_azimuth(): diff --git a/tests/core/rf/test_dop.py b/tests/core/rf/test_dop.py index d407906..4c2380a 100644 --- a/tests/core/rf/test_dop.py +++ b/tests/core/rf/test_dop.py @@ -31,9 +31,7 @@ def test_geometry_matrix_toa_2d(self): # Unit vectors should point from position to anchors # Check one manually - expected_dir = -(anchors[0] - position) / np.linalg.norm( - anchors[0] - position - ) + expected_dir = -(anchors[0] - position) / np.linalg.norm(anchors[0] - position) assert np.allclose(H[0], expected_dir) def test_geometry_matrix_toa_3d(self): @@ -120,9 +118,7 @@ def test_dop_poor_geometry(self): def test_dop_3d(self): """Test DOP computation in 3D.""" # Cube corners - anchors = np.array( - [[0, 0, 0], [10, 0, 0], [0, 10, 0], [0, 0, 10]], dtype=float - ) + anchors = np.array([[0, 0, 0], [10, 0, 0], [0, 10, 0], [0, 0, 10]], dtype=float) position = np.array([5.0, 5.0, 5.0]) H = compute_geometry_matrix(anchors, position, "toa") @@ -262,15 +258,11 @@ def test_dop_center_vs_edge(self): anchors = np.array([[0, 0], [10, 0], [10, 10], [0, 10]], dtype=float) # Center position - H_center = compute_geometry_matrix( - anchors, np.array([5.0, 5.0]), "toa" - ) + H_center = compute_geometry_matrix(anchors, np.array([5.0, 5.0]), "toa") dop_center = compute_dop(H_center) # Edge position - H_edge = compute_geometry_matrix( - anchors, np.array([1.0, 1.0]), "toa" - ) + H_edge = compute_geometry_matrix(anchors, np.array([1.0, 1.0]), "toa") dop_edge = compute_dop(H_edge) # Center should have better DOP @@ -389,4 +381,3 @@ def test_weighted_dop_book_eq_4103(self): if __name__ == "__main__": pytest.main([__file__, "-v"]) - diff --git a/tests/core/rf/test_measurement_models.py b/tests/core/rf/test_measurement_models.py index f42322e..982da14 100644 --- a/tests/core/rf/test_measurement_models.py +++ b/tests/core/rf/test_measurement_models.py @@ -232,14 +232,14 @@ def test_simulate_rtt_no_noise(self): rtt, info = simulate_rtt_measurement(anchor, agent) # True range should be 15m - assert np.isclose(info['true_range'], 15.0) + assert np.isclose(info["true_range"], 15.0) # RTT should be 2 * 15 / c expected_rtt = 2.0 * 15.0 / SPEED_OF_LIGHT assert np.isclose(rtt, expected_rtt) # Range estimate should match true range - assert np.isclose(info['range_estimate'], 15.0, atol=1e-6) + assert np.isclose(info["range_estimate"], 15.0, atol=1e-6) def test_simulate_rtt_with_processing_time(self): """Test simulate_rtt_measurement with processing time.""" @@ -256,7 +256,7 @@ def test_simulate_rtt_with_processing_time(self): assert np.isclose(rtt, expected_rtt) # Range estimate should still be correct - assert np.isclose(info['range_estimate'], 15.0, atol=1e-6) + assert np.isclose(info["range_estimate"], 15.0, atol=1e-6) def test_simulate_rtt_with_noise(self): """Test simulate_rtt_measurement with noise (Eq. 4.9).""" @@ -269,12 +269,13 @@ def test_simulate_rtt_with_noise(self): errors = [] for _ in range(100): rtt, info = simulate_rtt_measurement( - anchor, agent, + anchor, + agent, processing_time=50e-9, processing_time_std=5e-9, clock_drift_std=2e-9, ) - errors.append(info['range_estimate'] - 15.0) + errors.append(info["range_estimate"] - 15.0) errors = np.array(errors) @@ -374,19 +375,19 @@ def test_simulate_rss_no_fading(self): ) # True distance should be 10m - assert np.isclose(info['true_distance'], 10.0) + assert np.isclose(info["true_distance"], 10.0) # RSS should match forward model expected_rss = -40.0 - 10 * 2.5 * np.log10(10.0) assert np.isclose(rss, expected_rss, atol=0.01) - assert np.isclose(info['rss_true'], expected_rss, atol=0.01) + assert np.isclose(info["rss_true"], expected_rss, atol=0.01) # No fading - assert info['omega_long_db'] == 0.0 - assert info['omega_short_db'] == 0.0 + assert info["omega_long_db"] == 0.0 + assert info["omega_short_db"] == 0.0 # Distance estimate should match true distance - assert np.isclose(info['distance_estimate'], 10.0, atol=0.01) + assert np.isclose(info["distance_estimate"], 10.0, atol=0.01) def test_simulate_rss_with_long_term_fading(self): """Test simulate_rss_measurement with long-term fading (Eq. 4.12).""" @@ -397,19 +398,18 @@ def test_simulate_rss_with_long_term_fading(self): # Simulate with 6 dB long-term fading std rss, info = simulate_rss_measurement( - anchor, agent, + anchor, + agent, p_ref_dbm=-40.0, path_loss_exp=2.5, sigma_long_db=6.0, ) # RSS should differ from true RSS by omega_long - assert np.isclose( - rss, info['rss_true'] + info['omega_long_db'], atol=1e-6 - ) + assert np.isclose(rss, info["rss_true"] + info["omega_long_db"], atol=1e-6) # omega_long should be non-zero - assert info['omega_long_db'] != 0.0 + assert info["omega_long_db"] != 0.0 def test_simulate_rss_short_term_gaussian_averaging(self): """Test that Gaussian short-term fading is reduced by averaging.""" @@ -422,28 +422,30 @@ def test_simulate_rss_short_term_gaussian_averaging(self): errors_no_avg = [] for _ in range(100): rss, info = simulate_rss_measurement( - anchor, agent, + anchor, + agent, p_ref_dbm=-40.0, path_loss_exp=2.5, sigma_short_linear=4.0, # Interpreted as dB std for gaussian_db n_samples_avg=1, short_fading_model="gaussian_db", ) - errors_no_avg.append(info['omega_short_db']) + errors_no_avg.append(info["omega_short_db"]) # With 10-sample averaging np.random.seed(42) errors_with_avg = [] for _ in range(100): rss, info = simulate_rss_measurement( - anchor, agent, + anchor, + agent, p_ref_dbm=-40.0, path_loss_exp=2.5, sigma_short_linear=4.0, n_samples_avg=10, short_fading_model="gaussian_db", ) - errors_with_avg.append(info['omega_short_db']) + errors_with_avg.append(info["omega_short_db"]) # Averaging should reduce std by sqrt(10) ≈ 3.16 std_no_avg = np.std(errors_no_avg) @@ -465,14 +467,15 @@ def test_simulate_rss_rayleigh_fading_statistics(self): fading_values_db = [] for _ in range(1000): rss, info = simulate_rss_measurement( - anchor, agent, + anchor, + agent, p_ref_dbm=-40.0, path_loss_exp=2.5, sigma_short_linear=sigma_rayleigh, n_samples_avg=1, short_fading_model="rayleigh", ) - fading_values_db.append(info['omega_short_db']) + fading_values_db.append(info["omega_short_db"]) fading_values_db = np.array(fading_values_db) @@ -501,28 +504,30 @@ def test_simulate_rss_rayleigh_averaging_reduces_variance(self): fading_no_avg = [] for _ in range(200): rss, info = simulate_rss_measurement( - anchor, agent, + anchor, + agent, p_ref_dbm=-40.0, path_loss_exp=2.5, sigma_short_linear=sigma_rayleigh, n_samples_avg=1, short_fading_model="rayleigh", ) - fading_no_avg.append(info['omega_short_db']) + fading_no_avg.append(info["omega_short_db"]) # With 10-sample averaging np.random.seed(42) fading_with_avg = [] for _ in range(200): rss, info = simulate_rss_measurement( - anchor, agent, + anchor, + agent, p_ref_dbm=-40.0, path_loss_exp=2.5, sigma_short_linear=sigma_rayleigh, n_samples_avg=10, short_fading_model="rayleigh", ) - fading_with_avg.append(info['omega_short_db']) + fading_with_avg.append(info["omega_short_db"]) # Averaging should significantly reduce variance std_no_avg = np.std(fading_no_avg) @@ -537,7 +542,8 @@ def test_simulate_rss_fading_model_none(self): agent = np.array([10.0, 0.0]) rss, info = simulate_rss_measurement( - anchor, agent, + anchor, + agent, p_ref_dbm=-40.0, path_loss_exp=2.5, sigma_short_linear=1.0, # Non-zero but model is 'none' @@ -545,8 +551,8 @@ def test_simulate_rss_fading_model_none(self): ) # No short-term fading despite non-zero sigma - assert info['omega_short_db'] == 0.0 - assert info['short_fading_model'] == "none" + assert info["omega_short_db"] == 0.0 + assert info["short_fading_model"] == "none" def test_simulate_rss_invalid_fading_model(self): """Test that invalid fading model raises ValueError.""" @@ -555,7 +561,8 @@ def test_simulate_rss_invalid_fading_model(self): with pytest.raises(ValueError, match="short_fading_model must be one of"): simulate_rss_measurement( - anchor, agent, + anchor, + agent, p_ref_dbm=-40.0, short_fading_model="invalid_model", ) @@ -566,16 +573,14 @@ def test_simulate_rss_returns_short_fading_model_in_info(self): agent = np.array([10.0, 0.0]) # Default model - rss, info = simulate_rss_measurement( - anchor, agent, p_ref_dbm=-40.0 - ) - assert info['short_fading_model'] == "rayleigh" + rss, info = simulate_rss_measurement(anchor, agent, p_ref_dbm=-40.0) + assert info["short_fading_model"] == "rayleigh" # Explicit Gaussian rss, info = simulate_rss_measurement( anchor, agent, p_ref_dbm=-40.0, short_fading_model="gaussian_db" ) - assert info['short_fading_model'] == "gaussian_db" + assert info["short_fading_model"] == "gaussian_db" def test_rss_fading_to_distance_error_eq413(self): """Test fading to distance error conversion (Eq. 4.13).""" @@ -607,21 +612,22 @@ def test_multiplicative_distance_error_eq413(self): agent = np.array([10.0, 0.0]) rss, info = simulate_rss_measurement( - anchor, agent, + anchor, + agent, p_ref_dbm=-40.0, path_loss_exp=2.5, sigma_long_db=6.0, ) # Check distance_error_factor matches Eq. 4.13 - total_fading = info['omega_long_db'] + info['omega_short_db'] + total_fading = info["omega_long_db"] + info["omega_short_db"] expected_factor = 10 ** (-total_fading / 25.0) - assert np.isclose(info['distance_error_factor'], expected_factor, atol=1e-6) + assert np.isclose(info["distance_error_factor"], expected_factor, atol=1e-6) # Check actual distance estimate follows multiplicative error # d̃ ≈ d * distance_error_factor - expected_distance = info['true_distance'] * info['distance_error_factor'] - assert np.isclose(info['distance_estimate'], expected_distance, rtol=1e-3) + expected_distance = info["true_distance"] * info["distance_error_factor"] + assert np.isclose(info["distance_estimate"], expected_distance, rtol=1e-3) def test_simulate_rss_monte_carlo(self): """Test RSS simulation produces expected statistics.""" @@ -634,12 +640,13 @@ def test_simulate_rss_monte_carlo(self): distance_errors = [] for _ in range(200): rss, info = simulate_rss_measurement( - anchor, agent, + anchor, + agent, p_ref_dbm=-40.0, path_loss_exp=2.5, sigma_long_db=6.0, ) - relative_error = (info['distance_estimate'] - 10.0) / 10.0 + relative_error = (info["distance_estimate"] - 10.0) / 10.0 distance_errors.append(relative_error) distance_errors = np.array(distance_errors) @@ -952,4 +959,3 @@ def test_aoa_handcheck_geometry(self): if __name__ == "__main__": pytest.main([__file__, "-v"]) - diff --git a/tests/core/rf/test_positioning.py b/tests/core/rf/test_positioning.py index 040c390..7662b7a 100644 --- a/tests/core/rf/test_positioning.py +++ b/tests/core/rf/test_positioning.py @@ -101,9 +101,7 @@ def test_toa_with_clock_bias(self): # Solve with clock bias estimation initial_guess = np.array([6.0, 6.0, 0.0]) # [x, y, clock_bias] - pos, bias, info = toa_solve_with_clock_bias( - anchors, ranges, initial_guess - ) + pos, bias, info = toa_solve_with_clock_bias(anchors, ranges, initial_guess) # Check position accuracy assert np.linalg.norm(pos - true_pos) < 1e-3 @@ -225,9 +223,7 @@ def test_tdoa_positioning_perfect_measurements(self): # Solve positioner = TDOAPositioner(anchors, reference_idx=0) - estimated_pos, info = positioner.solve( - tdoa, initial_guess=np.array([6.0, 6.0]) - ) + estimated_pos, info = positioner.solve(tdoa, initial_guess=np.array([6.0, 6.0])) # Should converge to true position assert info["converged"] @@ -249,9 +245,7 @@ def test_tdoa_positioning_with_noise(self): tdoa = np.array(tdoa) + np.random.randn(3) * 0.05 # 5cm noise positioner = TDOAPositioner(anchors, reference_idx=0) - estimated_pos, info = positioner.solve( - tdoa, initial_guess=np.array([5.0, 5.0]) - ) + estimated_pos, info = positioner.solve(tdoa, initial_guess=np.array([5.0, 5.0])) # Should be close to true position error = np.linalg.norm(estimated_pos - true_pos) @@ -273,9 +267,7 @@ def test_tdoa_different_reference(self): tdoa = np.array(tdoa) positioner = TDOAPositioner(anchors, reference_idx=1) - estimated_pos, info = positioner.solve( - tdoa, initial_guess=np.array([6.0, 6.0]) - ) + estimated_pos, info = positioner.solve(tdoa, initial_guess=np.array([6.0, 6.0])) assert info["converged"] assert np.linalg.norm(estimated_pos - true_pos) < 1e-3 @@ -296,9 +288,7 @@ def test_aoa_positioning_perfect_measurements_2d(self): # Solve positioner = AOAPositioner(anchors) - estimated_pos, info = positioner.solve( - aoa, initial_guess=np.array([5.0, 5.0]) - ) + estimated_pos, info = positioner.solve(aoa, initial_guess=np.array([5.0, 5.0])) # Should converge to true position assert info["converged"] @@ -333,9 +323,7 @@ def test_aoa_positioning_square_anchors_2d(self): aoa = aoa_angle_vector(anchors, true_pos, include_elevation=False) positioner = AOAPositioner(anchors) - estimated_pos, info = positioner.solve( - aoa, initial_guess=np.array([6.0, 6.0]) - ) + estimated_pos, info = positioner.solve(aoa, initial_guess=np.array([6.0, 6.0])) assert info["converged"] assert np.linalg.norm(estimated_pos - true_pos) < 1e-2 @@ -391,8 +379,7 @@ def test_aoa_weighting_uniform_sigma_psi_2d(self): # Solve with sigma_psi (should compute W_a) estimated_pos, info = positioner.solve( - aoa, initial_guess=np.array([5.0, 5.0]), - sigma_psi=np.deg2rad(2.0) + aoa, initial_guess=np.array([5.0, 5.0]), sigma_psi=np.deg2rad(2.0) ) # Should converge and include weight matrix in info @@ -415,8 +402,7 @@ def test_aoa_weighting_heterogeneous_sigma_psi_2d(self): sigma_psi_per_anchor = np.deg2rad([1.0, 2.0, 5.0, 10.0]) estimated_pos, info = positioner.solve( - aoa, initial_guess=np.array([5.0, 5.0]), - sigma_psi=sigma_psi_per_anchor + aoa, initial_guess=np.array([5.0, 5.0]), sigma_psi=sigma_psi_per_anchor ) assert info["converged"] @@ -440,7 +426,8 @@ def test_aoa_weighting_direct_sigma_tan_psi_2d(self): # Direct tan(psi) std estimated_pos, info = positioner.solve( - aoa, initial_guess=np.array([5.0, 5.0]), + aoa, + initial_guess=np.array([5.0, 5.0]), sigma_tan_psi=0.1, residual="tan", ) @@ -457,9 +444,7 @@ def test_transformed_domain_sigmas_rejected_for_angle_residual(self): positioner = AOAPositioner(anchors) with pytest.raises(ValueError, match="residual='tan'"): - positioner.solve( - aoa, initial_guess=np.array([5.0, 5.0]), sigma_tan_psi=0.1 - ) + positioner.solve(aoa, initial_guess=np.array([5.0, 5.0]), sigma_tan_psi=0.1) def test_aoa_weighting_3d_with_sigmas(self): """Test 3D AOA with sigma_theta and sigma_psi weighting.""" @@ -473,9 +458,10 @@ def test_aoa_weighting_3d_with_sigmas(self): positioner = AOAPositioner(anchors) estimated_pos, info = positioner.solve( - aoa, initial_guess=np.array([6.0, 6.0, 1.0]), + aoa, + initial_guess=np.array([6.0, 6.0, 1.0]), sigma_theta=np.deg2rad(1.0), - sigma_psi=np.deg2rad(2.0) + sigma_psi=np.deg2rad(2.0), ) assert info["converged"] @@ -509,15 +495,12 @@ def test_aoa_weighting_improves_accuracy_heterogeneous_noise(self): positioner = AOAPositioner(anchors) # Unweighted - est_uw, _ = positioner.solve( - aoa_noisy, initial_guess=np.array([5.0, 5.0]) - ) + est_uw, _ = positioner.solve(aoa_noisy, initial_guess=np.array([5.0, 5.0])) errors_unweighted.append(np.linalg.norm(est_uw - true_pos)) # Weighted with known noise est_w, _ = positioner.solve( - aoa_noisy, initial_guess=np.array([5.0, 5.0]), - sigma_psi=sigma_rad + aoa_noisy, initial_guess=np.array([5.0, 5.0]), sigma_psi=sigma_rad ) errors_weighted.append(np.linalg.norm(est_w - true_pos)) @@ -541,9 +524,10 @@ def test_aoa_weighting_explicit_weights_override_sigma(self): explicit_weights = np.eye(4) * 5.0 estimated_pos, info = positioner.solve( - aoa, initial_guess=np.array([5.0, 5.0]), + aoa, + initial_guess=np.array([5.0, 5.0]), weights=explicit_weights, - sigma_psi=np.deg2rad(2.0) # Should be ignored + sigma_psi=np.deg2rad(2.0), # Should be ignored ) # Explicit weights should be used @@ -563,11 +547,11 @@ def test_aoa_weight_matrix_computation(self): # Compute expected variances using error propagation # var(tan ψ) = sec^4(ψ) * var(ψ) - var_psi = sigma_psi ** 2 + var_psi = sigma_psi**2 expected_var = [] for psi_i in psi: sec_sq = 1 + np.tan(psi_i) ** 2 - expected_var.append(sec_sq ** 2 * var_psi) + expected_var.append(sec_sq**2 * var_psi) expected_weights = 1.0 / np.array(expected_var) assert np.allclose(np.diag(W), expected_weights) @@ -604,8 +588,11 @@ def test_jacobian_2d_finite_difference(self): # Compare np.testing.assert_allclose( - H_analytical, H_numerical, rtol=1e-5, atol=1e-8, - err_msg="2D Jacobian mismatch vs finite difference" + H_analytical, + H_numerical, + rtol=1e-5, + atol=1e-8, + err_msg="2D Jacobian mismatch vs finite difference", ) def test_jacobian_3d_finite_difference(self): @@ -637,8 +624,11 @@ def test_jacobian_3d_finite_difference(self): # Compare np.testing.assert_allclose( - H_analytical, H_numerical, rtol=1e-5, atol=1e-8, - err_msg="3D Jacobian mismatch vs finite difference" + H_analytical, + H_numerical, + rtol=1e-5, + atol=1e-8, + err_msg="3D Jacobian mismatch vs finite difference", ) def test_jacobian_f_partial_derivatives(self): @@ -915,11 +905,11 @@ def test_build_tdoa_covariance_uniform_noise(self): cov = build_tdoa_covariance(sigmas, ref_idx=0) # Diagonal: 2 * sigma^2 - expected_diag = 2 * sigma ** 2 + expected_diag = 2 * sigma**2 assert np.allclose(np.diag(cov), expected_diag) # Off-diagonal: sigma^2 - expected_offdiag = sigma ** 2 + expected_offdiag = sigma**2 for i in range(3): for j in range(3): if i != j: @@ -957,14 +947,15 @@ def test_tdoa_positioning_with_correlated_covariance(self): # Generate noisy ranges range_noise = np.random.randn(len(anchors)) * sigmas noisy_ranges = np.array( - [np.linalg.norm(true_pos - anchors[i]) + range_noise[i] - for i in range(len(anchors))] + [ + np.linalg.norm(true_pos - anchors[i]) + range_noise[i] + for i in range(len(anchors)) + ] ) # Compute TDOA tdoa_noisy = np.array( - [noisy_ranges[i] - noisy_ranges[0] - for i in range(1, len(anchors))] + [noisy_ranges[i] - noisy_ranges[0] for i in range(1, len(anchors))] ) positioner = TDOAPositioner(anchors, reference_idx=0) @@ -972,7 +963,8 @@ def test_tdoa_positioning_with_correlated_covariance(self): # Correlated weighting try: est_corr, info = positioner.solve( - tdoa_noisy, initial_guess=np.array([10.0, 10.0]), + tdoa_noisy, + initial_guess=np.array([10.0, 10.0]), covariance=cov_corr, ) if info["converged"]: @@ -983,7 +975,8 @@ def test_tdoa_positioning_with_correlated_covariance(self): # Identity weighting try: est_id, info = positioner.solve( - tdoa_noisy, initial_guess=np.array([10.0, 10.0]), + tdoa_noisy, + initial_guess=np.array([10.0, 10.0]), covariance=cov_id, ) if info["converged"]: @@ -1018,7 +1011,7 @@ def test_fang_perfect_measurements(self): # Solve pos, info = toa_fang_solver(anchors, ranges) - assert info['method'] == 'Fang_TOA' + assert info["method"] == "Fang_TOA" assert np.linalg.norm(pos - true_pos) < 1e-6 def test_fang_with_noise(self): @@ -1098,15 +1091,17 @@ def test_chan_perfect_measurements(self): # Compute true ranges and TDOA ranges = np.linalg.norm(anchors - true_pos, axis=1) d_ref = ranges[ref_idx] - tdoa = np.array([ranges[i] - d_ref for i in range(len(anchors)) if i != ref_idx]) + tdoa = np.array( + [ranges[i] - d_ref for i in range(len(anchors)) if i != ref_idx] + ) # Solve pos, info = tdoa_chan_solver(anchors, tdoa, ref_idx=ref_idx) - assert info['method'] == 'Chan_TDOA' + assert info["method"] == "Chan_TDOA" assert np.linalg.norm(pos - true_pos) < 1e-4 # Reference distance should be close to true - assert np.abs(info['reference_distance'] - d_ref) < 1e-3 + assert np.abs(info["reference_distance"] - d_ref) < 1e-3 def test_chan_with_noise(self): """Test Chan's solver with noisy TDOA measurements.""" @@ -1122,7 +1117,9 @@ def test_chan_with_noise(self): # Compute noisy TDOA d_ref = ranges_noisy[ref_idx] - tdoa = np.array([ranges_noisy[i] - d_ref for i in range(len(anchors)) if i != ref_idx]) + tdoa = np.array( + [ranges_noisy[i] - d_ref for i in range(len(anchors)) if i != ref_idx] + ) pos, info = tdoa_chan_solver(anchors, tdoa, ref_idx=ref_idx) @@ -1148,7 +1145,9 @@ def test_chan_with_covariance(self): # Compute TDOA d_ref = ranges_noisy[ref_idx] - tdoa = np.array([ranges_noisy[i] - d_ref for i in range(len(anchors)) if i != ref_idx]) + tdoa = np.array( + [ranges_noisy[i] - d_ref for i in range(len(anchors)) if i != ref_idx] + ) # Solve with covariance (WLS) pos, info = tdoa_chan_solver(anchors, tdoa, ref_idx=ref_idx, covariance=cov) @@ -1164,7 +1163,9 @@ def test_chan_different_reference(self): ranges = np.linalg.norm(anchors - true_pos, axis=1) d_ref = ranges[ref_idx] - tdoa = np.array([ranges[i] - d_ref for i in range(len(anchors)) if i != ref_idx]) + tdoa = np.array( + [ranges[i] - d_ref for i in range(len(anchors)) if i != ref_idx] + ) pos, info = tdoa_chan_solver(anchors, tdoa, ref_idx=ref_idx) @@ -1203,7 +1204,9 @@ def test_chan_vs_iwls_consistency(self): # Perfect measurements ranges = np.linalg.norm(anchors - true_pos, axis=1) d_ref = ranges[ref_idx] - tdoa = np.array([ranges[i] - d_ref for i in range(len(anchors)) if i != ref_idx]) + tdoa = np.array( + [ranges[i] - d_ref for i in range(len(anchors)) if i != ref_idx] + ) # Chan's solver chan_pos, _ = tdoa_chan_solver(anchors, tdoa, ref_idx=ref_idx) @@ -1253,6 +1256,3 @@ def test_aoa_wrong_measurement_size(self): if __name__ == "__main__": pytest.main([__file__, "-v"]) - - - diff --git a/tests/core/rf/test_solve_batch_keeps_the_failures.py b/tests/core/rf/test_solve_batch_keeps_the_failures.py index c39adff..cb885e3 100644 --- a/tests/core/rf/test_solve_batch_keeps_the_failures.py +++ b/tests/core/rf/test_solve_batch_keeps_the_failures.py @@ -65,12 +65,14 @@ def _outcome(script, **kwargs): def test_a_converged_fix_that_landed_far_away_is_a_failure(): """The AOA case: `converged=True` at 1e11 m is not a measurement.""" - out = _outcome([ - ([1.0, 1.0], True), - ([1e11, 1e11], True), # converged, and absurd - ([3.0, 3.0], True), - ([4.0, 4.0], True), - ]) + out = _outcome( + [ + ([1.0, 1.0], True), + ([1e11, 1e11], True), # converged, and absurd + ([3.0, 3.0], True), + ([4.0, 4.0], True), + ] + ) assert out.n_failed == 1 assert not out.solved[1] assert out.max_solved_m < DIVERGENCE_M, ( @@ -86,12 +88,14 @@ def test_a_fix_that_never_left_the_initial_guess_is_a_failure(): scores as the distance from the seed to the truth -- which is why all three methods returned an identical 6.77 m on the collinear beacons. """ - out = _outcome([ - (GUESS, True), # stalled, but says it converged - ([2.0, 2.0], True), - ([3.0, 3.0], True), - ([4.0, 4.0], True), - ]) + out = _outcome( + [ + (GUESS, True), # stalled, but says it converged + ([2.0, 2.0], True), + ([3.0, 3.0], True), + ([4.0, 4.0], True), + ] + ) assert out.stalled[0] assert not out.solved[0] assert out.n_failed == 1 @@ -99,12 +103,14 @@ def test_a_fix_that_never_left_the_initial_guess_is_a_failure(): def test_a_raise_is_a_failure_and_does_not_become_a_zero_error(): """NaN, not 0.0 -- a dropped fix must not read as a perfect one.""" - out = _outcome([ - (None, True), # raises - ([2.0, 2.0], True), - ([3.0, 3.0], True), - ([4.0, 4.0], True), - ]) + out = _outcome( + [ + (None, True), # raises + ([2.0, 2.0], True), + ([3.0, 3.0], True), + ([4.0, 4.0], True), + ] + ) assert np.isnan(out.errors[0]) assert not out.solved[0] assert out.n_failed == 1 @@ -121,12 +127,14 @@ def test_errors_stay_aligned_with_the_measurements(): successes gives an array that is still a valid array, still plottable, and silently paired with the wrong per-position quantity. """ - out = _outcome([ - ([1.0, 1.0], True), - (None, True), - ([3.0, 3.0], True), - ([1e11, 1e11], True), - ]) + out = _outcome( + [ + ([1.0, 1.0], True), + (None, True), + ([3.0, 3.0], True), + ([1e11, 1e11], True), + ] + ) gdop = np.array([10.0, 20.0, 30.0, 40.0]) assert len(out.errors) == len(TRUTH) == len(gdop) @@ -144,12 +152,14 @@ def test_the_median_includes_failures_and_the_mean_does_not(): accuracy of the fixes that happened to work. `mean_solved_m` is the other question and needs the failures gone. """ - out = _outcome([ - (GUESS, True), # stalled at 0,0: error |(1,1)| = sqrt(2) - (GUESS, True), # stalled at 0,0: error |(2,2)| = 2 sqrt(2) - ([3.0, 3.0], True), # exact - ([4.0, 4.0], True), # exact - ]) + out = _outcome( + [ + (GUESS, True), # stalled at 0,0: error |(1,1)| = sqrt(2) + (GUESS, True), # stalled at 0,0: error |(2,2)| = 2 sqrt(2) + ([3.0, 3.0], True), # exact + ([4.0, 4.0], True), # exact + ] + ) # Two stalls and two exact fixes, so the median sits between them and the # mean over the successes is 0. One stall would not discriminate: the # median of [sqrt(2), 0, 0, 0] is 0, and the assertion would hold whether diff --git a/tests/core/sensors/__init__.py b/tests/core/sensors/__init__.py index 022688f..73f7ecd 100644 --- a/tests/core/sensors/__init__.py +++ b/tests/core/sensors/__init__.py @@ -1,3 +1 @@ # Tests for core.sensors module (IMU, PDR, wheel odometry, calibration, etc.) - - diff --git a/tests/core/sensors/test_gravity_eq6_8.py b/tests/core/sensors/test_gravity_eq6_8.py index ae43c9b..847349f 100644 --- a/tests/core/sensors/test_gravity_eq6_8.py +++ b/tests/core/sensors/test_gravity_eq6_8.py @@ -25,7 +25,7 @@ class TestGravityMagnitudeEq6_8(unittest.TestCase): """ Test suite for Book Eq. (6.8) gravity magnitude computation. - + Reference values computed from WGS-84 formula: g(φ) = 9.7803 * (1 + 0.0053024·sin²(φ) - 0.000005·sin²(2φ)) """ @@ -33,25 +33,26 @@ class TestGravityMagnitudeEq6_8(unittest.TestCase): def test_gravity_at_equator(self): """ Test gravity magnitude at equator (φ = 0°). - + At equator: sin(0) = 0 sin(2·0) = 0 g(0) = 9.7803 * (1 + 0 - 0) = 9.7803 m/s² - + This is the minimum gravity on Earth (strongest centrifugal effect). """ lat_rad = 0.0 # Equator g = gravity_magnitude_eq6_8(lat_rad) # Expected: exactly 9.7803 m/s² - self.assertAlmostEqual(g, 9.7803, places=6, - msg="Gravity at equator should be 9.7803 m/s²") + self.assertAlmostEqual( + g, 9.7803, places=6, msg="Gravity at equator should be 9.7803 m/s²" + ) def test_gravity_at_45_degrees(self): """ Test gravity magnitude at 45° latitude. - + At 45° (π/4 rad): sin(π/4) = √2/2 ≈ 0.7071 sin²(π/4) = 0.5 @@ -61,7 +62,7 @@ def test_gravity_at_45_degrees(self): = 9.7803 * (1 + 0.0026512 - 0.000005) = 9.7803 * 1.0026462 ≈ 9.8062 m/s² - + This is close to the commonly used approximation g = 9.81 m/s². """ lat_rad = np.deg2rad(45.0) # 45° North @@ -69,15 +70,20 @@ def test_gravity_at_45_degrees(self): # Expected: approximately 9.8062 m/s² expected_g = 9.7803 * (1 + 0.0053024 * 0.5 - 0.000005 * 1.0) - self.assertAlmostEqual(g, expected_g, places=6, - msg="Gravity at 45° should match hand-calculated value") - self.assertAlmostEqual(g, 9.8062, places=3, - msg="Gravity at 45° should be approximately 9.806 m/s²") + self.assertAlmostEqual( + g, + expected_g, + places=6, + msg="Gravity at 45° should match hand-calculated value", + ) + self.assertAlmostEqual( + g, 9.8062, places=3, msg="Gravity at 45° should be approximately 9.806 m/s²" + ) def test_gravity_at_north_pole(self): """ Test gravity magnitude at North Pole (φ = 90°). - + At North Pole (π/2 rad): sin(π/2) = 1 sin²(π/2) = 1 @@ -87,7 +93,7 @@ def test_gravity_at_north_pole(self): = 9.7803 * (1 + 0.0053024) = 9.7803 * 1.0053024 ≈ 9.8322 m/s² - + This is the maximum gravity on Earth (no centrifugal effect). """ lat_rad = np.pi / 2 # North Pole (90°) @@ -95,15 +101,23 @@ def test_gravity_at_north_pole(self): # Expected: approximately 9.8322 m/s² expected_g = 9.7803 * (1 + 0.0053024) - self.assertAlmostEqual(g, expected_g, places=6, - msg="Gravity at North Pole should match hand-calculated value") - self.assertAlmostEqual(g, 9.8322, places=3, - msg="Gravity at pole should be approximately 9.832 m/s²") + self.assertAlmostEqual( + g, + expected_g, + places=6, + msg="Gravity at North Pole should match hand-calculated value", + ) + self.assertAlmostEqual( + g, + 9.8322, + places=3, + msg="Gravity at pole should be approximately 9.832 m/s²", + ) def test_gravity_at_south_pole(self): """ Test gravity magnitude at South Pole (φ = -90°). - + Gravity should be symmetric: g(-90°) = g(+90°). """ lat_rad_south = -np.pi / 2 # South Pole (-90°) @@ -112,13 +126,17 @@ def test_gravity_at_south_pole(self): g_south = gravity_magnitude_eq6_8(lat_rad_south) g_north = gravity_magnitude_eq6_8(lat_rad_north) - self.assertAlmostEqual(g_south, g_north, places=10, - msg="Gravity should be symmetric at North/South poles") + self.assertAlmostEqual( + g_south, + g_north, + places=10, + msg="Gravity should be symmetric at North/South poles", + ) def test_gravity_increases_from_equator_to_pole(self): """ Test that gravity increases monotonically from equator to pole. - + Physical expectation: - Minimum at equator (strongest centrifugal force) - Maximum at poles (no centrifugal force) @@ -130,14 +148,17 @@ def test_gravity_increases_from_equator_to_pole(self): # Check monotonic increase for i in range(len(gravities) - 1): - self.assertGreater(gravities[i + 1], gravities[i], - msg=f"Gravity should increase from {latitudes_deg[i]}° " - f"to {latitudes_deg[i + 1]}°") + self.assertGreater( + gravities[i + 1], + gravities[i], + msg=f"Gravity should increase from {latitudes_deg[i]}° " + f"to {latitudes_deg[i + 1]}°", + ) def test_gravity_symmetric_north_south(self): """ Test that gravity is symmetric between Northern/Southern hemispheres. - + g(+φ) should equal g(-φ) for any latitude φ. """ test_latitudes_deg = [10, 25, 40, 55, 70, 85] @@ -149,13 +170,17 @@ def test_gravity_symmetric_north_south(self): g_north = gravity_magnitude_eq6_8(lat_rad_north) g_south = gravity_magnitude_eq6_8(lat_rad_south) - self.assertAlmostEqual(g_north, g_south, places=10, - msg=f"Gravity should be symmetric at ±{lat_deg}°") + self.assertAlmostEqual( + g_north, + g_south, + places=10, + msg=f"Gravity should be symmetric at ±{lat_deg}°", + ) def test_gravity_variation_range(self): """ Test that gravity variation is within expected WGS-84 range. - + Expected range: [9.78, 9.84] m/s² (approximately). Total variation: ~0.052 m/s² (~0.5% of g). """ @@ -167,59 +192,64 @@ def test_gravity_variation_range(self): g_max = max(gravities) # Check bounds - self.assertGreater(g_min, 9.77, - msg="Minimum gravity should be above 9.77 m/s²") - self.assertLess(g_max, 9.85, - msg="Maximum gravity should be below 9.85 m/s²") + self.assertGreater(g_min, 9.77, msg="Minimum gravity should be above 9.77 m/s²") + self.assertLess(g_max, 9.85, msg="Maximum gravity should be below 9.85 m/s²") # Check variation variation = g_max - g_min - self.assertAlmostEqual(variation, 0.0519, places=2, - msg="Gravity variation should be approximately 0.052 m/s²") + self.assertAlmostEqual( + variation, + 0.0519, + places=2, + msg="Gravity variation should be approximately 0.052 m/s²", + ) def test_gravity_at_typical_city_latitudes(self): """ Test gravity at representative city latitudes. - + Validates against known approximate values for common locations. """ # Tokyo, Japan: 35.6762° N g_tokyo = gravity_magnitude_eq6_8(np.deg2rad(35.6762)) - self.assertAlmostEqual(g_tokyo, 9.7976, places=3, - msg="Tokyo gravity should be ~9.798 m/s²") + self.assertAlmostEqual( + g_tokyo, 9.7976, places=3, msg="Tokyo gravity should be ~9.798 m/s²" + ) # New York City, USA: 40.7128° N g_nyc = gravity_magnitude_eq6_8(np.deg2rad(40.7128)) - self.assertAlmostEqual(g_nyc, 9.8023, places=3, - msg="NYC gravity should be ~9.802 m/s²") + self.assertAlmostEqual( + g_nyc, 9.8023, places=3, msg="NYC gravity should be ~9.802 m/s²" + ) # Singapore: 1.3521° N (near equator) g_singapore = gravity_magnitude_eq6_8(np.deg2rad(1.3521)) - self.assertAlmostEqual(g_singapore, 9.7804, places=3, - msg="Singapore gravity should be ~9.780 m/s²") + self.assertAlmostEqual( + g_singapore, 9.7804, places=3, msg="Singapore gravity should be ~9.780 m/s²" + ) # London, UK: 51.5074° N g_london = gravity_magnitude_eq6_8(np.deg2rad(51.5074)) - self.assertAlmostEqual(g_london, 9.8117, places=3, - msg="London gravity should be ~9.812 m/s²") + self.assertAlmostEqual( + g_london, 9.8117, places=3, msg="London gravity should be ~9.812 m/s²" + ) class TestGravityMagnitudeWithFallback(unittest.TestCase): """ Test suite for gravity_magnitude() with automatic fallback. - + Verifies backward compatibility and flexible API. """ def test_default_fallback_when_no_latitude(self): """ Test that default gravity is returned when lat_rad=None. - + This ensures backward compatibility with existing code. """ g = gravity_magnitude(lat_rad=None, default_g=9.81) - self.assertEqual(g, 9.81, - msg="Should return default_g when lat_rad is None") + self.assertEqual(g, 9.81, msg="Should return default_g when lat_rad is None") def test_default_fallback_with_custom_default(self): """ @@ -227,8 +257,9 @@ def test_default_fallback_with_custom_default(self): """ custom_g = 9.798 g = gravity_magnitude(lat_rad=None, default_g=custom_g) - self.assertEqual(g, custom_g, - msg="Should return custom default_g when lat_rad is None") + self.assertEqual( + g, custom_g, msg="Should return custom default_g when lat_rad is None" + ) def test_eq6_8_when_latitude_provided(self): """ @@ -239,18 +270,19 @@ def test_eq6_8_when_latitude_provided(self): # Should match Eq. (6.8) result, NOT default g_expected = gravity_magnitude_eq6_8(lat_rad) - self.assertAlmostEqual(g, g_expected, places=10, - msg="Should use Eq. (6.8) when lat_rad provided") - self.assertNotEqual(g, 9.81, - msg="Should NOT return default when latitude provided") + self.assertAlmostEqual( + g, g_expected, places=10, msg="Should use Eq. (6.8) when lat_rad provided" + ) + self.assertNotEqual( + g, 9.81, msg="Should NOT return default when latitude provided" + ) def test_default_parameter_values(self): """ Test that default parameters work as expected. """ g = gravity_magnitude() # No arguments - self.assertEqual(g, 9.81, - msg="Should default to 9.81 m/s² with no arguments") + self.assertEqual(g, 9.81, msg="Should default to 9.81 m/s² with no arguments") class TestGravityMagnitudeFromDegrees(unittest.TestCase): @@ -268,8 +300,12 @@ def test_degree_conversion_at_45(self): g_from_deg = gravity_magnitude_from_lat_deg(lat_deg) g_from_rad = gravity_magnitude_eq6_8(lat_rad) - self.assertAlmostEqual(g_from_deg, g_from_rad, places=10, - msg="Degree and radian versions should match") + self.assertAlmostEqual( + g_from_deg, + g_from_rad, + places=10, + msg="Degree and radian versions should match", + ) def test_degree_conversion_at_multiple_latitudes(self): """ @@ -283,8 +319,12 @@ def test_degree_conversion_at_multiple_latitudes(self): g_from_deg = gravity_magnitude_from_lat_deg(lat_deg) g_from_rad = gravity_magnitude_eq6_8(lat_rad) - self.assertAlmostEqual(g_from_deg, g_from_rad, places=10, - msg=f"Degree/radian mismatch at {lat_deg}°") + self.assertAlmostEqual( + g_from_deg, + g_from_rad, + places=10, + msg=f"Degree/radian mismatch at {lat_deg}°", + ) class TestGravityEdgeCases(unittest.TestCase): @@ -300,8 +340,9 @@ def test_very_small_latitude(self): g = gravity_magnitude_eq6_8(lat_rad) # Should be very close to equator value (9.7803) - self.assertAlmostEqual(g, 9.7803, places=4, - msg="Near-zero latitude should give equator gravity") + self.assertAlmostEqual( + g, 9.7803, places=4, msg="Near-zero latitude should give equator gravity" + ) def test_negative_latitudes(self): """ @@ -323,10 +364,11 @@ def test_large_latitude_array(self): # Should work without errors gravities = [gravity_magnitude_from_lat_deg(lat) for lat in latitudes_deg] - self.assertEqual(len(gravities), 1000, - msg="Should handle array-like inputs") - self.assertTrue(all(9.77 < g < 9.85 for g in gravities), - msg="All gravity values should be in valid range") + self.assertEqual(len(gravities), 1000, msg="Should handle array-like inputs") + self.assertTrue( + all(9.77 < g < 9.85 for g in gravities), + msg="All gravity values should be in valid range", + ) class TestGravityIntegrationWithCh6Algorithms(unittest.TestCase): @@ -337,7 +379,7 @@ class TestGravityIntegrationWithCh6Algorithms(unittest.TestCase): def test_strapdown_propagation_use_case(self): """ Simulate strapdown propagation use case. - + Test that gravity magnitude can be computed and used in typical strapdown integration workflow. """ @@ -355,7 +397,7 @@ def test_strapdown_propagation_use_case(self): def test_pdr_gravity_removal_use_case(self): """ Simulate PDR gravity removal use case. - + Test Eq. (6.47) usage: a_dynamic = a_mag - g """ lat_rad = np.deg2rad(35.0) @@ -374,7 +416,7 @@ def test_pdr_gravity_removal_use_case(self): def test_backward_compatibility_no_latitude(self): """ Test that old code without latitude still works. - + Ensures no breaking changes to existing Ch6 examples. """ # Old code path: no latitude provided @@ -386,7 +428,7 @@ def test_backward_compatibility_no_latitude(self): def test_new_code_with_latitude(self): """ Test that new code with latitude uses Eq. (6.8). - + Validates book-accurate path for updated examples. """ # New code path: latitude provided @@ -402,4 +444,3 @@ def test_new_code_with_latitude(self): if __name__ == "__main__": unittest.main() - diff --git a/tests/core/sensors/test_sensors_calibration.py b/tests/core/sensors/test_sensors_calibration.py index 633d2a8..1ed90fa 100644 --- a/tests/core/sensors/test_sensors_calibration.py +++ b/tests/core/sensors/test_sensors_calibration.py @@ -78,9 +78,7 @@ def test_allan_variance_overlapping_vs_non_overlapping(self) -> None: taus_fixed = np.logspace(-1, 0, 20) taus_over, adev_over = allan_variance(x, fs, taus=taus_fixed, overlapping=True) - taus_non, adev_non = allan_variance( - x, fs, taus=taus_fixed, overlapping=False - ) + taus_non, adev_non = allan_variance(x, fs, taus=taus_fixed, overlapping=False) # Both should produce results assert len(adev_over) > 0 @@ -325,7 +323,9 @@ def test_characterize_imu_multi_axis(self) -> None: assert "gyro" in results assert "accel" in results assert isinstance(results["gyro"]["angle_random_walk"], (float, np.floating)) - assert isinstance(results["accel"]["velocity_random_walk"], (float, np.floating)) + assert isinstance( + results["accel"]["velocity_random_walk"], (float, np.floating) + ) def test_characterize_imu_realistic_parameters(self) -> None: """Test with realistic IMU noise parameters.""" @@ -616,4 +616,3 @@ def test_allan_variance_single_tau(self) -> None: if __name__ == "__main__": unittest.main() - diff --git a/tests/core/sensors/test_sensors_constraints.py b/tests/core/sensors/test_sensors_constraints.py index 2259134..221343a 100644 --- a/tests/core/sensors/test_sensors_constraints.py +++ b/tests/core/sensors/test_sensors_constraints.py @@ -381,4 +381,3 @@ def test_nhc_zero_velocity(self) -> None: if __name__ == "__main__": unittest.main() - diff --git a/tests/core/sensors/test_sensors_environment.py b/tests/core/sensors/test_sensors_environment.py index 5a32a11..6343622 100644 --- a/tests/core/sensors/test_sensors_environment.py +++ b/tests/core/sensors/test_sensors_environment.py @@ -190,9 +190,9 @@ def test_mag_heading_inverts_the_frames_own_forward_convention(self) -> None: recovered = mag_heading(mag, 0.0, 0.0, frame=frame) - assert np.isclose(recovered, psi, atol=1e-9), ( - f"{frame.map_frame}: {psi} -> {direction} -> {recovered}" - ) + assert np.isclose( + recovered, psi, atol=1e-9 + ), f"{frame.map_frame}: {psi} -> {direction} -> {recovered}" def test_mag_heading_with_declination(self) -> None: """Test heading with magnetic declination correction.""" @@ -225,20 +225,32 @@ def test_mag_heading_tilt_invariant(self) -> None: tilt body = Ry(pitch) @ Rx(roll) @ level, then checks the recovered heading is the same for any tilt. """ + def Rx(a): - return np.array([[1, 0, 0], [0, np.cos(a), np.sin(a)], [0, -np.sin(a), np.cos(a)]]) + return np.array( + [[1, 0, 0], [0, np.cos(a), np.sin(a)], [0, -np.sin(a), np.cos(a)]] + ) def Ry(a): - return np.array([[np.cos(a), 0, -np.sin(a)], [0, 1, 0], [np.sin(a), 0, np.cos(a)]]) + return np.array( + [[np.cos(a), 0, -np.sin(a)], [0, 1, 0], [np.sin(a), 0, np.cos(a)]] + ) m_level = np.array([18.0, 7.0, 35.0]) target = np.arctan2(m_level[1], m_level[0]) - for roll, pitch in [(0.0, 0.0), (0.3, 0.0), (0.0, 0.4), (0.3, -0.4), (-0.5, 0.6), (0.7, 0.7)]: + for roll, pitch in [ + (0.0, 0.0), + (0.3, 0.0), + (0.0, 0.4), + (0.3, -0.4), + (-0.5, 0.6), + (0.7, 0.7), + ]: mag_b = Ry(pitch) @ Rx(roll) @ m_level heading = mag_heading(mag_b, roll, pitch) - assert np.isclose(heading, target, atol=1e-9), ( - f"heading not tilt-invariant at roll={roll}, pitch={pitch}" - ) + assert np.isclose( + heading, target, atol=1e-9 + ), f"heading not tilt-invariant at roll={roll}, pitch={pitch}" def test_mag_heading_wrapping(self) -> None: """Test that heading is wrapped to [-π, π].""" @@ -565,4 +577,3 @@ def test_floor_change_large_jump(self) -> None: if __name__ == "__main__": unittest.main() - diff --git a/tests/core/sensors/test_sensors_imu_models.py b/tests/core/sensors/test_sensors_imu_models.py index c27ed0d..dbc6213 100644 --- a/tests/core/sensors/test_sensors_imu_models.py +++ b/tests/core/sensors/test_sensors_imu_models.py @@ -327,4 +327,3 @@ def test_negative_measurements(self) -> None: if __name__ == "__main__": unittest.main() - diff --git a/tests/core/sensors/test_sensors_pdr.py b/tests/core/sensors/test_sensors_pdr.py index effbfb2..1c68a0d 100644 --- a/tests/core/sensors/test_sensors_pdr.py +++ b/tests/core/sensors/test_sensors_pdr.py @@ -518,9 +518,7 @@ def test_step_length_formula_explicit(self) -> None: # Book Eq. (6.49): # SL = [L_offset + a*(h - h_ref) + b*(SF - SF_ref)*(h/h_ref)] * c - L_expected = ( - L_offset + a * (h - h_ref) + b * (SF - SF_ref) * (h / h_ref) - ) * c + L_expected = (L_offset + a * (h - h_ref) + b * (SF - SF_ref) * (h / h_ref)) * c assert np.isclose(L, L_expected) @@ -560,7 +558,7 @@ def test_weinberg_basic_computation(self) -> None: L = step_length_weinberg(f_window, G_w) # ptp = 4.0, so L = 0.5 * 4.0^0.25 = 0.5 * 1.414... ≈ 0.707 - expected = G_w * (4.0 ** 0.25) + expected = G_w * (4.0**0.25) assert np.isclose(L, expected, atol=0.01) def test_weinberg_monotonicity(self) -> None: @@ -600,7 +598,7 @@ def test_weinberg_custom_power(self) -> None: L = step_length_weinberg(f_window, G_w, power=power_custom) # ptp = 8.0, L = 0.5 * 8.0^0.5 = 0.5 * 2.828... ≈ 1.414 - expected = G_w * (8.0 ** power_custom) + expected = G_w * (8.0**power_custom) assert np.isclose(L, expected, atol=0.01) def test_weinberg_eps_floor(self) -> None: @@ -613,7 +611,7 @@ def test_weinberg_eps_floor(self) -> None: L = step_length_weinberg(f_flat, G_w, eps=eps) # Should use eps as ptp: L = G_w * eps^0.25 - expected = G_w * (eps ** 0.25) + expected = G_w * (eps**0.25) assert np.isclose(L, expected, atol=0.01) def test_weinberg_invalid_inputs(self) -> None: @@ -640,7 +638,7 @@ def test_calibrate_gain_basic(self) -> None: G_w = calibrate_weinberg_gain(ptp_per_step, distance_known) # Verify: sum of step lengths should equal distance - total_length = G_w * np.sum(ptp_per_step ** 0.25) + total_length = G_w * np.sum(ptp_per_step**0.25) assert np.isclose(total_length, distance_known, atol=0.01) def test_calibrate_gain_consistency(self) -> None: @@ -651,7 +649,7 @@ def test_calibrate_gain_consistency(self) -> None: G_w = calibrate_weinberg_gain(ptp_per_step, distance_known) # Each step should be 0.5m - step_length = G_w * (4.0 ** 0.25) + step_length = G_w * (4.0**0.25) assert np.isclose(step_length, 0.5, atol=0.01) def test_calibrate_gain_more_steps_same_distance(self) -> None: @@ -733,4 +731,3 @@ def test_accel_magnitude_large_values(self) -> None: if __name__ == "__main__": unittest.main() - diff --git a/tests/core/sensors/test_sensors_strapdown.py b/tests/core/sensors/test_sensors_strapdown.py index 2f3e691..f8148ab 100644 --- a/tests/core/sensors/test_sensors_strapdown.py +++ b/tests/core/sensors/test_sensors_strapdown.py @@ -483,4 +483,3 @@ def test_long_integration_drift(self) -> None: if __name__ == "__main__": unittest.main() - diff --git a/tests/core/sensors/test_sensors_types.py b/tests/core/sensors/test_sensors_types.py index 35c774d..5ded520 100644 --- a/tests/core/sensors/test_sensors_types.py +++ b/tests/core/sensors/test_sensors_types.py @@ -135,9 +135,7 @@ def test_wheel_speed_series_valid_construction(self) -> None: def test_wheel_speed_series_immutability(self) -> None: """Test that WheelSpeedSeries is frozen.""" n = 10 - wheel = WheelSpeedSeries( - t=np.linspace(0, 1, n), v_s=np.zeros((n, 3)), meta={} - ) + wheel = WheelSpeedSeries(t=np.linspace(0, 1, n), v_s=np.zeros((n, 3)), meta={}) with pytest.raises(Exception): wheel.v_s = np.ones((n, 3)) @@ -265,16 +263,12 @@ def test_nav_state_qpvp_invalid_q_shape(self) -> None: def test_nav_state_qpvp_invalid_v_shape(self) -> None: """Test NavStateQPVP rejects wrong velocity shape.""" with pytest.raises(ValueError, match="v must have shape"): - NavStateQPVP( - q=np.array([1.0, 0.0, 0.0, 0.0]), v=np.zeros(2), p=np.zeros(3) - ) + NavStateQPVP(q=np.array([1.0, 0.0, 0.0, 0.0]), v=np.zeros(2), p=np.zeros(3)) def test_nav_state_qpvp_invalid_p_shape(self) -> None: """Test NavStateQPVP rejects wrong position shape.""" with pytest.raises(ValueError, match="p must have shape"): - NavStateQPVP( - q=np.array([1.0, 0.0, 0.0, 0.0]), v=np.zeros(3), p=np.zeros(4) - ) + NavStateQPVP(q=np.array([1.0, 0.0, 0.0, 0.0]), v=np.zeros(3), p=np.zeros(4)) class TestNavStateQPVPBias(unittest.TestCase): @@ -399,4 +393,3 @@ def test_metadata_flexibility(self) -> None: if __name__ == "__main__": unittest.main() - diff --git a/tests/core/sensors/test_sensors_wheel_odometry.py b/tests/core/sensors/test_sensors_wheel_odometry.py index 8653566..fb5c5d1 100644 --- a/tests/core/sensors/test_sensors_wheel_odometry.py +++ b/tests/core/sensors/test_sensors_wheel_odometry.py @@ -159,11 +159,13 @@ def test_lever_arm_equation_6_11_misaligned(self) -> None: # Rotation matrix: 90° about z (S-frame rotated 90° relative to A-frame) angle = np.pi / 2 - C_S_A = np.array([ - [np.cos(angle), -np.sin(angle), 0], - [np.sin(angle), np.cos(angle), 0], - [0, 0, 1] - ]) + C_S_A = np.array( + [ + [np.cos(angle), -np.sin(angle), 0], + [np.sin(angle), np.cos(angle), 0], + [0, 0, 1], + ] + ) v_a = wheel_speed_to_attitude_velocity(v_s, omega, lever_arm, C_S_A) @@ -185,7 +187,9 @@ def test_misaligned_frames_identity_rotation(self) -> None: # Explicit identity C_S_A_identity = np.eye(3) - v_a_explicit = wheel_speed_to_attitude_velocity(v_s, omega, lever_arm, C_S_A_identity) + v_a_explicit = wheel_speed_to_attitude_velocity( + v_s, omega, lever_arm, C_S_A_identity + ) np.testing.assert_array_almost_equal(v_a_default, v_a_explicit) @@ -196,11 +200,7 @@ def test_misaligned_frames_180deg_rotation(self) -> None: lever_arm = np.zeros(3) # 180° rotation about z-axis - C_S_A = np.array([ - [-1, 0, 0], - [0, -1, 0], - [0, 0, 1] - ]) + C_S_A = np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]]) v_a = wheel_speed_to_attitude_velocity(v_s, omega, lever_arm, C_S_A) @@ -432,4 +432,3 @@ def test_negative_velocity(self) -> None: if __name__ == "__main__": unittest.main() - diff --git a/tests/core/sim/__init__.py b/tests/core/sim/__init__.py index 310867a..ddad4a0 100644 --- a/tests/core/sim/__init__.py +++ b/tests/core/sim/__init__.py @@ -1,2 +1 @@ """Unit tests for core.sim module.""" - diff --git a/tests/core/sim/test_noise_pink.py b/tests/core/sim/test_noise_pink.py index 676b3b1..4c28cd0 100644 --- a/tests/core/sim/test_noise_pink.py +++ b/tests/core/sim/test_noise_pink.py @@ -120,7 +120,9 @@ def test_allan_deviation_with_combined_noise(self): # Generate combined white + pink noise (realistic IMU scenario) white = rng.standard_normal(N) * 0.3 # ARW component (dominant at short tau) - pink = pink_noise_1f_fft(N, fs, rng=rng) * 1.0 # BI component (dominant at mid tau) + pink = ( + pink_noise_1f_fft(N, fs, rng=rng) * 1.0 + ) # BI component (dominant at mid tau) combined = white + pink # Compute Allan deviation @@ -209,7 +211,12 @@ def test_scaling_factor_correctness(self): bi_factor = 0.5 # non-standard factor pink_scaled = scale_to_bias_instability( - pink_unit, target_bi_rad_s, allan_variance, tau_grid, fs, bi_factor=bi_factor + pink_unit, + target_bi_rad_s, + allan_variance, + tau_grid, + fs, + bi_factor=bi_factor, ) # Compute Allan deviation @@ -281,4 +288,3 @@ def test_full_pipeline(self): if __name__ == "__main__": unittest.main() - diff --git a/tests/core/slam/__init__.py b/tests/core/slam/__init__.py index 66e5037..f658fca 100644 --- a/tests/core/slam/__init__.py +++ b/tests/core/slam/__init__.py @@ -5,5 +5,3 @@ Author: Li-Ta Hsu Date: 2024 """ - - diff --git a/tests/core/slam/test_bundle_adjustment_smoke.py b/tests/core/slam/test_bundle_adjustment_smoke.py index abc28a6..aebe1c8 100644 --- a/tests/core/slam/test_bundle_adjustment_smoke.py +++ b/tests/core/slam/test_bundle_adjustment_smoke.py @@ -30,7 +30,7 @@ class TestBundleAdjustmentSmoke: """Smoke tests for bundle adjustment pipeline. - + NOTE: Bundle adjustment is tested in ch7_slam/example_bundle_adjustment.py These tests are skipped as BA requires careful setup and the example demonstrates it. """ @@ -42,25 +42,35 @@ def simple_ba_scenario(self): # Camera intrinsics (simple pinhole, minimal distortion) intrinsics = CameraIntrinsics( - fx=500.0, fy=500.0, cx=320.0, cy=240.0, - k1=0.0, k2=0.0, p1=0.0, p2=0.0 # No distortion for simplicity + fx=500.0, + fy=500.0, + cx=320.0, + cy=240.0, + k1=0.0, + k2=0.0, + p1=0.0, + p2=0.0, # No distortion for simplicity ) # Ground truth camera poses (3 poses in a line, looking forward) # Format: [x, y, z, yaw] for simplicity (2D motion, camera at z=0) - true_poses = np.array([ - [0.0, 0.0, 0.0, 0.0], - [1.0, 0.0, 0.0, 0.0], - [2.0, 0.0, 0.0, 0.0], - ]) + true_poses = np.array( + [ + [0.0, 0.0, 0.0, 0.0], + [1.0, 0.0, 0.0, 0.0], + [2.0, 0.0, 0.0, 0.0], + ] + ) # Ground truth 3D landmarks (features in front of cameras) - true_landmarks = np.array([ - [1.0, -0.5, 3.0], - [1.0, 0.5, 3.0], - [2.0, -0.3, 4.0], - [2.0, 0.3, 4.0], - ]) + true_landmarks = np.array( + [ + [1.0, -0.5, 3.0], + [1.0, 0.5, 3.0], + [2.0, -0.3, 4.0], + [2.0, 0.3, 4.0], + ] + ) # Generate observations (pixel coordinates) observations = [] # (pose_idx, landmark_idx, pixel_uv) @@ -98,7 +108,9 @@ def simple_ba_scenario(self): # Generate noisy initial estimates initial_poses = true_poses + np.random.normal(0, 0.05, true_poses.shape) - initial_landmarks = true_landmarks + np.random.normal(0, 0.1, true_landmarks.shape) + initial_landmarks = true_landmarks + np.random.normal( + 0, 0.1, true_landmarks.shape + ) return { "intrinsics": intrinsics, @@ -109,7 +121,9 @@ def simple_ba_scenario(self): "observations": observations, } - @pytest.mark.skip(reason="BA tested in example_bundle_adjustment.py - integration test needs proper 3D/4D pose handling") + @pytest.mark.skip( + reason="BA tested in example_bundle_adjustment.py - integration test needs proper 3D/4D pose handling" + ) def test_bundle_adjustment_reduces_reprojection_error(self, simple_ba_scenario): """Smoke test: BA should reduce reprojection error.""" data = simple_ba_scenario @@ -130,22 +144,26 @@ def test_bundle_adjustment_reduces_reprojection_error(self, simple_ba_scenario): graph.add_variable(f"landmark_{i}", landmark) # Add prior on first pose (anchor) - graph.add_factor(create_prior_factor( - "pose_0", - initial_poses[0], - np.diag([1e6, 1e6, 1e6, 1e6]) # Strong prior - )) + graph.add_factor( + create_prior_factor( + "pose_0", + initial_poses[0], + np.diag([1e6, 1e6, 1e6, 1e6]), # Strong prior + ) + ) # Add reprojection factors pixel_covariance = np.diag([1.0, 1.0]) for pose_idx, landmark_idx, observed_pixel in observations: - graph.add_factor(create_reprojection_factor( - f"pose_{pose_idx}", - f"landmark_{landmark_idx}", - observed_pixel, - intrinsics, - pixel_covariance, - )) + graph.add_factor( + create_reprojection_factor( + f"pose_{pose_idx}", + f"landmark_{landmark_idx}", + observed_pixel, + intrinsics, + pixel_covariance, + ) + ) # Optimize initial_error = graph.compute_error() @@ -167,8 +185,9 @@ def test_bundle_adjustment_reduces_reprojection_error(self, simple_ba_scenario): # Weak assertion: Error should reduce by at least 10% improvement = (initial_error - final_error) / initial_error - assert improvement > 0.10, \ - f"BA error reduction {improvement*100:.1f}% below 10% threshold" + assert ( + improvement > 0.10 + ), f"BA error reduction {improvement*100:.1f}% below 10% threshold" @pytest.mark.skip(reason="BA tested in example_bundle_adjustment.py") def test_bundle_adjustment_improves_pose_accuracy(self, simple_ba_scenario): @@ -188,22 +207,29 @@ def test_bundle_adjustment_improves_pose_accuracy(self, simple_ba_scenario): for i, landmark in enumerate(initial_landmarks): graph.add_variable(f"landmark_{i}", landmark) - graph.add_factor(create_prior_factor( - "pose_0", initial_poses[0], np.diag([1e6, 1e6, 1e6, 1e6]) - )) + graph.add_factor( + create_prior_factor( + "pose_0", initial_poses[0], np.diag([1e6, 1e6, 1e6, 1e6]) + ) + ) for pose_idx, landmark_idx, observed_pixel in observations: - graph.add_factor(create_reprojection_factor( - f"pose_{pose_idx}", f"landmark_{landmark_idx}", - observed_pixel, intrinsics, np.diag([1.0, 1.0]), - )) + graph.add_factor( + create_reprojection_factor( + f"pose_{pose_idx}", + f"landmark_{landmark_idx}", + observed_pixel, + intrinsics, + np.diag([1.0, 1.0]), + ) + ) optimized_vars, _ = graph.optimize(method="gauss_newton", max_iterations=10) # Extract optimized poses - optimized_poses = np.array([ - optimized_vars[f"pose_{i}"] for i in range(len(true_poses)) - ]) + optimized_poses = np.array( + [optimized_vars[f"pose_{i}"] for i in range(len(true_poses))] + ) # Compute RMSEs initial_rmse = compute_rmse(initial_poses[:, :3], true_poses[:, :3]) @@ -214,8 +240,9 @@ def test_bundle_adjustment_improves_pose_accuracy(self, simple_ba_scenario): # Lenient assertion: Should show some improvement or stay similar # (BA may not converge fully with numerical Jacobians in few iterations) - assert optimized_rmse <= initial_rmse * 1.2, \ - "BA should not significantly worsen pose estimates" + assert ( + optimized_rmse <= initial_rmse * 1.2 + ), "BA should not significantly worsen pose estimates" @pytest.mark.skip(reason="BA tested in example_bundle_adjustment.py") def test_bundle_adjustment_improves_landmark_accuracy(self, simple_ba_scenario): @@ -235,22 +262,29 @@ def test_bundle_adjustment_improves_landmark_accuracy(self, simple_ba_scenario): for i, landmark in enumerate(initial_landmarks): graph.add_variable(f"landmark_{i}", landmark) - graph.add_factor(create_prior_factor( - "pose_0", initial_poses[0], np.diag([1e6, 1e6, 1e6, 1e6]) - )) + graph.add_factor( + create_prior_factor( + "pose_0", initial_poses[0], np.diag([1e6, 1e6, 1e6, 1e6]) + ) + ) for pose_idx, landmark_idx, observed_pixel in observations: - graph.add_factor(create_reprojection_factor( - f"pose_{pose_idx}", f"landmark_{landmark_idx}", - observed_pixel, intrinsics, np.diag([1.0, 1.0]), - )) + graph.add_factor( + create_reprojection_factor( + f"pose_{pose_idx}", + f"landmark_{landmark_idx}", + observed_pixel, + intrinsics, + np.diag([1.0, 1.0]), + ) + ) optimized_vars, _ = graph.optimize(method="gauss_newton", max_iterations=10) # Extract optimized landmarks - optimized_landmarks = np.array([ - optimized_vars[f"landmark_{i}"] for i in range(len(true_landmarks)) - ]) + optimized_landmarks = np.array( + [optimized_vars[f"landmark_{i}"] for i in range(len(true_landmarks))] + ) # Compute RMSEs initial_rmse = compute_rmse(initial_landmarks, true_landmarks) @@ -260,8 +294,9 @@ def test_bundle_adjustment_improves_landmark_accuracy(self, simple_ba_scenario): print(f"[BA LANDMARK] Optimized RMSE: {optimized_rmse:.4f} m") # Lenient assertion - assert optimized_rmse <= initial_rmse * 1.2, \ - "BA should not significantly worsen landmark estimates" + assert ( + optimized_rmse <= initial_rmse * 1.2 + ), "BA should not significantly worsen landmark estimates" @pytest.mark.skip(reason="BA tested in example_bundle_adjustment.py") @pytest.mark.slow @@ -283,15 +318,22 @@ def test_bundle_adjustment_with_more_iterations(self, simple_ba_scenario): for i, landmark in enumerate(initial_landmarks): graph.add_variable(f"landmark_{i}", landmark) - graph.add_factor(create_prior_factor( - "pose_0", initial_poses[0], np.diag([1e6, 1e6, 1e6, 1e6]) - )) + graph.add_factor( + create_prior_factor( + "pose_0", initial_poses[0], np.diag([1e6, 1e6, 1e6, 1e6]) + ) + ) for pose_idx, landmark_idx, observed_pixel in observations: - graph.add_factor(create_reprojection_factor( - f"pose_{pose_idx}", f"landmark_{landmark_idx}", - observed_pixel, intrinsics, np.diag([1.0, 1.0]), - )) + graph.add_factor( + create_reprojection_factor( + f"pose_{pose_idx}", + f"landmark_{landmark_idx}", + observed_pixel, + intrinsics, + np.diag([1.0, 1.0]), + ) + ) # Optimize with more iterations initial_error = graph.compute_error() @@ -303,18 +345,22 @@ def test_bundle_adjustment_with_more_iterations(self, simple_ba_scenario): final_error = graph.compute_error() # Extract results - optimized_poses = np.array([ - optimized_vars[f"pose_{i}"] for i in range(len(true_poses)) - ]) - optimized_landmarks = np.array([ - optimized_vars[f"landmark_{i}"] for i in range(len(true_landmarks)) - ]) + optimized_poses = np.array( + [optimized_vars[f"pose_{i}"] for i in range(len(true_poses))] + ) + optimized_landmarks = np.array( + [optimized_vars[f"landmark_{i}"] for i in range(len(true_landmarks))] + ) # Compute improvements - pose_improvement = 1 - (compute_rmse(optimized_poses[:, :3], true_poses[:, :3]) / - compute_rmse(initial_poses[:, :3], true_poses[:, :3])) - landmark_improvement = 1 - (compute_rmse(optimized_landmarks, true_landmarks) / - compute_rmse(initial_landmarks, true_landmarks)) + pose_improvement = 1 - ( + compute_rmse(optimized_poses[:, :3], true_poses[:, :3]) + / compute_rmse(initial_poses[:, :3], true_poses[:, :3]) + ) + landmark_improvement = 1 - ( + compute_rmse(optimized_landmarks, true_landmarks) + / compute_rmse(initial_landmarks, true_landmarks) + ) error_reduction = 1 - final_error / initial_error print("\n[BA EXTENDED]") @@ -324,8 +370,9 @@ def test_bundle_adjustment_with_more_iterations(self, simple_ba_scenario): print(f" Iterations: {len(error_history)}") # With more iterations, should show meaningful improvement - assert error_reduction > 0.20, \ - f"BA should reduce error by >20% with more iterations, got {error_reduction*100:.1f}%" + assert ( + error_reduction > 0.20 + ), f"BA should reduce error by >20% with more iterations, got {error_reduction*100:.1f}%" class TestBundleAdjustmentRegressionThresholds: @@ -352,12 +399,16 @@ def test_ba_error_reduction_threshold(self): for l_idx, lm in enumerate(true_landmarks): lm_cam = (T_cam_inv @ np.append(lm, 1.0))[:3] if lm_cam[2] > 0: - pixel = project_point(intrinsics, lm_cam) + np.random.normal(0, 0.3, 2) + pixel = project_point(intrinsics, lm_cam) + np.random.normal( + 0, 0.3, 2 + ) observations.append((p_idx, l_idx, pixel)) # Noisy initial estimates initial_poses = true_poses + np.random.normal(0, 0.03, true_poses.shape) - initial_landmarks = true_landmarks + np.random.normal(0, 0.05, true_landmarks.shape) + initial_landmarks = true_landmarks + np.random.normal( + 0, 0.05, true_landmarks.shape + ) # Build and optimize graph = FactorGraph() @@ -366,12 +417,20 @@ def test_ba_error_reduction_threshold(self): for i, lm in enumerate(initial_landmarks): graph.add_variable(f"landmark_{i}", lm) - graph.add_factor(create_prior_factor("pose_0", initial_poses[0], np.diag([1e6]*4))) + graph.add_factor( + create_prior_factor("pose_0", initial_poses[0], np.diag([1e6] * 4)) + ) for p_idx, l_idx, pixel in observations: - graph.add_factor(create_reprojection_factor( - f"pose_{p_idx}", f"landmark_{l_idx}", pixel, intrinsics, np.diag([1, 1]) - )) + graph.add_factor( + create_reprojection_factor( + f"pose_{p_idx}", + f"landmark_{l_idx}", + pixel, + intrinsics, + np.diag([1, 1]), + ) + ) initial_error = graph.compute_error() graph.optimize(method="gauss_newton", max_iterations=15) @@ -380,10 +439,10 @@ def test_ba_error_reduction_threshold(self): error_reduction = (initial_error - final_error) / initial_error # Regression threshold: At least 10% error reduction - assert error_reduction > 0.10, \ - f"BA error reduction {error_reduction*100:.1f}% below 10% regression threshold" + assert ( + error_reduction > 0.10 + ), f"BA error reduction {error_reduction*100:.1f}% below 10% regression threshold" if __name__ == "__main__": pytest.main([__file__, "-v", "-s"]) - diff --git a/tests/core/slam/test_camera.py b/tests/core/slam/test_camera.py index 64a20a1..dd54d3a 100644 --- a/tests/core/slam/test_camera.py +++ b/tests/core/slam/test_camera.py @@ -137,8 +137,7 @@ def test_off_center_projection(self): def test_with_distortion(self): """Test projection with lens distortion.""" intrinsics = CameraIntrinsics( - fx=500, fy=500, cx=320, cy=240, - k1=-0.1, k2=0.01, p1=0.001, p2=0.001 + fx=500, fy=500, cx=320, cy=240, k1=-0.1, k2=0.01, p1=0.001, p2=0.001 ) point_3d = np.array([1.0, 0.5, 5.0]) @@ -160,11 +159,13 @@ def test_batch_projection(self): """Test projection of multiple points.""" intrinsics = CameraIntrinsics(fx=500, fy=500, cx=320, cy=240) - points_3d = np.array([ - [0.0, 0.0, 5.0], - [1.0, 0.0, 5.0], - [0.0, 1.0, 5.0], - ]) + points_3d = np.array( + [ + [0.0, 0.0, 5.0], + [1.0, 0.0, 5.0], + [0.0, 1.0, 5.0], + ] + ) pixels = project_point(intrinsics, points_3d) assert pixels.shape == (3, 2) @@ -290,11 +291,13 @@ def test_batch_reprojection_error(self): """Test computing errors for multiple points.""" intrinsics = CameraIntrinsics(fx=500, fy=500, cx=320, cy=240) - points_3d = np.array([ - [0.0, 0.0, 5.0], - [1.0, 0.0, 5.0], - [0.0, 1.0, 5.0], - ]) + points_3d = np.array( + [ + [0.0, 0.0, 5.0], + [1.0, 0.0, 5.0], + [0.0, 1.0, 5.0], + ] + ) # Project all points projected = project_point(intrinsics, points_3d) @@ -329,8 +332,7 @@ def test_create_simple_intrinsics(self): def test_create_with_distortion(self): """Test creating intrinsics with distortion coefficients.""" intrinsics = CameraIntrinsics( - fx=500, fy=500, cx=320, cy=240, - k1=-0.1, k2=0.01, p1=0.001, p2=0.001 + fx=500, fy=500, cx=320, cy=240, k1=-0.1, k2=0.01, p1=0.001, p2=0.001 ) assert intrinsics.k1 == -0.1 @@ -353,16 +355,17 @@ class TestIntegration: def test_project_unproject_roundtrip(self): """Test full roundtrip: 3D point → pixel → 3D point.""" intrinsics = CameraIntrinsics( - fx=500, fy=500, cx=320, cy=240, - k1=-0.05, k2=0.005, p1=0.001, p2=0.001 + fx=500, fy=500, cx=320, cy=240, k1=-0.05, k2=0.005, p1=0.001, p2=0.001 ) # Original 3D points - points_original = np.array([ - [0.5, 0.3, 5.0], - [1.0, -0.5, 4.0], - [-0.5, 0.8, 6.0], - ]) + points_original = np.array( + [ + [0.5, 0.3, 5.0], + [1.0, -0.5, 4.0], + [-0.5, 0.8, 6.0], + ] + ) # Project to pixels pixels = project_point(intrinsics, points_original) @@ -385,12 +388,14 @@ def test_reprojection_error_minimization_concept(self): observed_pixel = project_point(intrinsics, point_true) # Test points at different positions - test_points = np.array([ - [1.0, 0.5, 5.0], # Correct point - [1.1, 0.5, 5.0], # Slightly off in X - [1.0, 0.6, 5.0], # Slightly off in Y - [1.0, 0.5, 5.2], # Slightly off in Z - ]) + test_points = np.array( + [ + [1.0, 0.5, 5.0], # Correct point + [1.1, 0.5, 5.0], # Slightly off in X + [1.0, 0.6, 5.0], # Slightly off in Y + [1.0, 0.5, 5.2], # Slightly off in Z + ] + ) errors = [] for test_point in test_points: @@ -545,4 +550,3 @@ def test_pure_rotation_is_degenerate(self): if __name__ == "__main__": pytest.main([__file__, "-v"]) - diff --git a/tests/core/slam/test_factors.py b/tests/core/slam/test_factors.py index 466a0de..6086983 100644 --- a/tests/core/slam/test_factors.py +++ b/tests/core/slam/test_factors.py @@ -274,7 +274,9 @@ def test_weak_prior_allows_movement(self): # Strong odometry factor connecting poses (expects 1m forward) strong_odom_info = np.diag([1e4, 1e4, 1e4]) # Very strong - odom = create_odometry_factor(0, 1, np.array([1.0, 0.0, 0.0]), information=strong_odom_info) + odom = create_odometry_factor( + 0, 1, np.array([1.0, 0.0, 0.0]), information=strong_odom_info + ) graph.add_factor(odom) # Optimize @@ -480,7 +482,9 @@ def test_scan_matching_integration(self): # Odometry measurement (noisy) odom_rel = np.array([0.9, 0.05, 0.02]) odom_cov = np.diag([0.1, 0.1, 0.01]) # Moderate uncertainty - odom = create_odometry_factor(0, 1, odom_rel, information=np.linalg.inv(odom_cov)) + odom = create_odometry_factor( + 0, 1, odom_rel, information=np.linalg.inv(odom_cov) + ) graph.add_factor(odom) # Simulated ICP measurement (more accurate) @@ -498,4 +502,3 @@ def test_scan_matching_integration(self): # Should be closer to [1.0, 0.0, 0.0] than to odometry dist_to_icp = np.linalg.norm(optimized[1] - np.array([1.0, 0.0, 0.0])) assert dist_to_icp < 0.05 # Very close to ICP measurement - diff --git a/tests/core/slam/test_frontend_2d.py b/tests/core/slam/test_frontend_2d.py index 2f9da6c..96171e3 100644 --- a/tests/core/slam/test_frontend_2d.py +++ b/tests/core/slam/test_frontend_2d.py @@ -38,12 +38,12 @@ def test_first_step_initialization(self): result = frontend.step(0, odom_delta, scan) # Should initialize at origin - np.testing.assert_allclose(result['pose_pred'], [0.0, 0.0, 0.0]) - np.testing.assert_allclose(result['pose_est'], [0.0, 0.0, 0.0]) + np.testing.assert_allclose(result["pose_pred"], [0.0, 0.0, 0.0]) + np.testing.assert_allclose(result["pose_est"], [0.0, 0.0, 0.0]) # Should mark as converged (initialization) - self.assertTrue(result['match_quality'].converged) - self.assertEqual(result['correction_magnitude'], 0.0) + self.assertTrue(result["match_quality"].converged) + self.assertEqual(result["correction_magnitude"], 0.0) # Submap should contain scan points self.assertEqual(len(frontend.submap), 2) @@ -78,7 +78,7 @@ def test_prediction_with_translation_only(self): result = frontend.step(1, odom_delta, scan) # Predicted pose should be (1, 0, 0) - np.testing.assert_allclose(result['pose_pred'], [1.0, 0.0, 0.0]) + np.testing.assert_allclose(result["pose_pred"], [1.0, 0.0, 0.0]) def test_prediction_with_rotation(self): """Test prediction with rotation.""" @@ -93,7 +93,9 @@ def test_prediction_with_rotation(self): result = frontend.step(1, odom_delta, scan) # Predicted pose should be (0, 0, π/2) - np.testing.assert_allclose(result['pose_pred'], [0.0, 0.0, np.pi / 2], atol=1e-6) + np.testing.assert_allclose( + result["pose_pred"], [0.0, 0.0, np.pi / 2], atol=1e-6 + ) def test_prediction_accumulates_over_steps(self): """Test that prediction accumulates correctly over multiple steps.""" @@ -111,7 +113,7 @@ def test_prediction_accumulates_over_steps(self): # Should be at (1.5, 0, 0) after 3 steps expected_pose = np.array([1.5, 0.0, 0.0]) - np.testing.assert_allclose(result['pose_est'][:2], expected_pose[:2], atol=0.1) + np.testing.assert_allclose(result["pose_est"][:2], expected_pose[:2], atol=0.1) class TestSlamFrontend2DScanToMapAlignment(unittest.TestCase): @@ -131,13 +133,11 @@ def test_scan_to_map_with_perfect_alignment(self): # Should have low residual (ICP may or may not converge with identical scans) # What matters is that residual is low - self.assertLess(result['match_quality'].residual, 0.5) + self.assertLess(result["match_quality"].residual, 0.5) # Estimated pose should be close to predicted pose np.testing.assert_allclose( - result['pose_est'][:2], - result['pose_pred'][:2], - atol=0.2 + result["pose_est"][:2], result["pose_pred"][:2], atol=0.2 ) def test_scan_to_map_with_small_drift(self): @@ -154,7 +154,9 @@ def test_scan_to_map_with_small_drift(self): result = frontend.step(1, odom_delta, scan) # Should converge - self.assertTrue(result['match_quality'].converged or result['match_quality'].residual < 0.5) + self.assertTrue( + result["match_quality"].converged or result["match_quality"].residual < 0.5 + ) def test_fallback_to_prediction_when_submap_too_small(self): """Test fallback to prediction when submap has too few points.""" @@ -169,10 +171,10 @@ def test_fallback_to_prediction_when_submap_too_small(self): result = frontend.step(1, odom_delta, scan) # Should NOT converge (fallback to prediction) - self.assertFalse(result['match_quality'].converged) + self.assertFalse(result["match_quality"].converged) # Pose estimate should equal prediction (no correction) - np.testing.assert_allclose(result['pose_est'], result['pose_pred']) + np.testing.assert_allclose(result["pose_est"], result["pose_pred"]) def test_fallback_to_prediction_on_empty_scan(self): """Test fallback when scan is empty or too small.""" @@ -188,7 +190,7 @@ def test_fallback_to_prediction_on_empty_scan(self): result = frontend.step(1, odom_delta, small_scan) # Should fallback to prediction - self.assertFalse(result['match_quality'].converged) + self.assertFalse(result["match_quality"].converged) class TestSlamFrontend2DMapUpdate(unittest.TestCase): @@ -230,7 +232,9 @@ def test_map_uses_estimated_pose_not_prediction(self): self.assertEqual(len(frontend.submap), 2) # First point should still be at origin - np.testing.assert_allclose(frontend.submap.points[0], initial_map_point, atol=1e-6) + np.testing.assert_allclose( + frontend.submap.points[0], initial_map_point, atol=1e-6 + ) class TestSlamFrontend2DInputValidation(unittest.TestCase): @@ -322,7 +326,7 @@ def test_straight_line_trajectory(self): # Should converge on most steps (though later steps might not due to accumulated map) # Just check no crashes - self.assertIsNotNone(result['pose_est']) + self.assertIsNotNone(result["pose_est"]) # Final pose should be approximately (5.0, 0, 0) final_pose = frontend.get_current_pose() @@ -360,7 +364,7 @@ def test_square_trajectory_with_rotations(self): result = frontend.step(i, odom_delta, curr_scan) # Just verify no crashes and results are reasonable - self.assertIsNotNone(result['pose_est']) + self.assertIsNotNone(result["pose_est"]) if __name__ == "__main__": diff --git a/tests/core/slam/test_icp_converges.py b/tests/core/slam/test_icp_converges.py index dee1c90..3354b27 100644 --- a/tests/core/slam/test_icp_converges.py +++ b/tests/core/slam/test_icp_converges.py @@ -25,12 +25,23 @@ def simple_scan(self): """Generate a simple rectangular scan pattern.""" np.random.seed(42) # Create rectangle with some internal points - scan = np.array([ - [0, 0], [1, 0], [2, 0], [3, 0], - [0, 1], [3, 1], - [0, 2], [3, 2], - [0, 3], [1, 3], [2, 3], [3, 3], - ], dtype=float) + scan = np.array( + [ + [0, 0], + [1, 0], + [2, 0], + [3, 0], + [0, 1], + [3, 1], + [0, 2], + [3, 2], + [0, 3], + [1, 3], + [2, 3], + [3, 3], + ], + dtype=float, + ) # Add small noise scan += np.random.normal(0, 0.01, scan.shape) return scan @@ -63,8 +74,12 @@ def test_icp_pure_translation_small(self, simple_scan): # Assertions assert converged, "ICP should converge for simple translation" assert iters < 20, f"ICP should converge quickly, took {iters} iterations" - np.testing.assert_allclose(pose, true_translation, atol=0.05, - err_msg="ICP should recover true translation") + np.testing.assert_allclose( + pose, + true_translation, + atol=0.05, + err_msg="ICP should recover true translation", + ) assert residual < 0.1, f"Final residual {residual} too high" def test_icp_pure_translation_large(self, simple_scan): @@ -81,8 +96,12 @@ def test_icp_pure_translation_large(self, simple_scan): # Assertions assert converged, "ICP should converge for large translation" - np.testing.assert_allclose(pose, true_translation, atol=0.1, - err_msg="ICP should recover true translation") + np.testing.assert_allclose( + pose, + true_translation, + atol=0.1, + err_msg="ICP should recover true translation", + ) assert residual < 1.0, f"Final residual {residual} too high" def test_icp_rotation_30deg(self, simple_scan): @@ -105,8 +124,12 @@ def test_icp_rotation_30deg(self, simple_scan): # Assertions assert converged, "ICP should converge with rotation and good guess" assert iters < 50, f"ICP should converge reasonably fast, took {iters}" - np.testing.assert_allclose(pose, true_pose, atol=0.15, - err_msg="ICP should recover rotation + translation") + np.testing.assert_allclose( + pose, + true_pose, + atol=0.15, + err_msg="ICP should recover rotation + translation", + ) assert residual < 1.0, f"Final residual {residual} too high" def test_icp_rotation_small(self, simple_scan): @@ -127,8 +150,9 @@ def test_icp_rotation_small(self, simple_scan): # Assertions assert converged, "ICP should converge with moderate rotation" - np.testing.assert_allclose(pose, true_pose, atol=0.15, - err_msg="ICP should recover moderate rotation") + np.testing.assert_allclose( + pose, true_pose, atol=0.15, err_msg="ICP should recover moderate rotation" + ) assert residual < 1.0, f"Final residual {residual} too high" def test_icp_with_good_initial_guess(self, dense_scan): @@ -139,22 +163,27 @@ def test_icp_with_good_initial_guess(self, dense_scan): # With moderate initial guess moderate_guess = np.array([1.0, 1.5, 0.1]) pose_moderate, iters_moderate, _, converged_moderate = icp_point_to_point( - dense_scan, target, initial_pose=moderate_guess, + dense_scan, + target, + initial_pose=moderate_guess, max_iterations=100, ) # With good initial guess good_guess = true_pose + np.array([0.1, 0.1, 0.05]) pose_with_init, iters_with_init, _, converged_with_init = icp_point_to_point( - dense_scan, target, initial_pose=good_guess, + dense_scan, + target, + initial_pose=good_guess, max_iterations=100, ) # Assertions assert converged_moderate, "ICP should converge with moderate guess" assert converged_with_init, "ICP should converge with good guess" - assert iters_with_init <= iters_moderate, \ - "Better initial guess should not increase iterations" + assert ( + iters_with_init <= iters_moderate + ), "Better initial guess should not increase iterations" # Good guess should be reasonably accurate np.testing.assert_allclose(pose_with_init, true_pose, atol=0.25) @@ -183,8 +212,12 @@ def test_icp_with_noise(self, simple_scan): # Assertions assert converged, "ICP should converge despite noise" # Allow larger tolerance due to noise - np.testing.assert_allclose(pose, true_pose, atol=0.25, - err_msg="ICP should approximately recover pose with noise") + np.testing.assert_allclose( + pose, + true_pose, + atol=0.25, + err_msg="ICP should approximately recover pose with noise", + ) # Residual will be higher due to noise assert residual < 5.0, f"Residual {residual} unexpectedly high" @@ -211,8 +244,9 @@ def test_icp_partial_overlap(self, dense_scan): # Assertions assert converged, "ICP should handle partial overlap" - np.testing.assert_allclose(pose, true_pose, atol=0.2, - err_msg="ICP should work with partial overlap") + np.testing.assert_allclose( + pose, true_pose, atol=0.2, err_msg="ICP should work with partial overlap" + ) def test_icp_fixed_seed_reproducibility(self, simple_scan): """Test that ICP is reproducible with fixed random seed.""" @@ -229,7 +263,9 @@ def test_icp_fixed_seed_reproducibility(self, simple_scan): ) # Should be identical - np.testing.assert_array_equal(pose1, pose2, err_msg="ICP should be deterministic") + np.testing.assert_array_equal( + pose1, pose2, err_msg="ICP should be deterministic" + ) assert iters1 == iters2, "Iteration count should be identical" assert res1 == res2, "Residual should be identical" assert conv1 == conv2, "Convergence status should be identical" @@ -243,11 +279,9 @@ def test_icp_accuracy_threshold_small_noise(self): np.random.seed(100) # Generate structured scan - scan = np.array([ - [i, j] - for i in np.linspace(0, 5, 10) - for j in np.linspace(0, 5, 10) - ]) + np.random.normal(0, 0.01, (100, 2)) + scan = np.array( + [[i, j] for i in np.linspace(0, 5, 10) for j in np.linspace(0, 5, 10)] + ) + np.random.normal(0, 0.01, (100, 2)) true_pose = np.array([1.0, 1.5, np.pi / 16]) # ~11 degrees target = se2_apply(true_pose, scan) @@ -262,10 +296,14 @@ def test_icp_accuracy_threshold_small_noise(self): # Regression threshold assert converged, "Must converge" pose_error = np.linalg.norm(pose[:2] - true_pose[:2]) - assert pose_error < 0.10, f"Position error {pose_error:.4f}m exceeds 10cm threshold" + assert ( + pose_error < 0.10 + ), f"Position error {pose_error:.4f}m exceeds 10cm threshold" angle_error = np.abs(pose[2] - true_pose[2]) - assert angle_error < np.deg2rad(5), f"Angle error {np.rad2deg(angle_error):.2f}deg exceeds 5deg threshold" + assert angle_error < np.deg2rad( + 5 + ), f"Angle error {np.rad2deg(angle_error):.2f}deg exceeds 5deg threshold" def test_icp_accuracy_threshold_with_noise(self): """Regression: ICP should achieve <15cm RMSE with 5cm noise.""" @@ -279,16 +317,15 @@ def test_icp_accuracy_threshold_with_noise(self): target_clean = se2_apply(true_pose, scan) target = target_clean + np.random.normal(0, 0.05, target_clean.shape) - pose, _, _, converged = icp_point_to_point( - scan, target, max_iterations=100 - ) + pose, _, _, converged = icp_point_to_point(scan, target, max_iterations=100) # Regression threshold (more lenient due to noise) assert converged, "Must converge" pose_error = np.linalg.norm(pose[:2] - true_pose[:2]) - assert pose_error < 0.15, f"Position error {pose_error:.4f}m exceeds 15cm threshold with noise" + assert ( + pose_error < 0.15 + ), f"Position error {pose_error:.4f}m exceeds 15cm threshold with noise" if __name__ == "__main__": pytest.main([__file__, "-v"]) - diff --git a/tests/core/slam/test_icp_residual_units.py b/tests/core/slam/test_icp_residual_units.py index 713bfda..e5c1753 100644 --- a/tests/core/slam/test_icp_residual_units.py +++ b/tests/core/slam/test_icp_residual_units.py @@ -69,7 +69,9 @@ def test_residual_does_not_grow_with_the_number_of_points(self): _, _, residual, _ = icp_point_to_point(scan, target, max_iterations=50) residuals[n] = residual - self.assertAlmostEqual(residuals[100], residuals[400], delta=0.3 * residuals[100]) + self.assertAlmostEqual( + residuals[100], residuals[400], delta=0.3 * residuals[100] + ) def test_residual_tracks_the_noise_it_is_measuring(self): """It is in metres, so it should read like the displacement it sees.""" @@ -111,16 +113,19 @@ def test_ungated_icp_can_be_dragged_far_off(self): """Documents why the gate is needed, so the next test means something.""" source, target = self._partial_overlap() _, _, ungated, _ = icp_point_to_point( - source, target, max_iterations=50, max_correspondence_distance=None) + source, target, max_iterations=50, max_correspondence_distance=None + ) _, _, gated, _ = icp_point_to_point( - source, target, max_iterations=50, max_correspondence_distance=1.0) + source, target, max_iterations=50, max_correspondence_distance=1.0 + ) self.assertLess(gated, ungated) def test_gating_keeps_the_alignment_near_the_truth(self): source, target = self._partial_overlap() pose, _, residual, _ = icp_point_to_point( - source, target, max_iterations=50, max_correspondence_distance=1.0) + source, target, max_iterations=50, max_correspondence_distance=1.0 + ) self.assertLess(np.linalg.norm(pose[:2]), 1.0) self.assertLess(residual, 0.5) diff --git a/tests/core/slam/test_loop_closure_2d.py b/tests/core/slam/test_loop_closure_2d.py index 538cbb7..672bd58 100644 --- a/tests/core/slam/test_loop_closure_2d.py +++ b/tests/core/slam/test_loop_closure_2d.py @@ -97,11 +97,19 @@ def test_loop_closure_with_similar_scans(self): # Create sequence with similar scans (loop closure scenario) # Use more points for better ICP convergence - scan_template = np.array([ - [1.0, -0.5], [1.0, 0.0], [1.0, 0.5], - [2.0, -0.5], [2.0, 0.0], [2.0, 0.5], - [3.0, -0.5], [3.0, 0.0], [3.0, 0.5], - ]) + scan_template = np.array( + [ + [1.0, -0.5], + [1.0, 0.0], + [1.0, 0.5], + [2.0, -0.5], + [2.0, 0.0], + [2.0, 0.5], + [3.0, -0.5], + [3.0, 0.0], + [3.0, 0.5], + ] + ) scans = [] poses = [] @@ -113,10 +121,18 @@ def test_loop_closure_with_similar_scans(self): scans.append(scan_template + noise) else: # Different scans - scans.append(np.array([ - [i + 0.5, -0.3], [i + 0.5, 0.0], [i + 0.5, 0.3], - [i + 1.5, -0.3], [i + 1.5, 0.0], [i + 1.5, 0.3], - ])) + scans.append( + np.array( + [ + [i + 0.5, -0.3], + [i + 0.5, 0.0], + [i + 0.5, 0.3], + [i + 1.5, -0.3], + [i + 1.5, 0.0], + [i + 1.5, 0.3], + ] + ) + ) loop_closures = detector.detect(scans, poses) @@ -159,11 +175,19 @@ def test_distance_gating_disabled_allows_far_matches(self): # Create similar scans with noise np.random.seed(42) - scan_template = np.array([ - [1.0, -0.5], [1.0, 0.0], [1.0, 0.5], - [2.0, -0.5], [2.0, 0.0], [2.0, 0.5], - [3.0, -0.5], [3.0, 0.0], [3.0, 0.5], - ]) + scan_template = np.array( + [ + [1.0, -0.5], + [1.0, 0.0], + [1.0, 0.5], + [2.0, -0.5], + [2.0, 0.0], + [2.0, 0.5], + [3.0, -0.5], + [3.0, 0.0], + [3.0, 0.5], + ] + ) scans = [] for _ in range(15): @@ -236,11 +260,19 @@ def test_icp_verification_accepts_pose_parameter(self): # Create similar scans with noise np.random.seed(42) - scan_template = np.array([ - [1.0, -0.3], [1.0, 0.0], [1.0, 0.3], - [2.0, -0.3], [2.0, 0.0], [2.0, 0.3], - [3.0, -0.3], [3.0, 0.0], [3.0, 0.3], - ]) + scan_template = np.array( + [ + [1.0, -0.3], + [1.0, 0.0], + [1.0, 0.3], + [2.0, -0.3], + [2.0, 0.0], + [2.0, 0.3], + [3.0, -0.3], + [3.0, 0.0], + [3.0, 0.3], + ] + ) scans = [] for _ in range(12): @@ -330,7 +362,9 @@ def test_square_trajectory_loop_closure(self): poses.append(np.array([x, y, 0.0])) # Create scan (wall at fixed distance) - scan = np.array([[3.0, -1.0], [3.0, 0.0], [3.0, 1.0]]) + np.random.normal(0, 0.01, (3, 2)) + scan = np.array([[3.0, -1.0], [3.0, 0.0], [3.0, 1.0]]) + np.random.normal( + 0, 0.01, (3, 2) + ) scans.append(scan) loop_closures = detector.detect(scans, poses) diff --git a/tests/core/slam/test_ndt.py b/tests/core/slam/test_ndt.py index 6ef7e26..3909614 100644 --- a/tests/core/slam/test_ndt.py +++ b/tests/core/slam/test_ndt.py @@ -470,14 +470,17 @@ def test_result_does_not_depend_on_step_size(self): poses = {} for step_size in (0.05, 0.1, 0.3, 0.5, 1.0): pose, _, _, converged = ndt_align( - source, target, voxel_size=1.0, step_size=step_size, + source, + target, + voxel_size=1.0, + step_size=step_size, max_iterations=200, ) poses[step_size] = pose assert converged, f"step_size={step_size} did not converge" - assert np.linalg.norm(pose[:2] - true_pose[:2]) < 0.1, ( - f"step_size={step_size} landed at {pose}, truth {true_pose}" - ) + assert ( + np.linalg.norm(pose[:2] - true_pose[:2]) < 0.1 + ), f"step_size={step_size} landed at {pose}, truth {true_pose}" spread = max( np.linalg.norm(a[:2] - b[:2]) @@ -604,7 +607,11 @@ def test_ndt_pipeline(self): # Step 3: Run alignment final_pose, iters, final_score, converged = ndt_align( - source, target, initial_pose=initial_pose, voxel_size=2.0, max_iterations=100 + source, + target, + initial_pose=initial_pose, + voxel_size=2.0, + max_iterations=100, ) # NDT should complete without errors @@ -614,4 +621,3 @@ def test_ndt_pipeline(self): # Step 4: Compute covariance cov = ndt_covariance(source, ndt_map, final_pose, voxel_size=2.0) assert cov.shape == (3, 3) - diff --git a/tests/core/slam/test_ndt_voxel_stats.py b/tests/core/slam/test_ndt_voxel_stats.py index 35612ba..fc8313b 100644 --- a/tests/core/slam/test_ndt_voxel_stats.py +++ b/tests/core/slam/test_ndt_voxel_stats.py @@ -27,14 +27,10 @@ def test_voxel_mean_accuracy(self): # Create points clustered in specific voxels # Voxel (0, 0): points around [0.2, 0.3] - voxel_00_points = np.array([ - [0.1, 0.2], [0.2, 0.3], [0.3, 0.4], [0.15, 0.25] - ]) + voxel_00_points = np.array([[0.1, 0.2], [0.2, 0.3], [0.3, 0.4], [0.15, 0.25]]) # Voxel (1, 1): points around [1.5, 1.6] - voxel_11_points = np.array([ - [1.4, 1.5], [1.5, 1.6], [1.6, 1.7], [1.5, 1.5] - ]) + voxel_11_points = np.array([[1.4, 1.5], [1.5, 1.6], [1.6, 1.7], [1.5, 1.5]]) points = np.vstack([voxel_00_points, voxel_11_points]) @@ -45,15 +41,17 @@ def test_voxel_mean_accuracy(self): assert (0, 0) in ndt_map, "Voxel (0,0) should exist" mean_00 = ndt_map[(0, 0)]["mean"] expected_mean_00 = np.mean(voxel_00_points, axis=0) - np.testing.assert_allclose(mean_00, expected_mean_00, atol=1e-10, - err_msg="Voxel (0,0) mean incorrect") + np.testing.assert_allclose( + mean_00, expected_mean_00, atol=1e-10, err_msg="Voxel (0,0) mean incorrect" + ) # Check voxel (1, 1) assert (1, 1) in ndt_map, "Voxel (1,1) should exist" mean_11 = ndt_map[(1, 1)]["mean"] expected_mean_11 = np.mean(voxel_11_points, axis=0) - np.testing.assert_allclose(mean_11, expected_mean_11, atol=1e-10, - err_msg="Voxel (1,1) mean incorrect") + np.testing.assert_allclose( + mean_11, expected_mean_11, atol=1e-10, err_msg="Voxel (1,1) mean incorrect" + ) def test_voxel_covariance_properties(self): """Validate that voxel covariances are symmetric and positive definite.""" @@ -70,25 +68,38 @@ def test_voxel_covariance_properties(self): cov = voxel_data["cov"] # Check shape - assert cov.shape == (2, 2), f"Covariance shape incorrect for voxel {voxel_key}" + assert cov.shape == ( + 2, + 2, + ), f"Covariance shape incorrect for voxel {voxel_key}" # Check symmetry - assert np.allclose(cov, cov.T), f"Covariance not symmetric for voxel {voxel_key}" + assert np.allclose( + cov, cov.T + ), f"Covariance not symmetric for voxel {voxel_key}" # Check positive definite (eigenvalues > 0) eigvals = np.linalg.eigvals(cov) - assert np.all(eigvals > 0), f"Covariance not positive definite for voxel {voxel_key}" + assert np.all( + eigvals > 0 + ), f"Covariance not positive definite for voxel {voxel_key}" def test_voxel_filtering_min_points(self): """Test that voxels with too few points are filtered out.""" np.random.seed(456) # Create sparse points (1-2 points per voxel) - points = np.array([ - [0.1, 0.1], # Voxel (0, 0) - 1 point - [1.5, 1.5], [1.6, 1.6], # Voxel (1, 1) - 2 points - [2.5, 2.5], [2.6, 2.6], [2.7, 2.7], [2.8, 2.8], # Voxel (2, 2) - 4 points - ]) + points = np.array( + [ + [0.1, 0.1], # Voxel (0, 0) - 1 point + [1.5, 1.5], + [1.6, 1.6], # Voxel (1, 1) - 2 points + [2.5, 2.5], + [2.6, 2.6], + [2.7, 2.7], + [2.8, 2.8], # Voxel (2, 2) - 4 points + ] + ) ndt_map = build_ndt_map(points, voxel_size=1.0, min_points_per_voxel=3) @@ -109,7 +120,9 @@ def test_ndt_score_computation(self): target = scan.copy() # Compute score at identity transformation (should be good) - score = ndt_score(scan, target, np.array([0, 0, 0]), voxel_size=0.5) # Smaller voxels + score = ndt_score( + scan, target, np.array([0, 0, 0]), voxel_size=0.5 + ) # Smaller voxels # Basic sanity checks assert not np.isnan(score), "NDT score should not be NaN" @@ -133,8 +146,12 @@ def test_ndt_alignment_from_good_initial_guess(self): initial_guess = np.array([0.45, 0.45, 0.04]) pose, iters, _, converged = ndt_align( - scan, target, initial_pose=initial_guess, - voxel_size=1.0, max_iterations=50, tolerance=1e-4 + scan, + target, + initial_pose=initial_guess, + voxel_size=1.0, + max_iterations=50, + tolerance=1e-4, ) assert converged, "NDT should converge from a good initial guess" @@ -156,8 +173,12 @@ def test_ndt_convergence_iterations_threshold(self): initial_guess = np.array([0.45, 0.45, 0.04]) pose, iters, _, converged = ndt_align( - scan, target, initial_pose=initial_guess, - voxel_size=1.0, max_iterations=50, tolerance=1e-4 + scan, + target, + initial_pose=initial_guess, + voxel_size=1.0, + max_iterations=50, + tolerance=1e-4, ) # Basic functionality test: should complete without errors @@ -168,4 +189,3 @@ def test_ndt_convergence_iterations_threshold(self): if __name__ == "__main__": pytest.main([__file__, "-v"]) - diff --git a/tests/core/slam/test_pose_graph_loop_closure_smoke.py b/tests/core/slam/test_pose_graph_loop_closure_smoke.py index 8f4bbf5..1e5813e 100644 --- a/tests/core/slam/test_pose_graph_loop_closure_smoke.py +++ b/tests/core/slam/test_pose_graph_loop_closure_smoke.py @@ -73,14 +73,17 @@ def square_trajectory_data(self): visible_pts = [] for lm in landmarks: # Transform landmark to sensor frame - lm_sensor = se2_apply(np.array([-pose[0], -pose[1], -pose[2]]), - lm.reshape(1, -1))[0] + lm_sensor = se2_apply( + np.array([-pose[0], -pose[1], -pose[2]]), lm.reshape(1, -1) + )[0] dist = np.linalg.norm(lm_sensor) if dist < sensor_range: visible_pts.append(lm_sensor) if len(visible_pts) > 5: - scan = np.array(visible_pts) + np.random.normal(0, 0.02, (len(visible_pts), 2)) + scan = np.array(visible_pts) + np.random.normal( + 0, 0.02, (len(visible_pts), 2) + ) scans.append(scan) else: # Fallback: generate some points if nothing visible @@ -95,11 +98,13 @@ def square_trajectory_data(self): rel_pose_true = se2_relative(true_poses[i], true_poses[i + 1]) # Add odometry noise (drift accumulates) - noise = np.array([ - np.random.normal(0, 0.1), # x noise - np.random.normal(0, 0.1), # y noise - np.random.normal(0, 0.02), # yaw noise - ]) + noise = np.array( + [ + np.random.normal(0, 0.1), # x noise + np.random.normal(0, 0.1), # y noise + np.random.normal(0, 0.02), # yaw noise + ] + ) rel_pose_noisy = rel_pose_true + noise # Accumulate odometry @@ -143,7 +148,8 @@ def test_full_slam_pipeline_reduces_error(self, square_trajectory_data): initial_guess = se2_relative(odometry_poses[i], odometry_poses[j]) rel_pose, iters, residual, converged = icp_point_to_point( - scans[i], scans[j], + scans[i], + scans[j], initial_pose=initial_guess, max_iterations=50, tolerance=1e-5, @@ -184,7 +190,9 @@ def test_full_slam_pipeline_reduces_error(self, square_trajectory_data): # 6. Assertions (smoke test thresholds - lenient for synthetic data) # Note: Loop closures may not always be detected depending on noise and distance - assert final_error <= initial_error * 1.1, "Optimization should not significantly increase error" + assert ( + final_error <= initial_error * 1.1 + ), "Optimization should not significantly increase error" # If loop closures were found, SLAM should improve if len(loop_closures) > 0: @@ -195,7 +203,9 @@ def test_full_slam_pipeline_reduces_error(self, square_trajectory_data): else: print(" No loop closures - SLAM may not improve much") # Without loop closures, just check it doesn't break - assert slam_rmse < 1.0, f"SLAM RMSE {slam_rmse:.3f}m unexpectedly high even without loop closures" + assert ( + slam_rmse < 1.0 + ), f"SLAM RMSE {slam_rmse:.3f}m unexpectedly high even without loop closures" def test_slam_without_loop_closure_still_works(self, square_trajectory_data): """Test pose graph optimization without loop closures (odometry-only).""" @@ -219,15 +229,18 @@ def test_slam_without_loop_closure_still_works(self, square_trajectory_data): # RMSE should be similar to odometry (no loop closure correction) odom_errors = compute_position_errors(true_poses[:, :2], odometry_poses[:, :2]) odom_rmse = compute_rmse(odom_errors) - no_lc_errors = compute_position_errors(true_poses[:, :2], optimized_poses[:, :2]) + no_lc_errors = compute_position_errors( + true_poses[:, :2], optimized_poses[:, :2] + ) no_lc_rmse = compute_rmse(no_lc_errors) print(f"\n[NO LOOP CLOSURE] Odometry RMSE: {odom_rmse:.4f} m") print(f"[NO LOOP CLOSURE] Smoothed RMSE: {no_lc_rmse:.4f} m") # Should not improve much without loop closure - assert np.abs(no_lc_rmse - odom_rmse) < 0.2, \ - "Without loop closure, RMSE should stay similar to odometry" + assert ( + np.abs(no_lc_rmse - odom_rmse) < 0.2 + ), "Without loop closure, RMSE should stay similar to odometry" def test_loop_closure_impact_quantified(self, square_trajectory_data): """Quantify the specific impact of loop closures on accuracy.""" @@ -244,7 +257,10 @@ def test_loop_closure_impact_quantified(self, square_trajectory_data): if np.linalg.norm(odometry_poses[i, :2] - odometry_poses[j, :2]) < 3.0: initial_guess = se2_relative(odometry_poses[i], odometry_poses[j]) rel_pose, _, residual, converged = icp_point_to_point( - scans[i], scans[j], initial_pose=initial_guess, max_iterations=50 + scans[i], + scans[j], + initial_pose=initial_guess, + max_iterations=50, ) if converged and residual < 2.0: loop_closures.append((i, j, rel_pose)) @@ -259,7 +275,9 @@ def test_loop_closure_impact_quantified(self, square_trajectory_data): loop_closures=None, prior_pose=true_poses[0], ) - optimized_no_lc, _ = graph_no_lc.optimize(method="gauss_newton", max_iterations=30) + optimized_no_lc, _ = graph_no_lc.optimize( + method="gauss_newton", max_iterations=30 + ) poses_no_lc = np.array([optimized_no_lc[i] for i in range(len(true_poses))]) errors_no_lc = compute_position_errors(true_poses[:, :2], poses_no_lc[:, :2]) rmse_no_lc = compute_rmse(errors_no_lc) @@ -271,9 +289,13 @@ def test_loop_closure_impact_quantified(self, square_trajectory_data): loop_closures=loop_closures, prior_pose=true_poses[0], ) - optimized_with_lc, _ = graph_with_lc.optimize(method="gauss_newton", max_iterations=30) + optimized_with_lc, _ = graph_with_lc.optimize( + method="gauss_newton", max_iterations=30 + ) poses_with_lc = np.array([optimized_with_lc[i] for i in range(len(true_poses))]) - errors_with_lc = compute_position_errors(true_poses[:, :2], poses_with_lc[:, :2]) + errors_with_lc = compute_position_errors( + true_poses[:, :2], poses_with_lc[:, :2] + ) rmse_with_lc = compute_rmse(errors_with_lc) print("\n[COMPARISON]") @@ -284,8 +306,9 @@ def test_loop_closure_impact_quantified(self, square_trajectory_data): # Loop closures should provide measurable improvement assert rmse_with_lc < rmse_no_lc, "Loop closures should improve accuracy" improvement = (rmse_no_lc - rmse_with_lc) / rmse_no_lc - assert improvement > 0.1, \ - f"Loop closure improvement {improvement*100:.1f}% below 10% threshold" + assert ( + improvement > 0.1 + ), f"Loop closure improvement {improvement*100:.1f}% below 10% threshold" class TestPoseGraphRegressionThresholds: @@ -296,12 +319,14 @@ def test_slam_accuracy_regression_threshold(self): np.random.seed(9999) # Simplified square trajectory (4 poses, one per corner) - true_poses = np.array([ - [0, 0, 0], - [5, 0, np.pi / 2], - [5, 5, np.pi], - [0, 5, -np.pi / 2], - ]) + true_poses = np.array( + [ + [0, 0, 0], + [5, 0, np.pi / 2], + [5, 5, np.pi], + [0, 5, -np.pi / 2], + ] + ) # Noisy odometry odometry_poses = true_poses + np.random.normal(0, 0.2, true_poses.shape) @@ -313,12 +338,15 @@ def test_slam_accuracy_regression_threshold(self): for pose in true_poses: visible = [] for lm in landmarks: - lm_sensor = se2_apply(np.array([-pose[0], -pose[1], -pose[2]]), - lm.reshape(1, -1))[0] + lm_sensor = se2_apply( + np.array([-pose[0], -pose[1], -pose[2]]), lm.reshape(1, -1) + )[0] if np.linalg.norm(lm_sensor) < 6.0: visible.append(lm_sensor) if len(visible) > 0: - scans.append(np.array(visible) + np.random.normal(0, 0.01, (len(visible), 2))) + scans.append( + np.array(visible) + np.random.normal(0, 0.01, (len(visible), 2)) + ) else: # Fallback: add some dummy points if nothing visible scans.append(np.random.rand(10, 2) * 3 - 1.5) @@ -331,7 +359,8 @@ def test_slam_accuracy_regression_threshold(self): # Detect loop closure (0 <-> 3, closing the square) rel_pose, _, residual, converged = icp_point_to_point( - scans[0], scans[3], + scans[0], + scans[3], initial_pose=se2_relative(odometry_poses[0], odometry_poses[3]), max_iterations=50, ) @@ -353,10 +382,10 @@ def test_slam_accuracy_regression_threshold(self): slam_rmse = compute_rmse(slam_errors) # Regression threshold: SLAM should achieve <20cm RMSE - assert slam_rmse < 0.20, \ - f"SLAM RMSE {slam_rmse:.4f}m exceeds 20cm regression threshold" + assert ( + slam_rmse < 0.20 + ), f"SLAM RMSE {slam_rmse:.4f}m exceeds 20cm regression threshold" if __name__ == "__main__": pytest.main([__file__, "-v", "-s"]) - diff --git a/tests/core/slam/test_scan_generation.py b/tests/core/slam/test_scan_generation.py index 5274fa7..0fe6c35 100644 --- a/tests/core/slam/test_scan_generation.py +++ b/tests/core/slam/test_scan_generation.py @@ -65,7 +65,7 @@ def test_no_intersection_parallel(self): ) self.assertIsNone(point) - self.assertEqual(distance, float('inf')) + self.assertEqual(distance, float("inf")) def test_no_intersection_behind_ray(self): """Test segment behind ray origin (no intersection).""" @@ -79,7 +79,7 @@ def test_no_intersection_behind_ray(self): ) self.assertIsNone(point) - self.assertEqual(distance, float('inf')) + self.assertEqual(distance, float("inf")) def test_no_intersection_beyond_segment(self): """Test ray passing beyond segment endpoints.""" @@ -93,7 +93,7 @@ def test_no_intersection_beyond_segment(self): ) self.assertIsNone(point) - self.assertEqual(distance, float('inf')) + self.assertEqual(distance, float("inf")) class TestScanGenerationWithOcclusion(unittest.TestCase): @@ -163,8 +163,9 @@ def test_occlusion_by_near_obstacle(self): self.assertGreater(min_dist, 2.5, "Distance should be ~3m") # Verify we DON'T see the far wall through the obstacle - self.assertFalse(np.any(distances > 8.0), - "Should not see far wall behind obstacle") + self.assertFalse( + np.any(distances > 8.0), "Should not see far wall behind obstacle" + ) def test_pillar_blocks_multiple_walls(self): """Test that a pillar blocks walls behind it in multiple directions.""" @@ -251,8 +252,7 @@ def test_min_max_range_filtering(self): # Set max_range=10 to exclude far wall scan = generate_scan_with_occlusion( - pose, walls, num_rays=360, max_range=10.0, min_range=0.1, - noise_std=0.0 + pose, walls, num_rays=360, max_range=10.0, min_range=0.1, noise_std=0.0 ) ranges = np.linalg.norm(scan, axis=1) @@ -297,5 +297,5 @@ def test_occlusion_reduces_point_count(self): self.assertGreater(len(scan_with_occlusion), 300) # Most rays hit -if __name__ == '__main__': +if __name__ == "__main__": unittest.main() diff --git a/tests/core/slam/test_scan_matching.py b/tests/core/slam/test_scan_matching.py index 9d256df..839d29b 100644 --- a/tests/core/slam/test_scan_matching.py +++ b/tests/core/slam/test_scan_matching.py @@ -281,9 +281,7 @@ def test_with_initial_guess(self): def test_rotation_and_translation(self): """Test ICP with rotation and translation.""" # Create a distinctive pattern - source = np.array( - [[0.0, 0.0], [1.0, 0.0], [2.0, 0.0], [0.0, 1.0], [1.0, 1.0]] - ) + source = np.array([[0.0, 0.0], [1.0, 0.0], [2.0, 0.0], [0.0, 1.0], [1.0, 1.0]]) # Apply known transformation true_pose = np.array([1.0, 2.0, np.pi / 6]) # 30° rotation @@ -300,7 +298,9 @@ def test_rotation_and_translation(self): def test_max_correspondence_distance(self): """Test ICP with max correspondence distance.""" source = np.array([[0.0, 0.0], [1.0, 0.0], [2.0, 0.0]]) - target = np.array([[0.1, 0.0], [1.1, 0.0], [10.0, 10.0]]) # Last point is outlier + target = np.array( + [[0.1, 0.0], [1.1, 0.0], [10.0, 10.0]] + ) # Last point is outlier pose, iters, residual, converged = icp_point_to_point( source, target, max_correspondence_distance=1.0 @@ -421,7 +421,9 @@ def test_high_uncertainty_with_few_correspondences(self): target = np.array([[0.1, 0.0]]) # Only one point nearby pose = np.array([0.0, 0.0, 0.0]) - cov = compute_icp_covariance(source, target, pose, max_correspondence_distance=1.0) + cov = compute_icp_covariance( + source, target, pose, max_correspondence_distance=1.0 + ) # Should have high uncertainty (large diagonal values) assert cov[0, 0] > 0.1 @@ -512,4 +514,3 @@ def test_icp_partial_overlap(self): # Translation should be roughly [0.5, 0.5] assert 0.0 <= pose[0] <= 1.5 assert 0.0 <= pose[1] <= 1.5 - diff --git a/tests/core/slam/test_se2.py b/tests/core/slam/test_se2.py index f144a9d..3758969 100644 --- a/tests/core/slam/test_se2.py +++ b/tests/core/slam/test_se2.py @@ -434,4 +434,3 @@ def test_repr(self): assert "Pose2" in repr_str assert "1.2345" in repr_str assert "2.3456" in repr_str - diff --git a/tests/core/test_angle_differences_are_wrapped.py b/tests/core/test_angle_differences_are_wrapped.py index f04b78e..d487662 100644 --- a/tests/core/test_angle_differences_are_wrapped.py +++ b/tests/core/test_angle_differences_are_wrapped.py @@ -81,11 +81,11 @@ def test_difference_helpers_are_correct_across_the_branch_cut(self) -> None: for name, fn in DIFF_HELPERS.items(): for a_deg, b_deg, want_deg in self.CASES: with self.subTest(helper=name, a=a_deg, b=b_deg): - got = np.rad2deg( - fn(np.deg2rad(a_deg), np.deg2rad(b_deg)) - ) + got = np.rad2deg(fn(np.deg2rad(a_deg), np.deg2rad(b_deg))) self.assertAlmostEqual( - float(got), want_deg, places=9, + float(got), + want_deg, + places=9, msg=f"{name}({a_deg}, {b_deg}) = {got}, want {want_deg}", ) @@ -99,9 +99,11 @@ def test_single_angle_helpers_agree_with_the_difference_helpers(self) -> None: d = np.deg2rad(a_deg) - np.deg2rad(b_deg) with self.subTest(a=a_deg, b=b_deg): self.assertAlmostEqual( - float(np.rad2deg(wrap_angle(d))), want_deg, places=9) + float(np.rad2deg(wrap_angle(d))), want_deg, places=9 + ) self.assertAlmostEqual( - float(np.rad2deg(wrap_heading(d))), want_deg, places=9) + float(np.rad2deg(wrap_heading(d))), want_deg, places=9 + ) def test_the_helpers_vectorise(self) -> None: """Several call sites pass whole arrays; none may silently degrade. @@ -131,7 +133,8 @@ def test_the_naive_reductions_really_are_wrong(self) -> None: self.assertAlmostEqual(float(np.rad2deg(raw)), 358.0, places=6) self.assertAlmostEqual( float(np.rad2deg(angle_diff(np.deg2rad(179.0), np.deg2rad(-179.0)))), - -2.0, places=9, + -2.0, + places=9, ) # The ch6 form: min(|d|, 2pi - |d|), negative once |d| exceeds 2pi. @@ -139,7 +142,8 @@ def test_the_naive_reductions_really_are_wrong(self) -> None: naive = min(d, 2 * np.pi - d) self.assertLess(naive, 0.0, "the ch6 form no longer misbehaves at 725 deg") self.assertAlmostEqual( - float(np.rad2deg(abs(wrap_angle(np.deg2rad(725.0))))), 5.0, places=9) + float(np.rad2deg(abs(wrap_angle(np.deg2rad(725.0))))), 5.0, places=9 + ) if __name__ == "__main__": diff --git a/tests/core/test_imu_forward_model.py b/tests/core/test_imu_forward_model.py index 84bd44d..b5b3c64 100644 --- a/tests/core/test_imu_forward_model.py +++ b/tests/core/test_imu_forward_model.py @@ -283,9 +283,15 @@ def test_no_free_fall_planar_motion(self) -> None: radius = 5.0 omega = 1.0 # rad/s - pos_map = np.column_stack([radius * np.cos(omega * t), radius * np.sin(omega * t), np.zeros(N)]) + pos_map = np.column_stack( + [radius * np.cos(omega * t), radius * np.sin(omega * t), np.zeros(N)] + ) vel_map = np.column_stack( - [-radius * omega * np.sin(omega * t), radius * omega * np.cos(omega * t), np.zeros(N)] + [ + -radius * omega * np.sin(omega * t), + radius * omega * np.cos(omega * t), + np.zeros(N), + ] ) # Yaw follows velocity direction @@ -318,4 +324,3 @@ def test_no_free_fall_planar_motion(self) -> None: if __name__ == "__main__": # Run tests with pytest pytest.main([__file__, "-v", "-s"]) - diff --git a/tests/core/test_imu_units.py b/tests/core/test_imu_units.py index 20772c2..2bad374 100644 --- a/tests/core/test_imu_units.py +++ b/tests/core/test_imu_units.py @@ -44,7 +44,7 @@ def test_deg_per_hour_to_rad_per_sec(self): # Also check that it's approximately 0.0028 deg/s result_deg_s = np.rad2deg(result) - self.assertAlmostEqual(result_deg_s, 10.0/3600.0, places=6) + self.assertAlmostEqual(result_deg_s, 10.0 / 3600.0, places=6) def test_deg_per_hour_to_rad_per_sec_acceptance_criterion(self): """Test acceptance criterion: 10 deg/hr = 0.0028 deg/s.""" @@ -248,15 +248,3 @@ def test_arw_reasonable_values(self): if __name__ == "__main__": unittest.main() - - - - - - - - - - - - diff --git a/tests/core/test_ins_state_ordering.py b/tests/core/test_ins_state_ordering.py index c1e75c4..1e5196e 100644 --- a/tests/core/test_ins_state_ordering.py +++ b/tests/core/test_ins_state_ordering.py @@ -332,4 +332,3 @@ def test_eq616_state_ordering_documentation(self): if __name__ == "__main__": unittest.main(verbosity=2) - diff --git a/tests/core/test_pdr_peak_detection.py b/tests/core/test_pdr_peak_detection.py index 3c62a75..42580f4 100644 --- a/tests/core/test_pdr_peak_detection.py +++ b/tests/core/test_pdr_peak_detection.py @@ -55,20 +55,16 @@ def test_detect_steps_no_motion(self): N = len(t) # Constant gravity only (no motion) - accel = np.column_stack([ - np.zeros(N), - np.zeros(N), - -9.81 * np.ones(N) - ]) + accel = np.column_stack([np.zeros(N), np.zeros(N), -9.81 * np.ones(N)]) step_indices, accel_processed = detect_steps_peak_detector( - accel, dt=0.01, g=9.81, - min_peak_height=1.0, min_peak_distance=0.3 + accel, dt=0.01, g=9.81, min_peak_height=1.0, min_peak_distance=0.3 ) # Should detect zero or very few steps - self.assertLess(len(step_indices), 5, - "Stationary signal should not detect many steps") + self.assertLess( + len(step_indices), 5, "Stationary signal should not detect many steps" + ) def test_detect_steps_synthetic_walking(self): """Detect steps in synthetic walking pattern.""" @@ -81,15 +77,10 @@ def test_detect_steps_synthetic_walking(self): walking_amplitude = 2.5 # m/s² accel_z = -9.81 + walking_amplitude * np.sin(2 * np.pi * step_freq * t) - accel = np.column_stack([ - np.zeros(N), - np.zeros(N), - accel_z - ]) + accel = np.column_stack([np.zeros(N), np.zeros(N), accel_z]) step_indices, accel_processed = detect_steps_peak_detector( - accel, dt=0.01, g=9.81, - min_peak_height=1.0, min_peak_distance=0.3 + accel, dt=0.01, g=9.81, min_peak_height=1.0, min_peak_distance=0.3 ) # Expected: ~120 steps (60s * 2 Hz) @@ -98,10 +89,16 @@ def test_detect_steps_synthetic_walking(self): # Allow 50-200% of expected (detector tuning dependent) self.assertGreater(detected_steps, 0, "Should detect nonzero steps") - self.assertGreater(detected_steps, expected_steps * 0.5, - f"Too few steps: {detected_steps} < {expected_steps * 0.5}") - self.assertLess(detected_steps, expected_steps * 2.0, - f"Too many steps: {detected_steps} > {expected_steps * 2.0}") + self.assertGreater( + detected_steps, + expected_steps * 0.5, + f"Too few steps: {detected_steps} < {expected_steps * 0.5}", + ) + self.assertLess( + detected_steps, + expected_steps * 2.0, + f"Too many steps: {detected_steps} > {expected_steps * 2.0}", + ) def test_detect_steps_refractory_period(self): """Test that min_peak_distance prevents double-counting.""" @@ -111,25 +108,24 @@ def test_detect_steps_refractory_period(self): # Two wider Gaussian peaks at t=5.0s and t=5.1s (wider to survive filtering) accel_z = -9.81 + 5.0 * ( - np.exp(-((t - 5.0)**2) / 0.1) + - np.exp(-((t - 5.1)**2) / 0.1) + np.exp(-((t - 5.0) ** 2) / 0.1) + np.exp(-((t - 5.1) ** 2) / 0.1) ) - accel = np.column_stack([ - np.zeros(N), - np.zeros(N), - accel_z - ]) + accel = np.column_stack([np.zeros(N), np.zeros(N), accel_z]) # With refractory period of 0.3s, should detect only 1 step step_indices, _ = detect_steps_peak_detector( - accel, dt=0.01, g=9.81, - min_peak_height=1.0, min_peak_distance=0.3, - lowpass_cutoff=None # Disable filter to preserve sharp peaks + accel, + dt=0.01, + g=9.81, + min_peak_height=1.0, + min_peak_distance=0.3, + lowpass_cutoff=None, # Disable filter to preserve sharp peaks ) - self.assertLessEqual(len(step_indices), 1, - "Refractory period should prevent double-counting") + self.assertLessEqual( + len(step_indices), 1, "Refractory period should prevent double-counting" + ) def test_detect_steps_without_filter(self): """Test peak detection without low-pass filter.""" @@ -144,17 +140,16 @@ def test_detect_steps_without_filter(self): noise = np.random.normal(0, 0.5, N) accel_z += noise - accel = np.column_stack([ - np.zeros(N), - np.zeros(N), - accel_z - ]) + accel = np.column_stack([np.zeros(N), np.zeros(N), accel_z]) # Detect without filter (lowpass_cutoff=None) step_indices, accel_processed = detect_steps_peak_detector( - accel, dt=0.01, g=9.81, - min_peak_height=1.0, min_peak_distance=0.3, - lowpass_cutoff=None + accel, + dt=0.01, + g=9.81, + min_peak_height=1.0, + min_peak_distance=0.3, + lowpass_cutoff=None, ) # Should still detect steps (but possibly noisier) @@ -173,17 +168,16 @@ def test_detect_steps_with_filter(self): noise = 0.8 * np.sin(2 * np.pi * 20 * t) accel_z += noise - accel = np.column_stack([ - np.zeros(N), - np.zeros(N), - accel_z - ]) + accel = np.column_stack([np.zeros(N), np.zeros(N), accel_z]) # Detect with filter (5 Hz cutoff) step_indices_filtered, _ = detect_steps_peak_detector( - accel, dt=0.01, g=9.81, - min_peak_height=1.0, min_peak_distance=0.3, - lowpass_cutoff=5.0 + accel, + dt=0.01, + g=9.81, + min_peak_height=1.0, + min_peak_distance=0.3, + lowpass_cutoff=5.0, ) # Expected: ~20 steps (10s * 2 Hz) @@ -192,8 +186,11 @@ def test_detect_steps_with_filter(self): # With filtering, should be close to expected self.assertGreater(detected_steps, 0) - self.assertLess(abs(detected_steps - expected_steps) / expected_steps, 0.5, - "Filtered detection should be within 50% of expected") + self.assertLess( + abs(detected_steps - expected_steps) / expected_steps, + 0.5, + "Filtered detection should be within 50% of expected", + ) def test_detect_steps_tunable_sensitivity(self): """Test that min_peak_height controls sensitivity.""" @@ -206,27 +203,24 @@ def test_detect_steps_tunable_sensitivity(self): walking_amplitude = 1.0 accel_z = -9.81 + walking_amplitude * np.sin(2 * np.pi * step_freq * t) - accel = np.column_stack([ - np.zeros(N), - np.zeros(N), - accel_z - ]) + accel = np.column_stack([np.zeros(N), np.zeros(N), accel_z]) # High threshold (2.0 m/s²) - should detect few/no steps steps_high_thresh, _ = detect_steps_peak_detector( - accel, dt=0.01, g=9.81, - min_peak_height=2.0, min_peak_distance=0.3 + accel, dt=0.01, g=9.81, min_peak_height=2.0, min_peak_distance=0.3 ) # Low threshold (0.5 m/s²) - should detect steps steps_low_thresh, _ = detect_steps_peak_detector( - accel, dt=0.01, g=9.81, - min_peak_height=0.5, min_peak_distance=0.3 + accel, dt=0.01, g=9.81, min_peak_height=0.5, min_peak_distance=0.3 ) # Low threshold should detect more steps - self.assertGreaterEqual(len(steps_low_thresh), len(steps_high_thresh), - "Lower threshold should detect more steps") + self.assertGreaterEqual( + len(steps_low_thresh), + len(steps_high_thresh), + "Lower threshold should detect more steps", + ) def test_detect_steps_input_validation(self): """Test input validation for detect_steps_peak_detector.""" @@ -249,4 +243,3 @@ def test_detect_steps_input_validation(self): if __name__ == "__main__": unittest.main() - diff --git a/tests/core/test_strapdown_stationary_imu.py b/tests/core/test_strapdown_stationary_imu.py index b9c22a2..c969906 100644 --- a/tests/core/test_strapdown_stationary_imu.py +++ b/tests/core/test_strapdown_stationary_imu.py @@ -371,4 +371,3 @@ def test_gravity_compensation_enu_vs_ned(self) -> None: if __name__ == "__main__": # Run tests with pytest pytest.main([__file__, "-v", "-s"]) - diff --git a/tests/docs/__init__.py b/tests/docs/__init__.py index 73756d7..63c63a2 100644 --- a/tests/docs/__init__.py +++ b/tests/docs/__init__.py @@ -8,5 +8,3 @@ test_ch6_examples.py - Tests Ch6 dataset README code blocks (future) test_ch8_examples.py - Tests Ch8 dataset README code blocks """ - - diff --git a/tests/docs/test_chapter_architecture_sections.py b/tests/docs/test_chapter_architecture_sections.py index ffd7bb9..8b5cd5f 100644 --- a/tests/docs/test_chapter_architecture_sections.py +++ b/tests/docs/test_chapter_architecture_sections.py @@ -103,7 +103,9 @@ def test_repo_section_matches_the_code(): """The top-level README's chapter-to-core map, same contract.""" text = (REPO_ROOT / "README.md").read_text(encoding="utf-8") blocks = _sections(text, REPO_BEGIN_MARKER, REPO_END_MARKER) - assert len(blocks) == 1, f"README.md should carry one generated section, found {len(blocks)}" + assert ( + len(blocks) == 1 + ), f"README.md should carry one generated section, found {len(blocks)}" expected = render_repo_section(chapter_dependencies(REPO_ROOT)) assert REPO_BEGIN_MARKER + blocks[0] + REPO_END_MARKER == expected, ( @@ -112,7 +114,9 @@ def test_repo_section_matches_the_code(): ) -@pytest.mark.parametrize("path", _markdown_files(), ids=lambda p: p.parent.name or "root") +@pytest.mark.parametrize( + "path", _markdown_files(), ids=lambda p: p.parent.name or "root" +) def test_mermaid_blocks_declare_a_diagram_type(path): """A Mermaid block that opens with a typo renders as an error box, not a diagram.""" for index, block in enumerate(_mermaid_blocks(path.read_text(encoding="utf-8"))): @@ -123,7 +127,9 @@ def test_mermaid_blocks_declare_a_diagram_type(path): ) -@pytest.mark.parametrize("path", _markdown_files(), ids=lambda p: p.parent.name or "root") +@pytest.mark.parametrize( + "path", _markdown_files(), ids=lambda p: p.parent.name or "root" +) def test_mermaid_blocks_name_real_paths(path): """Every repository path a diagram label names has to exist. @@ -140,9 +146,9 @@ def test_mermaid_blocks_name_real_paths(path): target = REPO_ROOT / name.rstrip("/") if not target.exists(): missing.append(name) - assert not missing, ( - f"{path.relative_to(REPO_ROOT)} draws paths that do not exist: {sorted(set(missing))}" - ) + assert ( + not missing + ), f"{path.relative_to(REPO_ROOT)} draws paths that do not exist: {sorted(set(missing))}" def test_the_deleted_diagram_directory_stays_deleted(): diff --git a/tests/docs/test_chapter_table_equation_ranges.py b/tests/docs/test_chapter_table_equation_ranges.py index 31ff536..27ac536 100644 --- a/tests/docs/test_chapter_table_equation_ranges.py +++ b/tests/docs/test_chapter_table_equation_ranges.py @@ -94,9 +94,10 @@ def test_chapter_equation_span_matches_the_index(chapter): (low_ch, low_n), (high_ch, high_n) = _table_rows()[chapter] indexed_low, indexed_high = _indexed_spans()[chapter] - assert (low_ch, high_ch) == (chapter, chapter), ( - f"Ch{chapter}'s row advertises equations from chapter {low_ch}-{high_ch}." - ) + assert (low_ch, high_ch) == ( + chapter, + chapter, + ), f"Ch{chapter}'s row advertises equations from chapter {low_ch}-{high_ch}." assert (low_n, high_n) == (indexed_low, indexed_high), ( f"README says Ch{chapter} covers Eqs. {chapter}.{low_n}-{chapter}.{high_n}, " f"but docs/equation_index.yml maps Eqs. {chapter}.{indexed_low}-" diff --git a/tests/docs/test_design_doc_is_historical.py b/tests/docs/test_design_doc_is_historical.py index cfef835..1fc5da9 100644 --- a/tests/docs/test_design_doc_is_historical.py +++ b/tests/docs/test_design_doc_is_historical.py @@ -59,8 +59,7 @@ # --- core/estimators: the solvers are functions, not step helpers ------- "gauss_newton_solve": "core.estimators.nonlinear_least_squares.gauss_newton", "gauss_newton_step": "core.estimators.nonlinear_least_squares.gauss_newton", - "levenberg_marquardt_step": - "core.estimators.nonlinear_least_squares.levenberg_marquardt", + "levenberg_marquardt_step": "core.estimators.nonlinear_least_squares.levenberg_marquardt", "solve_fgo": "core.estimators.factor_graph.FactorGraph.optimize", "gradient_descent_step": None, # Factor's interface was named differently in the end. @@ -72,8 +71,7 @@ "aoa_bearing": "core.rf.measurement_models.aoa_azimuth", "simulate_rf_measurements": None, # --- core/sensors ------------------------------------------------------- - "ZaruMeasurementModel": - "core.sensors.constraints.ZaruMeasurementModelPlaceholder", + "ZaruMeasurementModel": "core.sensors.constraints.ZaruMeasurementModelPlaceholder", "InsWheelProcessModel": None, "WheelSpeedMeasurementModel": None, # --- core/sim: the trajectory generators live in scripts/ instead ------- @@ -93,8 +91,7 @@ "raytrace_likelihood": None, # --- outside core/ ------------------------------------------------------ "run_multisensor_ekf": None, - "generate_fusion_2d_imu_uwb_dataset": - "scripts/generate_ch8_fusion_2d_imu_uwb_dataset.py", + "generate_fusion_2d_imu_uwb_dataset": "scripts/generate_ch8_fusion_2d_imu_uwb_dataset.py", } #: Names the extraction picks up that are not API claims. @@ -125,9 +122,7 @@ def _core_names(): for path in sorted(REPO_ROOT.glob("core/**/*.py")): tree = ast.parse(path.read_text(encoding="utf-8")) for node in ast.walk(tree): - if isinstance( - node, (ast.FunctionDef, ast.AsyncFunctionDef, ast.ClassDef) - ): + if isinstance(node, (ast.FunctionDef, ast.AsyncFunctionDef, ast.ClassDef)): names.add(node.name) return names @@ -148,9 +143,7 @@ def _doc_api_names(text): """ found = set(re.findall(r"^\s*def\s+(\w+)\s*\(", text, re.M)) found |= set(re.findall(r"^\s*class\s+(\w+)", text, re.M)) - found |= set( - re.findall(r"^\s*(\w+)\([^)]*\)\s*(?:->|[-–—]|$)", text, re.M) - ) + found |= set(re.findall(r"^\s*(\w+)\([^)]*\)\s*(?:->|[-–—]|$)", text, re.M)) return {n for n in found if not n.startswith("_")} - NOT_API_CLAIMS @@ -221,9 +214,9 @@ def test_superseded_module_register_is_exact(): for old, new in SUPERSEDED_MODULES.items(): if new is not None: - assert (REPO_ROOT / new).exists(), ( - f"SUPERSEDED_MODULES maps {old} -> {new}, which does not exist." - ) + assert ( + REPO_ROOT / new + ).exists(), f"SUPERSEDED_MODULES maps {old} -> {new}, which does not exist." def test_nothing_cites_a_design_doc_section_that_does_not_exist(): diff --git a/tests/docs/test_documented_paths_exist.py b/tests/docs/test_documented_paths_exist.py index ef54042..29ee877 100644 --- a/tests/docs/test_documented_paths_exist.py +++ b/tests/docs/test_documented_paths_exist.py @@ -96,7 +96,9 @@ def test_documented_python_paths_resolve(document): assert not missing, ( f"{_name(document)} cites {len(missing)} path(s) that do not exist:\n " - + "\n ".join(f"{p} (line {', '.join(map(str, ls))})" for p, ls in missing.items()) + + "\n ".join( + f"{p} (line {', '.join(map(str, ls))})" for p, ls in missing.items() + ) + "\n\nA renamed file is the usual cause -- point the document at the new " "name rather than deleting the line. If the document names the file as " "future work the reader is expected to write, say so in the prose and " @@ -145,7 +147,9 @@ def test_documented_bare_filenames_resolve(document): assert not missing, ( f"{_name(document)} names {len(missing)} file(s) that do not exist:\n " - + "\n ".join(f"{n} (line {', '.join(map(str, ls))})" for n, ls in missing.items()) + + "\n ".join( + f"{n} (line {', '.join(map(str, ls))})" for n, ls in missing.items() + ) + "\n\nUsually a rename the document did not follow. Point it at the " "current name; if the file is future work, say so and list it in " "ASPIRATIONAL." @@ -182,7 +186,9 @@ def test_documented_modules_resolve(document): assert not missing, ( f"{_name(document)} runs {len(missing)} module(s) that do not exist:\n " - + "\n ".join(f"{m} (line {', '.join(map(str, ls))})" for m, ls in missing.items()) + + "\n ".join( + f"{m} (line {', '.join(map(str, ls))})" for m, ls in missing.items() + ) + "\n\nIf the feature was never built, remove the worked example rather " "than leaving a command the reader cannot run." ) diff --git a/tests/docs/test_examples_run_from_chapter_dir.py b/tests/docs/test_examples_run_from_chapter_dir.py index 4f10096..b4b8349 100644 --- a/tests/docs/test_examples_run_from_chapter_dir.py +++ b/tests/docs/test_examples_run_from_chapter_dir.py @@ -44,10 +44,11 @@ #: One dataset-reading example per chapter that has one, with arguments that #: force the dataset path to be exercised rather than the inline-data branch. DATASET_EXAMPLES = [ - ("ch2_coords.example_coordinate_transforms", - ("--data", "ch2_coords_san_francisco")), - ("ch3_estimators.example_ekf_range_bearing", - ("--data", "ch3_estimator_nonlinear")), + ( + "ch2_coords.example_coordinate_transforms", + ("--data", "ch2_coords_san_francisco"), + ), + ("ch3_estimators.example_ekf_range_bearing", ("--data", "ch3_estimator_nonlinear")), # --compare-geometry is the silent-skip path: three hardcoded dataset names, # each `continue`d past with a message if the path does not resolve. ("ch4_rf_point_positioning.example_comparison", ("--compare-geometry",)), @@ -88,9 +89,8 @@ def test_example_finds_its_dataset_from_its_chapter_directory(module, args): f"{module} ran from its chapter directory and reported {found} -- its " "dataset did not resolve. Dataset paths must go through " "core.utils.resolve_data_path, which tries the working directory and " - "then the repository root; a bare Path(\"data/sim\") / name only works " - "when the reader happens to be standing at the root.\n\n" - + output[-2000:] + 'then the repository root; a bare Path("data/sim") / name only works ' + "when the reader happens to be standing at the root.\n\n" + output[-2000:] ) @@ -109,6 +109,5 @@ def test_the_same_example_is_clean_from_the_repository_root(module, args): assert run.process.returncode == 0, output[-2000:] assert not [m for m in FAILURE_MARKERS if m in output], ( f"{module} does not resolve its dataset even from the repository root, " - "so the chapter-directory check next door proves nothing.\n\n" - + output[-2000:] + "so the chapter-directory check next door proves nothing.\n\n" + output[-2000:] ) diff --git a/tests/docs/test_readme_example_output.py b/tests/docs/test_readme_example_output.py index cbff4f9..ce35b1f 100644 --- a/tests/docs/test_readme_example_output.py +++ b/tests/docs/test_readme_example_output.py @@ -104,9 +104,7 @@ (re.compile(r"\S*[/\\]figs[/\\]?\S*"), ""), ] -MARKER = re.compile( - r"^\s*$", re.M -) +MARKER = re.compile(r"^\s*$", re.M) ELISION = "..." WILDCARD = "~" @@ -268,7 +266,7 @@ def test_readme_transcript_is_what_the_example_prints(case): if hit >= 0: cursor = hit + 1 else: - nearby = "\n ".join(live[max(0, cursor - 2):cursor + 8]) + nearby = "\n ".join(live[max(0, cursor - 2) : cursor + 8]) pytest.fail( f"{readme.relative_to(REPO_ROOT).as_posix()} shows a line that " f"`python -m {module} {' '.join(args)}`.strip() no longer " @@ -281,8 +279,9 @@ def test_readme_transcript_is_what_the_example_prints(case): ) -@pytest.mark.parametrize("readme", sorted(REPO_ROOT.glob("ch*_*/README.md")), - ids=lambda p: p.parent.name) +@pytest.mark.parametrize( + "readme", sorted(REPO_ROOT.glob("ch*_*/README.md")), ids=lambda p: p.parent.name +) def test_unmarked_transcripts_match_the_register(readme): """No new unchecked transcript blocks.""" chapter = readme.parent.name diff --git a/tests/docs/test_readme_file_structure.py b/tests/docs/test_readme_file_structure.py index 1189f85..bbf7e06 100644 --- a/tests/docs/test_readme_file_structure.py +++ b/tests/docs/test_readme_file_structure.py @@ -132,8 +132,12 @@ def test_file_structure_lists_every_example(chapter): @pytest.mark.parametrize("chapter", _chapters(), ids=lambda p: p.name) def test_file_structure_entries_exist(chapter): """Every file the tree names resolves, at both levels of nesting.""" - absent = [name for name in _entries(_block(chapter)) if not (REPO_ROOT / name).exists()] - assert not absent, f"{chapter.name}/README.md's File Structure names absent files: {absent}" + absent = [ + name for name in _entries(_block(chapter)) if not (REPO_ROOT / name).exists() + ] + assert ( + not absent + ), f"{chapter.name}/README.md's File Structure names absent files: {absent}" def test_the_parser_reads_the_shapes_that_fooled_it(): @@ -161,9 +165,9 @@ def test_the_parser_reads_the_shapes_that_fooled_it(): assert "ch7_slam/example_pose_graph_slam.py" in entries assert "ch7_slam/figs/slam_with_maps.png" in entries, "nested entry lost" assert ".dev/ch7_prompts_1-6_COMPLETE.md" in entries, "hyphenated name truncated" - assert not any("ch7_prompt*" in e or e == ".dev/ch7_prompt" for e in entries), ( - "a glob was truncated into a claim about a concrete file" - ) - assert EXAMPLE.findall(block) == ["example_pose_graph_slam.py"], ( - "test_example_*.py must not read as an example" - ) + assert not any( + "ch7_prompt*" in e or e == ".dev/ch7_prompt" for e in entries + ), "a glob was truncated into a claim about a concrete file" + assert EXAMPLE.findall(block) == [ + "example_pose_graph_slam.py" + ], "test_example_*.py must not read as an example" diff --git a/tests/example_runner.py b/tests/example_runner.py index 7aa8384..51ffc5e 100644 --- a/tests/example_runner.py +++ b/tests/example_runner.py @@ -87,24 +87,26 @@ def run_example(module: str, *args: str, cwd: str = None) -> ExampleRun: figs_root = _figs_root() env = os.environ.copy() - env.update({ - "MPLBACKEND": "Agg", - "PYTHONPATH": str(WORKSPACE_ROOT), - # Send the figures to scratch. Without this these tests rewrite the - # tracked chX_*/figs/*, so every run manufactured a diff -- which is - # why slam_with_maps.png kept turning up in git status. The examples - # still name their own chX/figs; core.eval mirrors in-repo writes under - # this root, preserving the repo-relative path. - "IPIN_FIGS_DIR": str(figs_root), - # Decode the child's stdout the same way on every machine. The examples - # legitimately print U+00B0 (tests/test_example_console_encoding.py - # keeps it legal on a legacy console), but on a CJK Windows host the - # child encodes it as a cp950 byte, which arrives here as U+FFFD -- - # while on the Ubuntu runner it arrives as the degree sign. Any test - # comparing this text against a file in the repo would then pass on CI - # and fail locally, for a reason that has nothing to do with the code. - "PYTHONIOENCODING": "utf-8", - }) + env.update( + { + "MPLBACKEND": "Agg", + "PYTHONPATH": str(WORKSPACE_ROOT), + # Send the figures to scratch. Without this these tests rewrite the + # tracked chX_*/figs/*, so every run manufactured a diff -- which is + # why slam_with_maps.png kept turning up in git status. The examples + # still name their own chX/figs; core.eval mirrors in-repo writes under + # this root, preserving the repo-relative path. + "IPIN_FIGS_DIR": str(figs_root), + # Decode the child's stdout the same way on every machine. The examples + # legitimately print U+00B0 (tests/test_example_console_encoding.py + # keeps it legal on a legacy console), but on a CJK Windows host the + # child encodes it as a cp950 byte, which arrives here as U+FFFD -- + # while on the Ubuntu runner it arrives as the degree sign. Any test + # comparing this text against a file in the repo would then pass on CI + # and fail locally, for a reason that has nothing to do with the code. + "PYTHONIOENCODING": "utf-8", + } + ) started_at = time.time() process = subprocess.run( diff --git a/tests/test_datasets_reproduce_from_their_recipe.py b/tests/test_datasets_reproduce_from_their_recipe.py index 1fc0964..a88a02c 100644 --- a/tests/test_datasets_reproduce_from_their_recipe.py +++ b/tests/test_datasets_reproduce_from_their_recipe.py @@ -65,46 +65,73 @@ #: null`` and that is not an omission. RECIPES = { "ch2_coords_san_francisco": ( - "generate_ch2_coordinate_transforms_dataset.py", ["--preset", "san_francisco"]), + "generate_ch2_coordinate_transforms_dataset.py", + ["--preset", "san_francisco"], + ), "ch3_estimator_nonlinear": ( - "generate_ch3_estimator_comparison_dataset.py", ["--preset", "nonlinear"]), + "generate_ch3_estimator_comparison_dataset.py", + ["--preset", "nonlinear"], + ), "ch3_estimator_high_nonlinear": ( - "generate_ch3_estimator_comparison_dataset.py", ["--preset", "high_nonlinearity"]), + "generate_ch3_estimator_comparison_dataset.py", + ["--preset", "high_nonlinearity"], + ), "ch4_rf_2d_square": ( - "generate_ch4_rf_2d_positioning_dataset.py", ["--preset", "baseline"]), + "generate_ch4_rf_2d_positioning_dataset.py", + ["--preset", "baseline"], + ), "ch4_rf_2d_optimal": ( - "generate_ch4_rf_2d_positioning_dataset.py", ["--preset", "optimal"]), + "generate_ch4_rf_2d_positioning_dataset.py", + ["--preset", "optimal"], + ), "ch4_rf_2d_linear": ( - "generate_ch4_rf_2d_positioning_dataset.py", ["--preset", "poor_geometry"]), + "generate_ch4_rf_2d_positioning_dataset.py", + ["--preset", "poor_geometry"], + ), "ch4_rf_2d_nlos": ( - "generate_ch4_rf_2d_positioning_dataset.py", ["--preset", "nlos"]), + "generate_ch4_rf_2d_positioning_dataset.py", + ["--preset", "nlos"], + ), "ch5_wifi_fingerprint_grid": ( - "generate_ch5_wifi_fingerprint_dataset.py", ["--preset", "baseline"]), + "generate_ch5_wifi_fingerprint_dataset.py", + ["--preset", "baseline"], + ), "ch5_wifi_fingerprint_dense": ( - "generate_ch5_wifi_fingerprint_dataset.py", ["--preset", "dense"]), + "generate_ch5_wifi_fingerprint_dataset.py", + ["--preset", "dense"], + ), "ch5_wifi_fingerprint_sparse": ( - "generate_ch5_wifi_fingerprint_dataset.py", ["--preset", "sparse"]), + "generate_ch5_wifi_fingerprint_dataset.py", + ["--preset", "sparse"], + ), "ch6_env_sensors_heading_altitude": ( - "generate_ch6_env_sensors_dataset.py", ["--preset", "baseline"]), + "generate_ch6_env_sensors_dataset.py", + ["--preset", "baseline"], + ), "ch6_foot_zupt_walk": ("generate_ch6_zupt_dataset.py", []), - "ch6_pdr_corridor_walk": ( - "generate_ch6_pdr_dataset.py", ["--preset", "baseline"]), + "ch6_pdr_corridor_walk": ("generate_ch6_pdr_dataset.py", ["--preset", "baseline"]), "ch6_strapdown_basic": ("generate_ch6_strapdown_dataset.py", []), "ch6_wheel_odom_square": ( - "generate_ch6_wheel_odom_dataset.py", ["--preset", "baseline"]), - "ch7_slam_2d_square": ( - "generate_ch7_slam_2d_dataset.py", ["--preset", "baseline"]), + "generate_ch6_wheel_odom_dataset.py", + ["--preset", "baseline"], + ), + "ch7_slam_2d_square": ("generate_ch7_slam_2d_dataset.py", ["--preset", "baseline"]), "ch7_slam_2d_high_drift": ( - "generate_ch7_slam_2d_dataset.py", ["--preset", "high_drift"]), + "generate_ch7_slam_2d_dataset.py", + ["--preset", "high_drift"], + ), "ch8_fusion_2d_imu_uwb": ("generate_ch8_fusion_2d_imu_uwb_dataset.py", []), # Explicit flags, not --preset nlos_severe: that preset is the 1.5 m bias # case and this dataset is the 0.8 m one. This is the invocation its own # README documents, and the one --all-variants uses. "ch8_fusion_2d_imu_uwb_nlos": ( "generate_ch8_fusion_2d_imu_uwb_dataset.py", - ["--nlos-anchors", "1", "2", "--nlos-bias", "0.8"]), + ["--nlos-anchors", "1", "2", "--nlos-bias", "0.8"], + ), "ch8_fusion_2d_imu_uwb_timeoffset": ( - "generate_ch8_fusion_2d_imu_uwb_dataset.py", ["--preset", "time_offset_50ms"]), + "generate_ch8_fusion_2d_imu_uwb_dataset.py", + ["--preset", "time_offset_50ms"], + ), } @@ -188,16 +215,28 @@ def test_regenerating_reproduces_the_shipped_arrays(dataset, tmp_path): shipped = DATA / dataset proc = subprocess.run( - [sys.executable, str(REPO_ROOT / "scripts" / generator), - *extra, "--output", str(tmp_path)], - capture_output=True, text=True, encoding="utf-8", errors="replace", - timeout=900, cwd=REPO_ROOT, - env={**__import__("os").environ, - "PYTHONIOENCODING": "utf-8", "MPLBACKEND": "Agg"}, - ) - assert proc.returncode == 0, ( - f"regenerating {dataset} failed:\n{proc.stdout[-1500:]}\n{proc.stderr[-1500:]}" + [ + sys.executable, + str(REPO_ROOT / "scripts" / generator), + *extra, + "--output", + str(tmp_path), + ], + capture_output=True, + text=True, + encoding="utf-8", + errors="replace", + timeout=900, + cwd=REPO_ROOT, + env={ + **__import__("os").environ, + "PYTHONIOENCODING": "utf-8", + "MPLBACKEND": "Agg", + }, ) + assert ( + proc.returncode == 0 + ), f"regenerating {dataset} failed:\n{proc.stdout[-1500:]}\n{proc.stderr[-1500:]}" # --output must win over --preset. It did not always: a preset overwrote it # unconditionally, so passing both regenerated the *shipped* dataset in diff --git a/tests/test_examples_import_this_checkout.py b/tests/test_examples_import_this_checkout.py index bf66138..2e21d82 100644 --- a/tests/test_examples_import_this_checkout.py +++ b/tests/test_examples_import_this_checkout.py @@ -130,9 +130,7 @@ def test_the_check_reads_the_shapes_it_has_to_distinguish(): # Bootstrap after the import: present, useless, and must not pass. reversed_order = ast.parse( - "import sys\n" - "from core.eval import save_figure\n" - "sys.path.insert(0, '.')\n" + "import sys\n" "from core.eval import save_figure\n" "sys.path.insert(0, '.')\n" ) assert _bootstrap_lines(reversed_order)[0] > _first_core_import(reversed_order) diff --git a/tests/test_jacobians.py b/tests/test_jacobians.py index 2b31799..550e1d6 100644 --- a/tests/test_jacobians.py +++ b/tests/test_jacobians.py @@ -17,23 +17,19 @@ ConstantVelocity2D, RangeMeasurement2D, RangeBearingMeasurement2D, - PositionMeasurement2D + PositionMeasurement2D, ) -def numerical_jacobian( - f: Callable, - x: np.ndarray, - epsilon: float = 1e-7 -) -> np.ndarray: +def numerical_jacobian(f: Callable, x: np.ndarray, epsilon: float = 1e-7) -> np.ndarray: """ Compute Jacobian numerically using central differences. - + Args: f: Function that takes x and returns y x: Point at which to compute Jacobian epsilon: Step size for finite differences - + Returns: Numerical Jacobian, shape (len(y), len(x)) """ @@ -81,14 +77,18 @@ def test_constant_velocity_1d_jacobian(self): F_analytical = model.F(dt) # Numerical Jacobian - def f(x_): return model.f(x_, dt=dt) + def f(x_): + return model.f(x_, dt=dt) + F_numerical = numerical_jacobian(f, x) # Compare np.testing.assert_allclose( - F_analytical, F_numerical, - rtol=1e-5, atol=1e-8, - err_msg=f"Jacobian mismatch at x={x}" + F_analytical, + F_numerical, + rtol=1e-5, + atol=1e-8, + err_msg=f"Jacobian mismatch at x={x}", ) def test_constant_velocity_2d_jacobian(self): @@ -107,14 +107,18 @@ def test_constant_velocity_2d_jacobian(self): F_analytical = model.F(dt) # Numerical - def f(x_): return model.f(x_, dt=dt) + def f(x_): + return model.f(x_, dt=dt) + F_numerical = numerical_jacobian(f, x) # Compare np.testing.assert_allclose( - F_analytical, F_numerical, - rtol=1e-5, atol=1e-8, - err_msg=f"Jacobian mismatch at x={x}" + F_analytical, + F_numerical, + rtol=1e-5, + atol=1e-8, + err_msg=f"Jacobian mismatch at x={x}", ) def test_motion_jacobian_shapes(self): @@ -135,18 +139,13 @@ class TestMeasurementModelJacobians: def test_range_measurement_jacobian(self): """Test range-only measurement Jacobian.""" - anchors = np.array([ - [0.0, 0.0], - [10.0, 0.0], - [10.0, 10.0], - [0.0, 10.0] - ]) + anchors = np.array([[0.0, 0.0], [10.0, 0.0], [10.0, 10.0], [0.0, 10.0]]) model = RangeMeasurement2D(anchors) test_states = [ np.array([5.0, 5.0, 1.0, 0.5]), # Center np.array([2.0, 3.0, 0.0, 0.0]), # Off-center - np.array([8.0, 7.0, -1.0, 1.5]), # Another point + np.array([8.0, 7.0, -1.0, 1.5]), # Another point ] for x in test_states: @@ -154,23 +153,23 @@ def test_range_measurement_jacobian(self): H_analytical = model.H(x) # Numerical - def h(x_): return model.h(x_) + def h(x_): + return model.h(x_) + H_numerical = numerical_jacobian(h, x) # Compare np.testing.assert_allclose( - H_analytical, H_numerical, - rtol=1e-4, atol=1e-7, - err_msg=f"Range Jacobian mismatch at x={x}" + H_analytical, + H_numerical, + rtol=1e-4, + atol=1e-7, + err_msg=f"Range Jacobian mismatch at x={x}", ) def test_range_bearing_measurement_jacobian(self): """Test range-bearing measurement Jacobian.""" - landmarks = np.array([ - [0.0, 0.0], - [20.0, 0.0], - [20.0, 20.0] - ]) + landmarks = np.array([[0.0, 0.0], [20.0, 0.0], [20.0, 20.0]]) model = RangeBearingMeasurement2D(landmarks) test_states = [ @@ -184,15 +183,19 @@ def test_range_bearing_measurement_jacobian(self): H_analytical = model.H(x) # Numerical - def h(x_): return model.h(x_) + def h(x_): + return model.h(x_) + H_numerical = numerical_jacobian(h, x) # Compare # Note: Bearing Jacobians can be sensitive, so slightly higher tolerance np.testing.assert_allclose( - H_analytical, H_numerical, - rtol=1e-3, atol=1e-6, - err_msg=f"Range-Bearing Jacobian mismatch at x={x}" + H_analytical, + H_numerical, + rtol=1e-3, + atol=1e-6, + err_msg=f"Range-Bearing Jacobian mismatch at x={x}", ) def test_position_measurement_jacobian(self): @@ -209,14 +212,18 @@ def test_position_measurement_jacobian(self): H_analytical = model.H(x) # Numerical - def h(x_): return model.h(x_) + def h(x_): + return model.h(x_) + H_numerical = numerical_jacobian(h, x) # Compare np.testing.assert_allclose( - H_analytical, H_numerical, - rtol=1e-5, atol=1e-8, - err_msg=f"Position Jacobian mismatch at x={x}" + H_analytical, + H_numerical, + rtol=1e-5, + atol=1e-8, + err_msg=f"Position Jacobian mismatch at x={x}", ) def test_measurement_jacobian_shapes(self): @@ -258,7 +265,9 @@ def test_range_at_anchor_singularity(self): assert np.allclose(H[0, :], 0.0), "Expected zero Jacobian at singularity" # Second row should be normal - assert not np.allclose(H[1, :], 0.0), "Non-singular measurement should have non-zero Jacobian" + assert not np.allclose( + H[1, :], 0.0 + ), "Non-singular measurement should have non-zero Jacobian" def test_range_bearing_at_landmark_singularity(self): """Test range-bearing at landmark position.""" @@ -340,11 +349,9 @@ def test_process_noise_scaling(self): Q2 = ConstantVelocity1D.Q(dt, q2) # Q should scale linearly with q - assert np.allclose(Q2, q2/q1 * Q1), "Q should scale linearly with q" + assert np.allclose(Q2, q2 / q1 * Q1), "Q should scale linearly with q" if __name__ == "__main__": # Run tests with pytest pytest.main([__file__, "-v", "--tb=short"]) - - diff --git a/tests/test_lint_debt_only_shrinks.py b/tests/test_lint_debt_only_shrinks.py index 6e4142e..c0bab41 100644 --- a/tests/test_lint_debt_only_shrinks.py +++ b/tests/test_lint_debt_only_shrinks.py @@ -8,28 +8,31 @@ black 237 of 288 files would be reformatted mypy 404 errors in core/ alone +Black passes now, and ruff is at 951. mypy is untouched at 406 errors in +core/, and is the honest remaining gap. + A reader who followed that section got thousands of complaints and reasonably concluded they had broken something. The README says what is true now; this holds the ruff half of it so the number cannot quietly grow back. -**Most of the original number was whitespace and is gone.** 4737 W293 (a blank -line containing spaces) and 131 W291 accounted for 83% of it. Ruff fixed 3961 -of them, and every one of the 3961 changed lines was verified to differ only by -trailing whitespace before the change was believed. +**Whitespace was 83% of the original number and is gone**, in two passes that +are worth telling apart. `ruff --fix` cleared 3961 of the 4868 W291/W293 and +refused the rest, because they sat inside string literals where whitespace is +content rather than layout. Running black then cleared 889 of the remaining +907 -- **black knows which triple-quoted strings are docstrings**, and +normalises those, where ruff could only see a string. A tool declining an +unsafe fix was right; the answer was a tool that could tell the difference, not +`--unsafe-fixes`. -**Ruff declined to fix the rest, and it was right to.** The 907 that remain sit -inside docstrings, where the whitespace is string content rather than layout -- -and in this repository that content is *printed*, because every example now -passes `description=__doc__` to argparse. A tool refusing an unsafe fix is not -an obstacle to work around with `--unsafe-fixes`. +The twelve W293 still here sit in argparse `epilog=` strings, which are not +docstrings and whose blank lines are printed. Black leaves them, correctly. **What is left is mostly not lint at all.** 727 of the remaining findings are UP006/UP045/UP035/UP007 -- `List[int]` for `list[int]`, `Optional[X]` for `X | None`. Those became legal only when the floor moved to 3.10, they are -mechanical, and they are worth doing in their own change rather than smuggled -into this one. The ~200 after that are the ones with actual content: 41 `zip()` -calls without `strict=`, which truncate silently to the shorter argument, are -the interesting ones. +mechanical, and they are worth doing in their own change. The ~140 after that +are the ones with actual content: 41 `zip()` calls without `strict=`, which +truncate silently to the shorter argument, are the interesting ones. Per-rule rather than a single total on purpose: a total lets ten fixed W293 pay for ten new B905, which is the opposite of what a ratchet is for. @@ -61,22 +64,23 @@ #: Findings per rule today. Only ever edit these downwards. #: -#: W293/W291 are docstring whitespace ruff will not safely fix, see the module -#: docstring. UP0xx are the annotation modernisations that the 3.10 floor made -#: available. B905 is the one group worth reading before fixing: `zip()` without -#: `strict=` truncates to the shorter argument without saying so. +#: UP0xx are the annotation modernisations that the 3.10 floor made available, +#: and are the bulk of what is left. B905 is the one group worth reading before +#: fixing: `zip()` without `strict=` truncates to the shorter argument without +#: saying so. The twelve remaining W293 sit inside argparse `epilog` strings, +#: where the whitespace is content that gets printed rather than layout -- +#: black leaves those alone, correctly, and so should you. BASELINE = { - "W293": 831, "UP006": 386, "UP045": 178, "UP035": 123, "I001": 84, - "W291": 70, "B905": 41, "UP007": 38, "B007": 28, "B028": 14, "E712": 13, + "W293": 12, "E731": 7, "B904": 6, "E741": 4, diff --git a/tests/test_python_floor_matches_syntax.py b/tests/test_python_floor_matches_syntax.py index 3154397..32dd088 100644 --- a/tests/test_python_floor_matches_syntax.py +++ b/tests/test_python_floor_matches_syntax.py @@ -45,9 +45,7 @@ REPO_ROOT = Path(__file__).resolve().parent.parent #: Names whose subscription is PEP 585, new in 3.9. -PEP585_NAMES = frozenset( - {"list", "dict", "tuple", "set", "frozenset", "type"} -) +PEP585_NAMES = frozenset({"list", "dict", "tuple", "set", "frozenset", "type"}) #: Directories that are not this package's source. EXEMPT_DIRS = (".git", ".claude", ".dev", "node_modules", "build", "dist") diff --git a/tests/test_repo_conventions.py b/tests/test_repo_conventions.py index cbde6a7..a26951a 100644 --- a/tests/test_repo_conventions.py +++ b/tests/test_repo_conventions.py @@ -346,7 +346,9 @@ def _bare_default_rng_lines(source: str): if not isinstance(node, ast.Call): continue func = node.func - name = func.attr if isinstance(func, ast.Attribute) else getattr(func, "id", None) + name = ( + func.attr if isinstance(func, ast.Attribute) else getattr(func, "id", None) + ) if name == "default_rng" and not node.args and not node.keywords: hits.append(node.lineno) return hits @@ -527,7 +529,7 @@ def test_no_test_functions_outside_the_tests_tree(source): assert not offenders, ( f"{relative} defines {len(offenders)} test_-prefixed function(s) that " - f"pytest never collects, because testpaths = [\"tests\"]: " + f'pytest never collects, because testpaths = ["tests"]: ' f"{', '.join(sorted(offenders))}. Rename to check_* or demo_* if it is a " f"by-hand self-check, or move it under tests/ if it is a real test." ) @@ -581,7 +583,7 @@ def test_preset_does_not_overwrite_an_explicit_output(script): assert not offenders, ( f"{relative} assigns output_dir a bare literal ({listed}), which " f"discards an explicit --output and rewrites the shipped dataset " - f"instead. Use `output_dir = output_dir or \"...\"` so the preset only " + f'instead. Use `output_dir = output_dir or "..."` so the preset only ' f"supplies a default, and make sure --output itself defaults to None." ) @@ -617,9 +619,9 @@ def test_core_library_takes_its_randomness_from_the_caller(module): # themselves and are covered by the uncollected-tests ratchet above. demo_lines = set() for node in ast.walk(tree): - if isinstance(node, (ast.FunctionDef, ast.AsyncFunctionDef)) and node.name.startswith( - ("check_", "demo_") - ): + if isinstance( + node, (ast.FunctionDef, ast.AsyncFunctionDef) + ) and node.name.startswith(("check_", "demo_")): for child in ast.walk(node): if hasattr(child, "lineno"): demo_lines.add(child.lineno) @@ -723,7 +725,8 @@ def test_chapter_directories_hold_only_examples(chapter): assert not offenders, ( f"{chapter.name} holds {len(offenders)} file(s) that are neither " - f"example_*.py nor __init__.py:\n " + "\n ".join(offenders) + f"example_*.py nor __init__.py:\n " + + "\n ".join(offenders) + "\n\nName a runnable demo example_.py so `ls " "chX/example_*.py` reports it. Move anything importable into core/." ) diff --git a/tools/__init__.py b/tools/__init__.py index 0f357e7..0c8f88d 100644 --- a/tools/__init__.py +++ b/tools/__init__.py @@ -1,3 +1 @@ # Tools for repository maintenance and CI/CD - - diff --git a/tools/chapter_dependencies.py b/tools/chapter_dependencies.py index a6b3190..92ddc60 100644 --- a/tools/chapter_dependencies.py +++ b/tools/chapter_dependencies.py @@ -117,7 +117,9 @@ def chapter_dependencies(root: Path | None = None) -> dict[str, dict[str, dict]] def _packages(per_example: dict[str, dict]) -> list[str]: """Distinct `core.` names the chapter depends on.""" - return sorted({".".join(m.split(".")[:2]) for v in per_example.values() for m in v["core"]}) + return sorted( + {".".join(m.split(".")[:2]) for v in per_example.values() for m in v["core"]} + ) def render_section(chapter: str, per_example: dict[str, dict]) -> str: @@ -170,7 +172,9 @@ def render_section(chapter: str, per_example: dict[str, dict]) -> str: return "\n".join(lines) -REPO_BEGIN_MARKER = "" +REPO_BEGIN_MARKER = ( + "" +) REPO_END_MARKER = "" # A package this many chapters or more import is shown as prose, not as edges. @@ -214,7 +218,9 @@ def render_repo_section(deps: dict[str, dict[str, dict]]) -> str: distinctive = [p for p in _packages(per) if p not in cross_cutting] topic = CHAPTER_TOPICS.get(chapter, chapter) target = "
".join(p.replace("core.", "core/") + "/" for p in distinctive) - lines.append(f' A{index}["{chapter}/
{topic}"] --> B{index}["{target}"]') + lines.append( + f' A{index}["{chapter}/
{topic}"] --> B{index}["{target}"]' + ) lines += [ ' S["imported by nearly every chapter
' + " · ".join(p.replace("core.", "core/") + "/" for p in sorted(cross_cutting)) @@ -253,14 +259,18 @@ def rewrite_readmes(root: Path | None = None) -> list[str]: text = readme.read_text(encoding="utf-8") if BEGIN_MARKER not in text: continue - new = _replace_between(text, BEGIN_MARKER, END_MARKER, render_section(chapter, per_example)) + new = _replace_between( + text, BEGIN_MARKER, END_MARKER, render_section(chapter, per_example) + ) if new != text: readme.write_text(new, encoding="utf-8") touched.append(str(readme.relative_to(root))) top = root / "README.md" text = top.read_text(encoding="utf-8") if REPO_BEGIN_MARKER in text: - new = _replace_between(text, REPO_BEGIN_MARKER, REPO_END_MARKER, render_repo_section(deps)) + new = _replace_between( + text, REPO_BEGIN_MARKER, REPO_END_MARKER, render_repo_section(deps) + ) if new != text: top.write_text(new, encoding="utf-8") touched.append("README.md") diff --git a/tools/check_all_datasets.py b/tools/check_all_datasets.py index 0582a03..11e7218 100644 --- a/tools/check_all_datasets.py +++ b/tools/check_all_datasets.py @@ -80,10 +80,10 @@ def extract_code_blocks(readme_path: Path) -> List[Tuple[str, str, int]]: for match in matches: code = match.group(1) - line_number = content[:match.start()].count("\n") + 1 + line_number = content[: match.start()].count("\n") + 1 # Find section name (look backwards for ## heading) - before = content[:match.start()] + before = content[: match.start()] section_matches = re.findall(r"##\s+(.+)", before) if section_matches: current_section = section_matches[-1].strip() @@ -146,7 +146,9 @@ def check_code_block(code: str, dataset_path: Path, verbose: bool = False) -> Di Dict with test results. """ # Create temporary script - with tempfile.NamedTemporaryFile(mode="w", suffix=".py", delete=False, encoding="utf-8") as f: + with tempfile.NamedTemporaryFile( + mode="w", suffix=".py", delete=False, encoding="utf-8" + ) as f: # Add necessary imports and setup test_code = f""" import sys @@ -260,13 +262,17 @@ def check_dataset(dataset_path: Path, verbose: bool = False) -> Dict: for i, (code, section, line_num) in enumerate(code_blocks, 1): if not is_runnable_code(code): if verbose: - print(f" [{i}/{len(code_blocks)}] Skipping snippet in '{section}' (not runnable)") + print( + f" [{i}/{len(code_blocks)}] Skipping snippet in '{section}' (not runnable)" + ) continue results["code_blocks_runnable"] += 1 if verbose: - print(f" [{i}/{len(code_blocks)}] Testing code in '{section}' (line {line_num})...") + print( + f" [{i}/{len(code_blocks)}] Testing code in '{section}' (line {line_num})..." + ) test_result = check_code_block(code, dataset_path, verbose) @@ -358,7 +364,9 @@ def main(): "7": ["ch7_slam"], "8": ["ch8_fusion"], } - datasets_to_test = {k: v for k, v in DATASETS.items() if k in chapter_map[args.chapter]} + datasets_to_test = { + k: v for k, v in DATASETS.items() if k in chapter_map[args.chapter] + } else: datasets_to_test = DATASETS @@ -388,13 +396,10 @@ def main(): # Exit with error code if any tests failed total_failed = sum( - 1 for results in all_results.values() - for result in results - if result["issues"] + 1 for results in all_results.values() for result in results if result["issues"] ) sys.exit(0 if total_failed == 0 else 1) if __name__ == "__main__": main() - diff --git a/tools/check_equation_index.py b/tools/check_equation_index.py index 5670497..3c1a98d 100644 --- a/tools/check_equation_index.py +++ b/tools/check_equation_index.py @@ -27,6 +27,7 @@ # Try to import yaml, fall back to basic parsing if not available try: import yaml + HAS_YAML = True except ImportError: HAS_YAML = False @@ -45,10 +46,10 @@ def find_project_root() -> Path: def parse_equation_index(index_path: Path) -> List[Dict]: """Parse the equation_index.yml file. - + Args: index_path: Path to equation_index.yml - + Returns: List of equation entries from the YAML file """ @@ -73,15 +74,15 @@ def parse_equation_index(index_path: Path) -> List[Dict]: def find_equation_references_in_code(root: Path) -> Dict[str, List[Tuple[str, int]]]: """Find all equation references in Python source files. - + Searches for patterns like: - "Eq. (2.1)" - "Eqs. (3.1)-(3.3)" - "Reference: Eq. (4.5)" - + Args: root: Project root directory - + Returns: Dict mapping equation IDs to list of (file_path, line_number) tuples """ @@ -89,11 +90,11 @@ def find_equation_references_in_code(root: Path) -> Dict[str, List[Tuple[str, in # Patterns to search for patterns = [ - r'Eq\.\s*\((\d+\.\d+)\)', # Eq. (2.1) - r'Eqs\.\s*\((\d+\.\d+)\)', # Eqs. (3.1) - captures first - r'Equation\s*\((\d+\.\d+)\)', # Equation (2.1) + r"Eq\.\s*\((\d+\.\d+)\)", # Eq. (2.1) + r"Eqs\.\s*\((\d+\.\d+)\)", # Eqs. (3.1) - captures first + r"Equation\s*\((\d+\.\d+)\)", # Equation (2.1) ] - combined_pattern = '|'.join(patterns) + combined_pattern = "|".join(patterns) # Search in core/ and ch*/ directories search_dirs = [root / "core"] + list(root.glob("ch*_*")) @@ -105,7 +106,7 @@ def find_equation_references_in_code(root: Path) -> Dict[str, List[Tuple[str, in for py_file in search_dir.rglob("*.py"): try: content = py_file.read_text(encoding="utf-8") - for line_num, line in enumerate(content.split('\n'), 1): + for line_num, line in enumerate(content.split("\n"), 1): for match in re.finditer(combined_pattern, line): eq_num = match.group(1) or match.group(2) or match.group(3) if eq_num: @@ -122,10 +123,10 @@ def find_equation_references_in_code(root: Path) -> Dict[str, List[Tuple[str, in def extract_equations_from_index(entries: List[Dict]) -> Set[str]: """Extract equation IDs from parsed index entries. - + Args: entries: List of equation entries from YAML - + Returns: Set of equation IDs (e.g., {"Eq. (2.1)", "Eq. (2.2)", ...}) """ @@ -142,11 +143,11 @@ def extract_equations_from_index(entries: List[Dict]) -> Set[str]: def check_file_paths(entries: List[Dict], root: Path) -> List[str]: """Check that file paths in the index exist. - + Args: entries: List of equation entries from YAML root: Project root directory - + Returns: List of error messages for missing files """ @@ -159,7 +160,9 @@ def check_file_paths(entries: List[Dict], root: Path) -> List[str]: if isinstance(file_info, dict): path = file_info.get("path", "") if path and not (root / path).exists(): - errors.append(f"Missing file: {path} (referenced by {entry.get('eq', 'unknown')})") + errors.append( + f"Missing file: {path} (referenced by {entry.get('eq', 'unknown')})" + ) return errors @@ -198,9 +201,7 @@ class that was since renamed or removed still looks green while documenting if file_path not in cache: try: - cache[file_path] = ast.parse( - file_path.read_text(encoding="utf-8") - ) + cache[file_path] = ast.parse(file_path.read_text(encoding="utf-8")) except (OSError, SyntaxError): cache[file_path] = None tree = cache[file_path] @@ -226,9 +227,10 @@ def _resolve_object(tree: ast.Module, dotted_name: str) -> bool: for segment in dotted_name.split("."): for node in scope: - if isinstance( - node, (ast.FunctionDef, ast.AsyncFunctionDef, ast.ClassDef) - ) and node.name == segment: + if ( + isinstance(node, (ast.FunctionDef, ast.AsyncFunctionDef, ast.ClassDef)) + and node.name == segment + ): scope = list(getattr(node, "body", [])) break if isinstance(node, ast.Assign) and any( @@ -318,17 +320,15 @@ def main(): parser = argparse.ArgumentParser( description="Check equation index consistency", formatter_class=argparse.RawDescriptionHelpFormatter, - epilog=__doc__ + epilog=__doc__, ) parser.add_argument( - "--verbose", "-v", - action="store_true", - help="Show detailed output" + "--verbose", "-v", action="store_true", help="Show detailed output" ) parser.add_argument( "--strict", action="store_true", - help="Fail if any equations in code are not in index" + help="Fail if any equations in code are not in index", ) args = parser.parse_args() @@ -426,11 +426,15 @@ def main(): print() # Determine exit code - if args.strict and (missing_from_index or path_errors or object_errors or unverified): + if args.strict and ( + missing_from_index or path_errors or object_errors or unverified + ): print("[FAILED] (strict mode)") return 1 elif path_errors or object_errors or unverified: - print("[WARNING] (unresolved file paths, object references, or unverified equations)") + print( + "[WARNING] (unresolved file paths, object references, or unverified equations)" + ) return 0 else: print("[PASSED]") @@ -439,4 +443,3 @@ def main(): if __name__ == "__main__": sys.exit(main()) - diff --git a/tools/compare_fusion_variants.py b/tools/compare_fusion_variants.py index 3999714..ee1763b 100644 --- a/tools/compare_fusion_variants.py +++ b/tools/compare_fusion_variants.py @@ -29,29 +29,31 @@ def load_fusion_dataset(dataset_path: Path) -> Dict: """Load a fusion dataset from directory. - + Args: dataset_path: Path to dataset directory. - + Returns: Dictionary with 'truth', 'imu', 'uwb', 'anchors', 'config' keys. """ data = {} - data['name'] = dataset_path.name - data['truth'] = dict(np.load(dataset_path / 'truth.npz')) - data['imu'] = dict(np.load(dataset_path / 'imu.npz')) - data['uwb'] = dict(np.load(dataset_path / 'uwb_ranges.npz')) - data['anchors'] = np.load(dataset_path / 'uwb_anchors.npy') + data["name"] = dataset_path.name + data["truth"] = dict(np.load(dataset_path / "truth.npz")) + data["imu"] = dict(np.load(dataset_path / "imu.npz")) + data["uwb"] = dict(np.load(dataset_path / "uwb_ranges.npz")) + data["anchors"] = np.load(dataset_path / "uwb_anchors.npy") - with open(dataset_path / 'config.json') as f: - data['config'] = json.load(f) + with open(dataset_path / "config.json") as f: + data["config"] = json.load(f) return data -def compare_trajectories(datasets: List[Dict], output_file: str = None, show: bool = False): +def compare_trajectories( + datasets: List[Dict], output_file: str = None, show: bool = False +): """Compare trajectories from multiple datasets. - + Args: datasets: List of dataset dictionaries. output_file: Output file path (None = display only). @@ -59,36 +61,49 @@ def compare_trajectories(datasets: List[Dict], output_file: str = None, show: bo """ fig, ax = plt.subplots(figsize=(12, 10)) - colors = ['blue', 'red', 'green', 'orange', 'purple', 'brown'] + colors = ["blue", "red", "green", "orange", "purple", "brown"] # Plot trajectories for i, data in enumerate(datasets): - truth = data['truth'] + truth = data["truth"] color = colors[i % len(colors)] - ax.plot(truth['p_xy'][:, 0], truth['p_xy'][:, 1], - '-', color=color, linewidth=2, alpha=0.7, - label=data['name']) + ax.plot( + truth["p_xy"][:, 0], + truth["p_xy"][:, 1], + "-", + color=color, + linewidth=2, + alpha=0.7, + label=data["name"], + ) # Plot anchors (from first dataset, assuming same for all) - anchors = datasets[0]['anchors'] - ax.plot(anchors[:, 0], anchors[:, 1], - 'k^', markersize=15, label='UWB Anchors', zorder=5) + anchors = datasets[0]["anchors"] + ax.plot( + anchors[:, 0], anchors[:, 1], "k^", markersize=15, label="UWB Anchors", zorder=5 + ) for i, anchor in enumerate(anchors): - ax.text(anchor[0], anchor[1] + 1, f'A{i}', - ha='center', fontsize=11, fontweight='bold') - - ax.set_xlabel('East (m)', fontsize=13) - ax.set_ylabel('North (m)', fontsize=13) - ax.set_title('Trajectory Comparison', fontsize=15, fontweight='bold') + ax.text( + anchor[0], + anchor[1] + 1, + f"A{i}", + ha="center", + fontsize=11, + fontweight="bold", + ) + + ax.set_xlabel("East (m)", fontsize=13) + ax.set_ylabel("North (m)", fontsize=13) + ax.set_title("Trajectory Comparison", fontsize=15, fontweight="bold") ax.legend(fontsize=11) ax.grid(True, alpha=0.3) - ax.axis('equal') + ax.axis("equal") plt.tight_layout() if output_file: - plt.savefig(output_file, dpi=150, bbox_inches='tight') + plt.savefig(output_file, dpi=150, bbox_inches="tight") print(f"Saved: {output_file}") if show: @@ -97,41 +112,44 @@ def compare_trajectories(datasets: List[Dict], output_file: str = None, show: bo plt.close() -def compare_range_errors(datasets: List[Dict], output_file: str = None, show: bool = False): +def compare_range_errors( + datasets: List[Dict], output_file: str = None, show: bool = False +): """Compare range error distributions across datasets. - + Args: datasets: List of dataset dictionaries. output_file: Output file path. show: If True, display plot interactively. """ n_datasets = len(datasets) - fig, axes = plt.subplots(n_datasets, 4, figsize=(16, 4*n_datasets)) + fig, axes = plt.subplots(n_datasets, 4, figsize=(16, 4 * n_datasets)) if n_datasets == 1: axes = axes.reshape(1, -1) for row, data in enumerate(datasets): - truth = data['truth'] - uwb = data['uwb'] - anchors = data['anchors'] - config = data['config'] + truth = data["truth"] + uwb = data["uwb"] + anchors = data["anchors"] + config = data["config"] - t_uwb = uwb['t'] - ranges = uwb['ranges'] - nlos_anchors = config.get('uwb', {}).get('nlos_anchors', []) + t_uwb = uwb["t"] + ranges = uwb["ranges"] + nlos_anchors = config.get("uwb", {}).get("nlos_anchors", []) # Interpolate truth to UWB timestamps - p_xy_uwb = np.column_stack([ - np.interp(t_uwb, truth['t'], truth['p_xy'][:, 0]), - np.interp(t_uwb, truth['t'], truth['p_xy'][:, 1]) - ]) + p_xy_uwb = np.column_stack( + [ + np.interp(t_uwb, truth["t"], truth["p_xy"][:, 0]), + np.interp(t_uwb, truth["t"], truth["p_xy"][:, 1]), + ] + ) # Compute true ranges - ranges_true = np.array([ - np.linalg.norm(p_xy_uwb - anchor, axis=1) - for anchor in anchors - ]).T + ranges_true = np.array( + [np.linalg.norm(p_xy_uwb - anchor, axis=1) for anchor in anchors] + ).T for col in range(4): ax = axes[row, col] @@ -140,33 +158,48 @@ def compare_range_errors(datasets: List[Dict], output_file: str = None, show: bo errors = ranges_i[valid_mask] - ranges_true[valid_mask, col] - color = 'red' if col in nlos_anchors else 'blue' - nlos_label = ' (NLOS)' if col in nlos_anchors else '' + color = "red" if col in nlos_anchors else "blue" + nlos_label = " (NLOS)" if col in nlos_anchors else "" - ax.hist(errors, bins=50, alpha=0.7, edgecolor='black', color=color) - ax.axvline(0, color='k', linestyle='--', linewidth=2) - ax.axvline(np.mean(errors), color='r', linestyle='-', - linewidth=2, label=f'μ={np.mean(errors):.3f}m') + ax.hist(errors, bins=50, alpha=0.7, edgecolor="black", color=color) + ax.axvline(0, color="k", linestyle="--", linewidth=2) + ax.axvline( + np.mean(errors), + color="r", + linestyle="-", + linewidth=2, + label=f"μ={np.mean(errors):.3f}m", + ) - ax.set_xlabel('Range Error (m)', fontsize=9) - ax.set_ylabel('Count', fontsize=9) + ax.set_xlabel("Range Error (m)", fontsize=9) + ax.set_ylabel("Count", fontsize=9) if row == 0: - ax.set_title(f'Anchor {col}{nlos_label}', fontsize=11, fontweight='bold') + ax.set_title( + f"Anchor {col}{nlos_label}", fontsize=11, fontweight="bold" + ) if col == 0: - ax.text(-0.15, 0.5, data['name'], transform=ax.transAxes, - fontsize=11, fontweight='bold', rotation=90, - va='center', ha='right') + ax.text( + -0.15, + 0.5, + data["name"], + transform=ax.transAxes, + fontsize=11, + fontweight="bold", + rotation=90, + va="center", + ha="right", + ) ax.legend(fontsize=8) - ax.grid(True, alpha=0.3, axis='y') + ax.grid(True, alpha=0.3, axis="y") - fig.suptitle('Range Error Comparison', fontsize=16, fontweight='bold') + fig.suptitle("Range Error Comparison", fontsize=16, fontweight="bold") plt.tight_layout() if output_file: - plt.savefig(output_file, dpi=150, bbox_inches='tight') + plt.savefig(output_file, dpi=150, bbox_inches="tight") print(f"Saved: {output_file}") if show: @@ -175,61 +208,71 @@ def compare_range_errors(datasets: List[Dict], output_file: str = None, show: bo plt.close() -def compare_imu_noise(datasets: List[Dict], output_file: str = None, show: bool = False): +def compare_imu_noise( + datasets: List[Dict], output_file: str = None, show: bool = False +): """Compare IMU noise characteristics across datasets. - + Args: datasets: List of dataset dictionaries. output_file: Output file path. show: If True, display plot interactively. """ n_datasets = len(datasets) - fig, axes = plt.subplots(n_datasets, 3, figsize=(15, 4*n_datasets), sharex=True) + fig, axes = plt.subplots(n_datasets, 3, figsize=(15, 4 * n_datasets), sharex=True) if n_datasets == 1: axes = axes.reshape(1, -1) for row, data in enumerate(datasets): - imu = data['imu'] - t = imu['t'] - accel_xy = imu['accel_xy'] - gyro_z = imu['gyro_z'] + imu = data["imu"] + t = imu["t"] + accel_xy = imu["accel_xy"] + gyro_z = imu["gyro_z"] # Accelerometer X - axes[row, 0].plot(t, accel_xy[:, 0], 'b-', linewidth=0.3, alpha=0.7) - axes[row, 0].set_ylabel('Accel X (m/s²)', fontsize=9) + axes[row, 0].plot(t, accel_xy[:, 0], "b-", linewidth=0.3, alpha=0.7) + axes[row, 0].set_ylabel("Accel X (m/s²)", fontsize=9) if row == 0: - axes[row, 0].set_title('Accelerometer X', fontsize=11, fontweight='bold') + axes[row, 0].set_title("Accelerometer X", fontsize=11, fontweight="bold") if row == 0: - axes[row, 0].text(-0.15, 0.5, data['name'], transform=axes[row, 0].transAxes, - fontsize=11, fontweight='bold', rotation=90, - va='center', ha='right') + axes[row, 0].text( + -0.15, + 0.5, + data["name"], + transform=axes[row, 0].transAxes, + fontsize=11, + fontweight="bold", + rotation=90, + va="center", + ha="right", + ) axes[row, 0].grid(True, alpha=0.3) # Accelerometer Y - axes[row, 1].plot(t, accel_xy[:, 1], 'g-', linewidth=0.3, alpha=0.7) - axes[row, 1].set_ylabel('Accel Y (m/s²)', fontsize=9) + axes[row, 1].plot(t, accel_xy[:, 1], "g-", linewidth=0.3, alpha=0.7) + axes[row, 1].set_ylabel("Accel Y (m/s²)", fontsize=9) if row == 0: - axes[row, 1].set_title('Accelerometer Y', fontsize=11, fontweight='bold') + axes[row, 1].set_title("Accelerometer Y", fontsize=11, fontweight="bold") axes[row, 1].grid(True, alpha=0.3) # Gyroscope Z - axes[row, 2].plot(t, gyro_z, 'r-', linewidth=0.3, alpha=0.7) - axes[row, 2].set_ylabel('Gyro Z (rad/s)', fontsize=9) + axes[row, 2].plot(t, gyro_z, "r-", linewidth=0.3, alpha=0.7) + axes[row, 2].set_ylabel("Gyro Z (rad/s)", fontsize=9) if row == 0: - axes[row, 2].set_title('Gyroscope Z', fontsize=11, fontweight='bold') + axes[row, 2].set_title("Gyroscope Z", fontsize=11, fontweight="bold") axes[row, 2].grid(True, alpha=0.3) if row == n_datasets - 1: - axes[row, 0].set_xlabel('Time (s)', fontsize=10) - axes[row, 1].set_xlabel('Time (s)', fontsize=10) - axes[row, 2].set_xlabel('Time (s)', fontsize=10) + axes[row, 0].set_xlabel("Time (s)", fontsize=10) + axes[row, 1].set_xlabel("Time (s)", fontsize=10) + axes[row, 2].set_xlabel("Time (s)", fontsize=10) - fig.suptitle('IMU Measurements Comparison', fontsize=16, fontweight='bold') + fig.suptitle("IMU Measurements Comparison", fontsize=16, fontweight="bold") plt.tight_layout() if output_file: - plt.savefig(output_file, dpi=150, bbox_inches='tight') + plt.savefig(output_file, dpi=150, bbox_inches="tight") print(f"Saved: {output_file}") if show: @@ -240,13 +283,13 @@ def compare_imu_noise(datasets: List[Dict], output_file: str = None, show: bool def print_comparison_summary(datasets: List[Dict]): """Print summary comparison table of datasets. - + Args: datasets: List of dataset dictionaries. """ - print("\n" + "="*80) + print("\n" + "=" * 80) print("DATASET COMPARISON SUMMARY") - print("="*80 + "\n") + print("=" * 80 + "\n") print(f"{'Parameter':<30} ", end="") for data in datasets: @@ -293,7 +336,7 @@ def print_comparison_summary(datasets: List[Dict]): print(f"{'NLOS anchors':<30} ", end="") for data in datasets: - nlos = data['config']['uwb'].get('nlos_anchors', []) + nlos = data["config"]["uwb"].get("nlos_anchors", []) print(f"{str(nlos):<22}", end="") print() @@ -310,17 +353,17 @@ def print_comparison_summary(datasets: List[Dict]): # Temporal calibration print(f"{'Time offset (ms)':<30} ", end="") for data in datasets: - offset = data['config']['temporal_calibration'].get('time_offset_sec', 0.0) + offset = data["config"]["temporal_calibration"].get("time_offset_sec", 0.0) print(f"{offset*1000:<22.1f}", end="") print() print(f"{'Clock drift (ppm)':<30} ", end="") for data in datasets: - drift = data['config']['temporal_calibration'].get('clock_drift', 0.0) + drift = data["config"]["temporal_calibration"].get("clock_drift", 0.0) print(f"{drift*1e6:<22.1f}", end="") print() - print("="*80 + "\n") + print("=" * 80 + "\n") def main(): @@ -353,35 +396,30 @@ def main(): data/sim/ch8_fusion_2d_imu_uwb \ data/sim/ch8_fusion_2d_imu_uwb_nlos \ --show - """ + """, ) parser.add_argument( - 'datasets', - type=str, - nargs='+', - help='Paths to dataset directories to compare' + "datasets", type=str, nargs="+", help="Paths to dataset directories to compare" ) parser.add_argument( - '--output', + "--output", type=str, - default='comparison', - help='Output file prefix (default: comparison)' + default="comparison", + help="Output file prefix (default: comparison)", ) parser.add_argument( - '--format', + "--format", type=str, - choices=['svg', 'png', 'pdf'], - default='svg', - help='Output format (default: svg)' + choices=["svg", "png", "pdf"], + default="svg", + help="Output format (default: svg)", ) parser.add_argument( - '--show', - action='store_true', - help='Display plots interactively' + "--show", action="store_true", help="Display plots interactively" ) args = parser.parse_args() @@ -425,5 +463,3 @@ def main(): if __name__ == "__main__": main() - - diff --git a/tools/plot_fusion_dataset.py b/tools/plot_fusion_dataset.py index 9986f9b..9d11511 100644 --- a/tools/plot_fusion_dataset.py +++ b/tools/plot_fusion_dataset.py @@ -23,48 +23,48 @@ def load_fusion_dataset(dataset_path: Path) -> Dict: """Load a fusion dataset from directory. - + Args: dataset_path: Path to dataset directory. - + Returns: Dictionary with 'truth', 'imu', 'uwb', 'anchors', 'config' keys. """ data = {} # Load ground truth - truth_file = dataset_path / 'truth.npz' + truth_file = dataset_path / "truth.npz" if truth_file.exists(): - data['truth'] = dict(np.load(truth_file)) + data["truth"] = dict(np.load(truth_file)) else: raise FileNotFoundError(f"truth.npz not found in {dataset_path}") # Load IMU - imu_file = dataset_path / 'imu.npz' + imu_file = dataset_path / "imu.npz" if imu_file.exists(): - data['imu'] = dict(np.load(imu_file)) + data["imu"] = dict(np.load(imu_file)) else: raise FileNotFoundError(f"imu.npz not found in {dataset_path}") # Load UWB ranges - uwb_file = dataset_path / 'uwb_ranges.npz' + uwb_file = dataset_path / "uwb_ranges.npz" if uwb_file.exists(): - data['uwb'] = dict(np.load(uwb_file)) + data["uwb"] = dict(np.load(uwb_file)) else: raise FileNotFoundError(f"uwb_ranges.npz not found in {dataset_path}") # Load UWB anchors - anchors_file = dataset_path / 'uwb_anchors.npy' + anchors_file = dataset_path / "uwb_anchors.npy" if anchors_file.exists(): - data['anchors'] = np.load(anchors_file) + data["anchors"] = np.load(anchors_file) else: raise FileNotFoundError(f"uwb_anchors.npy not found in {dataset_path}") # Load config - config_file = dataset_path / 'config.json' + config_file = dataset_path / "config.json" if config_file.exists(): with open(config_file) as f: - data['config'] = json.load(f) + data["config"] = json.load(f) else: raise FileNotFoundError(f"config.json not found in {dataset_path}") @@ -73,88 +73,123 @@ def load_fusion_dataset(dataset_path: Path) -> Dict: def plot_trajectory(data: Dict, ax: Optional[plt.Axes] = None) -> plt.Axes: """Plot 2D trajectory with anchors. - + Args: data: Dataset dictionary from load_fusion_dataset. ax: Optional existing axes (creates new if None). - + Returns: Matplotlib axes object. """ if ax is None: fig, ax = plt.subplots(figsize=(10, 8)) - truth = data['truth'] - anchors = data['anchors'] - config = data['config'] + truth = data["truth"] + anchors = data["anchors"] + config = data["config"] # Plot trajectory - ax.plot(truth['p_xy'][:, 0], truth['p_xy'][:, 1], - 'b-', linewidth=2, label='Ground Truth', alpha=0.8) + ax.plot( + truth["p_xy"][:, 0], + truth["p_xy"][:, 1], + "b-", + linewidth=2, + label="Ground Truth", + alpha=0.8, + ) # Mark start and end - ax.plot(truth['p_xy'][0, 0], truth['p_xy'][0, 1], - 'go', markersize=12, label='Start', zorder=10) - ax.plot(truth['p_xy'][-1, 0], truth['p_xy'][-1, 1], - 'ro', markersize=12, label='End', zorder=10) + ax.plot( + truth["p_xy"][0, 0], + truth["p_xy"][0, 1], + "go", + markersize=12, + label="Start", + zorder=10, + ) + ax.plot( + truth["p_xy"][-1, 0], + truth["p_xy"][-1, 1], + "ro", + markersize=12, + label="End", + zorder=10, + ) # Plot anchors - nlos_anchors = config.get('uwb', {}).get('nlos_anchors', []) + nlos_anchors = config.get("uwb", {}).get("nlos_anchors", []) for i, anchor in enumerate(anchors): if i in nlos_anchors: - color = 'red' - marker = '^' - label = f'Anchor {i} (NLOS)' if i == nlos_anchors[0] else None + color = "red" + marker = "^" + label = f"Anchor {i} (NLOS)" if i == nlos_anchors[0] else None else: - color = 'green' - marker = '^' - label = f'Anchor {i} (Clean)' if i == 0 or (i == 1 and not nlos_anchors) else None - - ax.plot(anchor[0], anchor[1], marker, markersize=15, - color=color, label=label, zorder=5) - ax.text(anchor[0], anchor[1] + 1, f'A{i}', - ha='center', fontsize=11, fontweight='bold') - - ax.set_xlabel('East (m)', fontsize=12) - ax.set_ylabel('North (m)', fontsize=12) - ax.set_title('2D Trajectory and UWB Anchors', fontsize=14, fontweight='bold') + color = "green" + marker = "^" + label = ( + f"Anchor {i} (Clean)" + if i == 0 or (i == 1 and not nlos_anchors) + else None + ) + + ax.plot( + anchor[0], + anchor[1], + marker, + markersize=15, + color=color, + label=label, + zorder=5, + ) + ax.text( + anchor[0], + anchor[1] + 1, + f"A{i}", + ha="center", + fontsize=11, + fontweight="bold", + ) + + ax.set_xlabel("East (m)", fontsize=12) + ax.set_ylabel("North (m)", fontsize=12) + ax.set_title("2D Trajectory and UWB Anchors", fontsize=14, fontweight="bold") ax.legend(fontsize=10) ax.grid(True, alpha=0.3) - ax.axis('equal') + ax.axis("equal") return ax def plot_velocity_heading(data: Dict, axes: Optional[np.ndarray] = None) -> np.ndarray: """Plot velocity and heading over time. - + Args: data: Dataset dictionary. axes: Optional 2x1 axes array (creates new if None). - + Returns: Axes array. """ if axes is None: fig, axes = plt.subplots(2, 1, figsize=(12, 8), sharex=True) - truth = data['truth'] - t = truth['t'] - v_xy = truth['v_xy'] - yaw = truth['yaw'] + truth = data["truth"] + t = truth["t"] + v_xy = truth["v_xy"] + yaw = truth["yaw"] # Velocity magnitude v_mag = np.linalg.norm(v_xy, axis=1) - axes[0].plot(t, v_mag, 'b-', linewidth=1.5) - axes[0].set_ylabel('Speed (m/s)', fontsize=11) - axes[0].set_title('Velocity and Heading Over Time', fontsize=13, fontweight='bold') + axes[0].plot(t, v_mag, "b-", linewidth=1.5) + axes[0].set_ylabel("Speed (m/s)", fontsize=11) + axes[0].set_title("Velocity and Heading Over Time", fontsize=13, fontweight="bold") axes[0].grid(True, alpha=0.3) # Heading - axes[1].plot(t, np.rad2deg(yaw), 'r-', linewidth=1.5) - axes[1].set_ylabel('Heading (degrees)', fontsize=11) - axes[1].set_xlabel('Time (s)', fontsize=11) + axes[1].plot(t, np.rad2deg(yaw), "r-", linewidth=1.5) + axes[1].set_ylabel("Heading (degrees)", fontsize=11) + axes[1].set_xlabel("Time (s)", fontsize=11) axes[1].grid(True, alpha=0.3) return axes @@ -162,37 +197,37 @@ def plot_velocity_heading(data: Dict, axes: Optional[np.ndarray] = None) -> np.n def plot_imu_measurements(data: Dict, axes: Optional[np.ndarray] = None) -> np.ndarray: """Plot IMU measurements (accel, gyro). - + Args: data: Dataset dictionary. axes: Optional 3x1 axes array (creates new if None). - + Returns: Axes array. """ if axes is None: fig, axes = plt.subplots(3, 1, figsize=(12, 10), sharex=True) - imu = data['imu'] - t = imu['t'] - accel_xy = imu['accel_xy'] - gyro_z = imu['gyro_z'] + imu = data["imu"] + t = imu["t"] + accel_xy = imu["accel_xy"] + gyro_z = imu["gyro_z"] # Accelerometer X - axes[0].plot(t, accel_xy[:, 0], 'b-', linewidth=0.5, alpha=0.7) - axes[0].set_ylabel('Accel X (m/s²)', fontsize=10) - axes[0].set_title('IMU Measurements', fontsize=13, fontweight='bold') + axes[0].plot(t, accel_xy[:, 0], "b-", linewidth=0.5, alpha=0.7) + axes[0].set_ylabel("Accel X (m/s²)", fontsize=10) + axes[0].set_title("IMU Measurements", fontsize=13, fontweight="bold") axes[0].grid(True, alpha=0.3) # Accelerometer Y - axes[1].plot(t, accel_xy[:, 1], 'g-', linewidth=0.5, alpha=0.7) - axes[1].set_ylabel('Accel Y (m/s²)', fontsize=10) + axes[1].plot(t, accel_xy[:, 1], "g-", linewidth=0.5, alpha=0.7) + axes[1].set_ylabel("Accel Y (m/s²)", fontsize=10) axes[1].grid(True, alpha=0.3) # Gyroscope Z - axes[2].plot(t, gyro_z, 'r-', linewidth=0.5, alpha=0.7) - axes[2].set_ylabel('Gyro Z (rad/s)', fontsize=10) - axes[2].set_xlabel('Time (s)', fontsize=11) + axes[2].plot(t, gyro_z, "r-", linewidth=0.5, alpha=0.7) + axes[2].set_ylabel("Gyro Z (rad/s)", fontsize=10) + axes[2].set_xlabel("Time (s)", fontsize=11) axes[2].grid(True, alpha=0.3) return axes @@ -200,11 +235,11 @@ def plot_imu_measurements(data: Dict, axes: Optional[np.ndarray] = None) -> np.n def plot_uwb_ranges(data: Dict, axes: Optional[np.ndarray] = None) -> np.ndarray: """Plot UWB range measurements per anchor. - + Args: data: Dataset dictionary. axes: Optional 2x2 axes array (creates new if None). - + Returns: Axes array. """ @@ -214,42 +249,48 @@ def plot_uwb_ranges(data: Dict, axes: Optional[np.ndarray] = None) -> np.ndarray else: axes = axes.flatten() - uwb = data['uwb'] - config = data['config'] + uwb = data["uwb"] + config = data["config"] - t_uwb = uwb['t'] - ranges = uwb['ranges'] - nlos_anchors = config.get('uwb', {}).get('nlos_anchors', []) + t_uwb = uwb["t"] + ranges = uwb["ranges"] + nlos_anchors = config.get("uwb", {}).get("nlos_anchors", []) for i in range(min(4, ranges.shape[1])): ranges_i = ranges[:, i] valid_mask = ~np.isnan(ranges_i) - color = 'red' if i in nlos_anchors else 'blue' - nlos_label = ' (NLOS)' if i in nlos_anchors else '' - - axes[i].plot(t_uwb[valid_mask], ranges_i[valid_mask], - 'o', color=color, markersize=3, alpha=0.6, - label=f'Measured{nlos_label}') - - axes[i].set_ylabel('Range (m)', fontsize=10) - axes[i].set_title(f'Anchor {i} Ranges', fontsize=11, fontweight='bold') + color = "red" if i in nlos_anchors else "blue" + nlos_label = " (NLOS)" if i in nlos_anchors else "" + + axes[i].plot( + t_uwb[valid_mask], + ranges_i[valid_mask], + "o", + color=color, + markersize=3, + alpha=0.6, + label=f"Measured{nlos_label}", + ) + + axes[i].set_ylabel("Range (m)", fontsize=10) + axes[i].set_title(f"Anchor {i} Ranges", fontsize=11, fontweight="bold") axes[i].legend(fontsize=9) axes[i].grid(True, alpha=0.3) if i >= 2: - axes[i].set_xlabel('Time (s)', fontsize=10) + axes[i].set_xlabel("Time (s)", fontsize=10) return axes def plot_range_errors(data: Dict, axes: Optional[np.ndarray] = None) -> np.ndarray: """Plot range measurement errors (measured - true). - + Args: data: Dataset dictionary. axes: Optional 2x2 axes array (creates new if None). - + Returns: Axes array. """ @@ -259,26 +300,27 @@ def plot_range_errors(data: Dict, axes: Optional[np.ndarray] = None) -> np.ndarr else: axes = axes.flatten() - truth = data['truth'] - uwb = data['uwb'] - anchors = data['anchors'] - config = data['config'] + truth = data["truth"] + uwb = data["uwb"] + anchors = data["anchors"] + config = data["config"] - t_uwb = uwb['t'] - ranges = uwb['ranges'] - nlos_anchors = config.get('uwb', {}).get('nlos_anchors', []) + t_uwb = uwb["t"] + ranges = uwb["ranges"] + nlos_anchors = config.get("uwb", {}).get("nlos_anchors", []) # Interpolate truth to UWB timestamps - p_xy_uwb = np.column_stack([ - np.interp(t_uwb, truth['t'], truth['p_xy'][:, 0]), - np.interp(t_uwb, truth['t'], truth['p_xy'][:, 1]) - ]) + p_xy_uwb = np.column_stack( + [ + np.interp(t_uwb, truth["t"], truth["p_xy"][:, 0]), + np.interp(t_uwb, truth["t"], truth["p_xy"][:, 1]), + ] + ) # Compute true ranges - ranges_true = np.array([ - np.linalg.norm(p_xy_uwb - anchor, axis=1) - for anchor in anchors - ]).T + ranges_true = np.array( + [np.linalg.norm(p_xy_uwb - anchor, axis=1) for anchor in anchors] + ).T for i in range(min(4, ranges.shape[1])): ranges_i = ranges[:, i] @@ -286,27 +328,41 @@ def plot_range_errors(data: Dict, axes: Optional[np.ndarray] = None) -> np.ndarr errors = ranges_i[valid_mask] - ranges_true[valid_mask, i] - nlos_label = ' (NLOS +bias)' if i in nlos_anchors else ' (Clean)' - - axes[i].hist(errors, bins=50, alpha=0.7, edgecolor='black', - color='red' if i in nlos_anchors else 'blue') - axes[i].axvline(0, color='k', linestyle='--', linewidth=2) - axes[i].axvline(np.mean(errors), color='r', linestyle='-', - linewidth=2, label=f'Mean: {np.mean(errors):.3f}m') - - axes[i].set_xlabel('Range Error (m)', fontsize=10) - axes[i].set_ylabel('Count', fontsize=10) - axes[i].set_title(f'Anchor {i}{nlos_label}', fontsize=11, fontweight='bold') + nlos_label = " (NLOS +bias)" if i in nlos_anchors else " (Clean)" + + axes[i].hist( + errors, + bins=50, + alpha=0.7, + edgecolor="black", + color="red" if i in nlos_anchors else "blue", + ) + axes[i].axvline(0, color="k", linestyle="--", linewidth=2) + axes[i].axvline( + np.mean(errors), + color="r", + linestyle="-", + linewidth=2, + label=f"Mean: {np.mean(errors):.3f}m", + ) + + axes[i].set_xlabel("Range Error (m)", fontsize=10) + axes[i].set_ylabel("Count", fontsize=10) + axes[i].set_title(f"Anchor {i}{nlos_label}", fontsize=11, fontweight="bold") axes[i].legend(fontsize=9) - axes[i].grid(True, alpha=0.3, axis='y') + axes[i].grid(True, alpha=0.3, axis="y") return axes -def plot_dataset_overview(dataset_path: str, output_dir: Optional[str] = None, - fmt: str = 'svg', show: bool = False): +def plot_dataset_overview( + dataset_path: str, + output_dir: Optional[str] = None, + fmt: str = "svg", + show: bool = False, +): """Create comprehensive overview plots for a fusion dataset. - + Args: dataset_path: Path to dataset directory. output_dir: Output directory for plots (default: same as dataset). @@ -324,7 +380,7 @@ def plot_dataset_overview(dataset_path: str, output_dir: Optional[str] = None, print(f"\nLoading dataset from: {dataset_path}") data = load_fusion_dataset(dataset_path) - config = data['config'] + config = data["config"] print(f" Duration: {config['dataset_info']['duration_sec']} s") print(f" IMU samples: {config['dataset_info']['imu_samples']}") print(f" UWB samples: {config['dataset_info']['uwb_samples']}") @@ -338,7 +394,7 @@ def plot_dataset_overview(dataset_path: str, output_dir: Optional[str] = None, plot_trajectory(data, ax=ax1) fig1.tight_layout() output_file1 = output_dir / f"trajectory.{fmt}" - fig1.savefig(output_file1, dpi=150, bbox_inches='tight') + fig1.savefig(output_file1, dpi=150, bbox_inches="tight") print(f" Saved: {output_file1}") # 2. Velocity and heading @@ -347,7 +403,7 @@ def plot_dataset_overview(dataset_path: str, output_dir: Optional[str] = None, plot_velocity_heading(data, axes=axes2) fig2.tight_layout() output_file2 = output_dir / f"velocity_heading.{fmt}" - fig2.savefig(output_file2, dpi=150, bbox_inches='tight') + fig2.savefig(output_file2, dpi=150, bbox_inches="tight") print(f" Saved: {output_file2}") # 3. IMU measurements @@ -356,7 +412,7 @@ def plot_dataset_overview(dataset_path: str, output_dir: Optional[str] = None, plot_imu_measurements(data, axes=axes3) fig3.tight_layout() output_file3 = output_dir / f"imu_measurements.{fmt}" - fig3.savefig(output_file3, dpi=150, bbox_inches='tight') + fig3.savefig(output_file3, dpi=150, bbox_inches="tight") print(f" Saved: {output_file3}") # 4. UWB ranges @@ -365,23 +421,23 @@ def plot_dataset_overview(dataset_path: str, output_dir: Optional[str] = None, plot_uwb_ranges(data, axes=axes4) fig4.tight_layout() output_file4 = output_dir / f"uwb_ranges.{fmt}" - fig4.savefig(output_file4, dpi=150, bbox_inches='tight') + fig4.savefig(output_file4, dpi=150, bbox_inches="tight") print(f" Saved: {output_file4}") # 5. Range errors print(" 5/5: Range errors") fig5, axes5 = plt.subplots(2, 2, figsize=(14, 10)) plot_range_errors(data, axes=axes5) - fig5.suptitle('UWB Range Measurement Errors', fontsize=14, fontweight='bold') + fig5.suptitle("UWB Range Measurement Errors", fontsize=14, fontweight="bold") fig5.tight_layout() output_file5 = output_dir / f"range_errors.{fmt}" - fig5.savefig(output_file5, dpi=150, bbox_inches='tight') + fig5.savefig(output_file5, dpi=150, bbox_inches="tight") print(f" Saved: {output_file5}") if show: plt.show() else: - plt.close('all') + plt.close("all") print(f"\nAll plots saved to: {output_dir}") print(f"Format: {fmt.upper()}\n") @@ -405,34 +461,34 @@ def main(): # Display plots interactively python %(prog)s data/sim/ch8_fusion_2d_imu_uwb --show - """ + """, ) parser.add_argument( - 'dataset', + "dataset", type=str, - help='Path to dataset directory (e.g., data/sim/ch8_fusion_2d_imu_uwb)' + help="Path to dataset directory (e.g., data/sim/ch8_fusion_2d_imu_uwb)", ) parser.add_argument( - '--output', + "--output", type=str, default=None, - help='Output directory for plots (default: same as dataset)' + help="Output directory for plots (default: same as dataset)", ) parser.add_argument( - '--format', + "--format", type=str, - choices=['svg', 'png', 'pdf'], - default='svg', - help='Output format (default: svg)' + choices=["svg", "png", "pdf"], + default="svg", + help="Output format (default: svg)", ) parser.add_argument( - '--show', - action='store_true', - help='Display plots interactively (default: save only)' + "--show", + action="store_true", + help="Display plots interactively (default: save only)", ) args = parser.parse_args() @@ -442,11 +498,9 @@ def main(): dataset_path=args.dataset, output_dir=args.output, fmt=args.format, - show=args.show + show=args.show, ) if __name__ == "__main__": main() - - diff --git a/tools/validate_dataset_docs.py b/tools/validate_dataset_docs.py index c47d6c3..0835258 100644 --- a/tools/validate_dataset_docs.py +++ b/tools/validate_dataset_docs.py @@ -35,17 +35,19 @@ class Colors: """ANSI color codes for terminal output.""" - GREEN = '\033[92m' - YELLOW = '\033[93m' - RED = '\033[91m' - BLUE = '\033[94m' - BOLD = '\033[1m' - END = '\033[0m' + + GREEN = "\033[92m" + YELLOW = "\033[93m" + RED = "\033[91m" + BLUE = "\033[94m" + BOLD = "\033[1m" + END = "\033[0m" + # Unicode-safe symbols (fallback for Windows) -CHECK = 'OK' # Was: ✓ -CROSS = 'X' # Was: ✗ -WARN = '!' # Was: ⚠ +CHECK = "OK" # Was: ✓ +CROSS = "X" # Was: ✗ +WARN = "!" # Was: ⚠ # Datasets whose documentation is genuinely incomplete, with what is missing. @@ -100,10 +102,10 @@ class Colors: def check_dataset_files(dataset_path: Path) -> Tuple[List[str], List[str]]: """Check for required dataset files. - + Args: dataset_path: Path to dataset directory. - + Returns: Tuple of (found_files, missing_files) """ @@ -131,8 +133,11 @@ def check_dataset_files(dataset_path: Path) -> Tuple[List[str], List[str]]: # it is accepted here rather than reported as a missing file. The naming # inconsistency is noted above. config = next( - (name for name in ("config.json", "metadata.json") - if (dataset_path / name).exists()), + ( + name + for name in ("config.json", "metadata.json") + if (dataset_path / name).exists() + ), None, ) if config: @@ -150,17 +155,17 @@ def check_dataset_files(dataset_path: Path) -> Tuple[List[str], List[str]]: def check_readme_sections(readme_path: Path) -> Dict[str, bool]: """Check which required sections are present in README. - + Args: readme_path: Path to README.md file. - + Returns: Dictionary mapping section names to presence (True/False). """ if not readme_path.exists(): return {section: False for section in REQUIRED_SECTIONS + RECOMMENDED_SECTIONS} - content = readme_path.read_text(encoding='utf-8') + content = readme_path.read_text(encoding="utf-8") results = {} for section in REQUIRED_SECTIONS + RECOMMENDED_SECTIONS: @@ -171,17 +176,17 @@ def check_readme_sections(readme_path: Path) -> Dict[str, bool]: def check_readme_code_blocks(readme_path: Path) -> Tuple[int, List[str]]: """Check for code examples in README. - + Args: readme_path: Path to README.md file. - + Returns: Tuple of (num_code_blocks, languages_found) """ if not readme_path.exists(): return 0, [] - content = readme_path.read_text(encoding='utf-8') + content = readme_path.read_text(encoding="utf-8") # Count code blocks code_blocks = content.count("```") @@ -201,17 +206,17 @@ def check_readme_code_blocks(readme_path: Path) -> Tuple[int, List[str]]: def check_parameter_table(readme_path: Path) -> bool: """Check if README contains a parameter effects table. - + Args: readme_path: Path to README.md file. - + Returns: True if parameter table found. """ if not readme_path.exists(): return False - content = readme_path.read_text(encoding='utf-8') + content = readme_path.read_text(encoding="utf-8") # A markdown table inside the parameter-effects section. # @@ -243,27 +248,27 @@ def check_parameter_table(readme_path: Path) -> bool: def validate_dataset(dataset_path: Path, verbose: bool = True) -> Tuple[bool, Dict]: """Validate a single dataset directory. - + Args: dataset_path: Path to dataset directory. verbose: Print detailed output. - + Returns: Tuple of (is_valid, results_dict) """ results = { - 'path': dataset_path, - 'dataset': dataset_path.name, - 'has_readme': False, - 'has_config': False, - 'has_data_files': False, - 'required_sections': {}, - 'recommended_sections': {}, - 'num_code_blocks': 0, - 'code_languages': [], - 'has_parameter_table': False, - 'warnings': [], - 'errors': [], + "path": dataset_path, + "dataset": dataset_path.name, + "has_readme": False, + "has_config": False, + "has_data_files": False, + "required_sections": {}, + "recommended_sections": {}, + "num_code_blocks": 0, + "code_languages": [], + "has_parameter_table": False, + "warnings": [], + "errors": [], } if verbose: @@ -272,14 +277,14 @@ def validate_dataset(dataset_path: Path, verbose: bool = True) -> Tuple[bool, Di # Check files found_files, missing_files = check_dataset_files(dataset_path) - results['has_config'] = any( + results["has_config"] = any( name in found_files for name in ("config.json", "metadata.json") ) - results['has_data_files'] = any("data files" in f for f in found_files) + results["has_data_files"] = any("data files" in f for f in found_files) if missing_files: for mf in missing_files: - results['errors'].append(f"Missing required file: {mf}") + results["errors"].append(f"Missing required file: {mf}") if verbose: print(f" {Colors.RED}[{CROSS}]{Colors.END} Missing: {mf}") else: @@ -288,10 +293,10 @@ def validate_dataset(dataset_path: Path, verbose: bool = True) -> Tuple[bool, Di # Check README readme_path = dataset_path / "README.md" - results['has_readme'] = readme_path.exists() + results["has_readme"] = readme_path.exists() if not readme_path.exists(): - results['errors'].append("Missing README.md") + results["errors"].append("Missing README.md") if verbose: print(f" {Colors.RED}[{CROSS}]{Colors.END} Missing README.md") return False, results @@ -302,47 +307,51 @@ def validate_dataset(dataset_path: Path, verbose: bool = True) -> Tuple[bool, Di # Check sections sections = check_readme_sections(readme_path) for section in REQUIRED_SECTIONS: - results['required_sections'][section] = sections[section] + results["required_sections"][section] = sections[section] if not sections[section]: - results['errors'].append(f"Missing required section: {section}") + results["errors"].append(f"Missing required section: {section}") if verbose: print(f" {Colors.RED}[{CROSS}]{Colors.END} Missing section: {section}") for section in RECOMMENDED_SECTIONS: - results['recommended_sections'][section] = sections[section] + results["recommended_sections"][section] = sections[section] if not sections[section]: - results['warnings'].append(f"Missing recommended section: {section}") + results["warnings"].append(f"Missing recommended section: {section}") - if verbose and not results['errors']: + if verbose and not results["errors"]: print(f" {Colors.GREEN}[{CHECK}]{Colors.END} All required sections present") # Check code examples num_blocks, languages = check_readme_code_blocks(readme_path) - results['num_code_blocks'] = num_blocks - results['code_languages'] = languages + results["num_code_blocks"] = num_blocks + results["code_languages"] = languages if num_blocks < 3: - results['warnings'].append(f"Only {num_blocks} code blocks (recommend ≥3)") + results["warnings"].append(f"Only {num_blocks} code blocks (recommend ≥3)") if verbose: - print(f" {Colors.YELLOW}[{WARN}]{Colors.END} Only {num_blocks} code blocks (recommend ≥3)") + print( + f" {Colors.YELLOW}[{WARN}]{Colors.END} Only {num_blocks} code blocks (recommend ≥3)" + ) elif verbose: print(f" {Colors.GREEN}[{CHECK}]{Colors.END} {num_blocks} code blocks found") - if 'python' not in languages: - results['warnings'].append("No Python loading examples found") + if "python" not in languages: + results["warnings"].append("No Python loading examples found") if verbose: print(f" {Colors.YELLOW}[{WARN}]{Colors.END} No Python loading examples") # Check parameter table - results['has_parameter_table'] = check_parameter_table(readme_path) - if not results['has_parameter_table']: - results['errors'].append("Missing parameter effects table") + results["has_parameter_table"] = check_parameter_table(readme_path) + if not results["has_parameter_table"]: + results["errors"].append("Missing parameter effects table") if verbose: - print(f" {Colors.RED}[{CROSS}]{Colors.END} Missing parameter effects table") + print( + f" {Colors.RED}[{CROSS}]{Colors.END} Missing parameter effects table" + ) elif verbose: print(f" {Colors.GREEN}[{CHECK}]{Colors.END} Parameter effects table present") - is_valid = len(results['errors']) == 0 + is_valid = len(results["errors"]) == 0 if verbose: if is_valid: @@ -350,7 +359,7 @@ def validate_dataset(dataset_path: Path, verbose: bool = True) -> Tuple[bool, Di else: print(f" {Colors.RED}{Colors.BOLD}Status: INVALID [{CROSS}]{Colors.END}") - if results['warnings']: + if results["warnings"]: print(f" {Colors.YELLOW}Warnings: {len(results['warnings'])}{Colors.END}") return is_valid, results @@ -358,10 +367,10 @@ def validate_dataset(dataset_path: Path, verbose: bool = True) -> Tuple[bool, Di def find_datasets(data_sim_path: Path) -> List[Path]: """Find all dataset directories in data/sim/. - + Args: data_sim_path: Path to data/sim/ directory. - + Returns: List of dataset directory paths. """ @@ -378,7 +387,7 @@ def find_datasets(data_sim_path: Path) -> List[Path]: suffixes = ("*.npz", "*.npy", "*.txt") for item in data_sim_path.iterdir(): - if not item.is_dir() or item.name.startswith('.'): + if not item.is_dir() or item.name.startswith("."): continue looks_like_dataset = ( (item / "README.md").exists() @@ -393,7 +402,7 @@ def find_datasets(data_sim_path: Path) -> List[Path]: def print_summary(results_list: List[Tuple[bool, Dict]]): """Print summary of validation results. - + Args: results_list: List of (is_valid, results_dict) tuples. """ @@ -409,17 +418,21 @@ def print_summary(results_list: List[Tuple[bool, Dict]]): print(f"Invalid datasets: {Colors.RED}{total_count - valid_count}{Colors.END}") if valid_count == total_count: - print(f"\n{Colors.GREEN}{Colors.BOLD}All datasets have complete documentation! [{CHECK}]{Colors.END}") + print( + f"\n{Colors.GREEN}{Colors.BOLD}All datasets have complete documentation! [{CHECK}]{Colors.END}" + ) else: - print(f"\n{Colors.RED}{Colors.BOLD}Some datasets need documentation fixes.{Colors.END}") + print( + f"\n{Colors.RED}{Colors.BOLD}Some datasets need documentation fixes.{Colors.END}" + ) print("\nDatasets needing attention:") for is_valid, results in results_list: if not is_valid: print(f" - {results['path'].name}: {len(results['errors'])} errors") # Print statistics - total_errors = sum(len(r['errors']) for _, r in results_list) - total_warnings = sum(len(r['warnings']) for _, r in results_list) + total_errors = sum(len(r["errors"]) for _, r in results_list) + total_warnings = sum(len(r["warnings"]) for _, r in results_list) print(f"\nTotal errors: {Colors.RED}{total_errors}{Colors.END}") print(f"Total warnings: {Colors.YELLOW}{total_warnings}{Colors.END}") @@ -443,32 +456,29 @@ def main(): # Strict mode (warnings treated as errors) python tools/validate_dataset_docs.py --strict - """ + """, ) parser.add_argument( - 'dataset', - nargs='?', - help='Specific dataset to check (default: check all)' + "dataset", nargs="?", help="Specific dataset to check (default: check all)" ) parser.add_argument( - '--quiet', '-q', - action='store_true', - help='Only print summary (no per-dataset details)' + "--quiet", + "-q", + action="store_true", + help="Only print summary (no per-dataset details)", ) parser.add_argument( - '--strict', - action='store_true', - help='Treat warnings as errors' + "--strict", action="store_true", help="Treat warnings as errors" ) parser.add_argument( - '--data-dir', + "--data-dir", type=str, - default='data/sim', - help='Path to data/sim directory (default: data/sim)' + default="data/sim", + help="Path to data/sim directory (default: data/sim)", ) args = parser.parse_args() @@ -501,10 +511,10 @@ def main(): is_valid, results = validate_dataset(dataset_path, verbose=not args.quiet) # In strict mode, treat warnings as errors - if args.strict and results['warnings']: + if args.strict and results["warnings"]: is_valid = False - results['errors'].extend(results['warnings']) - results['warnings'] = [] + results["errors"].extend(results["warnings"]) + results["warnings"] = [] results_list.append((is_valid, results)) @@ -521,11 +531,13 @@ def main(): # optional sections. if args.strict: failing = sorted( - results['dataset'] for is_valid, results in results_list if not is_valid + results["dataset"] for is_valid, results in results_list if not is_valid ) if failing: - print(f"\n{Colors.YELLOW}{Colors.BOLD}Strict mode: recommended " - f"sections missing{Colors.END}") + print( + f"\n{Colors.YELLOW}{Colors.BOLD}Strict mode: recommended " + f"sections missing{Colors.END}" + ) for name in failing: print(f" - {name}") print( @@ -544,12 +556,14 @@ def main(): # become valid also fails it, so the register cannot quietly grow stale -- # the same reasoning as the ratchets in tests/test_repo_conventions.py. unregistered = sorted( - results['dataset'] for is_valid, results in results_list - if not is_valid and results['dataset'] not in KNOWN_INCOMPLETE + results["dataset"] + for is_valid, results in results_list + if not is_valid and results["dataset"] not in KNOWN_INCOMPLETE ) fixed = sorted( - results['dataset'] for is_valid, results in results_list - if is_valid and results['dataset'] in KNOWN_INCOMPLETE + results["dataset"] + for is_valid, results in results_list + if is_valid and results["dataset"] in KNOWN_INCOMPLETE ) if unregistered: @@ -561,22 +575,26 @@ def main(): "line saying what it still needs." ) if fixed: - print(f"\n{Colors.GREEN}{Colors.BOLD}Now valid, so drop from " - f"KNOWN_INCOMPLETE:{Colors.END}") + print( + f"\n{Colors.GREEN}{Colors.BOLD}Now valid, so drop from " + f"KNOWN_INCOMPLETE:{Colors.END}" + ) for name in fixed: print(f" - {name}") if not unregistered and not fixed: registered = sum( - 1 for is_valid, results in results_list - if not is_valid and results['dataset'] in KNOWN_INCOMPLETE + 1 + for is_valid, results in results_list + if not is_valid and results["dataset"] in KNOWN_INCOMPLETE + ) + print( + f"\n{Colors.GREEN}{Colors.BOLD}[PASSED]{Colors.END} " + f"no new gaps ({registered} known, listed in KNOWN_INCOMPLETE)" ) - print(f"\n{Colors.GREEN}{Colors.BOLD}[PASSED]{Colors.END} " - f"no new gaps ({registered} known, listed in KNOWN_INCOMPLETE)") return 1 if (unregistered or fixed) else 0 if __name__ == "__main__": sys.exit(main()) -