4、离散卡尔曼滤波推导：系统模型、预测步骤、更新步骤、卡尔曼增益
各位同学，欢迎来到第四讲。
说实话，卡尔曼滤波的推导，是很多人入门导航融合时最头疼的一关。我当年刚接触时，看着那一堆矩阵和协方差，也是一脸懵。但后来在项目里摔打了几次，慢慢就摸到门道了。
今天咱们就把这个「硬骨头」啃下来。我会带着大家，从系统模型开始，一步步推导出预测、更新和卡尔曼增益。你跟着我的思路走，保证能理清楚。
4.1 系统模型：你得先知道系统长什么样
做卡尔曼滤波，第一步就是建模。说白了，你得用数学语言告诉计算机：这个系统是怎么演变的？传感器是怎么观测的？
离散卡尔曼滤波假设系统是线性的，模型分两部分：
1. 状态方程（预测模型）
描述状态如何随时间变化：
x_k = F * x_{k-1} + B * u_k + w_kx_k：k时刻的状态向量（比如位置、速度）F：状态转移矩阵（描述上一时刻状态如何影响当前）B：控制输入矩阵u_k：控制输入（比如加速度指令）w_k：过程噪声，服从N(0, Q)
2. 观测方程（测量模型）
描述传感器如何观测状态：
z_k = H * x_k + v_kz_k：观测向量（比如GPS位置）H：观测矩阵（把状态映射到观测空间）v_k：观测噪声，服从N(0, R)
4.2 预测步骤：先猜一个大概
预测步骤，就是利用系统模型，根据上一时刻的最优估计，来「猜」当前时刻的状态。说白了，就是先蒙一个。
预测分两步：
1. 状态预测
x̂_k|k-1 = F * x̂_{k-1|k-1} + B * u_k这里 x̂_k|k-1 表示：基于k-1时刻的信息，对k时刻状态的预测。注意，这里没有加噪声项，因为噪声的期望是0。
2. 协方差预测
P_k|k-1 = F * P_{k-1|k-1} * F^T + Q协方差矩阵 P 代表我们对状态估计的不确定度。预测会让不确定度增大（因为加了过程噪声Q）。
4.3 更新步骤：用观测来修正
预测完了，我们有了一个「猜测值」。现在传感器送来一个观测值，我们得用这个观测来修正之前的猜测。
更新步骤的核心公式：
x̂_k|k = x̂_k|k-1 + K_k * (z_k - H * x̂_k|k-1)这个公式很直观：
z_k - H * x̂_k|k-1叫「残差」或「新息」，就是实际观测和预测观测的差值K_k就是卡尔曼增益，它决定了我们有多相信这个残差
同时，协方差也要更新：
P_k|k = (I - K_k * H) * P_k|k-1更新后，协方差会变小，说明我们对状态的把握更精确了。
4.4 卡尔曼增益：信任的权重
卡尔曼增益 K_k 是整个滤波器的灵魂。它决定了预测和观测谁更可信。
它的计算公式是：
K_k = P_k|k-1 * H^T * (H * P_k|k-1 * H^T + R)^{-1}这个公式看着复杂，但本质很简单：
- 分子
P_k|k-1 * H^T：代表预测的不确定度映射到观测空间 - 分母
H * P_k|k-1 * H^T + R：代表预测不确定度 + 观测噪声
说白了，卡尔曼增益就是「预测不确定度」和「总不确定度」的比值。
极端情况分析：
| 情况 | K_k 取值 | 含义 |
|---|---|---|
| 观测噪声R → 0 | K_k → H^{-1} | 完全相信观测，忽略预测 |
| 预测协方差P → 0 | K_k → 0 | 完全相信预测，忽略观测 |
4.5 完整算法流程
把上面这些串起来，离散卡尔曼滤波的完整流程就是：
- 初始化：设定初始状态
x̂_0和初始协方差P_0 - 预测：计算
x̂_k|k-1和P_k|k-1 - 计算增益：计算
K_k - 更新：用观测
z_k修正状态和协方差 - 循环：k = k+1，回到步骤2
嗯，这里要注意：这个流程是递归的，每一时刻只需要上一时刻的状态和协方差，不需要历史数据。这也是卡尔曼滤波高效的原因之一。
好了，这一讲的内容就到这里。下一讲我们会用一个具体的导航融合案例，把今天推导的公式全部跑一遍。到时候你就知道，这些公式在代码里是怎么实现的了。