专业知识的应用04.误差状态与松耦合怎么串起来

前言

前两篇分别讲了卡尔曼在本项目里怎么用、惯导解算用到了哪些知识。

但组合导航是一个整体：惯导负责用 IMU 递推名义状态，卡尔曼负责估计误差并用 GNSS 观测去修正；两者通过「误差注入 + 重置」形成闭环，使得下一段递推的初值始终是「组合后的最优值」，不会一路漂下去。

本篇把前两篇串成「一个系统」，讲清楚名义状态与误差状态分别是什么、数据怎么流、松耦合在本项目里具体指什么，以及这些在代码里对应谁调用谁。

这是实现层面的核心，看懂这一篇，整个组合导航的主循环就通了。

名义状态与误差状态

名义状态：对外输出的「当前位姿」

名义状态（Nominal State）就是组合系统对外输出的当前位置、速度、姿态以及零偏估计。

在代码里对应 state/state.h 中的 NominalState：pos（纬经高）、vel（NED 速度）、quat（体到导航系的四元数）、bg（陀螺零偏）、ba（加速度计零偏）。用户调用 getPose() 拿到的就是这份名义状态。

名义状态不是卡尔曼直接估计出来的，而是由两条线共同维护：

1. 惯导递推：每次有 IMU 数据时，用零偏补偿后的 IMU 做惯导解算，得到新的位置、速度、姿态；这部分结果先更新在 Pins 内部的 InsState 里，再通过 syncFromINS() 同步到名义状态（只同步 pos、vel、quat，不同步零偏）。所以在只有 IMU、没有 GNSS 的时候，名义状态就是惯导解算的结果。
2. 误差注入：当有 GNSS 观测时，卡尔曼用位置残差做一次观测更新，得到最新的误差状态估计；然后用误差状态去修正名义状态（位置、速度、姿态、零偏都按误差做一次校正），再把误差状态置零，并把校正后的名义状态写回 INS，作为下一段递推的初值。所以在 GNSS 更新之后，名义状态 = 惯导结果 + 卡尔曼估计的误差修正。

因此，名义状态 = 惯导递推的结果 + 卡尔曼误差校正；零偏只在 GNSS 更新时通过 correctPose() 注入，平时由滤波器在预测步中随误差状态一起传播。

误差状态：卡尔曼估计的「惯导结果的误差」

误差状态（Error State）是卡尔曼滤波器估计的对象，表示「当前惯导解算结果相对于真实状态的误差」。

在代码里对应 state/state.h 中的 GIState：posDelta、velDelta、attDelta（NED 下左扰动的旋转矢量）、bg、ba（零偏误差）。滤波器内部维护这份误差状态和其 15×15 协方差矩阵 P。

误差状态的含义是：真实状态 ≈ 名义状态 − 误差状态（位置、速度、姿态用减，零偏用加，具体符号与扰动定义一致）。

所以观测更新后，我们用估计出的误差去修正名义状态：位置、速度减去误差，姿态用 exp(δφ) 左乘名义四元数，零偏加上误差。

修正完成后，误差状态会被重置为零（filter_.reset()），因为「误差已经注入到名义状态里了」，下一时刻的误差状态应该表示的是「新名义状态」相对于真实的误差，而不是累积旧误差。

同时，INS 的当前状态也会被重置为校正后的名义状态（pins_.resetState(NominalState2InsState(state_))），这样下一段 IMU 递推的初值就是「组合后的最优值」，惯导不会带着未校正的偏差继续积分。

这就是 误差状态卡尔曼滤波（ESKF） 在本项目里的用法：估计的是误差，用误差修正名义状态，然后误差状态置零、INS 与名义状态对齐，形成闭环。

从系统角度看，误差状态并不是“长期存在的状态”，而是一个只在修正瞬间发挥作用的中间量；修正完成后，系统的所有信息都回到名义状态和 INS 内部.

数据流：主循环与四种时间对齐模式

组合导航的主入口是 PoseEstimator::updatePose()。

