4. 卡尔曼滤波基础：状态空间模型、线性卡尔曼滤波、扩展卡尔曼滤波（EKF）、无迹卡尔曼滤波（UKF）
各位同学，今天我们聊点硬核的——卡尔曼滤波。说实话，我在做组合导航之前，觉得卡尔曼滤波就是个数学公式堆砌的东西。直到第一次在RTK+INS紧耦合项目里调参数调到头秃，才真正理解它的精髓。
卡尔曼滤波说白了，就是一套“预测+修正”的递归算法。你想想看，我们做定位的时候，GPS有噪声，IMU有漂移，怎么把这两坨不完美的数据揉在一起？卡尔曼滤波就是干这个的。
4.1 状态空间模型
先讲状态空间模型。这是卡尔曼滤波的“骨架”。
我个人习惯把状态空间模型拆成两个方程：
- 状态方程：描述系统怎么演化。比如IMU的角速度积分出姿态，加速度积分出速度。
- 观测方程：描述传感器怎么测量。比如GPS给出位置，但位置是状态的一部分。
数学上长这样：
x_k = F * x_{k-1} + B * u_k + w_k
z_k = H * x_k + v_k这里x是状态向量，F是状态转移矩阵，B是控制输入矩阵，u是控制量，w是过程噪声。z是观测向量，H是观测矩阵，v是观测噪声。
重要：w和v都是高斯白噪声，均值0，协方差矩阵分别是Q和R。这两个矩阵是调参的关键，我后面会讲。
我在项目中遇到过一个问题：状态向量选什么？很多人一股脑把位置、速度、姿态、陀螺零偏、加速度计零偏全塞进去。结果状态维度太高，计算量爆炸，而且有些状态根本不可观。我的建议是：只选你需要的。比如紧耦合里，状态向量通常包括位置误差、速度误差、姿态误差、IMU零偏，就够了。
4.2 线性卡尔曼滤波
线性卡尔曼滤波是基础。它假设系统是线性的，噪声是高斯白噪声。
算法分两步走：
- 预测：用状态方程预测当前状态和协方差。
- 更新：用观测值修正预测结果。
代码实现很简单：
// 预测
x_pred = F * x_prev;
P_pred = F * P_prev * F' + Q;
// 更新
K = P_pred * H' * inv(H * P_pred * H' + R);
x_est = x_pred + K * (z - H * x_pred);
P_est = (I - K * H) * P_pred;这里K是卡尔曼增益。说白了，K决定了你更相信预测还是更相信观测。如果观测噪声R小，K就大，更相信观测。反之亦然。
小技巧：调参的时候，我习惯先固定Q，然后调R。R调好了再微调Q。千万别两个一起调，否则你会疯的。
线性卡尔曼滤波的局限性很明显——现实世界哪有那么多线性系统？IMU的旋转是非线性的，GPS的观测模型也是非线性的。所以我们需要扩展卡尔曼滤波。
4.3 扩展卡尔曼滤波（EKF）
EKF的核心思想：线性化。把非线性函数在估计点附近做一阶泰勒展开，然后套用线性卡尔曼滤波的框架。
数学上：
x_k = f(x_{k-1}, u_k) + w_k
z_k = h(x_k) + v_k这里f和h都是非线性函数。我们需要计算它们的雅可比矩阵：
F_k = ∂f/∂x | x_{k-1}
H_k = ∂h/∂x | x_k_pred然后代入线性卡尔曼滤波的公式。
我曾经在EKF上踩过一个坑：雅可比矩阵算错了。当时用的是四元数表示姿态，雅可比矩阵推导了整整两天，结果代码里一个符号写反了，导致滤波发散。嗯，这里要注意：雅可比矩阵一定要仔细验证。我建议用数值微分先验证一下解析结果。
警告：EKF的线性化误差在高非线性系统中会很大。比如大角度机动时，一阶近似可能不够。这时候可以考虑迭代EKF（IEKF）或者直接上UKF。
4.4 无迹卡尔曼滤波（UKF）
UKF的思路和EKF完全不同。它不线性化，而是用一组采样点（sigma点）来近似概率分布。
具体步骤：
- 根据当前状态和协方差，生成2n+1个sigma点（n是状态维度）。
- 把每个sigma点通过非线性函数f传播，得到预测的sigma点。
- 从预测的sigma点中恢复预测均值和协方差。
- 同样的方法处理观测更新。
代码实现比EKF复杂一些：
// 生成sigma点
lambda = alpha^2 * (n + kappa) - n;
sigma_points = [x, x + sqrt((n+lambda)*P), x - sqrt((n+lambda)*P)];
// 传播sigma点
sigma_pred = f(sigma_points);
// 恢复均值和协方差
x_pred = sum(W_m * sigma_pred);
P_pred = sum(W_c * (sigma_pred - x_pred) * (sigma_pred - x_pred)') + Q;UKF的好处是：不需要计算雅可比矩阵。对于强非线性系统，UKF的精度通常比EKF高。代价是计算量稍大，但现代处理器完全扛得住。
我的经验：在RTK+INS紧耦合中，如果IMU的角速度变化剧烈（比如无人机做急转弯），EKF容易发散，UKF就稳得多。但UKF的参数（alpha, beta, kappa）需要调，我一般用alpha=0.001, beta=2, kappa=0作为起点。
4.5 三种滤波器的对比
| 特性 | 线性卡尔曼滤波 | EKF | UKF |
|---|---|---|---|
| 适用系统 | 线性 | 弱非线性 | 强非线性 |
| 计算复杂度 | O(n^3) | O(n^3) | O(n^3) |
| 需要雅可比 | 否 | 是 | 否 |
| 精度 | 最优（线性） | 一阶近似 | 二阶近似 |
| 实现难度 | 低 | 中 | 中高 |
你可能会问：那是不是UKF永远比EKF好？不一定。如果系统接近线性，EKF的精度和UKF差不多，但EKF更快。而且EKF的雅可比矩阵能提供一些物理意义，方便调试。
我个人习惯：先用EKF快速验证算法，再根据实际效果决定是否切换到UKF。毕竟在嵌入式平台上，代码的稳定性和可调试性也很重要。
避坑指南：我曾经在UKF里忘了对协方差矩阵做对称化处理，结果P矩阵变得不对称，滤波直接炸了。记得每次更新后加一句：P = (P + P') / 2。
好了，卡尔曼滤波的基础就讲到这里。下一节我们会把这些滤波器应用到RTK+INS紧耦合中，到时候你会看到它们真正的威力。
公众号:蓝海资料掘金营，微信deep3321