企业官网建设流程全解析

做惯性导航相关方案有一阵子了，每次调试完一轮数据回来，我都会重新翻一遍误差状态卡尔曼滤波（ESKF）的笔记。这个名字起得很精准：真正被“滤”的并不是位置、速度和姿态本身，而是它们相对某条理想参考轨迹的偏差。最近我把这套理论重新整理了一遍，趁思路还清楚，先把主干部分写出来。这篇定位是初步整理，后续会随着实测结果和新的问题持续更新，所以有些细节可能还不够完整，但基本框架和理解路径是稳定的。

如果你正在做基于IMU的定位、机器人里程计或者组合导航，又不想一上来就啃大段公式，这篇应该能帮你少走点弯路。我不会把所有推导都抄一遍，重点放在“为什么工程上要这么设计”以及“自己动手实现时最容易卡住的地方”。

1. 为什么惯性导航项目里，ESKF几乎成了默认选择

1.1 标准KF直接滤全状态会遇到的麻烦

一开始做惯性导航的人，最容易想到的思路是把位置、速度、姿态、陀螺仪零偏、加速度计零偏全部塞进一个状态向量，然后直接用卡尔曼滤波去估计。这个思路本身没问题，但真正落地的时候会发现相当难受：姿态是一个四元数或者旋转矩阵，它不是欧几里得空间里的普通向量，你不能随随便便对它做加法。四元数加一个增量之后大概率不再归一化，旋转矩阵加一个增量之后也不再是旋转矩阵。这些操作都需要额外的归一化甚至重新正交化，而卡尔曼滤波的协方差更新根本不会理会这些约束。

还有一个更本质的问题：系统方程的非线性程度远比想象中高，尤其是姿态变化剧烈的时候。如果直接对全状态做线性化，工作点离真实状态可能已经偏离出了线性化有效的范围。EKF在这种场景下勉强能用，但协方差矩阵很容易变得不健康，甚至出现负定。

1.2 ESKF的基本思路：真状态等于标称状态加上误差状态

ESKF的思路很巧：把状态拆成两份。标称状态（nominal state）完全由IMU测量递推得到，它是一条“看起来正确”的轨迹，不参与卡尔曼滤波。剩下的误差状态（error state）才真正进入卡尔曼滤波，它描述的是真状态与标称状态之间的偏差。真状态等于标称状态与误差状态按某种方式叠加。

对于位置和速度，这个叠加就是普通的加法；对于姿态，误差状态用一个三维的小角度扰动来表示，叠加的方式是四元数乘法。也就是说误差状态里的姿态部分是一个小量，小到可以用线性化处理。

这样做的好处非常明显：误差状态的噪声特性更接近高斯分布，线性化也更为合理。因为误差状态的幅值本来就小，线性化不会偏离太远。标称轨迹负责把大尺度、强非线性的运动全部消化掉，卡尔曼滤波只需要盯着这个“小偏差”做修正。整体鲁棒性远好于直接对全状态滤波。

1.3 误差状态小到什么程度，公式才能简化

这里有个容易被忽略的前提：误差状态必须是真的“小”。姿态误差通常要求小于十几度，这样三角函数的近似和小角度旋转的假设才成立。如果你系统的初始姿态偏差有三十度以上，ESKF的线性化基础就开始动摇，协方差更新也不再可靠。

因此，工程上很少有人会让ESKF在姿态完全未知的情况下冷启动。一般会有加速度计水平对齐、磁力计或视觉尺度估计来给出一个还不错的初始姿态。剩下的小误差交给ESKF慢慢收敛。这个思路贯穿整个算法，后面调参时也会不断遇到。

2. 动笔前必须定清楚的几件事：坐标系、姿态参数化、误差定义

2.1 坐标系约定影响的是整个推导和代码的符号

惯性导航里最常见的坐标约定有两套：NED（北东地）和ENU（东北天）。我自己的实现里习惯用ENU，也就是东北天坐标，IMU所在的body坐标系定义为前右下或前左上，代码里要明确注释清楚。坐标轴方向一旦搞反，整个推导看哪哪不对，最后往往只能靠试错。

特别要注意的一点是重力向量在世界坐标系里的表示。ENU下重力是负的Z方向，NED下是正的Z方向。这个符号错一个，位置递推会在几秒内完全爆炸。我建议在代码里把坐标系定义写成全局常量，并且在每一个IMU测量进入递推函数之前做一次维度检查，而不是让这个隐患散落在各个模块里。

