GNSS 杠杆臂因子：解算参考点不能直接当机体位置

GNSS 接收机输出的是设备配置所声明的解算参考点，不是天然等同于 IMU 原点、后轴中心或算法定义的 base_link。这个点可能是天线参考点（ARP），也可能是经过相位中心偏移与变化模型修正后的参考点，必须以接收机配置、天线模型和输出协议为准。天线装在车顶，机体转弯时，即使机体参考点瞬时不动，GNSS 参考点也会沿圆弧移动。若直接写位置残差 $z - p$ ，直线匀速数据上可能看不出问题；一到连续转弯、掉头或坡道，优化器只能用错误的平移、姿态或 IMU bias 去解释确定性的杆臂运动。

问题也不靠“调低 GNSS 权重”解决。应先固定坐标、状态扰动和测量时刻，预测正确的物理点，再处理异常值与协方差。整条主线可以压成一句话：测量属于哪个点，因子就预测哪个点。

1. 从 SE(3) 状态预测 GNSS 参考点

设世界系为 $W$ ，机体或 IMU 系为 $B$ ，接收机输出所对应的 GNSS 参考点为 $G$ 。状态

T_{W B} = [R_{W B} p_{W B} \0 1] \in S E (3)

中， $R_{W B}$ 把 $B$ 中向量旋到 $W$ ， $p_{W B}$ 是 $B$ 原点在 $W$ 中的位置。杆臂 $l =^{B} p_{G} -^{B} p_{B}$ 从机体原点指向解算参考点，并用 $B$ 表达。预测模型只有一行：

h (T_{W B}) = p_{W B} + R_{W B} l .

以“预测减测量”定义残差 $r = h - z_{G}^{W}$ ，代价为

E_{G} = ρ! (r^{T} Σ_{G}^{- 1} r) .

若实现用 $z - h$ ，雅可比整体变号；只要残差与梯度自洽即可。真正危险的是公式、数值差分和优化器各用一种约定。

测量还须先转到 $W$ 。经纬度与椭球高可按选定参考椭球直接转换为 ECEF，不需要局部原点；若要构造局部 ENU，则应选定固定地理原点，用原点的 ECEF 坐标完成平移并按原点经纬度旋转。接收机给的是正高还是椭球高也必须核对，不能漏掉大地水准面改正。算法若用 NED、地图系或首帧 IMU 系还需继续变换；ENU/NED 不只是交换数组下标，竖轴符号也不同。杆臂数据则要确认指向机械底座、ARP，还是天线模型采用的参考点。真实相位中心会随频率和卫星方向变化，不能在未说明天线校准模型时把它当成一个固定机械点；厘米级系统必须让测量定义与杆臂终点一致。

杆臂也影响 Doppler 速度。令 $^{B} ω_{W B}$ 表示机体 $B$ 相对世界系 $W$ 的角速度并用 $B$ 表达，且杆臂刚性固定于 $B$ ，则

v_{G}^{W} = v_{B}^{W} + R_{W B} (^{B} ω_{W B} \times l) .

因此同时融合 GNSS 速度时，不能只在位置因子里补杆臂；还要先确认接收机报告的速度属于哪个参考点，避免对已经换算过的输出重复补偿。

2. 旋转雅可比：先说扰动在哪一侧

记 $[a]_{\times} b = a \times b$ 。采用右扰动

