4. 卡尔曼滤波核心思想回顾

在进入水下组合导航的EKF实现之前，有必要把标准卡尔曼滤波（KF）的骨架重新梳理一遍。EKF并不是另一套算法，而是KF在非线性系统上的一阶线性化推广；如果对KF的“预测—更新”节奏、卡尔曼增益的作用机理、以及“最优”二字的成立条件没有清晰认识，后续在SINS/DVL组合中调整Q、R矩阵时就会失去判断依据。

一、状态空间模型：把导航问题写成两组方程

卡尔曼滤波处理的对象是离散时间线性高斯系统，其标准形式由状态方程和量测方程共同描述：

x_k = F_{k-1} · x_{k-1} + B_{k-1} · u_{k-1} + w_{k-1},   w ~ N(0, Q)
z_k = H_k · x_k + v_k,                                    v ~ N(0, R)

其中 x 为状态向量，F 为状态转移矩阵，u 为已知控制输入，B 为输入矩阵，w 为过程噪声；z 为量测向量，H 为量测矩阵，v 为量测噪声。两条关键假设是：w 与 v 均为零均值高斯白噪声且互不相关，初始状态 x₀ 服从高斯分布且与噪声独立。正是“线性 + 高斯”这两个条件，使得滤波后的状态后验分布始终保持高斯形式，只需递推均值和协方差两个量即可完整描述。

映射到水下组合导航，状态通常取15维误差状态：姿态误差（3）、速度误差（3）、位置误差（3）、陀螺零偏（3）、加速度计零偏（3）。量测则来自DVL的对地速度或位置差。这里要特别注意约定：本课程统一采用“导航系为东北天（ENU）、误差状态定义为 估计值减真值”的约定，姿态误差的正负号、DVL速度量测的参考系都以此为准，后续章节的H矩阵与符号均与此保持一致。

二、预测与更新：两拍循环

KF每个滤波周期分为两步。预测步（时间更新）用系统模型把上一时刻的估计外推到当前时刻：

预测均值： x̂_k⁻ = F_{k-1} · x̂_{k-1}⁺ + B_{k-1} · u_{k-1}
预测协方差：P_k⁻ = F_{k-1} · P_{k-1}⁺ · F_{k-1}ᵀ + Q_{k-1}

更新步（量测更新）在拿到量测 z_k 后，用新息修正预测值：

新息：      ỹ_k = z_k − H_k · x̂_k⁻
新息协方差：S_k = H_k · P_k⁻ · H_kᵀ + R_k
卡尔曼增益：K_k = P_k⁻ · H_kᵀ · S_k⁻¹
更新均值：  x̂_k⁺ = x̂_k⁻ + K_k · ỹ_k
更新协方差：P_k⁺ = (I − K_k · H_k) · P_k⁻

理解这两步的关键在于协方差的流动：预测步中 P 被 F 放大并被 Q 注入不确定性，因此 P⁻ 通常比 P⁺ 大；更新步中 P 被量测“压缩”，因此 P⁺ 小于 P⁻。若某个滤波周期没有有效量测（例如DVL失锁），则只执行预测步，协方差持续增长——这正是组合导航中“失锁期间误差发散”的数学表现。

三、卡尔曼增益到底在做什么

把更新式改写一下更容易看清本质：x̂⁺ = (I − K·H)·x̂⁻ + K·z。也就是说，后验估计是“预测值”和“量测值”的加权平均，而 K 就是权重。当 R 很大（量测噪声大、不可信）时，S 变大，K 趋近于零，滤波器几乎不理会量测，输出贴近预测；当 P⁻ 很大（预测已经不可信，比如长时间没有量测修正）时，K 趋近于 H⁻¹，滤波器大幅采纳量测。因此 K 不是人为设定的参数，而是由 P⁻ 与 R 的相对大小自动计算出的“信任分配比例”。

从另一个角度看，K 还决定了新息对状态的影响强度，而新息 ỹ 本身是“量测实际值减去按当前状态预测出的量测值”。在水下组合导航调试中，观察新息序列是否为零均值白噪声，是判断滤波器是否健康的重要手段：若新息持续偏置，往往意味着H矩阵符号错误、杆臂未补偿或DVL安装角标定不准；若新息幅值远超 S 的平方根，则可能是R设置过小或存在未建模的故障。

四、“最优估计”成立的条件与工程含义

KF被称为线性高斯系统的最优估计，其“最优”有明确所指：在均方误差意义下最小，即最小化 E[(x − x̂)(x − x̂)ᵀ] 的迹。这一最优性依赖三个前提：系统模型线性且准确、噪声统计特性（Q、R）与实际一致、初始状态统计特性已知。三者中任何一个不满足，滤波器就不再严格最优，甚至可能发散。

这对水下组合导航的实践含义很直接：Q 和 R 不是“调参玄学”，而是对真实误差源的统计描述。例如MEMS陀螺的零偏不稳定性对应姿态误差过程噪声，DVL的速度精度对应R的对角元素。工程上常用的一段做法是：先根据器件手册给出Q、R的初值，再用实测的静止对准数据和匀速直航数据检查新息统计量，若归一化新息平方（NIS）长期偏离理论卡方分布，则回头修正Q、R。下表给出一个教学示例的初值设定思路（数值为合理假设，非实测结果）：

把上述内容压缩成一句话：状态空间模型定义了“系统怎么演化、我们能看到什么”，预测与更新定义了“如何递推地融合先验与量测”，卡尔曼增益定义了“融合时信谁多一点”，而线性高斯假设保证了这种融合在均方误差意义下最优。理解了这四者的关系，再去看EKF中雅可比矩阵的推导和Q、R的整定，就不会觉得是另一门学问了。