4. 卡尔曼滤波基础回顾

在飞行器高动态导航中，卡尔曼滤波之所以能成为状态估计的“基准工具”，根源在于它把线性高斯假设下的最优估计问题，转化为一组可以递推执行的矩阵运算。理解这一节，关键不是背公式，而是看清三个问题：状态如何传播、观测如何修正、协方差如何随增益变化。

一、线性高斯假设到底假设了什么

卡尔曼滤波的推导建立在两个核心条件上。第一是线性：状态方程和观测方程对状态量都是线性的，可写成

x_k = F_{k-1} x_{k-1} + B_{k-1} u_{k-1} + w_{k-1}
z_k = H_k x_k + v_k

其中 x 为状态向量，F 为状态转移矩阵，B 为控制输入矩阵，u 为已知控制量，H 为观测矩阵。第二是高斯：过程噪声 w 与观测噪声 v 都服从零均值高斯分布，且互不相关，协方差分别为 Q 和 R。若初始状态也服从高斯分布，则整个递推过程中状态的后验分布始终保持高斯，均值和协方差即可完整描述它。

对飞行器导航而言，这意味着：陀螺、加速度计、GNSS、气压高度等量测误差若近似零均值高斯，且系统在工作点附近可线性化，卡尔曼滤波就是合理的起点。若噪声明显非高斯（如强多径、脉冲干扰），或系统强非线性，则需要扩展卡尔曼、无迹卡尔曼或粒子滤波等方法。

二、递推公式：预测与更新交替进行

卡尔曼滤波按“预测—更新”循环推进。设上一时刻后验估计为 x̂_{k-1|k-1}，协方差为 P_{k-1|k-1}。

预测步先根据动力学把状态和不确定度推到当前时刻：

x̂_{k|k-1} = F_{k-1} x̂_{k-1|k-1} + B_{k-1} u_{k-1}
P_{k|k-1} = F_{k-1} P_{k-1|k-1} F_{k-1}^T + Q_{k-1}

这一步只依赖系统模型，尚未使用当前观测。P 的增大反映“时间越久、预测越不确定”。

更新步引入观测 z_k，先算新息（残差）与增益，再修正状态和协方差：

y_k = z_k - H_k x̂_{k|k-1}
S_k = H_k P_{k|k-1} H_k^T + R_k
K_k = P_{k|k-1} H_k^T S_k^{-1}
x̂_{k|k} = x̂_{k|k-1} + K_k y_k
P_{k|k} = (I - K_k H_k) P_{k|k-1}

其中 y_k 为新息，S_k 为新息协方差，K_k 为卡尔曼增益。新息代表“观测与预测的差异”，增益决定把这一差异按多大比例注入状态。

三、增益与协方差的物理含义

卡尔曼增益 K 是预测不确定度与观测不确定度的“权衡器”。当 P_{k|k-1} 很大而 R 很小时，K 接近 H^{-1}，滤波器更相信观测；当 P_{k|k-1} 很小而 R 很大时，K 接近零，滤波器更相信模型预测。因此 K 不是固定权重，而是随每一步的 P 和 R 自动调整。

协方差更新则体现“观测带来的信息收益”。P_{k|k} 通常小于 P_{k|k-1}，说明融合观测后不确定度下降。若某状态长期不可观测，对应 P 会持续增大，增益也随之变化，这正是判断滤波器是否“发散”的重要线索。

四、一个可手算的示例

设一维位置状态，F=1，H=1，Q=0.01，R=0.25。上一时刻 x̂=10.0，P=0.04。预测后 P_{k|k-1}=0.04+0.01=0.05。若观测 z=10.6，则新息 y=10.6-10.0=0.6，S=0.05+0.25=0.30，K=0.05/0.30≈0.1667。更新后 x̂=10.0+0.1667×0.6≈10.10，P=(1-0.1667)×0.05≈0.0417。可以看到，观测把位置从 10.0 拉向 10.6，但只拉了约 1/6，因为观测噪声相对预测不确定度更大。

若把 R 改为 0.01，则 S=0.06，K≈0.833，更新后 x̂≈10.50，P≈0.0083。观测更可信时，状态更靠近观测，协方差下降更明显。这一对比说明：K 和 P 的变化直接由 Q、R 的相对大小决定。

五、工程实现中的几个关键点

第一，Q 和 R 的设定决定滤波性格。Q 过小会使滤波器过度自信、跟不上机动；Q 过大则滤波结果抖动。R 过小会放大观测异常；R 过大则响应迟钝。实际调参常从传感器数据手册的噪声密度出发，再根据新息序列是否白噪声来微调。

第二，协方差对称性与正定性。理论上 P 应保持对称正定，但浮点运算可能破坏对称性。工程上常在每步更新后执行 P=(P+Pᵀ)/2，必要时用 Joseph 形式 P=(I−KH)P(I−KH)ᵀ+KRKᵀ 提升数值稳定性。

第三，新息检验。若 y 的统计特性偏离零均值高斯，说明模型或噪声假设有问题。可计算归一化新息平方 yᵀS⁻¹y，并与卡方分布阈值比较，用于检测观测异常或滤波器发散。

总结来看，卡尔曼滤波的递推公式并不复杂，真正需要掌握的是：预测步让不确定度增长，更新步用观测把不确定度压缩，而增益 K 正是这两者之间的动态平衡点。理解 K 与 P 的联动关系，就抓住了卡尔曼滤波的核心。