41-EFT.WP.Comms.Navigation v1.0 | Chapter 7: Kinematics & Dynamics — IMU / Odometry / Vision

Home ／ Docs-Technical WhitePaper ／ 41-EFT.WP.Comms.Navigation v1.0

Chapter 7: Kinematics & Dynamics — IMU / Odometry / Vision

I. Objectives & Applicability

• Provide minimal usable continuous/discrete kinematic–dynamic models, error-state linearization, IMU noise & calibration, preintegration (factor-graph), wheel odometry and visual measurements (mono/stereo/depth) with fusion interfaces.
• All formulas/symbols/definitions are in English and wrapped in backticks. When coupled with arrival-time channels (e.g., fused with TOA/TDOA/FOA/CP), use the selected T_arr convention and explicitly record gamma(ell) and d ell in dataset cards.

II. State & Process Models (Continuous/Discrete)

• S70-1 (State vector)
  x = [ p_WB, v_WB, q_WB, b_g, b_a ], with p_WB ∈ ℝ^3, v_WB ∈ ℝ^3, q_WB ∈ SO(3), b_g,b_a ∈ ℝ^3. Optionally augment x ← [x, s_vo, Δt_cam, Δt_wheel] (mono scale and timing offsets).
• S70-2 (Continuous-time model)
  \dot p_WB = v_WB
  \dot v_WB = R(q_WB) ( a_m − b_a − n_a ) + g_W
  \dot q_WB = 0.5 · Ω( ω_m − b_g − n_g ) · q_WB
  \dot b_g = n_{wg}, \dot b_a = n_{wa}
  where a_m, ω_m are IMU measurements, n_* Gaussian noises, and Ω(·) the quaternion angular-velocity operator.
• S70-3 (Discretization — zero-order hold)
  With step Δt:
  x_{k+1} ≈ f(x_k,u_k,Δt) = [ p_k + v_k Δt + 0.5( R_k( a_m−b_a ) + g_W )Δt^2; v_k + ( R_k( a_m−b_a ) + g_W )Δt; q_k ⊗ Exp( (ω_m−b_g)Δt ); b_{g,k}; b_{a,k} ].
  Linearization gives transition F_k and noise map G_k, with Q_k = G_k Q_c G_k^T Δt.

III. Error-State & Linearization (ES-EKF)

• S70-4 (Error parametrization)
  δx = [ δp, δv, δθ, δb_g, δb_a ], with small-angle δθ (left-multiplicative SO(3) error).
• S70-5 (Error dynamics)
  δ\dot x = A · δx + L · n, where
  A = [[0,I,0,0,0],[0,0,-R( a_m−b_a )×, -R, -I],[0,0,-(ω_m−b_g)×, -I, 0],[0,0,0,0,0],[0,0,0,0,0]] (schematic), n = [n_a,n_g,n_{wa},n_{wg}].
• S70-6 (Discrete propagation)
  P_{k+1} = Φ_k P_k Φ_k^T + Q_k, with Φ_k = expm(A_k Δt).

IV. IMU Noise, Stability & Calibration

• S70-7 (Noise models)
  Gyro/acc measurement noise densities σ_g, σ_a and bias random walks σ_{wg}, σ_{wa}; estimate Q_c via Allan deviation σ_y(τ).
• S70-8 (Temperature / misalignment / scale)
  ω_true = M_g ω_m + o_g + T_g(T), a_true = M_a a_m + o_a + T_a(T), with calibration matrices M_*, biases o_*, and temperature terms T_*.
• M7-1 (IMU calibration workflow)
  Static/turntable/multi-pose data → estimate M_g,M_a,o_g,o_a,σ_* → emit calibration & noise cards.

V. Preintegration (Factor Graph / VIO)

• S70-9 (Preintegrated quantities)
  Over window [k,k+1], define ΔR, Δv, Δp with covariance and Jacobians w.r.t. b_g,b_a; first-order bias corrections applied.
• S70-10 (Preintegration residuals)
  r_ΔR = Log( (ΔR_meas)^{-1} · ( R_k^T R_{k+1} ) )
  r_Δv = R_k^T ( v_{k+1} − v_k − g_W Δt ) − Δv_meas − J_{vg} δb_g − J_{va} δb_a
  r_Δp = R_k^T ( p_{k+1} − p_k − v_k Δt − 0.5 g_W Δt^2 ) − Δp_meas − J_{pg} δb_g − J_{pa} δb_a
• M7-2 (Preintegration build)
  Input IMU stream, calib, σ_* → accumulate ΔR,Δv,Δp,Σ_Δ + Jacobians and write factors into the graph.

VI. Wheel Odometry

• S70-11 (Measurement model)
  y_w = V_B + ε_w or differential-drive y_w = [v_x, ω_z]^T + ε, tied to ground constraints v_y≈0 and no-slip assumptions.
• S70-12 (Residual)
  r_w = V_B(x) − y_w, with V_B(x) = [ (R_WB^T v_WB)_x, (ω_B)_z ]^T.
• S70-13 (Systematics)
  Wheel radius/gear and sync errors → scale s_w and bias b_w; slip handled via mixture likelihood or Huber/Tukey reweighting.

VII. Vision (VO/VIO)

• S70-14 (Projection model)
  Pinhole: u = Π( K · ( R_BC ( R_WB^T ( P_W − p_WB ) ) + t_BC ) ), where K intrinsics, T_BC=[R_BC,t_BC].