外部按时间顺序喂入 IMU 和 GNSS 数据（addImuData、addGnssData），每次在合适的时机调用 updatePose()，内部根据当前 IMU 时间和当前 GNSS 时间的关系，决定「先传播还是先更新、要不要在 IMU 区间内插值到 GNSS 时刻再更新」。

updatePose() 本身并不关心“算法细节”，它做的只是根据 IMU 与 GNSS 的时间关系，选择传播与更新的顺序，从而保证观测在正确的时间点被使用。

时间对齐的四种模式

项目以 IMU 时间为基准。ImuTimeAligner::getMode(prevImuData_, currImuData_, gnssData_, ...) 会给出四种模式：

1. Curr：GNSS 时间在当前 IMU 时刻之前。先做 GNSS 更新和误差注入，再做当前 IMU 的传播。
2. Next：GNSS 时间在当前 IMU 时刻之后。先做当前 IMU 的传播，再做 GNSS 更新和误差注入。
3. Internal：GNSS 时间落在上一帧 IMU 与当前帧 IMU 之间。先用插值得到 GNSS 时刻的「虚拟 IMU」，传播到 GNSS 时刻；在该时刻做 GNSS 更新和误差注入；再用剩余时间从 GNSS 时刻传播到当前 IMU 时刻。
4. No：没有有效 GNSS 或时间差过大。只做 IMU 传播，不做 GNSS 更新。

这样做的目的，是让 GNSS 观测在正确的时刻被使用，避免时间错位带来的额外误差。

单步传播：propagate(imuData)

每次「用一帧 IMU 做一次递推」时，都会调用 propagate(imuData)，其顺序是：

1. 零偏补偿：comPenImu(imuData) 用当前名义状态里的 bg、ba 对原始 IMU 做补偿，得到补偿后的 IMU。
2. 惯导解算：pins_.insUpdate(comPenImuData) 用补偿后的 IMU 更新 INS 内部的状态（位置、速度、姿态）。
3. 误差状态预测：filter_.predict(pins_.getState(), comPenImuData, dt) 用当前惯导状态和 IMU 数据更新误差状态和协方差 P（Φ、G、Q 参与的那一步）。
4. 同步到名义状态：syncFromINS() 把 INS 当前的位置、速度、姿态拷贝到名义状态 state_（零偏不变）。

所以每来一帧 IMU，名义状态中的 pos/vel/quat 会跟着惯导走一步，误差状态和 P 会跟着卡尔曼预测走一步；零偏在名义状态里要等 GNSS 更新时才会被修正。

GNSS 更新与误差注入：gnssUpdate() + correctPose()

当逻辑判断「这一拍要做 GNSS 更新」时，会先调用 gnssUpdate()，再调用 correctPose()。

gnssUpdate() 做的事：

1. 用当前名义状态和天线杆臂 antlever_ 计算「天线相位中心在 LLH 下的位置」：fixCurrPos = state_.pos + NDE2LLhMatrix(...) * Cbn * antlever_（Cbn 即 state_.quat.toRotationMatrix()）。
2. 计算位置差：posErr = fixCurrPos - gnssData_.pos（LLH 下）。
3. 把位置差转到 NED：dz = LLh2NEDMatrix(...) * posErr，得到观测残差。
4. 调用 filter_.update(dz)，用 dz 做一次卡尔曼观测更新，更新误差状态和 P。

correctPose() 做的事：

1. 从滤波器取出当前误差状态 dx = filter_.getState()。
2. 用误差修正名义状态：位置减 NDE2LLh * posDelta，速度减 velDelta，零偏加 bg/ba，姿态用 exp(attDelta) 左乘原四元数（左扰动）。
3. 调用 filter_.reset() 把误差状态置零。
4. 调用 pins_.resetState(NominalState2InsState(state_)) 把 INS 状态设为校正后的名义状态。

这样，GNSS 观测被用来修正名义状态和零偏，并让 INS 与名义状态重新对齐；下一段 IMU 递推就从「校正后的状态」开始，形成闭环。