2.2 姿态参数化：标称用四元数，误差状态用三维小向量

ESKF里最常见的搭配是：标称姿态用四元数，误差姿态用三维向量表示。四元数负责无奇异地表达完整姿态，三维小向量负责线性化。两者通过指数映射联系起来。

约定好误差的定义方式很重要。我习惯用右乘的形式定义：

真四元数 = 标称四元数 ⊗ 误差四元数

于是可以认为：

误差四元数 ≈ [1, 0.5 * δθ]ᵀ

其中δθ就是那个三维小扰动向量。如果你采用左乘定义，后面推导的F矩阵符号会整体反转。参考文献里左右乘都有，没有统一标准，自己实现时只要全程一致即可。最容易踩坑的是调试阶段混合了不同论文里的公式，最后姿态误差的符号对不上。

2.3 状态向量的组织方式

一个典型的15维ESKF状态向量是这样组织的：

δx = [δp, δv, δθ, δb_g, δb_a]ᵀ

其中：

• δp：位置误差，单位米
• δv：速度误差，单位米/秒
• δθ：姿态误差，单位弧度
• δb_g：陀螺仪零偏误差，单位弧度/秒
• δb_a：加速度计零偏误差，单位米/秒²

有些系统还会把重力向量或接收机时钟误差扩进来，变成18维或者21维，但基本骨架都一样。最开始实现的时候建议先跑通15维，再去考虑扩展状态。状态越多，协方差矩阵越大，可观测性也越差，不是越大越好。

3. 标称递推与误差协方差传播的分工

3.1 标称状态是怎么被IMU往前推的

标称状态的递推就是纯粹的IMU积分。假设已经读到陀螺仪测量值ω_m和加速度计测量值a_m，先扣除当前估计的零偏：

ω = ω_m - b_g a = a_m - b_a

然后用以下方式更新标称姿态、速度和位置：

