4. 卡尔曼滤波基础：状态空间模型、线性卡尔曼滤波推导、滤波发散的原因与初值敏感性
各位同学，今天我们聊卡尔曼滤波。说实话，这玩意儿在导航融合里太常用了。我入行那会儿，第一次接触卡尔曼滤波，觉得数学公式一堆，头大。后来做项目做多了，发现核心思想其实很朴素——就是“预测+修正”的循环。
你想想看，我们做导航融合，本质上就是要把多个传感器的数据揉在一起，得到一个更准、更稳的估计。卡尔曼滤波，就是干这个的经典工具。但工具再好，用不对也会翻车。今天我就把基础讲透，顺便聊聊那些年我踩过的坑。
4.1 状态空间模型：你得先知道“状态”是什么
卡尔曼滤波的第一步，是建立状态空间模型。说白了，就是你要用数学语言描述“系统是怎么演化的”以及“观测是怎么得到的”。
我个人习惯把状态空间模型拆成两个方程：
- 状态方程：描述系统内部状态如何随时间变化。比如，一个运动物体的位置和速度。
- 观测方程：描述传感器观测值跟状态之间的关系。比如，GPS测到的位置跟真实位置的关系。
用数学写出来，就是：
状态方程：x_k = A * x_{k-1} + B * u_k + w_k
观测方程：z_k = H * x_k + v_k这里：
x_k是k时刻的状态向量（比如位置、速度）A是状态转移矩阵B是控制输入矩阵u_k是控制输入（比如加速度）w_k是过程噪声（建模为高斯白噪声）z_k是观测向量H是观测矩阵v_k是观测噪声（也是高斯白噪声）
4.2 线性卡尔曼滤波推导：预测与修正的循环
线性卡尔曼滤波的推导，其实不复杂。它分两步走：预测（Time Update）和修正（Measurement Update）。
第一步：预测
用上一时刻的最优估计，预测当前时刻的状态和误差协方差。
状态预测：x_{k|k-1} = A * x_{k-1|k-1} + B * u_k
协方差预测：P_{k|k-1} = A * P_{k-1|k-1} * A^T + Q这里 Q 是过程噪声协方差矩阵。嗯，这个Q怎么设，后面我会讲，是个大坑。
第二步：修正
用实际观测值来修正预测值，得到最优估计。
卡尔曼增益：K_k = P_{k|k-1} * H^T * (H * P_{k|k-1} * H^T + R)^{-1}
状态更新：x_{k|k} = x_{k|k-1} + K_k * (z_k - H * x_{k|k-1})
协方差更新：P_{k|k} = (I - K_k * H) * P_{k|k-1}你看，卡尔曼增益 K_k 决定了“你更相信预测还是观测”。如果观测噪声小（R小），K就大，更相信观测；反之更相信预测。
4.3 滤波发散的原因：为什么卡尔曼滤波会“飘”
理论上，卡尔曼滤波是收敛的。但实际工程中，我见过太多次滤波发散的情况。说白了，就是估计值越来越离谱，最后完全偏离真实值。
为什么会这样？我总结了几点：
- 模型不准确：状态方程或观测方程跟实际系统不匹配。比如，你假设运动是匀速的，但实际有加速度。
- 噪声统计特性不准确：Q和R矩阵设得不对。Q设太小，滤波器过于相信模型；R设太小，滤波器过于相信观测。
- 数值计算误差：协方差矩阵P失去正定性或对称性，导致计算崩溃。
- 初值设置不当：这个我重点讲。
4.4 初值敏感性：别小看初始状态
卡尔曼滤波对初值敏感吗？答案是：敏感，但没那么可怕。关键在于你怎么设。
初值包括两部分：
- 初始状态估计 x0：比如位置、速度的初始值。
- 初始误差协方差 P0：表示你对初始估计的不确定性。
我个人的建议是：
- x0 尽量准：如果可能，用第一次观测值来初始化。比如GPS第一次定位结果。
- P0 设大一点：如果你对初始状态不确定，就把P0设大。这样滤波器一开始会“谦虚”地相信观测，快速收敛。
举个例子：
// 初始化示例
x0 = [0, 0] // 假设初始位置和速度都为0
P0 = [[100, 0], [0, 100]] // 不确定性很大，协方差设大如果你把P0设得太小（比如接近0），滤波器会认为初始状态非常准，后面观测数据很难修正它。这就是初值敏感性的一种表现。
4.5 避坑指南：实战中的几个要点
最后，我分享几个实战中总结的要点：
- Q和R的调参：没有万能公式。我一般先根据传感器手册的噪声指标设初值，然后通过仿真或实测数据微调。
- 数值稳定性：用平方根滤波或UD分解，避免协方差矩阵非正定。我在嵌入式平台上吃过这个亏。
- 异常检测：加一个卡方检验，检测观测值是否异常。如果残差太大，暂时不用这个观测更新。
- 初始化策略：如果系统启动时没有可靠初值，可以用多次观测做最小二乘估计，得到初始状态。
好了，这一章的内容就这些。卡尔曼滤波是导航融合的基石，理解透了，后面学扩展卡尔曼滤波、无迹卡尔曼滤波就轻松多了。下一章我们聊非线性滤波，到时候见。