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.)