4. 松组合核心算法：卡尔曼滤波器设计、状态方程与量测方程、开环与闭环校正
好，咱们进入松组合最核心的部分了。说实话，搞组合导航这么多年，卡尔曼滤波器这块我踩过的坑最多。你想想看，一个滤波器设计得好不好，直接决定了你的组合导航系统能不能用。今天我就把我在项目里摸爬滚打的经验，掰开了揉碎了讲给你听。
4.1 卡尔曼滤波器设计——到底要估什么？
卡尔曼滤波器的本质，说白了就是一个“数据融合器”。它把GPS和惯导的数据揉在一起，取长补短。但设计之前，你得先想清楚一个问题：我要估计什么？
我个人习惯把状态量分成三类：
- 导航状态：位置、速度、姿态（这是必须的）
- 惯性器件误差：陀螺零偏、加速度计零偏（这些不估，系统迟早漂飞）
- 辅助传感器误差：比如GPS的钟差、钟漂（看情况加）
核心原则：状态量不是越多越好。我在项目中见过有人把20多个状态全塞进去，结果滤波器发散得一塌糊涂。记住，可观测性才是王道。
4.2 状态方程——系统怎么动？
状态方程描述的是系统自身的演化规律。对于松组合来说，我们通常用惯导的误差方程作为状态方程。嗯，这里要注意，我们估计的是误差，不是绝对量。
一个典型的15维状态向量长这样：
X = [δp, δv, φ, ε, ∇]^T
其中：
δp = [δL, δλ, δh] —— 位置误差（纬度、经度、高度）
δv = [δv_e, δv_n, δv_u] —— 速度误差（东、北、天）
φ = [φ_e, φ_n, φ_u] —— 姿态误差角（失准角）
ε = [ε_x, ε_y, ε_z] —— 陀螺零偏
∇ = [∇_x, ∇_y, ∇_z] —— 加速度计零偏状态方程写成矩阵形式：
X_k = Φ_{k,k-1} · X_{k-1} + W_{k-1}这里的Φ是状态转移矩阵，W是系统噪声。我在实际项目中，最头疼的就是这个Φ矩阵的离散化。你想想看，连续系统到离散系统，一步算错，后面全完蛋。
我的经验：状态转移矩阵的计算，建议用泰勒展开到二阶项。一阶近似在低动态场景下还行，但如果你做的是无人机或者车载导航，二阶项必须加上。我曾经因为这个吃过亏，后来就学乖了。
4.3 量测方程——观测怎么来？
量测方程描述的是观测值和状态量之间的关系。松组合的量测，就是GPS和惯导输出的差值。
具体来说：
Z = H · X + V
其中：
Z = [p_GPS - p_INS, v_GPS - v_INS]^T
H 是量测矩阵
V 是量测噪声量测矩阵H的设计，其实挺有讲究的。我见过不少新手直接把H设成单位阵，这不对。你得根据你的状态定义，把位置和速度的对应关系写清楚。
| 量测类型 | H矩阵对应行 | 说明 |
|---|---|---|
| 位置量测 | H(1:3, 1:3) = I | 直接对应位置误差状态 |
| 速度量测 | H(4:6, 4:6) = I | 直接对应速度误差状态 |
| 其他状态 | H(1:6, 7:15) = 0 | 不可直接观测 |
注意：GPS和惯导的时间同步问题。我踩过这个坑——GPS数据有100ms的延迟，惯导数据是实时的，两者直接相减，量测值全是错的。解决办法：要么做时间戳对齐，要么在量测方程里加一个时间延迟状态。
4.4 开环校正 vs 闭环校正——两种哲学
这是松组合设计里一个绕不开的选择题。两种方式我都用过，各有各的适用场景。
开环校正
开环校正，说白了就是只估计，不反馈。卡尔曼滤波器算出来的误差，只用来修正输出，但不反馈给惯导解算模块。
- 优点：滤波器设计简单，稳定性好，不容易发散
- 缺点：惯导误差会一直累积，滤波器状态量会越来越大
- 适用场景：短时间导航、高精度惯导（误差本身很小）
闭环校正
闭环校正则是既估计，又反馈。滤波器算出的误差，直接反馈到惯导解算模块，把惯导的误差“清零”。
- 优点：惯导始终保持在低误差状态，滤波器状态量不会发散
- 缺点：反馈回路可能引入震荡，设计不好会炸
- 适用场景：长时间导航、低精度惯导（MEMS）
我的建议：如果你用的是光纤陀螺或者激光陀螺这种高精度惯导，开环校正完全够用。但如果你做的是消费级MEMS，老老实实用闭环。我曾经在一个车载项目里试过开环，结果跑了10分钟，位置误差就飘到几百米了。
4.5 核心逻辑流程图
下面这张图，是我自己总结的松组合卡尔曼滤波器核心流程。你仔细看看，每一步都有讲究。
4.6 实际项目中的避坑指南
最后，我把自己这些年踩过的坑总结一下，你写代码的时候多留个心眼：
- 初始化要谨慎：P矩阵（协方差矩阵）的初值别乱设。设太大，滤波器收敛慢；设太小，滤波器可能永远收敛不到真值。我一般取GPS精度的平方作为位置协方差初值。
- Q矩阵和R矩阵要调参：系统噪声Q和量测噪声R，这两个矩阵是滤波器的“灵魂”。别指望一次调好，我通常会在仿真环境里跑几十组参数，选效果最好的。
- 注意野值：GPS偶尔会跳点，这时候量测值Z会突然变得很大。如果不做野值检测，滤波器瞬间就发散了。我的做法是：如果|Z| > 3σ，直接丢弃这次量测更新。
- 开环闭环切换要平滑：有些系统需要开环和闭环切换使用。切换的时候，状态量和协方差矩阵要处理好，不然会出现跳变。我曾经因为这个，在试飞的时候差点把飞机搞失控了。
一个小技巧：调试卡尔曼滤波器的时候，把新息序列（innovation sequence）打印出来看看。如果新息是零均值白噪声，说明滤波器设计对了。如果新息有明显的趋势项，那你的模型肯定有问题。
好了，松组合的核心算法就讲到这里。这些内容都是我一个个项目、一次次调试换来的经验。你回去写代码的时候，多想想我今天说的这些点，能少走不少弯路。
公众号:蓝海资料掘金营，微信deep3321