数据流小结

文字描述的数据流如下，读者可以据此画一张简图：

• IMU 数据 → 零偏补偿 → INS 解算（更新位置、速度、姿态）→ 结果同步到名义状态（pos/vel/quat）；同时用同一帧 IMU 和当前 INS 状态做误差状态预测（更新误差状态与 P）。
• GNSS 数据（位置）→ 与当前名义状态（加杆臂）求差 → 得到 NED 下的观测残差 dz → 卡尔曼观测更新（更新误差状态与 P）→ 误差注入（用误差状态修正名义状态与零偏）→ 误差状态置零、INS 重置为名义状态。
• 对外输出：始终是名义状态（getPose()）。

因此，名义状态 = 惯导递推 + 误差注入；误差状态 = 卡尔曼估计，只在内部和校正时使用，校正后置零。

松耦合：只用 GNSS 位置作为观测

松耦合（Loose Coupling）在本项目里指：只用 GNSS 已经解算好的位置（如单点定位或 RTK 的结果）作为观测，与惯导解算的位置做差得到观测残差，再交给卡尔曼做融合。我们不使用 GNSS 的原始观测量（伪距、伪距率、载波相位等），也不在滤波器里估计 GNSS 的钟差、模糊度等状态。

这样做的好处是接口简单：GNSS 端只需输出「某一时刻的 LLH 位置（及是否有效）」；组合端只需在对应时刻算「当前名义状态对应的天线位置 − GNSS 位置」，得到 dz，再调用 filter_.update(dz)。不需要和 GNSS 接收机的原始观测接口、不需要额外的状态维数，适合练手和离线复现。代价是信息利用得不如紧耦合充分（紧耦合会把伪距、载波等一起建进观测方程），但作为学习和复习组合导航原理已经足够。本项目在 pose_estimator 里融合 IMU 与 GNSS 的方式，就是这种「只融合位置」的松耦合。

代码对应小结

• 名义状态：PoseEstimator::state_，类型为 NominalState（pos, vel, quat, bg, ba）。对外通过 getPose() 返回。
• 误差状态：GILooseFilter 内部维护，类型为 GIState（posDelta, velDelta, attDelta, bg, ba）；通过 getState() 取出，仅在 correctPose() 中使用。
• 主循环：updatePose() 根据 ImuTimeAligner::getMode(...) 选择 Curr/Next/Internal/No，在合适顺序下调用 propagate()、gnssUpdate()、correctPose()。
• 传播：propagate(imuData) → comPenImu → pins_.insUpdate → filter_.predict → syncFromINS。
• GNSS 更新：gnssUpdate() 计算 dz（含杆臂）并调用 filter_.update(dz)；correctPose() 用误差状态修正 state_，再 filter_.reset()、pins_.resetState(...)。
• 状态定义：state/state.h 中的 NominalState、GIState、InsState，以及 NominalState2InsState 用于在名义状态与 INS 状态之间拷贝（不含零偏）。

谁调用谁、数据怎么流，按上述顺序在代码里走一遍，就能和本篇的「名义状态 vs 误差状态」「数据流」「松耦合」一一对应。

总结

1. 名义状态 vs 误差状态：名义状态是对外输出的位置、速度、姿态和零偏，由惯导递推与卡尔曼误差注入共同得到；误差状态是卡尔曼估计的惯导结果误差，用于修正名义状态，修正后置零，INS 同时重置为名义状态，形成闭环。
2. 数据流与闭环：IMU → 零偏补偿 → INS 解算 → 名义状态同步 + 误差状态预测；GNSS 位置 → 观测残差 dz → 卡尔曼更新 → 误差注入 → 误差状态置零、INS 重置。主循环 updatePose() 按 IMU/GNSS 时间关系选择传播与更新的顺序，必要时在 IMU 区间内插值到 GNSS 时刻再更新。
3. 松耦合：只用 GNSS 解算后的位置作为观测，不用伪距/载波；接口简单，与 pose_estimator 中 IMU + GNSS 的融合方式一致。