4、卡尔曼滤波推导（上）：从贝叶斯滤波到卡尔曼滤波，状态预测与观测更新

各位同学好，我是你们的导航算法讲师。今天我们来啃一块硬骨头——卡尔曼滤波的数学推导。

说实话，我当年第一次看卡尔曼滤波的论文时，也是一头雾水。满篇的矩阵、协方差、高斯分布，感觉像在看天书。但后来我在做组合导航项目时，被IMU的噪声折磨得够呛，才真正理解了这套理论的精妙之处。

嗯，咱们不搞那些花里胡哨的。今天我就带你从最朴素的贝叶斯滤波出发，一步步走到卡尔曼滤波。你想想看，这其实就是一个「猜谜游戏」——我们怎么用数学来「猜」出系统的最优状态。

4.1 从贝叶斯滤波说起

贝叶斯滤波，说白了就是一套「根据观测结果更新信念」的数学框架。我习惯把它理解成：

• 先验：我对系统状态的初始猜测
• 似然：当前观测值有多「像」我的猜测
• 后验：结合观测后，我对状态的修正

公式很简单，但威力巨大：

P(x|z) = P(z|x) * P(x) / P(z)

这里P(x|z)就是后验概率，P(x)是先验，P(z|x)是似然。分母P(z)是个归一化常数，保证概率和为1。

我在做车载组合导航时遇到过一个问题：GPS信号突然变差，如果只相信GPS，定位会跳来跳去；如果只相信IMU，误差会越积越大。贝叶斯滤波的思路就是——两边都听，但谁靠谱就多信谁一点。

4.2 从贝叶斯到卡尔曼：高斯假设

贝叶斯滤波虽然优雅，但有个致命问题——计算量太大。为什么呢？因为你需要维护整个概率分布，这在连续状态空间里几乎不可能。

卡尔曼老爷子想了个聪明的办法：假设所有噪声都是高斯分布。这样一来，我们只需要维护两个东西：

• 均值 μ：系统状态的最优估计
• 协方差 P：这个估计的不确定性

你想想看，一个高斯分布只需要两个参数就能描述，这比维护整个概率密度函数省了多少事！

4.3 状态预测：时间更新

预测阶段，说白了就是「根据模型猜下一步」。假设我们有线性系统：

x_k = F * x_{k-1} + B * u_k + w_k
z_k = H * x_k + v_k

其中w_k是过程噪声，v_k是观测噪声，都假设为零均值高斯分布。

预测公式其实就两行：

// 状态预测
x_pred = F * x_est + B * u

// 协方差预测
P_pred = F * P_est * F^T + Q

这里Q是过程噪声协方差矩阵。Q越大，说明我们对模型越不信任，预测的不确定性就越大。

4.4 观测更新：测量更新

观测更新阶段，就是「用实际测量值修正预测」。核心公式如下：

// 卡尔曼增益
K = P_pred * H^T * (H * P_pred * H^T + R)^(-1)

// 状态更新
x_est = x_pred + K * (z - H * x_pred)

// 协方差更新
P_est = (I - K * H) * P_pred

这里R是观测噪声协方差矩阵。R越大，说明观测越不可靠，卡尔曼增益K就越小，我们越不相信观测值。

你仔细看这个公式：

• z - H*x_pred 叫「新息」或「残差」——预测值和实际观测的差距
• K 就是「信任分配器」——多大程度上用新息修正预测

说白了，卡尔曼滤波就是在做一件事：用新息乘以一个权重，然后加到预测值上。这个权重K就是卡尔曼增益。

4.5 知识体系总览

为了让你对本章内容有个整体把握，我画了一张流程图：

4.6 一个简单的例子

假设我们要估计一辆汽车的位置。系统模型是：

位置 = 上一时刻位置 + 速度 * dt + 噪声

观测是GPS给出的位置，但GPS有噪声。

预测阶段：

x_pred = x_est + v * dt
P_pred = P_est + Q   // 这里F=1，所以简化了

观测更新阶段：

K = P_pred / (P_pred + R)
x_est = x_pred + K * (z - x_pred)
P_est = (1 - K) * P_pred

你看，当GPS噪声R很大时，K很小，我们基本不相信GPS，位置主要靠模型预测。当模型噪声Q很大时，P_pred变大，K接近1，我们更相信GPS。

4.7 本章小结

今天我们走完了从贝叶斯滤波到卡尔曼滤波的推导之路。核心就三点：

• 贝叶斯滤波是理论根基，但计算量大
• 高斯假设让问题简化，只需维护均值和协方差
• 预测+更新两步走，卡尔曼增益K决定信任分配

下一章我们会继续深入，讨论卡尔曼滤波的数学推导细节，以及在实际工程中如何调参。嗯，今天就到这里，希望你能把这几行核心公式刻在脑子里。