• S70-15 (Feature residual)
  r_uv = u_meas − Π(·); take Jacobians w.r.t. x, T_BC, K; rolling-shutter handled by a time-row model.
• S70-16 (Scale & timing offsets)
  Mono introduces s_vo; camera–IMU offset Δt_cam warps reprojection timing.
• M7-3 (Extrinsic & timing calibration)
  Hand–eye/self-cal/time-alignment → estimate T_BC, Δt_cam with uncertainties.

VIII. Fusion Framework & Numerics

• S70-17 (Filtering/smoothing)
  ES-EKF/UKF and factor-graph sliding-window MAP; IMU preintegration + vision/odometry/geometry factors jointly optimized.
• S70-18 (Marginalization & windowing)
  Schur-marginalize stale keyframes to a prior; use sparse Cholesky/LM with robust kernels.
• M7-4 (Fusion pipeline)
  ingest → time_sync → imu_propagate → factors_build → optimize → out_states, producing x̂, P or trajectory + cov.

IX. Coupling with Chapters 4/5/6

• S70-19 (Channel fusion)
  TOA/TDOA/AOA/FOA/CP residuals plus this chapter’s priors form H, Σ_y; FOA vertical gains align with Chapter 5.
• S70-20 (Sync priors)
  Inject cov([b_t, \dot b_t]) from Chapter 6 into the state or observation covariance.
• S70-21 (Geometry design)
  Choose observation geometry and keyframe policy per Chapter 5’s GDOP/cond(F).

X. Data Contract (Required/Recommended Fields for This Chapter)

unit_system: "SI"

imu:

sigmas: {sigma_g: "<rad/s/√Hz>", sigma_a: "<m/s^2/√Hz>", sigma_wg: "<rad/s^2/√Hz>", sigma_wa: "<m/s^3/√Hz>"}

calib: {Mg: "<3x3>", Ma: "<3x3>", og: "<3>", oa: "<3>", temp_model: "<optional>"}

gravity_W: [0, 0, -9.80665]

wheel:

model: "vx_omega|full_kinematic"

scale: "<s_w>", bias: "<b_w>", slip_flag: "<bool|score>"

vision:

camera: {K: "<3x3>", dist: "<coeffs>", model: "pinhole|fisheye"}

extrinsics: {T_BC: "<SE3>", cov: "<6x6>"}

timing: {dt_cam: "<s>", mode: "global|rolling"}

state0:

p_WB: "<m>", v_WB: "<m/s>", q_WB: "<w,x,y,z>", bg: "<rad/s>", ba: "<m/s^2>"

fusion:

window: {size: n, strategy: "keyframe|fixed-lag"}

robust: {loss: "Huber|Cauchy", params: {...}}

covariance:

Qc: "<process PSD>", P0: "<prior>", Σ_meas: "<block-diagonal or sparse>"

references:

- "EFT.WP.Comms.Navigation v1.0:Ch.2 S20-*"

- "EFT.WP.Comms.Navigation v1.0:Ch.4 S40-*"

- "EFT.WP.Comms.Navigation v1.0:Ch.5 S50-*"

- "EFT.WP.Comms.Navigation v1.0:Ch.6 S60-*"

XI. Implementation Bindings (Interface Prototypes)

• I7-1 imu_propagate(x, P, imu_stream, calib, dt) -> {x_pred, P_pred}
• I7-2 imu_preintegrate(imu_stream, calib) -> {ΔR, Δv, Δp, Σ_Δ, J_Δ}
• I7-3 build_wheel_factor(y_w, model, cov) -> {factor_w}
• I7-4 build_vision_factor(tracks, K, T_BC, timing) -> {factors_vo}
• I7-5 calibrate_extrinsics(sync_data) -> {T_BC, Δt_cam, cov}
• I7-6 synchronize_timestamps(streams, mode) -> {aligned_streams, Δt}
• I7-7 run_filter(init, factors, priors) -> {x̂, P}
• I7-8 run_smoother(window, factors, priors) -> {traj, cov}

XII. Quality Gates (This Chapter)

• Q1 Dimensions/units: pass check_dim; SI columns present (rad, m, s, m/s, m/s^2).
• Q2 Noise consistency: σ_g, σ_a, σ_{wg}, σ_{wa} consistent with Allan fits; process Q_c consistent with discrete Q_k.
• Q3 Extrinsics/timing: dataset explicitly records T_BC, Δt_cam, Δt_wheel estimates/uncertainties and updates factors accordingly.
• Q4 Preintegration correctness: covariances/Jacobians of ΔR,Δv,Δp include first-order bias updates; numerically stable (avoid singularities).
• Q5 Robustness: slip/occlusion/rolling shutter anomalies enter via nlos/slip flags or robust losses; marginalized priors remain well-conditioned.

XIII. Cross-Volume References & Anchors (This Chapter)

1. Cross-volume (fixed style): this volume Ch. 2 (terminology/metrology), Ch. 4 (observation models), Ch. 5 (geometry & GDOP), Ch. 6 (sync chain).
2. Anchors:
   • Minimal statements: S70-1—S70-21
   • Workflows: M7-1—M7-4
   • Interfaces: I7-1—I7-8

XIV. Summary
This chapter delivers end-to-end modeling and bindings from continuous/discrete dynamics, error-state formulation, IMU noise & calibration, to preintegration and odometry/vision measurements. Through unified contracts, quality gates, and interfaces, kinematic priors are injected stably into fusion frameworks and coordinated with geometry/sync/channel observations (Chs. 4–6), supporting fusion estimation (Ch. 8) and experiment design (Ch. 10).