T^{'} = T Exp (δ ξ^{\land}), δ ξ = (δ ρ, δ θ),

其中增量在机体系表达。一阶近似有

h (T^{'}) ≃ p + R δ ρ + R (I + [δ θ] \times) l & = h (T) + R δ ρ - R [l] \times δ θ .

对 $r = h - z$ ，局部增量顺序为平移、旋转时，

J_{right} = [R - R [l]_{\times}] .

若采用左扰动 $T^{'} = Exp (δ ξ^{\land}) T$ ，小量在世界系表达，整个 GNSS 参考点被左乘：

h (T^{'}) ≃ h (T) + δ ρ - [h (T)]_{\times} δ θ,

于是

J_{left} = [I - [h (T)]_{\times}] .

若状态分别更新为 $R \leftarrow R Exp (δ θ)$ 、 $p \leftarrow p + δ p$ ，平移块是 $I$ ，旋转块仍是 $- R [l]_{\times}$ ；若变量顺序是旋转、平移，还要交换列块。所谓“符号写反”往往只是残差方向、扰动侧或增量排列不同。

每次修改 retraction 都应做中心差分：

J_{i}^{num} = \frac{r ( T ⊞ ϵ e _{i} ) - r ( T ⊞ ( - ϵ e _{i} ))}{2 ϵ} .

测试位姿不能只取单位阵；应使用非零位置、非单位姿态和三轴均非零的杆臂，并扫描几个步长，避免二阶项或浮点消减掩盖错误。

3. 有旋转块，不等于姿态和杆臂都可观

单个三维位置因子最多提供三维信息。虽然 $R l$ 让残差对姿态敏感，但 GNSS 并未直接测量姿态；若 $p$ 可自由变化，它可以抵消 $R$ 的改动。姿态信息来自 GNSS 与 IMU 预积分、视觉/LiDAR 相对位姿或轮速共同形成的整张图。

若把杆臂也作为状态， $\partial r / \partial l = R$ 满秩仍不代表杆臂可辨识。每个 $p_{k}$ 若可自由调整，可用 $δ p_{k} = - R_{k} δ l$ 消去任意杆臂变化。真正的信息来自跨时刻刚体约束和姿态激励：纯直线、恒姿态时杆臂误差近似固定位置偏移；只绕一根轴旋转，沿该轴的杆臂分量仍弱。地面车缺少 roll/pitch 时，竖直杆臂还会与高程偏差、天线模型误差耦合。

在线标定应有多方向角运动和合理外参先验。离线可对窗口线性化系统做 Schur complement，查看杆臂、时间偏移相关块的特征值谱与弱方向。若结果始终贴着先验，只能说明数据没提供信息，不能反过来证明先验正确。

4. 时间不同步会伪装成杆臂误差

ROS 消息到达时间包含链路与调度延迟，不能代替 GNSS 解算时刻。若测量属于 $t_{g}$ ，却绑在 $t_{g} + Δ t$ 的状态上，小时间偏移造成

δ p_{G}^{W} ≃ (v_{B}^{W} + R_{W B} (^{B} ω_{W B} \times l)) Δ t .

直线高速段表现为沿速度方向的误差，急转段还出现与角速度相关的横向误差，后者极像杆臂错误 $R δ l$ 。数据若只有单一速度和转弯半径，两者会高度相关。

后端应把轨迹评估到测量时刻。平移插值加旋转 SLERP 在关键帧足够密、区间运动平缓时，通常能减少绑定最近关键帧造成的时间量化误差，但它不是对高动态运动的无条件改进；应通过运动模型和回放残差验证。已有连续时间轨迹或 IMU 模型时则使用其正式接口。输入层需统一 GPS time、UTC、闰秒和跨周语义，并保留原始时间戳供回放。

排错时画残差对车速、角速度的条件统计，并将世界系残差旋回机体系 $R^{T} r$ 。机体系中近似固定的偏差更像杆臂；随速度变号更像同步。不要同时无先验地开放杆臂、时间偏移和 GNSS 全局偏置，它们会互相解释。

5. 多频解不等于固定协方差

多频接收机能改善电离层建模与模糊度固定，但 RTK fixed、float、DGPS、单点解的分布仍不同，遮挡和多路径也会产生非高斯偏差。融合层应读取每历元 solution status、改正龄期和三维协方差；HDOP 只是几何稀释指标，不能单独当米制方差。

接收机若给 ENU 协方差，世界系旋转为 $R_{W \leftarrow E}$ ，则

Σ_{W} = R_{W \leftarrow E} Σ_{ENU} R_{W \leftarrow E}^{T} .

均值和协方差必须一起变换。高程通常弱于水平，强设各向同性权重会把竖直多路径传给 IMU bias。若只有标准差，可用明确标注的对角近似，但不能假装相关项确实为零。

用 $Σ = L L^{T}$ 做 Cholesky 白化：

r ~ = L^{- 1} r, J ~ = L^{- 1} J,

不要显式求逆。输入层应拒绝 NaN、非对称或非正定协方差，并记录原因。连续 GNSS 历元的多路径误差常有时间相关性；将高频解全部当独立白噪声会重复计算信息，应通过降采样、偏置状态或创新自相关检查处理。

6. 门限、鲁棒核与开关不是同一层

无有效解、时间倒退和非法协方差应在建图前拒绝。对其余测量，先用尚未纳入当前 GNSS 测量的先验状态协方差 $P^{-}$ 构造创新协方差

S = H P^{-} H^{T} + Σ_{G},

其中 $H$ 是当前预测点对门限检验状态的雅可比。然后计算

d^{2} = r^{T} S^{- 1} r,

只有在创新近似零均值高斯、 $S$ 与当前测量相互独立地预测得到且满秩时， $d^{2}$ 才近似服从三自由度卡方分布，可据此选择门限。若提前用同一测量更新过 $P$ ，再拿后验协方差门限会重复使用信息；强非线性、相关多路径或降维观测也需要重新标定分布或自由度。只用 $Σ_{G}$ 会忽略状态自身不确定度；固定米制门限则无法统一 fixed 与 float 解。

通过门限后的重尾残差可使用 Huber 或 Cauchy。鲁棒核应作用于白化后的平方马氏距离，参数才是无量纲尺度。持续成段的有偏解可加开关变量：

E_{k} = ρ! (∣ s_{k} r ~_{k} ∣^{2}) + λ (1 - s_{k})^{2}, s_{k} \in [0, 1] .

先验项防止优化器无代价关闭所有 GNSS。连续历元可共享开关或加时间平滑，避免每个高频测量都增加一个自由变量。监控必须暴露拒绝数、鲁棒权重和开关值，否则系统静默关闭 GNSS 后仍可能短时靠惯性维持。

7. Eigen 实现：把约定写到变量名里

下面片段采用 $r = h - z$ 、右扰动和 $(δ ρ, δ θ)$ 顺序：

cpp

#include <Eigen/Cholesky>
#include <Eigen/Core>
#include <Eigen/Geometry>

#include <algorithm>
#include <cmath>
#include <stdexcept>

struct LinearizedGnss {
  Eigen::Vector3d whitened_residual;
  Eigen::Matrix<double, 3, 6> whitened_jacobian;
};

Eigen::Matrix3d skew(const Eigen::Vector3d& a) {
  Eigen::Matrix3d A;
  A << 0.0, -a.z(), a.y(),
       a.z(), 0.0, -a.x(),
      -a.y(), a.x(), 0.0;
  return A;
}

LinearizedGnss linearize(
    const Eigen::Quaterniond& q_WB,
    const Eigen::Vector3d& p_WB,
    const Eigen::Vector3d& lever_B,
    const Eigen::Vector3d& z_WG,
    const Eigen::Matrix3d& covariance_W) {
  if (!q_WB.coeffs().allFinite()) {
    throw std::invalid_argument("non-finite GNSS orientation");
  }
  const double q_squared_norm = q_WB.squaredNorm();
  if (!std::isfinite(q_squared_norm) || q_squared_norm < 1e-24) {
    throw std::invalid_argument("degenerate GNSS orientation");
  }
  if (!p_WB.allFinite() || !lever_B.allFinite() || !z_WG.allFinite() ||
      !covariance_W.allFinite()) {
    throw std::invalid_argument("non-finite GNSS factor input");
  }

  const double covariance_scale =
      std::max(1.0, covariance_W.cwiseAbs().maxCoeff());
  const double asymmetry =
      (covariance_W - covariance_W.transpose())
          .cwiseAbs()
          .maxCoeff();
  if (asymmetry > 1e-12 * covariance_scale) {
    throw std::invalid_argument("non-symmetric GNSS covariance");
  }

  const Eigen::Matrix3d R_WB = q_WB.normalized().toRotationMatrix();
  const Eigen::Vector3d residual =
      p_WB + R_WB * lever_B - z_WG;

  Eigen::Matrix<double, 3, 6> J;
  J.leftCols<3>() = R_WB;
  J.rightCols<3>() = -R_WB * skew(lever_B);

  Eigen::LLT<Eigen::Matrix3d> llt(covariance_W);
  if (llt.info() != Eigen::Success) {
    throw std::runtime_error("invalid GNSS covariance");
  }
  const Eigen::Matrix3d L = llt.matrixL();
  return LinearizedGnss{
      L.triangularView<Eigen::Lower>().solve(residual),
      L.triangularView<Eigen::Lower>().solve(J)};
}

接入 Ceres、GTSAM 或自研后端时，实际 plus() 必须与该推导相同。评审应能回答四元数方向、杆臂表达系、协方差表达系、白化位置、核函数输入，以及因子绑定插值状态还是最近关键帧。用 q_WB、lever_B 这类名字，比在传感器边界传 q、ext 更容易阻止轴向错误。

8. 离线验收要同时看参考点、同步和一致性

验收不能只看 ATE。错误杆臂可能被轨迹对齐吸收，也可能牺牲姿态和 bias 换取更好看的位置。固定一组含静止、左右转、掉头、坡道和不同速度的 bag，逐层回归：

随机位姿、非轴对齐杆臂和 SPD 协方差下，对实际 plus() 做中心差分，检查原始及白化雅可比。
用已知 ECEF/ENU 点测试地理原点、ENU/NED 轴和协方差旋转；非法状态都应有可统计的拒绝原因。
静态段检查 $R^{T} (z - p)$ 是否接近实测杆臂；静态数据只做符号自检，不宣称可观。
对比“无杆臂”“固定实测杆臂”“有先验在线杆臂”，同时报告 GNSS 参考点与机体点误差、姿态、bias、创新和杆臂后验。
人工注入正负时间偏移，确认残差随速度和角速度按公式变化；修正后不同速度片段应回到同一基线。
注入跳变、连续偏置、协方差低报及 fixed/float 切换，核对门限、权重、开关和健康告警，系统不能靠关闭全部 GNSS 获得“成功”。
按解状态画白化创新、NIS 和自相关。长期系统性偏大或偏小应先查协方差与相关性模型，而不是直接改核参数。

真值也必须转换到同一物理点：全站仪棱镜、动捕标记或另一根 GNSS 天线都有自己的外参。最终通过标准应是解析与数值线性化一致；杆臂在有激励的独立数据上可复现；时间偏移不会被杆臂吸收；创新与声明协方差大体一致；异常解可识别且不会拖走轨迹。

$p + R l$ 虽然只有一行，周围却连着坐标、扰动、时钟、协方差和物理参考点。任一契约含糊，优化器都会用别的状态替它圆谎。把这些契约变成单测、异常计数和固定 bag 回归，GNSS 参考点测量才会成为机体状态可靠的绝对约束。