41-EFT.WP.Comms.Navigation v1.0 | 第8章 融合估计：EKF/UKF/粒子/因子图

目录 ／ 文档-技术白皮书 ／ 41-EFT.WP.Comms.Navigation v1.0

第8章 融合估计：EKF/UKF/粒子/因子图

I. 目标与适用域

• 给出融合估计的最小可用框架与可执行流程：误差状态 EKF、UKF、粒子滤波（PF），以及因子图/最小二乘的 MAP 平滑；覆盖观测通道 TOA/TDOA/AOA/FOA/RSS/CP 与第7章动力学先验的联合。
• 公式/符号/定义统一英文并用反引号；凡含到达时量，必须沿用既定 T_arr 口径并显式记录 gamma(ell) 与 d ell。

II. 统一状态—观测基线

• S80-1（状态与过程）：x_k = f(x_{k-1}, u_{k-1}) + w_{k-1}，w ~ 𝒩(0,Q)；x 至少含第7章 x = [p_WB, v_WB, q_WB, b_g, b_a] 与必要的时钟项 [b_t, \dot b_t]。
• S80-2（观测模型）：y_k = h(x_k, θ, ν; x_design) + ε_k，ε ~ 𝒩(0, Σ_y) 或稳健混合；通道见第4章。
• S80-3（雅可比与测量矩阵）：F = ∂f/∂x，H = ∂h/∂x；第10章测量矩阵 J = ∂y/∂θ 用于设计优化但本章以内参 H 为主。

III. 误差状态 EKF（ES-EKF）

• S80-4（预测）：
  x̂_{k|k-1} = f(x̂_{k-1|k-1}, u_{k-1})，P_{k|k-1} = F_{k-1} P_{k-1|k-1} F_{k-1}^T + Q_{k-1}。
• S80-5（更新）：
  S_k = H_k P_{k|k-1} H_k^T + R_k；K_k = P_{k|k-1} H_k^T S_k^{-1}；
  x̂_{k|k} = x̂_{k|k-1} ⊕ ( K_k r_k )，P_{k|k} = (I − K_k H_k) P_{k|k-1}，r_k = y_k − h(x̂_{k|k-1})。
  姿态采用左乘/李代数更新 ⊕。
• M8-1（EKF 步进流程）：propagate(x̂,P) → build H,R → gate → update → normalize(q)。

IV. 无迹卡尔曼滤波（UKF）

• S80-6（σ 点生成）：以 α, β, κ 设定 λ = α^2(n+κ) − n，构造 χ_i 与权重 W_i^{(m)}, W_i^{(c)}。
• S80-7（传播与更新）：
  传播 χ_i 经 f(·) 得 χ_i^-；x̂^- = Σ W_i^{(m)} χ_i^-，P^- = Σ W_i^{(c)} (χ_i^- − x̂^-)(·)^T + Q；
  量测预测同理，经 h(·) 得 ŷ、S、P_{xy}，卡尔曼增益 K = P_{xy} S^{-1} 更新。
• M8-2（UKF 配置）：记录 α, β, κ 与李群上的映射（姿态使用 Exp/Log）。

V. 粒子滤波（PF）

• S80-8（重要性采样）：从 proposal q(x_k|x_{k-1},y_k) 采样；权重更新 w_k^i ∝ w_{k-1}^i · p(y_k|x_k^i) · p(x_k^i|x_{k-1}^i)/q(·)。
• S80-9（退化与重采样）：有效粒子数 N_eff = 1/Σ(w_k^i)^2；若 N_eff < τ_resamp · N，执行系统/分层/残差重采样。
• M8-3（PF 管线）：propagate → weight → normalize → resample(可选) → estimate；支持 Rao-Blackwellized PF（位置 PF + 线性子状态 EKF）。

VI. 因子图 / MAP 平滑（滑窗）

• S80-10（目标函数）：最小化
  J(ξ) = ‖r_0(ξ)‖^2_{P_0^{-1}} + Σ_k ‖r_f(ξ_k,ξ_{k+1})‖^2_{Q_k^{-1}} + Σ_m ‖r_m(ξ_k)‖^2_{R_m^{-1}}，
  其中 ξ 为状态在李代数的最小参数化，r_f 为过程因子（如第7章预积分），r_m 为各观测因子（第4章）。
• S80-11（求解与稀疏性）：高斯-牛顿/LM，稀疏列分解/PCG；鲁棒核 Huber/Cauchy/Tukey。
• S80-12（边缘化）：Schur 消元旧关键帧形成先验因子；保持数值稳定与信息守恒。
• M8-4（图优化流程）：build_factors → robustify → linearize → solve → relinearize(触发) → marginalize。

VII. 数据关联与鲁棒性

