MSCKF中真实状态、标称状态与误差状态：从定义到代码的完整解析

1. 三种状态到底在解决什么问题如果你正在啃MSCKF相关的论文或者开源代码大概率会在前几页就被三个词拦住真实状态true state、标称状态nominal state、误差状态error state。我第一次看的时候也是一头雾水心想不就是一个位姿和速度吗为什么要拆成三份来写后来自己动手推了一遍公式、又对着代码逐行核对才明白这套拆分不是故弄玄虚而是整个滤波框架能不能跑通的地基。先把结论摆在这儿MSCKF本质上是一个**误差状态卡尔曼滤波Error-State Kalman Filter, ESKF**的变体。它不去直接估计机器人的真实位姿而是估计真实状态和标称状态之间的那个小偏差也就是误差状态。这么做的核心动机有三个一是让姿态这种带约束的量旋转矩阵正交、四元数单位模在滤波中不被破坏二是让线性化点始终靠近真值避免大误差下的线性化失真三是让误差状态始终保持在小量级数值上更稳定。这篇文章我打算把这三个状态从定义、关系、传播、更新到代码落地完整捋一遍。适合谁看如果你已经知道卡尔曼滤波的基本公式但一看到MSCKF里那堆带波浪号、带hat的符号就头疼那这篇就是写给你的。我会尽量用生活化的类比把抽象概念落地同时把关键公式的来龙去脉讲清楚最后给出一份可以直接对照代码的实操清单。在展开之前先明确一个贯穿全文的记号约定这个约定在绝大多数MSCKF文献里是通用的不带任何修饰的符号比如 $x$表示真实状态是理论上存在但永远无法精确知道的量。带 hat 的符号比如 $\hat{x}$表示标称状态是滤波器维护的、我们认为最接近真值的那个估计。带 tilde 的符号比如 $\tilde{x}$表示误差状态定义为真实状态与标称状态之间的偏差。三者满足一个核心关系$x \hat{x} \oplus \tilde{x}$这里的 $\oplus$ 不是普通加法而是针对不同状态量定义的复合运算。对于向量位置、速度它就是普通加法对于姿态它是四元数或旋转矩阵的乘法。这个细节非常关键后面会专门讲。2. 真实状态、标称状态、误差状态的定义与关系2.1 真实状态存在但不可知的那个上帝视角真实状态 $x$ 是系统在物理世界中的实际状态包括位置、速度、姿态、以及各种传感器偏置陀螺仪偏置、加速度计偏置。它客观存在但我们永远无法直接获得。为什么因为所有传感器都有噪声所有模型都有误差。IMU测出来的角速度和加速度本身就带着偏置和随机游走相机观测到的特征点像素坐标也有噪声。所以真实状态在滤波里扮演的角色是理论参照物。我们写公式、做推导时一切都是以真实状态为基准展开的但在实际计算中我们从不直接操作它。你可以把它理解成物理课本里的理想质点——用来建立方程但不会出现在代码里。真实状态的动力学由非线性连续时间模型描述$$\dot{x} f(x, u, w)$$其中 $u$ 是IMU输入角速度、加速度$w$ 是过程噪声。这个模型是精确的物理规律问题在于我们不知道 $x$也不知道 $w$。2.2 标称状态滤波器真正维护的那份估计标称状态 $\hat{x}$ 是滤波器实际维护、实际传播、实际输出的状态。它通过把IMU测量值代入运动学方程积分得到不包含任何随机噪声项。换句话说标称状态是确定性的那部分它假设IMU读数就是真值虽然我们知道不是。标称状态的传播方程是$$\dot{\hat{x}} f(\hat{x}, u, 0)$$注意这里噪声项 $w$ 被置为零。这就是标称状态和真实状态在动力学上的唯一区别真实状态受噪声驱动标称状态不受。也正因为如此两者会随着时间逐渐偏离这个偏离量就是误差状态。标称状态在代码里通常就是一个结构体装着当前时刻的位姿、速度、偏置。每次IMU来一帧数据就用它做一次积分更新这个结构体。它是看得见摸得着的也是最终对外输出的位姿来源。2.3 误差状态真正被卡尔曼滤波估计的对象误差状态 $\tilde{x}$ 定义为真实状态与标称状态之间的偏差。对于位置、速度、偏置这类向量量$$\tilde{x} x - \hat{x}$$对于姿态这种流形上的量不能直接相减而是定义在切空间上$$\tilde{\theta} \text{Log}(R^T \hat{R})$$或者用四元数写成 $\tilde{\theta} 2 \cdot \text{Vec}(q^{-1} \otimes \hat{q})$ 之类的形式不同文献约定略有差异但本质都是把姿态误差映射到三维旋转向量。误差状态才是卡尔曼滤波真正估计的东西。滤波器的状态向量、协方差矩阵、观测更新全部围绕 $\tilde{x}$ 展开。为什么因为误差状态始终是小量接近零线性化在这个点上非常准确而且误差状态的维度是固定的姿态误差永远是三维不管用四元数还是旋转矩阵表示不会出现过参数化问题。2.4 三者的复合关系与注入操作三者关系可以总结成一句话真实状态 标称状态 复合 误差状态。$$x \hat{x} \oplus \tilde{x}$$这个 $\oplus$ 对不同量有不同含义。位置、速度、偏置用普通加法姿态用四元数乘法或旋转矩阵乘法。写成具体形式位置$p \hat{p} \tilde{p}$速度$v \hat{v} \tilde{v}$姿态$R \hat{R} \cdot \text{Exp}(\tilde{\theta})$偏置$b \hat{b} \tilde{b}$滤波更新之后我们得到的是误差状态的估计 $\hat{\tilde{x}}$ 和它的协方差。这时候需要把误差状态注入回标称状态得到新的标称状态然后把误差状态清零等待下一轮。这个操作叫injection或reset$$\hat{x} \leftarrow \hat{x} \oplus \hat{\tilde{x}}, \quad \tilde{x} \leftarrow 0$$注入之后误差状态归零协方差矩阵保持不变或者做一次小的重映射取决于姿态误差的参数化方式。这一步是ESKF区别于标准EKF的关键操作也是新手最容易写错的地方。注意注入操作必须在观测更新之后、下一轮传播之前完成。如果顺序搞反误差状态会累积两次导致估计发散。3. 为什么要拆成三种状态误差状态滤波的核心优势3.1 姿态的流形约束问题姿态不是普通的三维向量它生活在SO(3)流形上。如果你直接用四元数四维做EKF状态会遇到两个麻烦一是四元数的单位模约束在滤波更新后会被破坏需要反复归一化而归一化本身会引入不一致二是四元数有四个分量但只有三个自由度协方差矩阵会奇异。用旋转矩阵九维更糟正交约束更难维持。误差状态滤波的巧妙之处在于标称状态用四元数或旋转矩阵维护保证它始终在流形上误差状态用三维旋转向量表示是切空间里的普通向量可以放心地用卡尔曼滤波处理。这样既避免了过参数化又保证了流形约束。打个比方地球表面是曲面但你在一小块区域内画地图可以用平面直角坐标系。误差状态就是那块局部平面地图标称状态是地球仪上的位置。地图是平的好算地球仪是圆的好看。两者配合各司其职。3.2 线性化点的稳定性卡尔曼滤波依赖线性化。标准EKF在标称状态处线性化如果标称状态和真实状态差得远线性化误差就大滤波可能发散。误差状态滤波的线性化点是误差状态空间的原点也就是 $\tilde{x}0$而误差状态始终是小量所以线性化始终在原点附近进行精度高得多。这一点在IMU积分场景下尤其重要。IMU积分误差随时间累积如果直接估计真实状态线性化点会越飘越远而误差状态滤波每轮都把误差注入标称状态并清零线性化点永远在原点稳定性大幅提升。3.3 数值精度与计算效率误差状态是小量用单精度浮点甚至定点数表示都不容易丢精度。而真实状态的数值可能很大比如位置到了几百米大数加小量容易产生舍入误差。把误差单独拎出来估计等于给小数部分单独开了高精度通道。计算效率上误差状态维度固定且通常较小15维左右位置3、速度3、姿态3、陀螺偏置3、加计偏置3协方差矩阵规模可控。MSCKF进一步把特征点排除在状态向量之外用观测模型边缘化掉状态维度不随特征点数量增长这是它能实时跑在嵌入式平台上的关键。3.4 三种状态的职责对照状态类型符号是否可观测在滤波中的角色代码中的体现真实状态$x$否理论参照用于推导不出现标称状态$\hat{x}$是估计值传播、输出、注入目标结构体变量误差状态$\tilde{x}$是被估计卡尔曼滤波状态向量状态向量与协方差这张表建议贴在显示器旁边写代码时随时对照能避免很多符号混淆。4. 误差状态的传播与观测更新实操4.1 连续时间误差状态动力学推导误差状态的传播方程不是拍脑袋写的而是从真实状态和标称状态的动力学方程相减得到的。以姿态为例真实姿态动力学$$\dot{R} R [\omega_m - b_g - n_g]_\times$$标称姿态动力学$$\dot{\hat{R}} \hat{R} [\omega_m - \hat{b}g]\times$$其中 $\omega_m$ 是陀螺测量值$b_g$ 是真实偏置$\hat{b}_g$ 是标称偏置$n_g$ 是测量噪声。两者相减并利用姿态误差的定义 $R \hat{R}\text{Exp}(\tilde{\theta})$经过一番推导这里省略中间步骤核心是用到Baker-Campbell-Hausdorff公式的一阶近似可以得到$$\dot{\tilde{\theta}} -[\omega_m - \hat{b}g]\times \tilde{\theta} - \tilde{b}_g - n_g$$这就是姿态误差的连续时间动力学。它是个线性方程系数矩阵就是角速度的反对称矩阵。类似地可以推出位置、速度、偏置的误差动力学最终拼成一个大的线性系统$$\dot{\tilde{x}} F \tilde{x} G n$$这个 $F$ 和 $G$ 就是误差状态转移矩阵和噪声驱动矩阵是后续离散化和协方差传播的基础。4.2 离散化与协方差传播连续时间系统要落地到代码必须离散化。常用的是零阶保持ZOH或者一阶近似。对于小时间步 $\Delta t$一阶近似足够$$\Phi I F \Delta t$$ $$Q_d G Q_c G^T \Delta t$$其中 $Q_c$ 是连续时间噪声协方差由IMU的噪声密度参数给出。协方差传播就是标准的卡尔曼预测步骤$$P_{k1} \Phi P_k \Phi^T Q_d$$这里有个实操细节$F$ 矩阵里包含角速度的反对称矩阵而角速度在 $\Delta t$ 内是变化的。高精度实现会用中值积分或者把 $\Delta t$ 切得更细。我实测下来对于一般的室内机器人$\Delta t$ 取IMU原始频率100-200Hz直接一阶近似就够用如果是高速运动场景建议用RK4或者分段积分。4.3 观测更新与误差注入MSCKF的观测是视觉特征点。特征点被多个相机帧观测到后构建重投影误差模型。这个模型对误差状态是线性的一阶近似形式为$$r H \tilde{x} n$$其中 $H$ 是观测雅可比$r$ 是重投影残差。标准卡尔曼更新$$K P H^T (H P H^T R)^{-1}$$ $$\hat{\tilde{x}} K r$$ $$P (I - K H) P$$更新完成后把 $\hat{\tilde{x}}$ 注入标称状态$$\hat{x} \leftarrow \hat{x} \oplus \hat{\tilde{x}}$$然后误差状态清零。注意姿态的注入要用四元数乘法位置速度偏置用加法。注入后协方差矩阵通常保持不变但如果姿态误差用的是旋转向量参数化注入后需要做一次协方差重映射因为切空间的参考点变了这一步在MSCKF的经典实现里通常用一阶近似处理。提示注入顺序和清零时机是调试时的高频错误点。建议在代码里加断言检查注入前后标称状态的变化量是否等于误差状态估计值。4.4 一份可对照的实操清单下面这份清单是我自己实现和调试MSCKF时总结的按顺序执行基本不会出大错初始化标称状态设为初始位姿误差状态清零协方差设一个合理的初始值位置速度可以大一点偏置给小一点。IMU传播每来一帧IMU先用标称状态积分更新标称状态同时计算 $\Phi$ 和 $Q_d$传播协方差。状态克隆当有新图像帧到来时把当前标称位姿克隆一份存入滑动窗口同时扩展协方差矩阵。特征跟踪与三角化跟踪特征点对满足条件的特征做三角化得到三维坐标。观测更新对每个特征点构建重投影残差和雅可比做卡尔曼更新得到误差状态估计。误差注入把误差状态注入标称状态清零误差状态必要时重映射协方差。边缘化滑动窗口满了就边缘化最老的帧保持状态维度恒定。每一步都有坑下面挑几个重点讲。5. 常见问题与排查技巧实录5.1 符号混淆导致的公式错误这是新手第一大坑。真实状态、标称状态、误差状态的符号如果不统一推导时很容易把 $\hat{R}$ 和 $R$ 搞混导致雅可比符号错误。我的建议是在纸上推导时每个符号都用不同颜色标注真实状态用黑色标称状态用蓝色误差状态用红色。推到一半发现颜色乱了说明符号用错了。代码里也一样变量命名要严格区分。比如R_true、R_hat、theta_tilde不要图省事写成R1、R2。我见过一个开源实现因为变量命名混乱姿态注入时用了错误的旋转方向导致滤波缓慢发散查了三天才找到。5.2 注入后协方差没有正确处理误差注入后标称状态变了误差状态的参考点也变了。如果姿态误差用的是旋转向量注入后协方差需要做一次重映射$$P \leftarrow J P J^T$$其中 $J$ 是注入操作的雅可比。很多简化实现直接忽略这一步在误差较小的时候问题不大但误差较大时会引入不一致。判断是否需要重映射的简单方法看注入前后姿态误差的模长如果经常超过0.1弧度就必须处理。5.3 线性化点漂移导致的发散虽然误差状态滤波的线性化点在原点但如果标称状态本身漂移严重比如IMU偏置估计不准误差状态就不再是小量线性化假设失效。这时候滤波会表现出更新量越来越大但估计越来越偏的症状。排查方法打印每轮更新后误差状态估计的模长。如果持续增长不收敛说明标称状态传播有问题优先检查IMU偏置的初始值和噪声参数。我一般会把陀螺偏置的初始协方差设得比实际预期大3-5倍给滤波器足够的调整空间。5.4 常见问题速查表症状可能原因排查方法解决思路滤波缓慢发散符号错误或注入顺序错检查注入前后状态变化统一符号约定加断言姿态估计抖动大协方差重映射缺失打印姿态误差模长补上重映射步骤更新量不收敛线性化点漂移打印误差状态模长调整偏置初始协方差特征点重投影误差大三角化精度不足检查三角化深度增加观测帧数或延迟三角化状态维度爆炸边缘化未生效检查滑动窗口大小修复边缘化逻辑5.5 几个独家避坑技巧第一先用仿真数据验证。不要一上来就跑真实数据先用仿真器生成带噪声的IMU和视觉数据真值已知可以精确对比标称状态和真实状态的偏差。仿真跑通了再上实机能省掉大量调试时间。第二把误差状态单独打印出来。很多人只关注最终位姿输出忽略了误差状态本身。其实误差状态的变化趋势最能反映滤波健康度。我习惯把误差状态的15个分量画成曲线正常情况应该是围绕零均值的小幅波动如果某个分量持续偏置说明对应的标称状态有问题。第三注入操作写成独立函数。不要散落在更新代码里封装成一个injectErrorState()函数输入标称状态和误差状态输出新的标称状态。这样既方便加断言也方便单元测试。第四协方差矩阵定期检查对称性和正定性。数值误差会让协方差矩阵慢慢失去对称性甚至变成非正定导致滤波崩溃。可以在每轮更新后做一次对称化 $P \leftarrow (P P^T)/2$代价很小但很有效。6. 从公式到代码的映射实践6.1 状态结构体的设计把三种状态映射到代码第一步是设计好数据结构。我的做法是定义两个结构体一个NominalState装标称状态一个ErrorState装误差状态。真实状态不定义因为它只存在于推导中。struct NominalState { Eigen::Vector3d p; // 位置 Eigen::Vector3d v; // 速度 Eigen::Quaterniond q; // 姿态 Eigen::Vector3d bg; // 陀螺偏置 Eigen::Vector3d ba; // 加计偏置 }; struct ErrorState { Eigen::Vector3d dp; // 位置误差 Eigen::Vector3d dv; // 速度误差 Eigen::Vector3d dtheta; // 姿态误差旋转向量 Eigen::Vector3d dbg; // 陀螺偏置误差 Eigen::Vector3d dba; // 加计偏置误差 };误差状态用一个15维向量表示协方差矩阵是15x15。注意姿态误差是三维旋转向量不是四元数这是关键。6.2 传播函数的实现要点传播函数做两件事更新标称状态传播协方差。标称状态更新用IMU积分姿态用四元数乘法// 标称状态传播 nominal.p nominal.v * dt 0.5 * (R * (a_m - nominal.ba) g) * dt * dt; nominal.v (R * (a_m - nominal.ba) g) * dt; nominal.q nominal.q * quaternionFromRotationVector((w_m - nominal.bg) * dt);协方差传播需要先构建 $F$ 和 $G$ 矩阵然后离散化。这部分代码比较长核心是把推导出的误差动力学系数填进矩阵。建议对照论文公式逐项填写填完用数值差分验证一遍。6.3 注入函数的实现注入函数是ESKF的灵魂必须写对void injectErrorState(NominalState nom, const ErrorState err) { nom.p err.dp; nom.v err.dv; nom.q nom.q * quaternionFromRotationVector(err.dtheta); nom.q.normalize(); nom.bg err.dbg; nom.ba err.dba; }注意姿态用四元数乘法且注入后要归一化。位置速度偏置直接加。注入后误差状态清零协方差按需重映射。6.4 调试与验证方法验证实现是否正确我常用三个手段。一是数值差分验证雅可比对观测模型用有限差分算出的雅可比和解析雅可比对比误差应该在1e-6量级。二是仿真闭环验证仿真数据跑完整流程对比估计轨迹和真值轨迹RMSE应该在合理范围。三是一致性检查用NEES归一化估计误差平方检验滤波一致性理想情况下NEES应该接近状态维度。这三个手段配合使用基本能覆盖绝大多数实现错误。我自己的经验是雅可比验证能抓出80%的bug剩下20%靠仿真和一致性检查。7. 一些个人体会这套三种状态的拆分刚接触时确实觉得绕但用熟之后会发现它其实是一种很优雅的工程妥协。真实状态负责讲道理标称状态负责干活误差状态负责纠偏三者各司其职把非线性、流形约束、数值稳定性这些棘手问题拆解成了可以分别处理的小问题。我踩过最大的坑是在姿态注入时搞错了旋转方向$q \otimes \delta q$ 写成了 $\delta q \otimes q$结果滤波在静止时还好一运动就发散。后来养成习惯每次写注入代码都先在纸上画一遍旋转顺序确认无误再敲键盘。这个习惯帮我省了无数调试时间。另外一个体会是不要迷信论文里的参数。IMU噪声密度、初始协方差这些论文给的都是典型值实际硬件差异很大。我的做法是先用 Allan 方差标定IMU噪声得到真实的噪声密度再根据标定结果设协方差。这一步花不了多少时间但能让滤波稳定性提升一个档次。最后分享一个小技巧如果你在实现MSCKF时觉得15维状态还是太多可以先从9维做起位置、速度、姿态把偏置当成已知常量。跑通之后再逐步加入偏置状态。这样分阶段实现每步都能验证比一上来就搞全状态要稳妥得多。等9维跑通了扩展到15维只是多填几个矩阵块的事。