第1章：状态空间模型——组合导航的数学骨架
做组合导航这些年，我最大的体会是：状态空间模型就是整个卡尔曼滤波的骨架。骨架搭歪了，后面再怎么调参都是白搭。今天咱们就把这个骨架彻底聊透。
1.1 连续时间系统状态方程
先说说连续时间系统。说白了，就是用微分方程描述一个系统的演化规律。
我习惯这样写：
ẋ(t) = F(t)·x(t) + G(t)·u(t) + L(t)·w(t)这里每个符号都有它的脾气：
- x(t) —— 状态向量，比如位置、速度、姿态角
- F(t) —— 状态转移矩阵，描述状态自身的演化规律
- u(t) —— 控制输入，比如IMU测到的加速度和角速度
- G(t) —— 控制输入矩阵，把控制量映射到状态空间
- w(t) —— 系统噪声，代表模型的不确定性
- L(t) —— 噪声驱动矩阵
核心理解：连续时间模型描述的是「真实物理世界」的演化。IMU的输出是连续的，地球自转也是连续的，所以用微分方程最自然。
我在项目中遇到过一个问题：有人直接把离散的IMU数据代入连续模型，结果发散得一塌糊涂。嗯，这里要注意——连续模型是理论基石，但实际实现必须离散化。
1.2 离散时间系统状态方程
计算机只能处理离散数据。所以我们要把连续方程变成离散形式：
xₖ = Φₖ₋₁·xₖ₋₁ + Bₖ₋₁·uₖ₋₁ + wₖ₋₁这里：
- Φₖ₋₁ —— 离散状态转移矩阵，由F(t)通过矩阵指数计算得到
- Bₖ₋₁ —— 离散控制输入矩阵
- wₖ₋₁ —— 离散系统噪声，协方差矩阵为Qₖ₋₁
离散化最常用的方法是一阶泰勒展开：
Φ ≈ I + F·Δt但说实话，如果Δt比较大（比如IMU输出频率只有10Hz），一阶近似就不够用了。我建议用矩阵指数函数精确计算：
Φ = expm(F·Δt)我的经验：在MATLAB里用expm()，在C++里用Eigen的unsupported模块。千万别手写矩阵指数，容易出bug。
1.3 观测方程
观测方程描述的是「传感器看到了什么」。比如GPS给出了位置，那观测方程就是：
zₖ = Hₖ·xₖ + vₖ- zₖ —— 观测向量，比如GPS的经纬高
- Hₖ —— 观测矩阵，把状态空间映射到观测空间
- vₖ —— 观测噪声，协方差矩阵为Rₖ
举个例子，如果你用GPS观测位置，状态向量是[位置, 速度, 姿态]，那H矩阵就是：
H = [I₃ₓ₃, 0₃ₓ₃, 0₃ₓ₃]为什么？因为GPS只观测位置，速度和姿态它看不到。
避坑指南：我曾经在融合GPS和IMU时，忘了考虑GPS的杆臂效应——GPS天线和IMU中心不重合。这个偏差如果不建模到观测方程里，定位精度会差好几米。
1.4 系统噪声与观测噪声建模
噪声建模是卡尔曼滤波里最「玄学」的部分。但说白了，就是回答两个问题：
- 系统噪声Q：模型有多不准？
- 观测噪声R：传感器有多不准？
系统噪声Q的建模
Q矩阵描述的是过程噪声。对于IMU来说，主要来源是：
- 加速度计的白噪声和零偏不稳定性
- 陀螺仪的白噪声和零偏不稳定性
- 随机游走
我一般这样建模：
Q = diag([σ_acc², σ_gyro², σ_bias_acc², σ_bias_gyro²])这些σ值怎么定？我的习惯是看IMU的数据手册，然后乘以一个经验系数（1.5~3倍）。为什么？因为数据手册给的是理想值，实际环境要恶劣得多。
观测噪声R的建模
R矩阵相对简单。对于GPS：
- 单点定位：σ ≈ 3~5米
- RTK：σ ≈ 0.02~0.05米
- 差分GPS：σ ≈ 0.5~1米
| 传感器类型 | 典型R值 | 备注 |
|---|---|---|
| GPS单点 | diag([9, 9, 15]) | 水平3m，高程稍差 |
| RTK | diag([0.04, 0.04, 0.09]) | 厘米级精度 |
| 里程计 | 0.1~0.5 m/s | 取决于轮速计质量 |
调参心得：Q和R的比值决定了滤波器的「信任倾向」。Q/R越大，滤波器越相信观测；反之越相信模型。我刚开始做的时候，总想把Q和R调得特别准，后来发现——保持Q/R比值在10~100之间，效果往往不错。
1.5 知识体系总览
下面这张图是我自己梳理的状态空间模型知识框架，你品品：
1.6 实战中的几个坑
最后分享几个我踩过的坑：
- 维度不匹配：状态向量和观测向量的维度一定要对得上。我见过有人把15维状态和3维观测硬凑，结果H矩阵写错了都不知道。
- 单位制混乱：IMU输出的是弧度，GPS输出的是度，不统一单位直接算——发散是必然的。
- 忘记初始化：状态协方差矩阵P₀的初始值很重要。设太小了滤波器收敛慢，设太大了前期震荡剧烈。
一个小技巧：刚开始调参时，先把Q和R都设成对角矩阵。等系统跑通了，再慢慢加非对角项。这样排查问题会快很多。
好了，状态空间模型就聊到这儿。记住一句话：模型决定上限，调参决定下限。把模型搭扎实了，后面的事就顺了。
公众号:蓝海资料掘金营，微信deep3321