• S80-13（门限与匹配）：马氏门控 d^2 = r^T S^{-1} r ≤ τ_gate；最近邻/联合兼容性/多假设跟踪（MHT）。
• S80-14（稳健似然）：混合高斯 p = Σ π_c 𝒩(·) 或 M-估计 ρ(·) 再加权；使用 NLOS_flag/slip_flag/occlusion 作软/硬屏蔽。
• M8-5（关联—鲁棒管线）：predict → gate → assign → robust-weight → update/solve。

VIII. 一致性与性能评估

• S80-15（NEES/NIS）：NEES = e^T P^{-1} e，NIS = r^T S^{-1} r；覆盖率 pass@coverage 应接近标称分位。
• S80-16（轨迹指标）：ATE/RPE、RMSE/CEP/PEB/SEB；因子图采用 χ^2 降幅与增量范数作为收敛判据。
• M8-6（评估流程）：输出一致性曲线、覆盖率统计与收敛报告。

IX. 与第5/6/7/10章的耦合

• S80-17（几何与观测）：第5章 GDOP/cond(F) 指导通道选择与布设；第4章提供 H,R 的构造。
• S80-18（同步先验）：第6章 cov([b_t,\dot b_t]) 并入 R 或状态先验。
• S80-19（动力学与预积分）：第7章提供 f,Q 与预积分因子；与本章 EKF/图优化直接对接。
• S80-20（实验设计）：第10章测量矩阵与 Fisher 信息用于 D/A/E-optimal 设计与传感资源分配。

X. 数据契约（本章必备/推荐字段）

unit_system: "SI"

fusion:

mode: "EKF|UKF|PF|FG" # 过滤或因子图

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

robust: {loss: "Huber|Cauchy|Tukey", tau_gate: "<χ2 quantile>"}

ekf:

linearization: "error-state"

ukf:

sigma_points: {alpha: 1e-3, beta: 2, kappa: 0}

pf:

N: 2048

resample: {scheme: "systematic|stratified|residual", Neff_ratio: 0.5}

factor_graph:

solver: {method: "GN|LM", damping: 1e-3, max_iter: 15, tol: 1e-6}

marginalization: {policy: "Schur", prior_cond_min: 1e-8}

channels:

y_channels: ["TOA","TDOA","AOA","FOA","RSS","CP","IMU","WHEEL","VO"]

Σ_y: "<block-diagonal or sparse>"

arrival_time:

convention: "pulled_const|integrand"

delta_form: "c_ref^-1 * ∫ n_eff dℓ" # 或 "∫ (n_eff/c_ref) dℓ"

gamma: "piecewise: free|fixture|substrate|device|environment"

d_ell: "m"

priors:

P0: "<state prior>", Qc: "<process PSD>", sync_cov: "<bb,bd,dd>"

logging:

consistency: {nees_win: 50, nis_win: 50}

XI. 实现绑定（接口原型）

• I8-1 ekf_step(x̂,P,u,y,H,R,Q) -> {x̂+, P+, stats}
• I8-2 ukf_step(x̂,P,u,y,f,h,Q,R,usp) -> {x̂+, P+, stats}
• I8-3 pf_step(particles, u, y, models, cfg) -> {particles+, est, Neff}
• I8-4 build_graph(factors, priors, cfg) -> {A,b,info}
• I8-5 optimize_graph(A,b,cfg) -> {δξ, cov, report}
• I8-6 marginalize(graph, keep_vars) -> {graph+, prior_factor}
• I8-7 assess_consistency(tracks, P, S) -> {NEES, NIS, pass@coverage}
• I8-8 fuse_batches(batches, mode) -> {traj, cov, metrics}

XII. 质量门（本章适用）

• Q1 口径一致：凡涉及 T_arr 的通道，数据卡必须含 convention/delta_form/gamma(ell)/d_ell。
• Q2 正定与数值稳定：P, Q, R 保持对称正定；LM 阻尼与先验条件数受控。
• Q3 一致性：NEES/NIS 覆盖率在标称±容差内；不合格触发线性化/噪声/鲁棒权重回退策略。
• Q4 稀疏与边缘化：边缘化先验的谱半径与条件数在阈内；图的稀疏结构不被破坏。
• Q5 单位/量纲：所有表达式通过 check_dim；角度 rad，频率 Hz，时间 s，距离 m。

XIII. 跨卷引用与本章锚点

1. 跨卷引用（固定写法）：见本卷第4章（观测模型）/第5章（几何与 GDOP）/第6章（同步链）/第7章（动力学），见《EFT.WP.Core.Metrology v1.0》Ch.1–3,5（单位/不确定度），见本卷第10章（测量矩阵与实验设计）。
2. 本章锚点：
   • 最小方程：S80-1—S80-16
   • 流程：M8-1—M8-6
   • 接口：I8-1—I8-8

XIV. 小结
本章形成从 EKF/UKF/PF 到因子图 MAP 平滑的统一融合估计栈，与第7章动力学、第四章观测、第五章几何与第六章同步先验无缝衔接；配合第10章的实验设计与信息度指标，可在跨场景/跨平台下稳定获得一致、鲁棒且可审计的定位与导航解。