lichun.qu/calibration
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

重构RTK-IMU标定链路并完成机械先验工程验证

Browse Source
This commit is contained in:
lichun.qu
2026-08-25 09:56:25 +08:00
parent c2da6dd192
commit d14ae74117
56 changed files with 118382 additions and 159 deletions
Download Patch File Download Diff File Expand all files Collapse all files
imu_lidar/rtk_imu_rotation.py
+301 -52
View File
@@ -2,7 +2,7 @@
from __future__ import annotations
from dataclasses import dataclass
from dataclasses import dataclass, replace
import numpy as np
from scipy.optimize import least_squares
@@ -31,6 +31,28 @@ class TimeOffsetAudit:
second_best_correlation: float
evaluated_samples: int
reliable: bool
method: str
peak_width_s: tuple[float, float]
per_session_offset_s: dict[str, float]
per_session_peak_correlation: dict[str, float]
@dataclass(frozen=True)
class BaselineConsistencyAudit:
"""Two-DOF audit using only the physically observed ANT1-to-ANT2 axis."""
baseline_axis_imu: np.ndarray
pair_count: int
residual_rms_deg: float
residual_median_deg: float
residual_p95_deg: float
per_session_rms_deg: dict[str, float]
per_session_p95_deg: dict[str, float]
per_session_axis_rms_deg: dict[str, np.ndarray]
gyro_bias_by_session_rad_s: dict[str, np.ndarray]
worst_pairs: tuple[dict[str, object], ...]
ok: bool
notes: tuple[str, ...]
@dataclass(frozen=True)
@@ -50,6 +72,10 @@ class RotationCalibrationResult:
information_singular_values: np.ndarray
per_session_rms_deg: dict[str, float]
loo_delta_deg: dict[str, float]
baseline_consistency: BaselineConsistencyAudit
observable_rotation_dof: int
full_attitude_observable: bool
legacy_full_attitude_numeric_ok: bool
ok: bool
notes: tuple[str, ...]
@@ -62,6 +88,8 @@ class _Pair:
delta_R_zero_bias: np.ndarray
J_bg: np.ndarray
weight: float
t0_s: float
t1_s: float
def _attitude_rows(rtk: RtkSeries, convention: GnhprConvention) -> tuple[np.ndarray, np.ndarray]:
@@ -91,14 +119,25 @@ def _attitude_rows(rtk: RtkSeries, convention: GnhprConvention) -> tuple[np.ndar
return unique_t, rotations
def _rtk_angular_speed(t: np.ndarray, rotations: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
def _rtk_heading_rate(t: np.ndarray, rotations: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
"""Return signed vehicle yaw rate from the ANT1-to-ANT2 azimuth.
ANT1-to-ANT2 points vehicle-right, so its clockwise heading increases when
mathematical body yaw decreases. Only this signed heading channel is
compared with IMU gyro_z; rotation about the baseline is unobservable.
"""
dt = np.diff(t)
valid = (dt >= 0.03) & (dt <= 0.5)
midpoint = 0.5 * (t[:-1] + t[1:])
speed = np.array(
[np.linalg.norm(so3_log(rotations[i].T @ rotations[i + 1])) for i in range(t.size - 1)]
) / np.maximum(dt, 1e-6)
return midpoint[valid], speed[valid]
baseline = rotations[:, :, 0]
heading = np.unwrap(np.arctan2(baseline[:, 0], baseline[:, 1]))
rate = -np.diff(heading) / np.maximum(dt, 1e-6)
valid = (
(dt >= 0.03)
& (dt <= 0.25)
& (np.abs(rate) >= np.deg2rad(0.5))
& (np.abs(rate) <= np.deg2rad(30.0))
)
return 0.5 * (t[:-1] + t[1:])[valid], rate[valid]
def _correlation(a: np.ndarray, b: np.ndarray) -> float:
@@ -115,45 +154,102 @@ def audit_time_offset(
search_half_width_s: float = 0.30,
step_s: float = 0.005,
) -> TimeOffsetAudit:
"""Estimate residual ``t_IMU - t_RTK`` from invariant angular-speed norms."""
"""Audit residual t_IMU - t_RTK from signed heading rate.
A broad correlation peak remains diagnostic. It is never applied unless
both peak separation and peak width pass.
"""
offsets = np.arange(-search_half_width_s, search_half_width_s + 0.5 * step_s, step_s)
session_series: list[tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]] = []
session_series: list[tuple[str, np.ndarray, np.ndarray, np.ndarray, np.ndarray]] = []
total_samples = 0
for session in sessions:
t_rtk, rotations = _attitude_rows(session.rtk, GNHPR_CANDIDATES[0])
midpoint, rtk_speed = _rtk_angular_speed(t_rtk, rotations)
imu_speed = np.linalg.norm(session.imu.gyro_rad_s, axis=1)
motion = rtk_speed > np.deg2rad(0.5)
midpoint = midpoint[motion]
rtk_speed = rtk_speed[motion]
midpoint, rtk_rate = _rtk_heading_rate(t_rtk, rotations)
imu_rate = session.imu.gyro_rad_s[:, 2]
if midpoint.size >= 20:
session_series.append((midpoint, rtk_speed, session.imu.t_s, imu_speed))
session_series.append(
(session.session_id, midpoint, rtk_rate, session.imu.t_s, imu_rate)
)
total_samples += int(midpoint.size)
if not session_series:
return TimeOffsetAudit(0.0, np.nan, np.nan, 0, False)
return TimeOffsetAudit(
0.0,
np.nan,
np.nan,
0,
False,
"signed_heading_rate_vs_imu_gyro_z",
(np.nan, np.nan),
{},
{},
)
scores = []
per_session_scores: dict[str, list[float]] = {
session_id: [] for session_id, *_ in session_series
}
for offset in offsets:
per_session = []
for midpoint, rtk_speed, imu_t, imu_speed in session_series:
for session_id, midpoint, rtk_rate, imu_t, imu_rate in session_series:
query = midpoint + offset
inside = (query >= imu_t[0]) & (query <= imu_t[-1])
if np.count_nonzero(inside) < 20:
per_session_scores[session_id].append(np.nan)
continue
interpolated = np.interp(query[inside], imu_t, imu_speed)
value = _correlation(rtk_speed[inside], interpolated)
interpolated = np.interp(query[inside], imu_t, imu_rate)
value = _correlation(rtk_rate[inside], interpolated)
per_session_scores[session_id].append(value)
if np.isfinite(value):
per_session.append(value)
scores.append(float(np.median(per_session)) if per_session else np.nan)
values = np.asarray(scores, dtype=float)
if not np.any(np.isfinite(values)):
return TimeOffsetAudit(0.0, np.nan, np.nan, total_samples, False)
return TimeOffsetAudit(
0.0,
np.nan,
np.nan,
total_samples,
False,
"signed_heading_rate_vs_imu_gyro_z",
(np.nan, np.nan),
{},
{},
)
best_index = int(np.nanargmax(values))
exclusion = np.abs(offsets - offsets[best_index]) >= 0.03
second = float(np.nanmax(values[exclusion])) if np.any(np.isfinite(values[exclusion])) else np.nan
peak = float(values[best_index])
reliable = bool(peak >= 0.35 and (not np.isfinite(second) or peak - second >= 0.015))
return TimeOffsetAudit(float(offsets[best_index]), peak, second, total_samples, reliable)
near_peak = np.flatnonzero(values >= peak - 0.005)
peak_width = (
(float(offsets[near_peak[0]]), float(offsets[near_peak[-1]]))
if near_peak.size
else (np.nan, np.nan)
)
per_session_offset = {}
per_session_peak = {}
for session_id, session_values in per_session_scores.items():
array = np.asarray(session_values, dtype=float)
if np.any(np.isfinite(array)):
index = int(np.nanargmax(array))
per_session_offset[session_id] = float(offsets[index])
per_session_peak[session_id] = float(array[index])
reliable = bool(
peak >= 0.5
and (not np.isfinite(second) or peak - second >= 0.015)
and np.isfinite(peak_width[0])
and peak_width[1] - peak_width[0] <= 0.03
)
return TimeOffsetAudit(
float(offsets[best_index]),
peak,
second,
total_samples,
reliable,
"signed_heading_rate_vs_imu_gyro_z",
peak_width,
per_session_offset,
per_session_peak,
)
def _nearest_index(times: np.ndarray, target: float) -> int:
@@ -175,6 +271,17 @@ def _make_pairs(
cache = {} if preintegration_cache is None else preintegration_cache
for session_index, session in enumerate(sessions):
t, rotations = _attitude_rows(session.rtk, convention)
dt = np.diff(t)
baseline = rotations[:, :, 0]
baseline_step = np.arccos(
np.clip(np.sum(baseline[:-1] * baseline[1:], axis=1), -1.0, 1.0)
)
broken_edge = (
(dt < 0.03)
| (dt > 0.25)
| (baseline_step / np.maximum(dt, 1e-6) > np.deg2rad(45.0))
)
broken_prefix = np.concatenate([[0], np.cumsum(broken_edge.astype(int))])
next_anchor = float(t[0])
for i in range(t.size - 1):
if t[i] + 1e-9 < next_anchor:
@@ -184,6 +291,8 @@ def _make_pairs(
j = _nearest_index(t, float(t[i] + duration))
if j <= i or abs(float(t[j] - t[i]) - duration) > 0.18:
continue
if broken_prefix[j] - broken_prefix[i] != 0:
continue
imu_t0 = float(t[i] + time_offset_s)
imu_t1 = float(t[j] + time_offset_s)
if imu_t0 < session.imu.t_s[0] or imu_t1 > session.imu.t_s[-1]:
@@ -212,9 +321,25 @@ def _make_pairs(
delta_R_zero_bias=preint.delta_R,
J_bg=preint.J_bg,
weight=weight,
t0_s=float(t[i]),
t1_s=float(t[j]),
)
)
return pairs
# Equalize total influence per session. Pair count and excitation otherwise
# let long/high-motion sessions dominate the shared rotation.
totals = {
session.session_id: sum(
pair.weight for pair in pairs if pair.session_id == session.session_id
)
for session in sessions
}
nonzero = [value for value in totals.values() if value > 0.0]
target = float(np.mean(nonzero)) if nonzero else 1.0
return [
replace(pair, weight=pair.weight * target / totals[pair.session_id])
for pair in pairs
if totals[pair.session_id] > 0.0
]
def _solve_core(sessions: list[RotationSession], pairs: list[_Pair]) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
@@ -290,6 +415,129 @@ def _solve_core(sessions: list[RotationSession], pairs: list[_Pair]) -> tuple[np
return r_x, biases, np.asarray(errors), covariance
def _solve_baseline_consistency(
sessions: list[RotationSession],
pairs: list[_Pair],
) -> BaselineConsistencyAudit:
"""Audit the two physically observable dual-antenna rotation DOFs.
The confirmed ANT1-to-ANT2 axis is IMU +X. For every interval, the angle
swept by the GNSS baseline must equal the angle swept by IMU +X under gyro
preintegration. Rotation about +X cancels from this invariant and is not
falsely scored as an RTK attitude residual.
"""
baseline_axis = np.array([1.0, 0.0, 0.0])
session_count = len(sessions)
def raw_errors(parameters: np.ndarray) -> np.ndarray:
biases = parameters.reshape(session_count, 3)
values = []
for pair in pairs:
corrected = apply_bias_jacobian_correction(
pair.delta_R_zero_bias,
pair.J_bg,
biases[pair.session_index],
)
observed = np.arccos(np.clip(pair.R_A[0, 0], -1.0, 1.0))
predicted = np.arccos(
np.clip(baseline_axis @ corrected @ baseline_axis, -1.0, 1.0)
)
values.append(predicted - observed)
return np.asarray(values)
def residual(parameters: np.ndarray) -> np.ndarray:
errors = raw_errors(parameters)
weighted = errors * np.sqrt(np.asarray([pair.weight for pair in pairs]))
return np.concatenate([weighted, parameters / 0.003])
initial = np.zeros(3 * session_count)
opt = least_squares(
residual,
initial,
loss="huber",
f_scale=np.deg2rad(0.25),
max_nfev=60,
)
biases = opt.x.reshape(session_count, 3)
errors_deg = np.degrees(raw_errors(opt.x))
per_session_rms = {}
per_session_p95 = {}
per_session_axis_rms = {}
for session in sessions:
selection = np.asarray(
[pair.session_id == session.session_id for pair in pairs], dtype=bool
)
values = errors_deg[selection]
per_session_rms[session.session_id] = (
float(np.sqrt(np.mean(values**2))) if values.size else np.nan
)
per_session_p95[session.session_id] = (
float(np.percentile(np.abs(values), 95.0)) if values.size else np.nan
)
axis_errors = []
for pair in np.asarray(pairs, dtype=object)[selection]:
corrected = apply_bias_jacobian_correction(
pair.delta_R_zero_bias,
pair.J_bg,
biases[pair.session_index],
)
axis_errors.append(np.degrees(so3_log(pair.R_A @ corrected.T)))
per_session_axis_rms[session.session_id] = (
np.sqrt(np.mean(np.asarray(axis_errors) ** 2, axis=0))
if axis_errors
else np.full(3, np.nan)
)
worst_indices = np.argsort(np.abs(errors_deg))[-20:][::-1]
worst_pairs = tuple(
{
"session_id": pairs[index].session_id,
"t0_s": pairs[index].t0_s,
"t1_s": pairs[index].t1_s,
"duration_s": pairs[index].t1_s - pairs[index].t0_s,
"baseline_angle_residual_deg": float(errors_deg[index]),
}
for index in worst_indices
)
rms = float(np.sqrt(np.mean(errors_deg**2)))
median = float(np.median(np.abs(errors_deg)))
p95 = float(np.percentile(np.abs(errors_deg), 95.0))
finite_session_rms = [
value for value in per_session_rms.values() if np.isfinite(value)
]
finite_session_p95 = [
value for value in per_session_p95.values() if np.isfinite(value)
]
ok = bool(
len(pairs) >= 20
and rms <= 1.0
and p95 <= 2.0
and (not finite_session_rms or max(finite_session_rms) <= 1.5)
and (not finite_session_p95 or max(finite_session_p95) <= 3.0)
)
return BaselineConsistencyAudit(
baseline_axis_imu=baseline_axis,
pair_count=len(pairs),
residual_rms_deg=rms,
residual_median_deg=median,
residual_p95_deg=p95,
per_session_rms_deg=per_session_rms,
per_session_p95_deg=per_session_p95,
per_session_axis_rms_deg=per_session_axis_rms,
gyro_bias_by_session_rad_s={
session.session_id: biases[index].copy()
for index, session in enumerate(sessions)
},
worst_pairs=worst_pairs,
ok=ok,
notes=(
"ANT1(main,left)->ANT2(secondary,right) is fixed to IMU +X",
"axis residual XYZ labels are baseline-spin(unobservable), baseline-elevation, heading",
"full rotation about the baseline is not identifiable from two antennas",
),
)
def solve_rtk_imu_rotation(
sessions: list[RotationSession] | tuple[RotationSession, ...],
*,
@@ -351,41 +599,28 @@ def solve_rtk_imu_rotation(
per_session[session.session_id] = (
float(np.sqrt(np.mean(np.asarray(values) ** 2))) if values else np.nan
)
baseline_audit = _solve_baseline_consistency(items, pairs)
loo = {}
if compute_loo and len(items) >= 3:
for omitted in items:
kept_items = [item for item in items if item.session_id != omitted.session_id]
kept_index = {item.session_id: index for index, item in enumerate(kept_items)}
kept_pairs = [
pair
replace(pair, session_index=kept_index[pair.session_id])
for pair in pairs
if pair.session_id != omitted.session_id
]
if len(kept_pairs) < 6:
loo[omitted.session_id] = np.nan
continue
def loo_residual(rotvec: np.ndarray) -> np.ndarray:
candidate = orthonormalize_rotation(so3_exp(rotvec))
rows = []
for pair in kept_pairs:
corrected = apply_bias_jacobian_correction(
pair.delta_R_zero_bias,
pair.J_bg,
biases[pair.session_index],
)
rows.append(
np.sqrt(pair.weight)
* so3_log(candidate.T @ pair.R_A @ candidate @ corrected.T)
)
return np.concatenate(rows)
loo_opt = least_squares(
loo_residual,
so3_log(rotation),
loss='huber',
f_scale=np.deg2rad(0.5),
max_nfev=30,
try:
loo_rotation, _, _, _ = _solve_core(kept_items, kept_pairs)
except ValueError:
loo[omitted.session_id] = np.nan
continue
loo[omitted.session_id] = float(
np.degrees(np.linalg.norm(so3_log(rotation.T @ loo_rotation)))
)
loo_rotation = orthonormalize_rotation(so3_exp(loo_opt.x))
loo[omitted.session_id] = float(np.degrees(np.linalg.norm(so3_log(rotation.T @ loo_rotation))))
rotation_cov = covariance[:3, :3]
std_deg = np.degrees(np.sqrt(np.maximum(np.diag(rotation_cov), 0.0)))
singular_values = np.linalg.svd(np.linalg.pinv(rotation_cov, rcond=1e-12), compute_uv=False)
@@ -395,7 +630,7 @@ def solve_rtk_imu_rotation(
finite_loo = [value for value in loo.values() if np.isfinite(value)]
sorted_scores = sorted(scores.values())
convention_gap = sorted_scores[1] - sorted_scores[0] if len(sorted_scores) > 1 else np.inf
ok = bool(
legacy_numeric_ok = bool(
len(pairs) >= 20
and rms <= 1.0
and p95 <= 2.0
@@ -403,19 +638,29 @@ def solve_rtk_imu_rotation(
and (not finite_loo or max(finite_loo) <= 1.0)
and convention_gap >= 0.05
)
# GNHPR supplies the ANT1-to-ANT2 direction but no independent rotation
# about that direction. A completed 3-D attitude is useful diagnostically,
# but cannot pass the full extrinsic-rotation gate from this dataset alone.
full_attitude_observable = False
ok = False
notes = [
"transform convention: p_RTK = R_RTK_IMU p_IMU",
f"residual time convention: t_IMU = t_RTK + {offset:+.6f} s",
f"GNHPR convention score gap={convention_gap:.4f} deg",
"GNHPR alternatives use zero-bias prescreen scores; only the winner is jointly refined",
"LOO is conditional: per-session gyro biases are held at their all-session estimates",
"LOO re-optimizes the remaining per-session gyro biases",
"legacy full-HPR rotation uses a zero-roll gauge completion and is diagnostic only",
"dual antennas do not observe rotation about the ANT1-to-ANT2 baseline",
]
if not time_audit.reliable:
notes.append("time-offset correlation was ambiguous; held residual offset at zero")
if convention is not GNHPR_CANDIDATES[0]:
notes.append("empirical best GNHPR convention differs from protocol expectation; manual verification required")
if not ok:
notes.append("rotation failed one or more strict acceptance gates")
if not baseline_audit.ok:
notes.append("the physically observable baseline consistency failed strict gates")
if not legacy_numeric_ok:
notes.append("the legacy gauge-completed rotation failed one or more numeric gates")
notes.append("full rotation is not accepted; translation must remain frozen")
return RotationCalibrationResult(
R_RTK_IMU=rotation,
rpy_deg=rpy_deg_xyz(rotation),
@@ -434,6 +679,10 @@ def solve_rtk_imu_rotation(
information_singular_values=singular_values,
per_session_rms_deg=per_session,
loo_delta_deg=loo,
baseline_consistency=baseline_audit,
observable_rotation_dof=2,
full_attitude_observable=full_attitude_observable,
legacy_full_attitude_numeric_ok=legacy_numeric_ok,
ok=ok,
notes=tuple(notes),
)