q ← q ⊗ Exp(ω * Δt) v ← v + (R(a + g_world_?) 反?? 实际上体重力项不需要再加，因为我们用的加速度计含有比力表示。要小心写法：如果a是世界系下的比力加上重力？不同论文写法规格差异很大。

为了避免歧义，这里给出我习惯的写法。令R是世界系到机体系的旋转矩阵，加速度计输出的是比力。那么机体系加速度在世界系的表达为R * a，再加上重力向量g_world。于是：

v ← v + (R(a) + g_world) * Δt p ← p + v * Δt + 0.5 * (R(a) + g_world) * Δt²

这里的关键坑在于，有的论文把重力直接整理进加速度向量，有的则单独保留，代码移植时交叉对照会非常痛苦。你只需要记住一个原则：IMU的加速度计测不到重力，所以机体相对于世界的真实加速度是比力在世界的投影加上重力。写代码时只要沿着这个定义，符号就不会乱。

3.2 误差状态为什么能用一段线性ODE表达

标称状态把非线性的大幅运动消化干净之后，剩下的误差状态满足的是线性微分方程。这个线性化过程完全可以手推一遍：

姿态误差的微分方程是：

δθ̇ = -[ω]× δθ - δb_g - n_g

速度误差的微分方程是：

δv̇ = -R [a]× δθ - R δb_a - R n_a

位置误差就是：

δṗ = δv

零偏的随机游走：

δḃ_g = n_bg δḃ_a = n_ba

这里[a]×表示向量a的反对称矩阵。这个推导的物理含义很直观：陀螺仪零偏会直接导致姿态积分持续飘移，姿态误差又通过旋转矩阵影响速度误差，加速度计零偏直接污染速度，位置误差只是速度误差的积分。

3.3 离散化与协方差传播

有了连续时间的线性微分方程，接下来就是离散化。一般不会去做高精度的积分，直接按一阶近似就够了，因为误差状态本身是小量，即使F矩阵差一点，卡尔曼增益也能通过协方差做一定程度的补偿。

离散化之后得到状态转移矩阵F和噪声传递矩阵G，然后做协方差预测：

P ← F P Fᵀ + G Q Gᵀ

这个步骤对代码性能的影响很大，尤其在更新频率很高的嵌入式平台上。常用的优化是把F分成一个一个的小块，直接手写乘法和加法，而不是调用通用矩阵库。工程上对F矩阵的计算损耗差异能到好几倍。

为了便于看懂，我这里给一个简化版的状态转移写法，实际实现时你还需要补上每个小分块对应的时变参数：

δp_{k+1} = δp_k + δv_k * Δt δv_{k+1} = δv_k + (-R [a]× δθ_k - R δb_a) * Δt δθ_{k+1} = R_δθ δθ_k - δb_g * Δt 其中R_δθ是一个与旋转增量有关的近似矩阵，在姿态快速变化时尤其重要。不能简单地把δθ当作三通道独立积分，旋转向量之间的耦合是真实存在的。

4. 观测更新与误差注入：为什么卡尔曼增益算在δx上反而更线性

4.1 观测量怎么映射进误差状态

ESKF的观测更新并不直接更新位置或姿态，而是先更新误差状态。假设外部观测给了一个位置测量y_pos，而当前的标称位置为p_nom，那么相对于标称轨迹的预测位置就是p_nom + δp。观测残差就是：

r = y_pos - p_nom - δp

因为δp很小，这个式子对δp而言是严格线性的。H矩阵取对应的选择矩阵即可，比如位置观测对应的H块就是单位阵。如果观测的是速度或者姿态，H矩阵会多一个R矩阵，但思路完全一样。

对于姿态观测，把误差状态里面的δθ转成一个小的四元数扰动，再按标称姿态组合出预测姿态，即可计算四元数残差，再映射回三维旋转向量。这部分实现细节较多，但每个细节都有清晰的定义，按部就班写就行。

4.2 标准卡尔曼更新循环的写法

更新过程与普通卡尔曼滤波没有任何区别：

S = H P Hᵀ + R K = P Hᵀ S⁻¹ δx = K r P = P - K H P

这里唯一值得注意的是，P更新用的是误差状态的雅可比，所以计算时维度是误差状态维度，而不是全状态维度。这意味着协方差矩阵可以更小，更新更快，数值上也更稳定。

更新完之后，需要把误差状态“注入”回标称状态：

p_nom ← p_nom + δp v_nom ← v_nom + δv q_nom ← q_nom ⊗ Exp(δθ) b_g ← b_g + δb_g b_a ← b_a + δb_a

注入完成之后，把误差状态清零，P要保留下来，不能清零，否则协方差信息就丢失了。这一步很多初学的同学容易写错：P传播阶段用到了上一时刻的误差协方差，注入状态清零时如果顺带把P也清零了，整个滤波器就再也没有修正能力。

4.3 协方差更新是用标准形式还是对称形式

代码中我建议用约瑟夫形式：

P ← (I - K H) P (I - K H)ᵀ + K R Kᵀ

虽然计算量稍微大一点，但数值对称性保持得更好。直接使用P ← P - KHP简洁，不等式会在大量迭代之后慢慢丢对称性，最终出现负特征值。保持浮点运算下的对称性在长时间运行场景中非常重要，尤其是嵌入式单精度运算时。

5. 真正花时间的工程细节：初值、单位、收敛与静止修正

5.1 初始协方差矩阵怎么设才不会很快发散

初始协方差P₀要反映你对初始状态的不确定度。位置和速度的初始不确定度可以选择比较小，因为一般启动时会有一个已知起点。姿态的初始不确定度则要谨慎：加速度计可以对齐横滚和俯仰，但对于偏航角，普通IMU本身完全没有参考，只能依赖外部输入。

我的实践习惯是，偏航的不确定度一开始给到5到10度，对应弧度大约0.087到0.175，平方之后填入协方差矩阵的对应块。如果你给得太小，滤波器会非常自信，外部观测想把它拉回来就需要很长时间。这里的关键是让观测更新有足够的“权限”去修正偏航，而不是让先验协方差把观测信号压死。

陀螺仪和加速度计的零偏初值通常给0，协方差却要给到物理上限，因为MEMS器件的零偏变化通常比预想的大。给一个保守的初始不确定度，滤波器会在前几秒内自动估计出一个合理的零偏初值。

5.2 单位问题会在半小时内毁掉一套系统

我见过太多案例，最后查来查去发现角度单位写错了。陀螺仪输出是度每秒，代码里却当成弧度每秒用；重力加速度写成9.8，但单位是m/s²还是cm/s²忘了换算。ESKF一旦混入单位错误，协方差矩阵里的量纲会迅速失衡，各状态之间的数值量级差异可以达到10的十几倍，数值求解直接崩坏。

我的做法是每个量和单位写在注释里，并且在输入接口处做统一换算，所有内部计算严格使用国际单位制。弧度、米、秒、米每秒、米每二次方秒，全部统一。这样排查问题时只要检查接口层，不必去翻主循环。

5.3 零速修正为什么是工程上的救命稻草

纯惯性导航的误差会随时间不断积累，哪怕传感器质量再好，位置漂移也只是快慢问题。在没有卫星或者GPS信号被遮挡的场景下，零速修正（ZUPT）是最简单也最有效的修正手段。

原理极简：当系统检测到载体处于静止状态时，真实速度应为零，于是把“速度为0”作为虚拟观测量放进卡尔曼滤波。这个观测无论是形式还是实现都比外部传感器简单得多，而且能够同时激励零偏估计的收敛。因为速度误差、姿态误差、零偏之间的耦合关系密切，静止时用零速度约束狠狠抑制漂移，零偏就会被拉出来。

静止检测本身也是一个不小的工程问题，要看加速度方差、角速度方差、时长条件，还要防止把匀速直线运动误判成静止。阈值太紧会错过修正机会，阈值太松会在运动时误触发，导致位置被错误地往零速度拉。我个人经验是，先看角速度幅值，再看加速度方差，用一阶滞后滤波平滑后再判断，误触发会少很多。

5.4 代码层面的几个小建议

建议把预测和更新的代码拆成独立的函数，输入输出都定义为结构体，方便单步调试。ESKF这种算法，错误的征兆往往要很久以后才爆发，不拆开根本找不到根因。以下是一个粗略的伪代码结构，实际工程里按这个骨架去填充即可。

def predict(nominal, error_cov, imu, dt): # 1. 扣除零偏 # 2. 更新标称姿态、速度、位置 # 3. 计算误差状态离散转移矩阵 F # 4. 更新误差协方差 P return nominal, error_cov def update(nominal, error_state, error_cov, z, H, R): # 1. 计算残差 # 2. 计算增益 K # 3. 误差状态修正 # 4. 误差注入标称状态 # 5. 清零误差状态 return nominal, error_state, error_cov

5.5 常见收敛异常与对应的排查方向

我把实际调试中遇到最多的几个异常列成了一张表，后续继续踩坑会不断往里补充。

6. 这份初稿还没展开的部分，以及我接下来的整理计划

6.1 松耦合与紧耦合的观测雅可比

目前把观测更新写得比较笼统，实际组合导航里，外部观测可能是卫星接收机给出的位置，也可能是视觉里程计给出的位姿，还可能是激光里程计的话题。不同观测源对应的H矩阵完全不同，特别是紧耦合场景，原始测量直接进因子图或者滤波方程，需要计算的雅可比会更复杂。这个部分预计会在下一篇重点展开。

6.2 迭代误差状态卡尔曼滤波（IESKF）

当观测方程本身非线性很强时，比如视觉重投影误差，一次性线性化效果不好。IESKF的做法是：更新完误差状态并注入标称状态之后，利用新的标称状态再做一次线性化和滤波更新，迭代若干次。这个方法在视觉惯性导航里几乎是标配，因为重投影误差对姿态的依赖性很强，一次线性化还不够。实现上比普通ESKF多一层循环，但公式结构没有本质变化。

6.3 与预积分类方法的对比

IMU预积分与ESKF解决的问题很像，都是处理小误差的递推与修正，但数学框架不完全一样。预积分在因子图里更自然，因为可以跨关键帧边缘化；ESKF则在实时递归滤波里更方便。两者各有适用场景，后续我会整理一组对比笔记。

6.4 传感器噪声特性的实验标定

理论里的白噪声和随机游走参数，在现实里其实很难精确测量。同一款型号在不同的温湿度环境下表现差异很大，甚至同一个传感器在不同时速移动状态下的噪声谱都不同。要真正落地，必须做一段静态数据采集，然后对Allan方差曲线做拟合，而不是直接用厂商手册给的参数。这个坑后续我会单独写详细标定流程。

这篇先写到这里。ESKF的内容远比初看起来要庞大，光是把预测部分吃透就需要不少时间。等我把组合导航观测、迭代更新和零速修正的实测数据都整理完，会继续在后续篇幅里补上。如果你也在对照公式写代码，建议先用仿真数据验证每一步，再上真实设备，经验会告诉你哪个环节最容易出偏差。