误差状态卡尔曼滤波（ESKF）在IMU与GNSS融合中的原理与实现

误差状态卡尔曼滤波（ESKF）在IMU与GNSS融合中的原理与实现

发布时间：2026/8/25 8:09:23
误差状态卡尔曼滤波（ESKF）在IMU与GNSS融合中的原理与实现 1. 从“状态”到“误差状态”为什么ESKF是融合IMU与GNSS的更优解在自动驾驶、机器人定位或者任何需要高精度动态姿态估计的领域IMU惯性测量单元和GNSS全球导航卫星系统的融合是一个经典且核心的问题。我们手里有两样东西一个是IMU它像是一个蒙着眼睛但感觉极其敏锐的运动员能通过自身的加速度计和陀螺仪感知到每一刻的“动作”角速度和线加速度但问题是它不知道自己一开始站在哪里而且随着时间推移它对自己位置的“感觉”会越来越飘误差会不断累积这叫“积分漂移”。另一个是GNSS它像是一个视力极好但反应有点慢的观察员能直接告诉你“你现在在地球上的经纬高坐标是多少”数据绝对准确在信号好的情况下但它的更新频率低比如10Hz而且容易被遮挡、产生跳变。所以很自然的想法就是让感觉敏锐但会迷路的运动员IMU和看得准但反应慢的观察员GNSS互相纠正得出一个既实时又准确的位置和姿态。卡尔曼滤波器KF及其各种变体就是让这两位“合作”的经典算法框架。但为什么是误差状态卡尔曼滤波器Error-State Kalman Filter, ESKF而不是直接用标准卡尔曼滤波器去融合IMU和GNSS的“全状态”位置、速度、姿态等呢这里有一个关键矛盾IMU的动力学模型由角速度和加速度积分得到姿态和位置是高度非线性的尤其是姿态部分用四元数或旋转矩阵表示。标准卡尔曼滤波器要求系统是线性的对于非线性系统我们常用扩展卡尔曼滤波器EKF即对非线性模型进行一阶泰勒展开线性化。然而当状态量包含姿态时直接在姿态空间如四元数上进行线性化会带来问题冗余性用于表示姿态的四元数是4维的但实际自由度只有3滚转、俯仰、偏航。这种过参数化会导致协方差矩阵奇异在滤波器中引发数值不稳定。非欧几里得空间姿态属于SO(3)特殊正交群其加法不封闭。简单地对四元数做“预测值 卡尔曼增益 * 创新”的运算结果可能不是一个合法的单位四元数破坏了约束。ESKF提供了一个优雅的解决方案。它的核心思想是我们不直接估计那个复杂的“全状态”而是去估计“全状态”的“误差”。这个“误差”通常很小并且在局部可以近似为一个在欧几里得空间R3中变化的向量。具体来说名义状态 (Nominal State)用一个纯粹的、理想的IMU动力学模型进行积分推进这个模型不考虑噪声。你可以把它想象成IMU“自以为”的状态轨迹。它包含位置、速度、姿态四元数等。误差状态 (Error State)真实状态与名义状态之间的微小差异。对于位置和速度误差就是三维向量差。对于姿态误差通常用一个三维的小角度扰动向量或等效旋转向量来表示对应到姿态四元数的局部切空间。真实状态 (True State)名义状态与误差状态的组合对于姿态是四元数乘法。ESKF的巧妙之处在于卡尔曼滤波器的主循环预测-更新完全在“误差状态”这个小小的、线性的、欧几里得空间里进行。我们预测误差如何传播然后用GNSS等观测来修正这个误差。修正后的误差被“注入”到名义状态中然后将误差状态重置为零开始下一轮循环。因为误差始终很小所以线性化假设在整个过程中都保持良好同时完美规避了在姿态流形上直接运算的麻烦。所以当你看到“IMUGNSS的误差状态卡尔曼滤波器ESKF的数学模型”这个标题时它指向的正是解决高精度、鲁棒性位姿估计中最核心的那个数学框架。接下来我们就一层层拆解这个模型的构成。2. ESKF状态空间定义名义状态、误差状态与真实状态构建数学模型的第一步是明确定义状态变量。在IMU/GNSS融合的ESKF中我们需要区分三个层次的状态。2.1 名义状态 (Nominal State)名义状态 (\mathbf{x}) 是一个理想化的、不考虑噪声的状态。它通常包含以下变量 [ \mathbf{x} [\mathbf{p}^T, \mathbf{v}^T, \mathbf{q}^T, \mathbf{b}_a^T, \mathbf{b}_g^T]^T ] 其中(\mathbf{p} \in \mathbb{R}^3)位置矢量在导航坐标系中例如东北天ENU。(\mathbf{v} \in \mathbb{R}^3)速度矢量在导航坐标系中。(\mathbf{q} \in \mathbb{S}^3)姿态四元数表示从载体坐标系b即IMU坐标系到导航坐标系n的旋转。它是一个单位四元数满足 (\mathbf{q} \otimes \mathbf{q}^* [1, 0, 0, 0]^T)其中 (\otimes) 表示四元数乘法。(\mathbf{b}_a \in \mathbb{R}^3)加速度计的零偏Bias。(\mathbf{b}_g \in \mathbb{R}^3)陀螺仪的零偏Bias。名义状态是IMU动力学模型直接积分推进的对象。它的运动学方程来源于刚体运动学和牛顿第二定律。2.2 误差状态 (Error State)误差状态 (\delta \mathbf{x}) 是真实状态与名义状态之间的微小差异。它是ESKF实际估计的量。 [ \delta \mathbf{x} [\delta \mathbf{p}^T, \delta \mathbf{v}^T, \delta \boldsymbol{\theta}^T, \delta \mathbf{b}_a^T, \delta \mathbf{b}_g^T]^T ] 其中(\delta \mathbf{p} \in \mathbb{R}^3)位置误差。(\delta \mathbf{v} \in \mathbb{R}^3)速度误差。(\delta \boldsymbol{\theta} \in \mathbb{R}^3)姿态误差。这是一个三维小角度向量其方向表示旋转轴模长表示旋转角度。它与姿态四元数的误差关系为(\mathbf{q}_{true} \approx \mathbf{q} \otimes \begin{bmatrix} 1 \ \frac{1}{2}\delta \boldsymbol{\theta} \end{bmatrix})。这里 (\delta \boldsymbol{\theta}) 存在于载体坐标系的切空间中。(\delta \mathbf{b}_a \in \mathbb{R}^3)加速度计零偏误差。(\delta \mathbf{b}_g \in \mathbb{R}^3)陀螺仪零偏误差。关键点误差状态的所有分量都是三维实向量生活在欧几里得空间 (\mathbb{R}^{15})假设15维状态中。这使得我们可以对其应用标准的线性卡尔曼滤波公式。2.3 真实状态 (True State) 及其与误差状态的关系真实状态 (\mathbf{x}_{true}) 是我们想知道的“地面真值”。它由名义状态和误差状态共同定义加性误差对于位置、速度、零偏 [ \mathbf{p}{true} \mathbf{p} \delta \mathbf{p} ] [ \mathbf{v}{true} \mathbf{v} \delta \mathbf{v} ] [ \mathbf{b}_{a, true} \mathbf{b}_a \delta \mathbf{b}a ] [ \mathbf{b}{g, true} \mathbf{b}_g \delta \mathbf{b}_g ]乘性误差对于姿态四元数 [ \mathbf{q}_{true} \mathbf{q} \otimes \delta \mathbf{q}(\delta \boldsymbol{\theta}) ] 其中 (\delta \mathbf{q}(\delta \boldsymbol{\theta}) \approx \begin{bmatrix} 1 \ \frac{1}{2}\delta \boldsymbol{\theta} \end{bmatrix}) 是由小角度误差向量 (\delta \boldsymbol{\theta}) 构造的扰动四元数。这个关系是ESKF的基石。名义状态由IMU数据“开环”积分可能会漂移。误差状态由ESKF“闭环”估计并保持很小用于持续修正名义状态。每次修正更新后误差状态被“注入”到名义状态然后自身重置为零准备进行下一轮的误差累积估计。实操心得状态维度的选择上面给出的是15维状态位置3速度3姿态3加速度计零偏3陀螺仪零偏3。这是最经典的配置。但在实际中你可能需要根据传感器和场景调整如果GNSS提供的是纯位置观测且不关心高度方向的速度有时会省略速度的状态但这会降低动态性能。如果IMU质量很高或者运行时间短零偏变化慢可以考虑将零偏建模为随机游走而不是高斯-马尔可夫过程这会影响状态方程。对于车载应用有时会加入轮速计模型状态量会相应增加。明确你的状态向量是设计滤波器的第一步。3. IMU动力学模型与误差状态系统方程推导系统方程描述了状态如何随时间演化。在ESKF中我们需要分别推导名义状态和误差状态的系统方程。3.1 名义状态系统方程连续时间名义状态的运动学由IMU的测量值驱动但忽略噪声。假设在k时刻我们获得了IMU的角速度测量 (\tilde{\boldsymbol{\omega}}_k) 和加速度测量 (\tilde{\mathbf{a}}_k)。连续时间下的名义状态微分方程如下位置位置的变化率就是速度。 [ \dot{\mathbf{p}} \mathbf{v} ]速度速度的变化率是加速度。这里加速度需要从载体坐标系转换到导航坐标系并减去重力加速度 (\mathbf{g})通常在导航系下为 ([0, 0, g]^T)。名义状态使用“理想”的测量值。 [ \dot{\mathbf{v}} \mathbf{R}(\mathbf{q}) \tilde{\mathbf{a}} - \mathbf{g} ] 其中 (\mathbf{R}(\mathbf{q})) 是由四元数 (\mathbf{q}) 构成的旋转矩阵用于将载体系下的矢量转换到导航系。姿态姿态四元数的微分方程由角速度决定。 [ \dot{\mathbf{q}} \frac{1}{2} \mathbf{q} \otimes \begin{bmatrix} 0 \ \tilde{\boldsymbol{\omega}} \end{bmatrix} ]零偏在名义状态方程中我们通常假设零偏是常数或者变化非常缓慢。因此其导数为零。 [ \dot{\mathbf{b}}_a \mathbf{0}, \quad \dot{\mathbf{b}}_g \mathbf{0} ]这些方程构成了一个常微分方程组ODE。在实际的代码实现中我们通常在离散时间步长 (\Delta t) 内对其进行积分例如使用龙格-库塔法来从k时刻的名义状态 (\mathbf{x}k) 预测k1时刻的名义状态 (\mathbf{x}{k1})。这个过程就是IMU的“前向传播”。3.2 误差状态系统方程连续时间及其线性化误差状态的演化才是卡尔曼滤波器预测步关心的核心。我们需要找到 (\delta \dot{\mathbf{x}} \mathbf{F} \delta \mathbf{x} \mathbf{G} \mathbf{w}) 这样的形式其中 (\mathbf{F}) 是误差状态转移矩阵(\mathbf{G}) 是噪声驱动矩阵(\mathbf{w}) 是系统噪声。推导过程涉及对真实状态方程进行一阶泰勒展开并利用真实状态、名义状态和误差状态之间的关系。这是一个关键的推导步骤这里给出结果和物理意义的解释。真实IMU的测量值包含零偏和白噪声 [ \tilde{\boldsymbol{\omega}}{true} \boldsymbol{\omega}{true} \mathbf{b}{g, true} \mathbf{n}g ] [ \tilde{\mathbf{a}}{true} \mathbf{a}{true} \mathbf{b}_{a, true} \mathbf{n}_a ] 其中 (\mathbf{n}_g, \mathbf{n}_a) 是高斯白噪声。将真实状态方程减去名义状态方程并忽略二阶及以上小量可以得到误差状态的连续时间微分方程[ \delta \dot{\mathbf{p}} \delta \mathbf{v} ] [ \delta \dot{\mathbf{v}} -\mathbf{R}[\tilde{\mathbf{a}}]_\times \delta \boldsymbol{\theta} \mathbf{R} \delta \mathbf{b}_a \mathbf{R} \mathbf{n}a ] [ \delta \dot{\boldsymbol{\theta}} -[\tilde{\boldsymbol{\omega}}]\times \delta \boldsymbol{\theta} \delta \mathbf{b}_g \mathbf{n}_g ] [ \delta \dot{\mathbf{b}}a \mathbf{n}{ba}, \quad \delta \dot{\mathbf{b}}g \mathbf{n}{bg} ]公式解读(\delta \dot{\mathbf{p}} \delta \mathbf{v})位置误差的增长直接来源于速度误差。这很直观。(\delta \dot{\mathbf{v}}) 方程速度误差的变化由两部分驱动(-\mathbf{R}[\tilde{\mathbf{a}}]_\times \delta \boldsymbol{\theta})这是最关键的一项。它表示姿态误差 (\delta \boldsymbol{\theta}) 如何影响速度。([\tilde{\mathbf{a}}]_\times) 是加速度测量值的反对称矩阵。因为姿态有误差导致我们把载体坐标系下的加速度矢量错误地旋转到了导航系这个旋转误差与加速度的大小成正比。加速度越大姿态误差对速度进而对位置的影响就越显著。这就是为什么在剧烈运动时纯惯性导航会迅速发散。(\mathbf{R} \delta \mathbf{b}_a \mathbf{R} \mathbf{n}_a)加速度计零偏误差 (\delta \mathbf{b}_a) 和噪声 (\mathbf{n}_a) 直接贡献为速度误差。(\delta \dot{\boldsymbol{\theta}}) 方程姿态误差的变化也由两部分驱动(-[\tilde{\boldsymbol{\omega}}]_\times \delta \boldsymbol{\theta})表示现有的姿态误差会在角速度的影响下发生“旋转”。这是误差的自身传播。(\delta \mathbf{b}_g \mathbf{n}_g)陀螺仪零偏误差和噪声直接积分成为姿态误差。陀螺仪的零偏是导致姿态漂移的元凶。零偏误差被建模为随机游走过程其导数等于白噪声 (\mathbf{n}{ba}, \mathbf{n}{bg})。将上述方程写成矩阵形式 (\delta \dot{\mathbf{x}} \mathbf{F}_c \delta \mathbf{x} \mathbf{G}_c \mathbf{w})我们就可以得到连续时间的误差状态转移矩阵 (\mathbf{F}_c) 和噪声驱动矩阵 (\mathbf{G}_c)。其中噪声向量 (\mathbf{w} [\mathbf{n}a^T, \mathbf{n}g^T, \mathbf{n}{ba}^T, \mathbf{n}{bg}^T]^T)。3.3 离散化与预测步协方差更新卡尔曼滤波器在计算机中运行是离散的。我们需要将连续时间系统方程离散化。假设IMU采样周期为 (\Delta t)且 (\Delta t) 很小通常采用一阶近似欧拉法或零阶保持ZOH假设。离散化的误差状态转移矩阵 (\mathbf{F}_k) 近似为 [ \mathbf{F}_k \approx \mathbf{I} \mathbf{F}_c \Delta t ] 离散化的噪声协方差矩阵 (\mathbf{Q}_k) 为 [ \mathbf{Q}k \int{0}^{\Delta t} \mathbf{F}_c(\tau) \mathbf{G}_c \mathbf{Q}_c \mathbf{G}_c^T \mathbf{F}_c(\tau)^T d\tau ] 其中 (\mathbf{Q}_c) 是连续时间噪声 (\mathbf{w}) 的功率谱密度矩阵。在实际应用中为了简化常常采用近似公式 [ \mathbf{Q}_k \approx \mathbf{G}_c \mathbf{Q}_c \mathbf{G}_c^T \Delta t ] 或者更精细地计算每个块矩阵的积分。有了 (\mathbf{F}_k) 和 (\mathbf{Q}_k)ESKF的预测步只针对误差状态就可以执行名义状态预测使用3.1节的运动学方程和IMU数据积分更新名义状态 (\mathbf{x}_{k1|k})。误差状态协方差预测 [ \mathbf{P}_{k1|k} \mathbf{F}k \mathbf{P}{k|k} \mathbf{F}_k^T \mathbf{Q}_k ] 注意这里预测的是误差状态 (\delta \mathbf{x}) 的协方差 (\mathbf{P})而不是名义状态的协方差。并且在预测步之后我们并不更新误差状态的均值因为误差状态的均值在上一轮更新后被重置为零了我们预测的是其不确定性协方差如何增长。踩坑实录离散化近似的陷阱使用简单的 (\mathbf{F}_k \mathbf{I} \mathbf{F}_c \Delta t) 和 (\mathbf{Q}_k \mathbf{G}_c \mathbf{Q}_c \mathbf{G}_c^T \Delta t) 在IMU频率高如200Hz且系统动态不强时问题不大。但在剧烈运动高角速度、高加速度时这种近似误差会变大导致预测的协方差 (\mathbf{P}) 不准确进而影响滤波器的增益和性能。一个常见的改进是使用预积分技术。IMU预积分在因子图优化中更常见但其思想对理解离散化有帮助它在两个关键帧之间对IMU测量进行积分得到一个相对运动约束这个约束本身对零偏更鲁棒并且积分过程可以更精确地处理噪声和离散化误差。在ESKF中如果运动剧烈需要考虑更高阶的离散化方法或减小预测步长。4. GNSS观测模型与ESKF更新步预测步基于IMU数据推演了状态和其不确定性而更新步则利用GNSS的观测数据来修正这些估计。GNSS接收机通常输出在导航坐标系如WGS-84经纬高或局部ENU坐标系下的位置信息有时也包含速度信息。4.1 GNSS观测方程假设在k时刻我们获得了GNSS的位置观测 (\mathbf{z}_p)。观测方程描述了观测值与状态量之间的关系。对于位置观测非常简单 [ \mathbf{z}p \mathbf{p}{true} \mathbf{v}_p \mathbf{p} \delta \mathbf{p} \mathbf{v}_p ] 其中 (\mathbf{v}_p) 是GNSS位置观测的高斯白噪声其协方差为 (\mathbf{R}_p)。这个噪声的大小取决于GNSS的信号质量如DOP值、信噪比可以通过接收机输出的精度指标或经验值来设定。如果我们还有GNSS速度观测 (\mathbf{z}_v)例如来自多普勒频移那么观测方程为 [ \mathbf{z}v \mathbf{v}{true} \mathbf{v}_v \mathbf{v} \delta \mathbf{v} \mathbf{v}_v ] 其中 (\mathbf{v}_v) 是速度观测噪声协方差为 (\mathbf{R}_v)。4.2 观测矩阵H的推导在卡尔曼滤波中我们需要观测矩阵 (\mathbf{H})它将状态空间映射到观测空间(\mathbf{z} \mathbf{H} \mathbf{x} \mathbf{v})。在ESKF中我们的状态是误差状态 (\delta \mathbf{x})观测方程需要围绕名义状态线性化。对于位置观测我们将观测方程在名义状态 (\mathbf{x})其误差状态均值为0处展开 [ \mathbf{z}p \mathbf{p}{true} \mathbf{v}_p (\mathbf{p} \delta \mathbf{p}) \mathbf{v}_p ] 我们可以将其重写为 [ \mathbf{z}_p - \mathbf{p} \delta \mathbf{p} \mathbf{v}_p ] 令创新Innovation或观测残差(\mathbf{y} \mathbf{z}p - \mathbf{p})它表示实际观测与基于名义状态的预测观测之间的差异。那么上式变为 [ \mathbf{y} \mathbf{H}p \delta \mathbf{x} \mathbf{v}p ] 其中观测矩阵 (\mathbf{H}p) 是一个 (3 \times 15) 的矩阵。因为 (\mathbf{y}) 只与 (\delta \mathbf{p}) 有关所以 [ \mathbf{H}p [\mathbf{I}{3\times3}, \mathbf{0}{3\times3}, \mathbf{0}{3\times3}, \mathbf{0}{3\times3}, \mathbf{0}{3\times3}] ] 即只有前3列对应 (\delta \mathbf{p})是单位矩阵其余都是零。同理对于速度观测有 [ \mathbf{z}v - \mathbf{v} \delta \mathbf{v} \mathbf{v}v ] 对应的观测矩阵 (\mathbf{H}v [\mathbf{0}{3\times3}, \mathbf{I}{3\times3}, \mathbf{0}{3\times3}, \mathbf{0}{3\times3}, \mathbf{0}{3\times3}])。如果同时使用位置和速度观测可以将观测向量和矩阵堆叠起来。4.3 ESKF更新步完整流程ESKF的更新步在误差状态空间中进行遵循标准卡尔曼滤波公式计算观测残差 [ \mathbf{y} \mathbf{z} - \mathbf{h}(\mathbf{x}) ] 其中 (\mathbf{h}(\mathbf{x})) 是利用当前名义状态 (\mathbf{x}) 计算出的预测观测值。对于GNSS位置就是 (\mathbf{p})。计算卡尔曼增益 [ \mathbf{K} \mathbf{P}{k1|k} \mathbf{H}^T (\mathbf{H} \mathbf{P}{k1|k} \mathbf{H}^T \mathbf{R})^{-1} ] 其中 (\mathbf{P}_{k1|k}) 是预测的误差状态协方差(\mathbf{H}) 是观测矩阵(\mathbf{R}) 是观测噪声协方差矩阵。更新误差状态及其协方差 [ \delta \mathbf{x}{k1|k1} \mathbf{K} \mathbf{y} ] [ \mathbf{P}{k1|k1} (\mathbf{I} - \mathbf{K} \mathbf{H}) \mathbf{P}_{k1|k} (\mathbf{I} - \mathbf{K} \mathbf{H})^T \mathbf{K} \mathbf{R} \mathbf{K}^T ] 后一式子为约瑟夫形式数值上更稳定注意这里更新的是误差状态的均值 (\delta \mathbf{x})。在更新之前它的期望是0更新之后它得到了一个非零值代表了基于最新观测对当前名义状态误差的最佳估计。误差状态注入与重置 这是ESKF区别于EKF的关键一步。我们将估计出的误差状态注入到名义状态中以修正名义状态 [ \mathbf{p} \leftarrow \mathbf{p} \delta \mathbf{p} ] [ \mathbf{v} \leftarrow \mathbf{v} \delta \mathbf{v} ] [ \mathbf{q} \leftarrow \mathbf{q} \otimes \delta \mathbf{q}(\delta \boldsymbol{\theta}) ] [ \mathbf{b}_a \leftarrow \mathbf{b}_a \delta \mathbf{b}_a ] [ \mathbf{b}g \leftarrow \mathbf{b}g \delta \mathbf{b}g ] 注入完成后名义状态得到了修正变得更接近真实状态。然后必须将误差状态重置为零 [ \delta \mathbf{x} \leftarrow \mathbf{0} ] 同时误差状态的协方差矩阵也需要反映这次重置。由于误差状态均值归零其协方差应更新为 [ \mathbf{P}{k1|k1} \leftarrow \mathbf{G} \mathbf{P}{k1|k1} \mathbf{G}^T ] 其中 (\mathbf{G}) 是注入过程的雅可比矩阵它描述了误差状态在注入后如何重新参数化。对于加性误差的部分位置、速度、零偏(\mathbf{G}) 对应块是单位阵。对于姿态部分由于我们使用了小角度近似进行注入注入后的新误差状态与注入前的误差状态之间存在一个线性关系其雅可比矩阵通常近似为单位阵因为误差很小或者是一个简单的矩阵。在许多实现中如果误差很小这一步有时被简化甚至忽略直接将更新后的 (\mathbf{P}{k1|k1}) 用于下一轮预测。但这在理论上是需要处理的尤其是当姿态修正量较大时。注意事项GNSS观测噪声R的设定GNSS的观测噪声 (\mathbf{R}) 不是固定值。在开阔天空下水平精度可能达到厘米级RTK高程精度稍差在高楼林立的城市峡谷噪声可能急剧增大甚至出现跳变多路径效应。一个成熟的融合系统需要自适应调整R矩阵。可以根据GNSS接收机输出的定位精度指标如HDOP、VDOP、卫星数、信噪比来动态缩放R。更高级的做法是使用新息检测Innovation Test或卡方检验来探测GNSS异常观测并在异常时增大R降低对该观测的信任度或直接拒绝该次观测。盲目使用固定的小R值在GNSS信号受干扰时一个坏点就足以把整个滤波器拉偏。5. 关键参数辨识过程噪声Q与观测噪声RESKF的性能极度依赖于两个核心的噪声协方差矩阵过程噪声协方差 (\mathbf{Q}) 和观测噪声协方差 (\mathbf{R})。它们分别代表了我们对IMU模型和GNSS观测的信任程度。5.1 过程噪声QIMU的不确定性(\mathbf{Q}) 矩阵来源于IMU测量噪声和零偏驱动噪声。它定义了误差状态在预测过程中会增长多快。(\mathbf{Q}) 通常是一个对角矩阵或块对角矩阵其元素需要根据IMU的 datasheet数据手册和实际测试来标定。角速度随机游走 (Angular Random Walk, ARW)对应陀螺仪白噪声 (\mathbf{n}_g) 的强度。单位通常是 (rad/s/\sqrt{Hz})。这描述了陀螺仪输出的“抖动”程度。加速度计随机游走 (Velocity Random Walk, VRW)对应加速度计白噪声 (\mathbf{n}_a) 的强度。单位通常是 (m/s^2/\sqrt{Hz})。零偏不稳定性 (Bias Instability)对应零偏驱动噪声 (\mathbf{n}{bg}, \mathbf{n}{ba}) 的强度。单位分别是 (rad/s/\sqrt{Hz}) 和 (m/s^2/\sqrt{Hz})。这描述了零偏随时间随机游走的快慢。从这些连续时间噪声谱密度参数可以推导出离散时间的过程噪声协方差矩阵 (\mathbf{Q}_k)。例如对于白噪声其离散化后的方差等于谱密度除以采样时间 (\Delta t)。一个简化的经验公式是 [ \mathbf{Q}_k \text{diag}( \sigma_a^2 \Delta t^2 \mathbf{I}_3, \sigma_a^2 \mathbf{I}3, \sigma_g^2 \Delta t^2 \mathbf{I}3, \sigma{ba}^2 \Delta t \mathbf{I}3, \sigma{bg}^2 \Delta t \mathbf{I}3 ) ] 其中 (\sigma_a, \sigma_g, \sigma{ba}, \sigma{bg}) 分别是加速度噪声、角速度噪声、加速度计零偏噪声、陀螺仪零偏噪声的标准差。这只是一个非常粗略的近似更准确的推导需要参考3.3节。实操心得Q矩阵的调参IMU的 datasheet 参数如ARW, VRW给出了一个基准但实际噪声往往更大。一个实用的标定方法是静止初始化将IMU静止放置一段时间例如1-5分钟采集数据。计算加速度计和陀螺仪输出的方差。这个方差反映了在零输入下IMU输出的波动它包含了白噪声和一部分零偏不稳定性。可以将这个方差作为初始的 (\mathbf{Q}) 中对应噪声项的参考。但要注意静止初始化得到的方差是测量值的方差它和ESKF中过程噪声Q的关系是间接的。Q描述的是状态尤其是误差状态演化的不确定性而测量方差是观测层面的噪声。通常我们会将静止初始化得到的方差乘以一个经验系数如10-100倍作为Q的初始值然后在真实数据上微调。调参的原则是如果滤波器输出过于“平滑”跟不上IMU的动态可能是Q太小了过于信任IMU模型如果输出噪声很大对GNSS观测反应迟钝可能是Q太大了过于不信任IMU模型。5.2 观测噪声RGNSS的可靠性(\mathbf{R}) 矩阵定义了GNSS观测的精度。如前所述它应该是自适应的。水平位置噪声在开阔天空单点定位SPP精度约为米级差分定位DGPS为亚米级实时动态定位RTK为厘米级。你可以设定一个基础值例如 (\sigma_{pos, h} 1.0 m)。高程位置噪声GNSS的高程精度通常比水平精度差1.5-2倍可以设为 (\sigma_{pos, v} 1.5 \sim 2.0 \times \sigma_{pos, h})。速度噪声如果使用多普勒速度其精度较高通常可以设为 (\sigma_{vel} 0.05 \sim 0.2 m/s)。那么对于位置观测(\mathbf{R}p \text{diag}(\sigma{pos,h}^2, \sigma_{pos,h}^2, \sigma_{pos,v}^2))。这个值应该根据GNSS输出的质量指标动态调整。例如可以设计一个函数 [ \sigma_{pos}^2 \sigma_{base}^2 \times (1 \alpha \times \text{HDOP}) \times \max(1, \frac{N_{sat,min}}{N_{sat}}) ] 其中 (\sigma_{base}^2) 是基础方差HDOP是水平精度因子(N_{sat}) 是卫星数(N_{sat,min}) 是最小有效卫星数(\alpha) 是调节系数。5.3 初始状态与协方差P0滤波器的启动也需要初始值初始名义状态 (\mathbf{x}_0)通常由第一帧有效的GNSS观测来初始化位置 (\mathbf{p}_0) 和速度 (\mathbf{v}_0)如果有速度观测。姿态 (\mathbf{q}_0) 的初始化是个挑战。在静止情况下可以通过加速度计测量重力方向来初始化滚转和俯仰角但航向角不可观需要磁力计或初始运动。在运动情况下可能需要借助GNSS速度方向来辅助或者直接设为零等待收敛。零偏初始化为0。初始误差状态协方差 (\mathbf{P}_0)这代表你对初始估计的不确定度。位置和速度的初始协方差可以设得较大例如位置10m速度1m/s反映GNSS初始观测的不确定性。姿态的初始协方差也设得较大例如每个轴10度。零偏的初始协方差可以设得较小因为你初始化为0不确定性不大。(\mathbf{P}_0) 通常设为一个对角矩阵。6. 实现细节、常见问题与进阶话题有了完整的数学模型将其转化为代码并稳定运行还需要处理许多工程细节。6.1 四元数与旋转的数值处理姿态表示使用四元数必须保证其始终是单位四元数。在名义状态积分四元数微分方程后需要对四元数进行归一化(\mathbf{q} \leftarrow \mathbf{q} / |\mathbf{q}|)。在误差状态注入时构造扰动四元数 (\delta \mathbf{q} [1, 0.5\delta \boldsymbol{\theta}^T]^T) 后与名义四元数做乘法结果也需要归一化。在计算旋转矩阵 (\mathbf{R}(\mathbf{q})) 或进行坐标变换时使用数值稳定的公式。6.2 误差状态重置的雅可比矩阵G在4.3节提到误差状态注入后其协方差矩阵需要更新(\mathbf{P} \leftarrow \mathbf{G} \mathbf{P} \mathbf{G}^T)。矩阵 (\mathbf{G}) 是注入函数对误差状态的雅可比矩阵。由于注入后误差状态被重置为0新的误差状态与旧的误差状态之间的关系是 对于加性状态p, v, b新误差 旧误差 - 估计出的误差均值。因此雅可比是单位阵的负值但因为我们关心的是协方差二阶矩负号在乘以其转置后会消失所以这部分对应的 (\mathbf{G}) 块是单位阵。 对于乘性状态姿态q新误差对应的扰动向量 (\delta \boldsymbol{\theta}{new}) 与旧误差 (\delta \boldsymbol{\theta}{old}) 和注入的估计值 (\delta \boldsymbol{\theta}{est}) 之间的关系更复杂。在小误差假设下近似有 (\delta \boldsymbol{\theta}{new} \approx \delta \boldsymbol{\theta}{old} - \delta \boldsymbol{\theta}{est})。因此姿态误差部分的 (\mathbf{G}) 块也可以近似为单位阵。 因此在许多实际实现中为了简化直接令 (\mathbf{G} \mathbf{I})。这意味着我们假设注入操作没有改变误差状态之间的线性关系。这在误差很小的情况下是合理的。但如果某次更新的修正量较大例如初始化时或GNSS失锁后重捕获这种近似会引入误差。更严谨的做法是推导精确的 (\mathbf{G}) 矩阵它通常是一个接近单位阵的矩阵其非对角线元素很小。6.3 异步传感器融合与时间对齐IMU频率100-500Hz远高于GNSS频率1-10Hz。ESKF的预测步IMU和更新步GNSS是异步的。标准做法是维护一个状态缓冲区。每当收到一个IMU数据就执行一次预测步更新名义状态和误差状态协方差。当收到一个GNSS数据时找到该GNSS数据时间戳之前最新的名义状态和误差状态协方差它们已经由IMU预测到了那个时刻。执行更新步得到修正后的误差状态注入并重置。继续处理后续的IMU数据。这里的关键是时间对齐。GNSS观测通常带有一个精确的时间戳。你需要确保IMU积分时使用的时间间隔 (\Delta t) 是准确的并且将状态预测到与GNSS观测完全一致的时间点上。微小的时序错误会导致明显的融合误差。6.4 应对GNSS失效与退化GNSS信号会中断隧道、地下车库或退化城市峡谷。在ESKF中如果长时间没有GNSS更新误差状态的协方差 (\mathbf{P}) 会随着预测步不断增大因为过程噪声 (\mathbf{Q}) 不断注入不确定性。此时滤波器相当于一个纯惯性导航系统INS其位置误差会立方级增长。失效检测可以通过判断卫星数、信噪比、DOP值或者使用新息检验计算 (\mathbf{y}^T (\mathbf{H}\mathbf{P}\mathbf{H}^T\mathbf{R})^{-1} \mathbf{y})它应服从卡方分布来检测GNSS观测是否异常。一旦检测到异常可以丢弃该次观测或者大幅增大其 (\mathbf{R}) 矩阵降低权重。零速修正ZUPT在车载或足式机器人中当检测到载体静止时通过IMU或轮速计可以将速度观测强制设为0并给予一个很小的观测噪声。这能有效抑制静止时的漂移。非完整约束NHC对于地面车辆假设其侧向和竖向速度为零在车身坐标系下。这可以作为一个虚拟的观测来约束速度误差尤其在GNSS失效时非常有用。6.5 ESKF与预积分、图优化的关系ESKF是一种基于滤波的、递推的融合方法。近年来基于图优化Graph Optimization的融合方法如VIO、LIO-SLAM也越来越流行。在这些方法中IMU预积分IMU Pre-integration技术被广泛使用。思想对比ESKF在每一个IMU时刻都进行状态预测并用最新的观测立即修正。图优化则是将一段时间内的所有IMU测量预积分成一个相对运动约束与多个时刻的观测如GNSS位置、视觉特征点一起构建一个全局优化问题一次性优化所有状态。图优化通常更精确尤其适合处理回环但计算量更大延迟更高。预积分与ESKF预积分的概念也可以被吸收到ESKF框架中。我们可以在两个GNSS观测之间对IMU数据进行预积分得到一个相对位置、速度、姿态的变化量。然后在ESKF的预测步中直接使用这个预积分量来更新名义状态并推导对应的误差状态转移矩阵和过程噪声。这种方法比传统的逐点积分更高效且对零偏的变化更鲁棒一些是连接滤波与优化思想的一座桥梁。IMU静止初始化得到的测量方差反映了传感器本身的噪声水平。而ESKF中的过程噪声Q描述的是状态误差演化的不确定性。前者是后者的重要输入和参考但Q还需要考虑模型误差如非重力加速度未被补偿、刻度因子误差等因此通常比单纯的测量方差要大。理解这两者的区别和联系对于正确标定滤波器至关重要。