误差状态卡尔曼滤波（ESKF）原理与应用：从传感器融合到机器人定位

1. 从“有意思”说起为什么误差状态卡尔曼滤波值得你花时间最近在整理一些传感器融合的老项目翻到ESKFError State Kalman Filter误差状态卡尔曼滤波的实现依然觉得这是个非常“有意思”的设计。说它有意思不是因为它多高深莫测恰恰相反当你理解了它的核心思想后会发现它用一种很巧妙的方式解决了很多我们在做惯性导航、机器人定位时遇到的棘手问题。比如用IMU惯性测量单元做姿态和位置估计时状态量里既有容易发散的位移、速度又有需要特殊处理的旋转四元数或旋转矩阵直接放在一个卡尔曼滤波框架里更新协方差矩阵的正定性、旋转的约束都很难保证。ESKF绕开了这个难题它不直接估计完整的状态而是去估计状态的“误差”。这个误差通常很小可以用一个简单的向量在欧式空间里表示所有标准卡尔曼滤波的线性假设在这里都变得非常合理。等你用这个小的误差去修正一个“名义状态”时又能得到高精度的完整状态估计。这种“分而治之”的思路既优雅又实用尤其是在处理IMU和视觉、激光雷达Lidar融合时几乎成了现代SLAM和自动驾驶定位系统的标配。如果你正在研究卡尔曼滤波、传感器融合或者被IMU预积分、相机-IMU标定里的数学搞得头大那么深入理解ESKF很可能就是你打通任督二脉的关键一步。2. ESKF的核心思想拆解名义状态、误差状态与真状态要搞懂ESKF首先得把三个核心概念理清楚名义状态、误差状态和真状态。这是理解整个算法框架的基石。2.1 三者的定义与关系我们可以用一个简单的类比来理解假设你要用手表记录时间。你的手表显示的时间是名义状态它由手表的内部机芯动力学模型驱动不断向前走。但由于机芯精度、温度变化等原因手表显示的时间和你手机上的标准时间真状态之间有一个微小的偏差这个偏差就是误差状态。在数学上我们通常用以下关系式来表达 [ \mathbf{x}{true} \mathbf{x}{nom} \boxplus \delta \mathbf{x} ] 这里的 (\mathbf{x}) 代表状态可能包含位置、速度、姿态四元数等(\boxplus) 是一个特殊的“加法”运算符。对于位置、速度这类欧式空间的状态(\boxplus) 就是普通的向量加法。但对于姿态四元数 (\mathbf{q})这个操作就复杂了它代表用误差状态一个三维的小角度旋转向量 (\delta \boldsymbol{\theta})去旋转名义姿态 [ \mathbf{q}{true} \mathbf{q}{nom} \otimes \mathbf{q}{\delta \boldsymbol{\theta}} ] 其中 (\otimes) 是四元数乘法(\mathbf{q}{\delta \boldsymbol{\theta}}) 是由小旋转向量 (\delta \boldsymbol{\theta}) 转换而来的四元数。关键在于误差状态 (\delta \mathbf{x}) 始终被设计为一个小的向量它的所有分量都在欧式空间中。这意味着无论名义状态里的旋转多复杂我们只需要在一个简单的、无约束的向量空间里对误差状态进行卡尔曼滤波的预测和更新。2.2 为什么这种“分离”是巧妙的这种设计的巧妙之处至少体现在三个方面线性化优势卡尔曼滤波要求系统模型和观测模型是线性的或者能进行合理的线性化。对于姿态动力学直接线性化四元数微分方程非常复杂且容易引入奇异性。而误差状态通常很小其动力学方程即误差如何传播可以在零点附近进行线性化这个线性化非常准确因为高阶项可以安全地忽略。这使得整个滤波过程始终在一个线性、良好的状态下运行。协方差管理的简化误差状态 (\delta \mathbf{x}) 的协方差矩阵 (\mathbf{P}) 描述的是这个小小误差向量的不确定性。由于误差状态空间是欧式的、无约束的(\mathbf{P}) 矩阵可以自由地增长、缩小并通过标准的卡尔曼公式更新完全不用担心它会变得不正定或者违反旋转矩阵的正交约束。我们只需要保证名义状态 (\mathbf{x}_{nom}) 自身满足约束例如定期归一化四元数即可。计算效率与数值稳定性误差状态维度通常比完整状态维度低例如用3维旋转向量代替4维四元数作为滤波状态且所有运算都在向量空间进行计算更高效。同时由于误差很小涉及到的雅可比矩阵计算也更稳定避免了在奇异点附近操作的风险。理解了这三个状态的关系我们就掌握了ESKF的“世界观”。接下来我们看看在这个世界观下滤波是如何一步步进行的。3. ESKF的完整工作流程预测、更新与重置ESKF的一个完整周期包含三个主要步骤预测、更新和重置。下图清晰地展示了数据流和状态转换过程flowchart TD A[上一周期最终状态br名义状态 x_nom, 误差状态 δx0] -- B[预测步骤] subgraph B [预测步骤] B1[IMU数据驱动br名义状态 x_nom 积分] -- B2[误差状态协方差 P 预测] end B -- C[等待观测] C -- D{是否有新观测?} D -- 是 -- E[更新步骤] subgraph E [更新步骤] E1[计算观测残差 z] -- E2[计算卡尔曼增益 K] E2 -- E3[更新误差状态 δx] E3 -- E4[更新协方差 P] end E -- F[重置步骤] subgraph F [重置步骤] F1[名义状态修正brx_nom x_nom ⊞ δx] -- F2[误差状态归零brδx ← 0] F2 -- F3[协方差矩阵重置brP G * P * Gᵀ] end F -- A D -- 否 -- C3.1 预测步骤名义状态积分与误差协方差传播预测步骤由IMU的角速度 (\boldsymbol{\omega}) 和加速度 (\mathbf{a}) 测量值驱动。名义状态预测这部分和传统的惯性导航解算完全一样。我们根据IMU的测量值对名义状态进行数值积分例如使用龙格-库塔法。对于姿态我们解算四元数微分方程对于位置和速度我们进行双重积分。这个过程中我们只使用IMU的测量值不考虑误差状态。预测后我们得到一个基于IMU动力学模型推算出来的名义状态 (\mathbf{x}_{nom, pred})。// 伪代码示例名义状态预测姿态部分 Quaternion q_prev x_nom.orientation; Vector3 omega_meas imu_data.gyro - bg_hat; // 减去估计的零偏 Quaternion dq Quaternion::fromAxisAngle(omega_meas * dt); x_nom_pred.orientation (q_prev * dq).normalized(); // 速度、位置预测略...误差状态协方差预测这是ESKF预测步骤的精髓。我们不再直接预测状态而是预测误差状态的协方差矩阵 (\mathbf{P})。我们需要推导出误差状态的连续时间动力学方程 (\dot{\delta \mathbf{x}} \mathbf{F} \delta \mathbf{x} \mathbf{G} \mathbf{i})其中 (\mathbf{F}) 是误差状态关于自身的雅可比矩阵系统矩阵(\mathbf{G}) 是噪声驱动矩阵(\mathbf{i}) 是IMU的噪声向量包括陀螺仪和加速度计的白噪声。然后我们将其离散化得到离散时间的状态转移矩阵 (\mathbf{F}_k) 和噪声协方差矩阵 (\mathbf{Q}k)。最后用标准卡尔曼滤波的预测公式更新协方差 [ \mathbf{P}{pred} \mathbf{F}k \mathbf{P}{k-1} \mathbf{F}_k^T \mathbf{Q}_k ] 这里的 (\mathbf{Q}_k) 就是过程噪声协方差它直接反映了你对IMU噪声角速度随机游走、加速度计随机游走等大小的信任程度。它的设置至关重要我们会在后面专门讨论。3.2 更新步骤利用观测修正误差当有其他传感器如GPS、视觉、激光雷达提供观测时我们进入更新步骤。关键点在于观测模型是基于真状态建立的但我们要用它来更新误差状态。计算观测残差首先我们用预测的名义状态 (\mathbf{x}{nom, pred}) 计算一个预期的观测值 (\mathbf{z}{pred})。然后将实际传感器读数 (\mathbf{z}{meas}) 与预期观测值比较得到残差 (\mathbf{y}) [ \mathbf{y} \mathbf{z}{meas} - \mathbf{z}_{pred} ] 注意这个残差本质上度量的是“真状态”与“名义状态”之间的差异在观测空间上的投影因此它天然地对应着误差状态 (\delta \mathbf{x})。构建观测矩阵我们需要知道误差状态 (\delta \mathbf{x}) 是如何影响观测残差 (\mathbf{y}) 的。这通过计算观测模型关于误差状态的雅可比矩阵 (\mathbf{H}) 得到 [ \mathbf{H} \frac{\partial \mathbf{z}{pred}}{\partial \delta \mathbf{x}} \bigg|{\delta \mathbf{x}0} ] 由于误差状态在名义状态处即 (\delta \mathbf{x}0)线性化这个雅可比矩阵的计算通常是直接且清晰的。执行卡尔曼更新有了残差 (\mathbf{y})、观测矩阵 (\mathbf{H})、预测的误差协方差 (\mathbf{P}{pred}) 以及观测噪声协方差 (\mathbf{R})我们就可以套用标准卡尔曼增益公式和状态更新公式 [ \mathbf{K} \mathbf{P}{pred} \mathbf{H}^T (\mathbf{H} \mathbf{P}{pred} \mathbf{H}^T \mathbf{R})^{-1} ] [ \delta \mathbf{x}{update} \mathbf{K} \mathbf{y} ] [ \mathbf{P}{update} (\mathbf{I} - \mathbf{K} \mathbf{H}) \mathbf{P}{pred} ] 这一步结束后我们得到了一个非零的、经过观测修正的误差状态估计(\delta \mathbf{x}{update}) 和更新后的误差协方差 (\mathbf{P}{update})。3.3 重置步骤将误差注入名义状态并归零更新之后误差状态 (\delta \mathbf{x}) 不再为零。重置步骤的目的就是将这个估计出的误差“吸收”到名义状态中然后将误差状态重置为零为下一个滤波周期做准备。名义状态注入使用我们之前定义的 (\boxplus) 运算符用估计的误差状态修正名义状态 [ \mathbf{x}{nom, final} \mathbf{x}{nom, pred} \boxplus \delta \mathbf{x}_{update} ] 对于位置直接相加。对于姿态四元数用估计的小旋转向量 (\delta \boldsymbol{\theta}) 构成四元数然后与名义四元数相乘。完成这一步后名义状态变得更接近真状态。误差状态归零将误差状态向量设为零向量(\delta \mathbf{x} \leftarrow \mathbf{0})。协方差矩阵重置这是最容易忽略但至关重要的一步。当我们把误差注入名义状态后误差状态的定义基准点发生了变化从更新前的名义状态变成了更新后的名义状态。因此描述误差不确定性的协方差矩阵 (\mathbf{P}) 也需要进行相应的变换。这个变换通过一个雅可比矩阵 (\mathbf{G}) 来完成 [ \mathbf{P}{final} \mathbf{G} \mathbf{P}{update} \mathbf{G}^T ] 矩阵 (\mathbf{G}) 描述了“旧”的误差状态如何映射到以新名义状态为基准的“新”误差状态。对于大多数欧式空间状态(\mathbf{G}) 是单位阵。但对于姿态误差旋转向量当注入的旋转 (\delta \boldsymbol{\theta}) 不是零时(\mathbf{G}) 会是一个非单位的矩阵其作用是保证协方差矩阵在新的误差状态原点附近仍然保持正确的几何意义。忽略这一步会导致滤波器的性能下降甚至发散。4. 关键参数剖析过程噪声Q与观测噪声R的实战设置滤波器调参永远是理论和实践的结合点。在ESKF中过程噪声协方差 (\mathbf{Q})和观测噪声协方差 (\mathbf{R})的设定直接决定了滤波器的“性格”是更相信动力学模型IMU还是更相信观测传感器。4.1 过程噪声QIMU噪声的离散化体现过程噪声 (\mathbf{Q}) 来源于IMU的测量噪声。它不是一个可以随意调整的“魔法数字”而应该从IMU的噪声特性推导出来。IMU的噪声通常用连续时间的功率谱密度PSD来刻画比如陀螺仪的角随机游走ARW和加速度计的速率随机游走VRW。我们需要将这些连续时间噪声特性通过离散化的系统动力学模型转化为离散时间的过程噪声协方差矩阵 (\mathbf{Q}_k)。一个简化的、针对误差状态中姿态、速度、位置的 (\mathbf{Q}) 矩阵推导如下 假设误差状态为 (\delta \mathbf{x} [\delta \boldsymbol{\theta}^T, \delta \mathbf{v}^T, \delta \mathbf{p}^T]^T)IMU噪声为角速度白噪声 (\mathbf{n}_g) 和加速度白噪声 (\mathbf{n}_a)其协方差强度分别为 (\sigma_g^2) 和 (\sigma_a^2)。在短时间 (\Delta t) 内离散化的 (\mathbf{Q}_k) 可以近似为 [ \mathbf{Q}_k \approx \begin{bmatrix} \sigma_g^2 \Delta t \mathbf{I}_3 \mathbf{0} \mathbf{0} \ \mathbf{0} \sigma_a^2 \Delta t \mathbf{I}_3 \mathbf{0} \ \mathbf{0} \mathbf{0} \mathbf{0} \end{bmatrix} ] 但这只是一个非常粗略的近似。更精确的推导需要考虑噪声如何通过动力学方程传播到各个误差状态。通常我们会根据误差状态的连续时间微分方程 [ \dot{\delta \mathbf{x}} \mathbf{F}_c \delta \mathbf{x} \mathbf{G}_c \mathbf{i} ] 其中 (\mathbf{i} [\mathbf{n}_g^T, \mathbf{n}_a^T]^T) 是连续时间噪声向量。然后通过离散化公式计算 (\mathbf{Q}_k) [ \mathbf{Q}k \int{0}^{\Delta t} \exp(\mathbf{F}_c \tau) \mathbf{G}_c \mathbf{Q}_c \mathbf{G}_c^T \exp(\mathbf{F}_c^T \tau) d\tau ] 其中 (\mathbf{Q}_c) 是连续时间噪声的强度矩阵对角线上是 (\sigma_g^2) 和 (\sigma_a^2)。对于大多数应用如果采样频率足够高IMU频率100Hz可以使用一阶近似简化计算许多开源库如GTSAM, Kalibr都提供了现成的函数。实操心得一开始可以IMU数据手册上给出的噪声密度Noise Density参数作为初始值。例如某IMU的陀螺仪噪声密度为4e-3 rad/s/√Hz那么其角随机游走 (\sigma_g) 就等于这个值。加速度计同理。在实际调试中可以围绕这个理论值进行微调。调大Q意味着你认为IMU模型不可靠、噪声大滤波器会更信任观测收敛快但可能受观测噪声影响大调小Q则更信任IMU短期精度平滑性好但观测修正作用弱系统误差如零偏可能导致估计发散。4.2 观测噪声R衡量传感器的可信度观测噪声协方差 (\mathbf{R}) 相对更直观它代表了观测传感器的不确定性。例如GPS可以根据接收机报告的HDOP水平精度因子、VDOP垂直精度因子和伪距误差来构造一个与当前位置相关的 (\mathbf{R}) 矩阵。静态时也可以通过采集一段静止数据计算其位置输出的方差来获得。视觉/激光里程计这个不确定性通常与特征点匹配质量、运动模糊、场景纹理等有关。在VO/VIO中常常会提供一个位姿估计的协方差。如果没有可以将其设为一个与平移量、旋转量成比例的固定值或者根据重投影误差来在线估计。零速修正ZUPT当检测到脚部或车辆静止时可以将速度观测的噪声设得非常小例如0.01 m/s告诉滤波器“此刻速度绝对应该是零”。一个常见的坑不同观测量的单位不同。确保你的 (\mathbf{R}) 矩阵对角线上的数值单位与观测残差 (\mathbf{y}) 的单位平方相匹配。例如位置残差单位是米那么 (\mathbf{R}) 中对应位置观测的值的单位就是 (m^2)姿态残差如果是角度弧度单位就是 (rad^2)。单位不一致会导致增益计算错误滤波器行为异常。5. 从理论到代码实现ESKF的注意事项与调试技巧理解了原理和流程动手实现时还会遇到一堆实际问题。这里分享几个关键的注意事项和调试技巧。5.1 姿态参数化与雅可比计算姿态的误差状态通常用一个三维旋转向量 (\delta \boldsymbol{\theta}) 表示它对应于轴-角表示中的“角度*轴”。在计算观测矩阵 (\mathbf{H}) 或状态转移矩阵 (\mathbf{F}) 时需要计算旋转对这个小向量的雅可比。核心公式一个三维向量 (\mathbf{v})在全局坐标系下相对于一个由旋转向量 (\delta \boldsymbol{\theta}) 表示的微小旋转的雅可比为 [ \frac{\partial (\mathbf{R} {\delta \boldsymbol{\theta}} \mathbf{v})}{\partial \delta \boldsymbol{\theta}} \bigg|{\delta \boldsymbol{\theta}0} \approx -\lfloor \mathbf{v} \rfloor\times ] 其中 (\lfloor \mathbf{v} \rfloor_\times) 是向量 (\mathbf{v}) 的叉乘矩阵反对称矩阵。这个公式在计算视觉重投影误差关于姿态误差的雅可比时非常常用。四元数更新在重置步骤进行四元数更新时即 (\mathbf{q}{new} \mathbf{q}{old} \otimes \mathbf{q}{\delta \boldsymbol{\theta}})要确保生成的小旋转四元数 (\mathbf{q}{\delta \boldsymbol{\theta}}) 是有效的。当 (|\delta \boldsymbol{\theta}|) 很小时可以使用近似公式 [ \mathbf{q}{\delta \boldsymbol{\theta}} \approx \begin{bmatrix} 1 \ \frac{1}{2} \delta \boldsymbol{\theta} \end{bmatrix} ] 然后记得对结果四元数进行归一化。5.2 初始化静止对齐与协方差设定滤波器的初始状态和协方差很重要。一个标准的做法是进行静止初始化将设备静止放置数秒。采集这段时间的IMU数据。加速度计数据的平均值除以重力加速度大小g可以用于估计初始俯仰和横滚角。注意这个方法无法估计航向角yaw初始航向可以设为零或由磁力计提供。计算这段时间加速度计和陀螺仪数据的方差。这里得到的测量方差和ESKF中的过程噪声Q有关系吗有间接关系但不能直接等同。静止初始化得到的测量方差反映了传感器在静止状态下的输出波动它包含了传感器的白噪声和可能的温度漂移等。而过程噪声Q建模的是状态演化过程中的不确定性它是由IMU噪声通过系统模型传播而来的。初始化方差可以作为设置陀螺仪和加速度计噪声密度(\sigma_g, \sigma_a)的一个参考进而推导出Q。但Q还包含了模型不准确等因素通常需要比纯测量方差稍大一些。初始协方差矩阵 (\mathbf{P}_0) 应该反映你对初始状态的置信度。位置、速度不确定性可以设大一些比如位置10m速度1m/s姿态不确定性尤其是航向也可以设大些。零偏的初始不确定性可以设为其典型变化范围。5.3 调试与性能评估可视化是关键绘制估计的轨迹、速度、姿态角并与参考轨迹如有或观测值对比。特别关注更新时刻的状态跳变是否平滑以及预测阶段的漂移情况。分析新息序列卡尔曼滤波的“新息”Innovation就是观测残差 (\mathbf{y})。在理想情况下新息序列应该是一个零均值的白噪声过程。你可以计算新息的自相关函数或者直接观察其曲线。如果新息有明显的时间相关性非白噪声说明你的模型系统模型或观测模型有未建模的动态或者噪声参数Q/R设置不当。检查协方差观察协方差矩阵 (\mathbf{P}) 的对角线元素各状态分量的方差。它们应该在更新后变小在预测阶段逐渐增大。如果协方差莫名其妙地急剧缩小或增长可能是数值计算问题或模型错误。处理不同步传感器IMU频率高100-500HzGPS/视觉频率低1-100Hz。在代码中需要维护一个状态缓冲区在IMU预测步骤中按高频率积分只在收到观测数据时才触发更新步骤。对于观测数据可能需要根据时间戳进行插值或对齐到最近的预测状态。实现一个稳定可靠的ESKF需要耐心和细致的调试。从简单的仿真环境开始比如用Matlab生成带噪声的IMU和GPS数据验证基本流程的正确性然后再接入真实的传感器数据是一个稳妥的路径。当你看到滤波器能够有效地融合高速但会漂移的IMU和低频但绝对准确的观测数据输出一条平滑而精确的轨迹时那种成就感就是对“有意思”这个词最好的诠释。