536 lines
24 KiB
Python
536 lines
24 KiB
Python
from __future__ import annotations
|
|
|
|
from types import SimpleNamespace
|
|
|
|
import numpy as np
|
|
import pytest
|
|
from scipy.spatial.transform import Rotation
|
|
|
|
from imu_lidar.contracts import ImuSeries
|
|
from rtk_imu.rtk_imu_engineering import (
|
|
G0,
|
|
HPR_DIRECT_ANGULAR_SIGMA_RAD,
|
|
ResidualAudit,
|
|
_all_hpr,
|
|
_engineering_gates,
|
|
_enu,
|
|
_height_reference,
|
|
_hpr_angular_sigma_rad,
|
|
_marginal_lever_information,
|
|
_motion_flags,
|
|
_nodes,
|
|
_resample_segments_with_multiplicity,
|
|
_segments,
|
|
solve_engineering_6dof,
|
|
)
|
|
from rtk_imu.rtk_imu_multisource import UnifiedSession
|
|
from rtk_imu.rtk_imu_node_graph import (
|
|
_additive_marginal_lever_information,
|
|
_free_residual,
|
|
_free_sparsity,
|
|
_hpr_sigma,
|
|
build_problem,
|
|
fit_states_at_fixed_lever,
|
|
initial_parameters as node_graph_initial_parameters,
|
|
jacobian_sparsity as node_graph_jacobian_sparsity,
|
|
residual as node_graph_residual,
|
|
solve_free_lever_many,
|
|
)
|
|
from tools.run_rtk_imu_mechanical_prior_heldout import _gate as heldout_gate
|
|
from tools.audit_rtk_imu_innovation_noise import _summarize as innovation_summary
|
|
from tools.audit_rtk_imu_propagation_bias_root_cause import _root_report
|
|
|
|
EARTH_RADIUS_M = 6_378_137.0
|
|
|
|
|
|
def _rows_from_motion(
|
|
session_id: str,
|
|
lever: np.ndarray,
|
|
R_RTK_IMU: np.ndarray,
|
|
rotation_at,
|
|
*,
|
|
duration_s: float = 10.0,
|
|
gga_altitude_offset_m: float | None = None,
|
|
) -> UnifiedSession:
|
|
imu_t = np.arange(0.0, duration_s + 0.005, 0.01)
|
|
rotations = Rotation.from_matrix(np.asarray([rotation_at(t) for t in imu_t]))
|
|
matrices = rotations.as_matrix()
|
|
relative = Rotation.from_matrix(np.einsum("nij,njk->nik", matrices[:-1].transpose(0, 2, 1), matrices[1:]))
|
|
gyro = np.empty((imu_t.size, 3))
|
|
gyro[:-1] = relative.as_rotvec() / 0.01
|
|
gyro[-1] = gyro[-2]
|
|
accel = np.einsum("nji,j->ni", matrices, np.array([0.0, 0.0, G0]))
|
|
|
|
hpr_t = np.arange(0.0, duration_s + 0.001, 0.1)
|
|
rows_hpr: list[dict[str, str]] = []
|
|
baseline_I = R_RTK_IMU.T[:, 0]
|
|
for t in hpr_t:
|
|
baseline = rotation_at(t) @ baseline_I
|
|
heading = np.degrees(np.arctan2(baseline[0], baseline[1])) % 360.0
|
|
pitch = np.degrees(np.arcsin(np.clip(baseline[2], -1.0, 1.0)))
|
|
rows_hpr.append({
|
|
"checksum_valid": "1", "heading_quality": "4", "t_device_s": str(t),
|
|
"heading_deg": str(heading), "pitch_deg": str(pitch),
|
|
})
|
|
|
|
rows_best: list[dict[str, str]] = []
|
|
rows_gga: list[dict[str, str]] = []
|
|
best_t = np.arange(0.0, duration_s + 0.001, 0.5)
|
|
dt_velocity = 1e-3
|
|
for t in best_t:
|
|
R_WI = rotation_at(t)
|
|
position = R_WI @ lever
|
|
before = rotation_at(max(t - dt_velocity, 0.0)) @ lever
|
|
after = rotation_at(min(t + dt_velocity, duration_s)) @ lever
|
|
denominator = min(t + dt_velocity, duration_s) - max(t - dt_velocity, 0.0)
|
|
velocity = (after - before) / denominator
|
|
lat = position[1] / EARTH_RADIUS_M * 180.0 / np.pi
|
|
lon = position[0] / EARTH_RADIUS_M * 180.0 / np.pi
|
|
rows_best.append({
|
|
"checksum_valid": "1", "position_fixed": "1", "t_device_s": str(t),
|
|
"lat_deg": str(lat), "lon_deg": str(lon), "altitude_m": str(50.0 + position[2]),
|
|
"doppler_velocity_valid": "1", "velocity_east_m_s": str(velocity[0]),
|
|
"velocity_north_m_s": str(velocity[1]), "vertical_speed_m_s": str(velocity[2]),
|
|
})
|
|
if gga_altitude_offset_m is not None:
|
|
rows_gga.append({
|
|
"checksum_valid": "1", "fix_quality": "4", "t_device_s": str(t),
|
|
"lat_deg": str(lat), "lon_deg": str(lon),
|
|
"altitude_msl_m": str(50.0 + position[2] + gga_altitude_offset_m),
|
|
})
|
|
|
|
rtk = {"BESTNAVA": rows_best, "GNHPR": rows_hpr}
|
|
if rows_gga:
|
|
rtk["GGA"] = rows_gga
|
|
return UnifiedSession(
|
|
session_id=session_id,
|
|
batch_id="synthetic",
|
|
imu=ImuSeries(t_s=imu_t, gyro_rad_s=gyro, acc_m_s2=accel),
|
|
imu_rpy_deg=np.zeros((imu_t.size, 3)),
|
|
imu_quaternion_wxyz=np.tile([1.0, 0.0, 0.0, 0.0], (imu_t.size, 1)),
|
|
imu_host_receive_utc_s=imu_t,
|
|
rtk_by_type=rtk,
|
|
)
|
|
|
|
|
|
def _constant_velocity_session(speed_m_s: float = 3.0) -> UnifiedSession:
|
|
imu_t = np.arange(0.0, 5.01, 0.01)
|
|
rows_best = []
|
|
rows_hpr = []
|
|
for t in np.arange(0.0, 5.01, 0.1):
|
|
rows_hpr.append({
|
|
"checksum_valid": "1", "heading_quality": "4", "t_device_s": str(t),
|
|
"heading_deg": "90", "pitch_deg": "0",
|
|
})
|
|
for t in np.arange(0.0, 5.01, 0.5):
|
|
rows_best.append({
|
|
"checksum_valid": "1", "position_fixed": "1", "t_device_s": str(t),
|
|
"lat_deg": "0", "lon_deg": str(speed_m_s * t / EARTH_RADIUS_M * 180.0 / np.pi),
|
|
"altitude_m": "50", "doppler_velocity_valid": "1",
|
|
"velocity_east_m_s": str(speed_m_s), "velocity_north_m_s": "0",
|
|
"vertical_speed_m_s": "0",
|
|
})
|
|
return UnifiedSession(
|
|
session_id="constant_velocity", batch_id="synthetic",
|
|
imu=ImuSeries(
|
|
t_s=imu_t, gyro_rad_s=np.zeros((imu_t.size, 3)),
|
|
acc_m_s2=np.tile([0.0, 0.0, G0], (imu_t.size, 1)),
|
|
),
|
|
imu_rpy_deg=np.zeros((imu_t.size, 3)),
|
|
imu_quaternion_wxyz=np.tile([1.0, 0.0, 0.0, 0.0], (imu_t.size, 1)),
|
|
imu_host_receive_utc_s=imu_t,
|
|
rtk_by_type={"BESTNAVA": rows_best, "GNHPR": rows_hpr},
|
|
)
|
|
|
|
|
|
def _audit(values: list[float]) -> ResidualAudit:
|
|
vector = np.asarray(values, dtype=float)
|
|
return ResidualAudit(20, vector, vector, float(np.linalg.norm(vector)), float(np.linalg.norm(vector)))
|
|
|
|
|
|
def test_constant_speed_straight_is_gravity_candidate_but_not_zupt() -> None:
|
|
session = _constant_velocity_session()
|
|
times = np.asarray([float(row["t_device_s"]) for row in session.rtk_by_type["BESTNAVA"]])
|
|
velocities = np.asarray([[3.0, 0.0, 0.0]] * len(times))
|
|
gravity_candidate, zupt_static = _motion_flags(session, 2.5, times, velocities)
|
|
assert gravity_candidate
|
|
assert not zupt_static
|
|
|
|
|
|
def test_bestnava_is_not_suppressed_by_earlier_gga_epochs() -> None:
|
|
session = _rows_from_motion(
|
|
"best_preferred", np.array([0.3, -0.2, 0.1]), np.eye(3),
|
|
lambda _: np.eye(3), gga_altitude_offset_m=100.0,
|
|
)
|
|
for row in session.rtk_by_type["GGA"]:
|
|
row["t_device_s"] = str(float(row["t_device_s"]) - 0.05)
|
|
reference = _height_reference([session])
|
|
assert reference is not None
|
|
nodes = _nodes(session, reference, 0.5)
|
|
assert nodes and all(node.source == "BESTNAVA" for node in nodes)
|
|
assert all(node.velocity_enu_m_s is not None for node in nodes)
|
|
|
|
def test_gga_msl_altitude_never_enters_bestnava_z_reference() -> None:
|
|
lever = np.array([0.3, -0.2, 0.4])
|
|
session = _rows_from_motion("mixed_height", lever, np.eye(3), lambda _: np.eye(3),
|
|
gga_altitude_offset_m=123.0)
|
|
reference = _height_reference([session])
|
|
assert reference is not None and reference[2] == pytest.approx(50.4)
|
|
best, best_mask = _enu(session.rtk_by_type["BESTNAVA"][1], "BESTNAVA", reference)
|
|
gga, gga_mask = _enu(session.rtk_by_type["GGA"][1], "GGA", reference)
|
|
assert best_mask.tolist() == [True, True, True]
|
|
assert gga_mask.tolist() == [True, True, False]
|
|
assert best[2] == pytest.approx(0.0)
|
|
assert gga[2] == pytest.approx(0.0)
|
|
|
|
|
|
def test_schur_observability_uses_marginal_lever_information() -> None:
|
|
J_l = np.array([
|
|
[1.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 0.1],
|
|
[1.0, 1.0, 0.0], [1.0, 0.0, 0.0],
|
|
])
|
|
J_n = np.array([[1.0], [0.0], [0.0], [1.0], [0.0]])
|
|
J = np.column_stack([J_l, J_n])
|
|
marginal, singular, condition, rank, weakest, covariance = _marginal_lever_information(J, np.ones(4))
|
|
H = J.T @ J
|
|
expected = H[:3, :3] - H[:3, 3:] @ np.linalg.pinv(H[3:, 3:]) @ H[3:, :3]
|
|
assert np.allclose(marginal, expected)
|
|
assert singular.shape == (3,) and rank == 3 and np.isfinite(condition)
|
|
assert abs(weakest[2]) > 0.99
|
|
assert covariance.shape == (3, 3)
|
|
|
|
|
|
def test_bootstrap_resampling_preserves_session_multiplicity() -> None:
|
|
segments = [SimpleNamespace(session_id="a"), SimpleNamespace(session_id="b")]
|
|
sampled = _resample_segments_with_multiplicity(segments, ["a", "a", "b"])
|
|
assert [segment.session_id for segment in sampled] == ["a", "a", "b"]
|
|
|
|
|
|
@pytest.mark.parametrize("fault", ["q5", "time_gap", "baseline_jump"])
|
|
def test_isolated_hpr_faults_do_not_split_r0_trajectory(fault: str) -> None:
|
|
session = _constant_velocity_session(0.0)
|
|
if fault == "q5":
|
|
session.rtk_by_type["GNHPR"][25]["heading_quality"] = "5"
|
|
elif fault == "time_gap":
|
|
session.rtk_by_type["GNHPR"] = [
|
|
row for row in session.rtk_by_type["GNHPR"]
|
|
if not 2.1 <= float(row["t_device_s"]) <= 2.9
|
|
]
|
|
else:
|
|
session.rtk_by_type["GNHPR"][25]["heading_deg"] = "270"
|
|
reference = _height_reference([session])
|
|
assert reference is not None
|
|
nodes = _nodes(session, reference, 0.5)
|
|
assert nodes
|
|
assert all(
|
|
right.continuity_id == left.continuity_id
|
|
for left, right in zip(nodes[:-1], nodes[1:])
|
|
)
|
|
if fault == "baseline_jump":
|
|
assert any(not node.hpr_factor_valid and node.hpr_factor_method == "isolated_outlier" for node in nodes)
|
|
|
|
|
|
def test_hpr_bridge_covariance_is_weaker_than_direct_and_limited_to_half_second() -> None:
|
|
assert _hpr_angular_sigma_rad("nearest_q4", 0.0) == pytest.approx(HPR_DIRECT_ANGULAR_SIGMA_RAD)
|
|
assert _hpr_angular_sigma_rad("bracket_interpolation", 0.2) > HPR_DIRECT_ANGULAR_SIGMA_RAD
|
|
assert _hpr_angular_sigma_rad("bracket_interpolation", 0.5) >= _hpr_angular_sigma_rad(
|
|
"bracket_interpolation", 0.2
|
|
)
|
|
assert np.isinf(_hpr_angular_sigma_rad("bracket_interpolation", 0.500001))
|
|
|
|
def test_short_hpr_dropout_uses_interpolated_factor_without_splitting_r0() -> None:
|
|
session = _constant_velocity_session(0.0)
|
|
session.rtk_by_type["GNHPR"] = [
|
|
row for row in session.rtk_by_type["GNHPR"]
|
|
if not 2.4 <= float(row["t_device_s"]) <= 2.6
|
|
]
|
|
reference = _height_reference([session])
|
|
assert reference is not None
|
|
nodes = _nodes(session, reference, 0.5)
|
|
bridged = [node for node in nodes if abs(node.t_s - 2.5) < 1e-9]
|
|
assert len(bridged) == 1
|
|
assert bridged[0].hpr_factor_valid
|
|
assert bridged[0].hpr_factor_method == "bracket_interpolation"
|
|
assert all(right.continuity_id == left.continuity_id for left, right in zip(nodes[:-1], nodes[1:]))
|
|
|
|
def test_position_time_backwards_still_splits_r0_trajectory() -> None:
|
|
session = _constant_velocity_session(0.0)
|
|
session.rtk_by_type["BESTNAVA"][5]["t_device_s"] = "1.75"
|
|
reference = _height_reference([session])
|
|
assert reference is not None
|
|
nodes = _nodes(session, reference, 0.5)
|
|
assert any(
|
|
right.continuity_id != left.continuity_id
|
|
for left, right in zip(nodes[:-1], nodes[1:])
|
|
)
|
|
|
|
|
|
def test_one_hz_timestamp_jitter_does_not_create_artificial_two_second_r0_gaps() -> None:
|
|
session = _constant_velocity_session(0.0)
|
|
rows = session.rtk_by_type["BESTNAVA"]
|
|
# Retain strict device-time monotonicity while making every second sample early.
|
|
for index, row in enumerate(rows):
|
|
row["t_device_s"] = str(float(row["t_device_s"]) - (0.002 if index % 2 else 0.0))
|
|
reference = _height_reference([session])
|
|
assert reference is not None
|
|
nodes = _nodes(session, reference, 1.0)
|
|
intervals = np.diff([node.t_s for node in nodes])
|
|
assert len(nodes) == 6
|
|
assert np.all(intervals > 0.75)
|
|
assert np.all(intervals < 1.25)
|
|
assert all(right.continuity_id == left.continuity_id for left, right in zip(nodes[:-1], nodes[1:]))
|
|
|
|
def test_large_residuals_fail_engineering_acceptance() -> None:
|
|
gates = _engineering_gates(
|
|
np.array([0.01, 0.01, 0.01]), np.array([100.0, 10.0, 1.0]), 3, 100.0,
|
|
_audit([1.0, 1.0]), _audit([1.0, 1.0, 1.0]), _audit([2.0, 2.0, 2.0]),
|
|
{"a": 0.01, "b": 0.02, "c": 0.03}, np.array([0.01, 0.01, 0.01]), True,
|
|
0.01, True, None, False, None,
|
|
)
|
|
assert not gates["gga_xy_rms_p95"]
|
|
assert not gates["bestnava_xyz_rms_p95"]
|
|
assert not gates["doppler_velocity_rms_p95"]
|
|
assert not all(gates.values())
|
|
|
|
|
|
def test_unobservable_free_solution_does_not_block_mechanical_prior_with_euclidean_delta() -> None:
|
|
gates = _engineering_gates(
|
|
np.array([0.01, 0.01, 0.01]), np.array([100.0, 10.0, 1.0]), 3, 100.0,
|
|
_audit([0.01, 0.01]), _audit([0.01, 0.01, 0.01]), _audit([0.01, 0.01, 0.01]),
|
|
{"a": 0.01, "b": 0.02, "c": 0.03}, np.array([0.01, 0.01, 0.01]), True,
|
|
0.01, True, np.array([10.0, 10.0, 10.0]), False, None,
|
|
)
|
|
assert gates["manual_lever_consistency"] is False
|
|
assert gates["free_manual_mahalanobis_consistency"] is True
|
|
observable_gates = _engineering_gates(
|
|
np.array([0.01, 0.01, 0.01]), np.array([100.0, 10.0, 1.0]), 3, 100.0,
|
|
_audit([0.01, 0.01]), _audit([0.01, 0.01, 0.01]), _audit([0.01, 0.01, 0.01]),
|
|
{"a": 0.01, "b": 0.02, "c": 0.03}, np.array([0.01, 0.01, 0.01]), True,
|
|
0.01, True, None, True, 12.0,
|
|
)
|
|
assert observable_gates["free_manual_mahalanobis_consistency"] is False
|
|
def test_yaw_only_recovers_xy_but_weak_z_is_rejected_and_transforms_are_inverse() -> None:
|
|
lever = np.array([0.30, -0.20, 0.10])
|
|
session = _rows_from_motion(
|
|
"yaw", lever, np.eye(3), lambda t: Rotation.from_euler("z", 0.20 * t).as_matrix()
|
|
)
|
|
result = solve_engineering_6dof(
|
|
[session], R_RTK_IMU=np.eye(3), sample_period_s=0.5,
|
|
run_loo=False, run_bootstrap=False, run_rotation_sensitivity=False,
|
|
)
|
|
assert result.l_I_m is not None
|
|
assert np.allclose(result.l_I_m[:2], lever[:2], atol=0.05)
|
|
assert result.lever_precision_rank < 3 or not result.engineering_acceptance_gates["lever_marginal_std"]
|
|
assert not result.engineering_6dof_accepted
|
|
assert result.T_RTK_IMU is not None and result.T_IMU_RTK is not None
|
|
assert np.allclose(result.T_RTK_IMU @ result.T_IMU_RTK, np.eye(4), atol=1e-8)
|
|
|
|
|
|
def test_pitch_excitation_recovers_vertical_lever_arm() -> None:
|
|
lever = np.array([0.28, -0.16, 0.42])
|
|
rotation_at = lambda t: Rotation.from_euler(
|
|
"xyz", [0.0, 0.12 * np.sin(0.55 * t), 0.16 * t]
|
|
).as_matrix()
|
|
session = _rows_from_motion("pitch", lever, np.eye(3), rotation_at)
|
|
result = solve_engineering_6dof(
|
|
[session], R_RTK_IMU=np.eye(3), sample_period_s=0.5,
|
|
run_loo=False, run_bootstrap=False, run_rotation_sensitivity=False,
|
|
)
|
|
assert result.l_I_m is not None
|
|
assert result.l_I_m[2] == pytest.approx(lever[2], abs=0.08)
|
|
|
|
|
|
def test_full_3d_excitation_with_nonzero_fixed_r2g_recovers_l_i() -> None:
|
|
lever = np.array([0.24, -0.18, 0.36])
|
|
fixed_rotation = Rotation.from_euler("xyz", [0.454, -0.003, 0.012], degrees=True).as_matrix()
|
|
rotation_at = lambda t: Rotation.from_euler(
|
|
"xyz", [0.16 * np.sin(0.37 * t), 0.18 * np.sin(0.51 * t), 0.18 * t]
|
|
).as_matrix()
|
|
session = _rows_from_motion(
|
|
"full_3d", lever, fixed_rotation, rotation_at, duration_s=15.0
|
|
)
|
|
result = solve_engineering_6dof(
|
|
[session], R_RTK_IMU=fixed_rotation, sample_period_s=0.5,
|
|
run_loo=False, run_bootstrap=False, run_rotation_sensitivity=False,
|
|
)
|
|
assert result.l_I_m is not None
|
|
assert np.allclose(result.l_I_m, lever, atol=0.08)
|
|
assert np.allclose(result.R_RTK_IMU, fixed_rotation)
|
|
assert result.T_RTK_IMU is not None and result.T_IMU_RTK is not None
|
|
assert np.allclose(result.T_RTK_IMU @ result.T_IMU_RTK, np.eye(4), atol=1e-8)
|
|
|
|
def test_mechanical_soft_prior_keeps_free_solution_and_reports_prior_solution() -> None:
|
|
lever = np.array([0.24, -0.18, 0.36])
|
|
reference = np.array([0.30, -0.18, 0.36])
|
|
rotation_at = lambda t: Rotation.from_euler(
|
|
"xyz", [0.14 * np.sin(0.37 * t), 0.16 * np.sin(0.51 * t), 0.18 * t]
|
|
).as_matrix()
|
|
session = _rows_from_motion("prior", lever, np.eye(3), rotation_at, duration_s=12.0)
|
|
result = solve_engineering_6dof(
|
|
[session], R_RTK_IMU=np.eye(3),
|
|
manual_l_I_m=reference,
|
|
manual_l_I_covariance_m2=np.diag([0.02**2, 0.02**2, 0.02**2]),
|
|
sample_period_s=0.5, run_loo=False, run_bootstrap=False,
|
|
run_rotation_sensitivity=False,
|
|
)
|
|
assert result.free_solution is not None
|
|
assert result.prior_constrained_solution is not None
|
|
assert result.translation_prior_applied
|
|
assert np.allclose(result.mechanical_reference_l_I_m, reference)
|
|
assert np.linalg.norm(result.prior_to_mechanical_delta_m) < np.linalg.norm(result.free_to_mechanical_delta_m)
|
|
assert np.allclose(result.l_I_m, result.prior_constrained_solution.l_I_m)
|
|
assert result.mechanical_reference_solution is not None
|
|
assert result.residual_comparison is not None
|
|
assert result.posterior_to_prior_covariance_ratio is not None
|
|
|
|
|
|
def test_manual_mean_and_covariance_must_be_provided_together() -> None:
|
|
session = _constant_velocity_session(0.0)
|
|
with pytest.raises(ValueError, match="must be provided together"):
|
|
solve_engineering_6dof([session], R_RTK_IMU=np.eye(3), manual_l_I_m=np.zeros(3))
|
|
|
|
|
|
def test_blockwise_schur_information_is_additive_across_independent_segments() -> None:
|
|
rng = np.random.default_rng(7)
|
|
lever_blocks = []
|
|
nuisance_blocks = []
|
|
row_count = 30
|
|
for scale in (1.0, 1e-3, 20.0):
|
|
lever_blocks.append(rng.normal(size=(row_count, 3)) * scale)
|
|
nuisance_blocks.append(rng.normal(size=(row_count, 15)) * scale)
|
|
jacobian = np.zeros((3 * row_count, 3 + 3 * 15))
|
|
for index, (lever, nuisance) in enumerate(zip(lever_blocks, nuisance_blocks)):
|
|
rows = slice(index * row_count, (index + 1) * row_count)
|
|
columns = slice(3 + index * 15, 3 + (index + 1) * 15)
|
|
jacobian[rows, :3] = lever
|
|
jacobian[rows, columns] = nuisance
|
|
residual = np.ones(jacobian.shape[0])
|
|
combined = _marginal_lever_information(jacobian, residual)[0]
|
|
expected = np.zeros((3, 3))
|
|
for lever, nuisance in zip(lever_blocks, nuisance_blocks):
|
|
local = np.column_stack([lever, nuisance])
|
|
expected += _marginal_lever_information(local, np.ones(row_count))[0]
|
|
assert np.allclose(combined, expected, rtol=1e-9, atol=1e-9)
|
|
|
|
|
|
def test_per_node_graph_has_finite_residual_and_matching_sparse_structure() -> None:
|
|
lever = np.array([0.24,-0.18,0.36])
|
|
rotation_at = lambda t: Rotation.from_euler('z',.12*t).as_matrix()
|
|
session = _rows_from_motion('node_graph',lever,np.eye(3),rotation_at,duration_s=10.)
|
|
segments = _segments([session],.5)
|
|
assert segments
|
|
problem = build_problem(segments[0],np.eye(3),lever)
|
|
x0 = node_graph_initial_parameters(problem)
|
|
value = node_graph_residual(problem,x0)
|
|
sparsity = node_graph_jacobian_sparsity(problem,x0)
|
|
assert x0.size == 15*len(problem.segment.nodes)
|
|
assert np.all(np.isfinite(value))
|
|
assert sparsity.shape == (value.size,x0.size)
|
|
assert sparsity.nnz > value.size
|
|
|
|
|
|
def test_node_graph_hpr_override_retains_bridge_extra_variance() -> None:
|
|
problem=SimpleNamespace(hpr_direct_angular_sigma_rad=.006)
|
|
direct=SimpleNamespace(hpr_angular_sigma_rad=HPR_DIRECT_ANGULAR_SIGMA_RAD)
|
|
assert _hpr_sigma(problem,direct)==pytest.approx(.006)
|
|
old=_hpr_angular_sigma_rad('bracket_interpolation',.4)
|
|
bridge=SimpleNamespace(hpr_angular_sigma_rad=old)
|
|
expected=np.sqrt(.006**2+old**2-HPR_DIRECT_ANGULAR_SIGMA_RAD**2)
|
|
assert _hpr_sigma(problem,bridge)==pytest.approx(expected)
|
|
|
|
|
|
def test_node_graph_free_lever_layout_extends_fixed_state_by_three() -> None:
|
|
lever=np.array([.24,-.18,.36])
|
|
rotation_at=lambda t: Rotation.from_euler('z',.12*t).as_matrix()
|
|
session=_rows_from_motion('node_graph_free',lever,np.eye(3),rotation_at,duration_s=5.)
|
|
problem=build_problem(_segments([session],.5)[0],np.eye(3),lever,.006)
|
|
value=np.concatenate([np.zeros(3),node_graph_initial_parameters(problem,np.zeros(3))])
|
|
residual=_free_residual(problem,value)
|
|
sparsity=_free_sparsity(problem,value)
|
|
assert np.all(np.isfinite(residual))
|
|
assert sparsity.shape==(residual.size,value.size)
|
|
|
|
|
|
def test_node_graph_block_schur_information_is_additive() -> None:
|
|
rng=np.random.default_rng(7); rows=24; nuisance=5
|
|
local=[rng.normal(size=(rows,3+nuisance)) for _ in range(2)]
|
|
global_jac=np.zeros((2*rows,3+2*nuisance))
|
|
global_jac[:rows,:3]=local[0][:,:3]
|
|
global_jac[:rows,3:3+nuisance]=local[0][:,3:]
|
|
global_jac[rows:,:3]=local[1][:,:3]
|
|
global_jac[rows:,3+nuisance:]=local[1][:,3:]
|
|
actual=_additive_marginal_lever_information(
|
|
global_jac,[0,rows,2*rows],[3,3+nuisance,3+2*nuisance])
|
|
expected=np.zeros((3,3))
|
|
for jac in local:
|
|
H=jac.T@jac
|
|
expected+=H[:3,:3]-H[:3,3:]@np.linalg.pinv(H[3:,3:])@H[3:,:3]
|
|
assert np.allclose(actual,expected,rtol=1e-10,atol=1e-10)
|
|
|
|
|
|
def test_node_graph_soft_lever_prior_adds_exact_information() -> None:
|
|
lever=np.array([.24,-.18,.36])
|
|
rotation_at=lambda t: Rotation.from_euler('z',.12*t).as_matrix()
|
|
session=_rows_from_motion('node_graph_prior',lever,np.eye(3),rotation_at,
|
|
duration_s=5.)
|
|
problem=build_problem(_segments([session],.5)[0],np.eye(3),lever,.006)
|
|
free=solve_free_lever_many([problem],lever,max_nfev=1)
|
|
covariance=np.diag([.02**2,.02**2,.03**2])
|
|
prior=solve_free_lever_many([problem],lever,max_nfev=1,
|
|
lever_prior_mean_m=lever,lever_prior_covariance_m2=covariance)
|
|
free_information=np.linalg.pinv(free.lever_covariance_m2,rcond=1e-9)
|
|
prior_information=np.linalg.pinv(prior.lever_covariance_m2,rcond=1e-9)
|
|
assert np.allclose(prior_information-free_information,
|
|
np.linalg.inv(covariance),rtol=1e-6,atol=1e-5)
|
|
|
|
|
|
def test_heldout_gate_does_not_accept_underdispersed_statistics() -> None:
|
|
vector={'count':1,'axis_rms':[0.,0.,0.],'axis_p95_abs':[0.,0.,0.],
|
|
'vector_rms':.01,'vector_p95':.02}
|
|
summary={'optimizer_converged_fraction':1.,
|
|
'global_chi_square_per_dof':.127,
|
|
'best_position_physical_m':vector,
|
|
'doppler_physical_m_s':vector,
|
|
'residual_by_factor':{
|
|
'hpr':{'p95_abs':1.3},
|
|
'imu_preintegration':{'p95_abs':.28}}}
|
|
result=heldout_gate(summary)
|
|
assert not result['passed']
|
|
assert not result['checks']['global_chi_square_per_dof_in_0p25_4']
|
|
|
|
|
|
def test_fixed_lever_retry_starts_from_previous_final_state() -> None:
|
|
lever=np.array([.24,-.18,.36])
|
|
rotation_at=lambda t: Rotation.from_euler('z',.12*t).as_matrix()
|
|
session=_rows_from_motion('node_graph_retry',lever,np.eye(3),rotation_at,
|
|
duration_s=5.)
|
|
problem=build_problem(_segments([session],.5)[0],np.eye(3),lever,.006)
|
|
state,first=fit_states_at_fixed_lever(problem,lever,max_nfev=1)
|
|
_,retry=fit_states_at_fixed_lever(
|
|
problem,lever,max_nfev=1,initial_state_values=state)
|
|
assert retry['initial_cost']==pytest.approx(first['cost'])
|
|
|
|
|
|
def test_innovation_summary_reports_vector_and_temporal_metrics() -> None:
|
|
records=[{'residual':np.array([float(i),0.,0.]),
|
|
'normalized':np.array([float(i),0.,0.]),
|
|
'normalized_factor_only':np.array([float(i),0.,0.]),
|
|
't_s':float(i)} for i in range(4)]
|
|
result=innovation_summary(records)
|
|
assert result['vector_p95']>result['vector_p50']
|
|
assert result['temporal_linear_drift_per_s'][0]==pytest.approx(1.)
|
|
assert np.asarray(result['empirical_covariance']).shape==(3,3)
|
|
|
|
|
|
def test_propagation_root_report_detects_common_constant_acceleration() -> None:
|
|
acceleration=np.array([.2,-.02,-.01]); position=[]; velocity=[]
|
|
for index,dt in enumerate((.8,1.,1.2,1.4)):
|
|
base={'interval_id':str(index),'session':'s','motion':'m','t_s':index,
|
|
'speed_bin':'slow','gyro_bin':'low','dt_s':dt,'R0_WI':np.eye(3)}
|
|
position.append({**base,'residual':.5*acceleration*dt*dt})
|
|
velocity.append({**base,'residual':acceleration*dt})
|
|
report,_=_root_report(position,velocity)
|
|
assert report['common_constant_acceleration_error_detected']
|
|
assert np.allclose(
|
|
report['overall']['difference_velocity_minus_position_m_s2']['bias'],0.)
|