3. 卡尔曼滤波基础：状态空间模型、五大公式推导、一维实例

各位同学，欢迎来到卡尔曼滤波的世界。说实话，我刚入行那会儿，看到卡尔曼滤波的公式就头疼——满屏的矩阵、协方差、增益，感觉像在看天书。但后来我发现，这东西说白了就是一个「猜得准，再修正」的循环过程。今天我们就把它拆开揉碎了讲清楚。

3.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) 是观测噪声

我个人习惯把这两个方程想象成「猜」和「看」的关系。状态方程是猜系统下一步会怎样，观测方程是看传感器实际测到了什么。两者结合，才能得到靠谱的估计。

3.2 五大公式推导：从猜想到修正

卡尔曼滤波的五大公式，其实就干两件事：预测和更新。我当年在调试车载组合导航时，就是靠这五个公式反复迭代，才把定位误差从米级压到厘米级。

先看预测部分（两个公式）：

// 公式1：状态预测
x_pred = A * x_est + B * u

// 公式2：协方差预测
P_pred = A * P_est * A^T + Q

这里 Q 是过程噪声协方差矩阵。它代表你对模型的信任程度——Q越大，说明你越不相信模型，更依赖观测。

再看更新部分（三个公式）：

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

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

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

这里 R 是观测噪声协方差矩阵。它代表你对传感器的信任程度——R越小，说明传感器越准，你越相信观测。

为什么会这样？你想想看：

• 卡尔曼增益 K 本质上是一个「权重分配器」
• 当观测噪声 R 很小时，K 会变大，更相信观测
• 当过程噪声 Q 很小时，K 会变小，更相信模型

3.3 一维实例：手把手带你跑通

理论说完了，我们来个最简单的例子——一维位置估计。假设你有一个物体在直线上运动，你只能通过一个带噪声的传感器测量它的位置。

状态向量只有位置 x，所以：

• A = 1（位置不变，除非有控制）
• H = 1（传感器直接测位置）
• B = 0（没有控制输入）

代码实现如下：

# Python 一维卡尔曼滤波示例
import numpy as np
import matplotlib.pyplot as plt

# 初始化
x_est = 0.0      # 初始状态估计
P_est = 1.0      # 初始协方差
Q = 0.01         # 过程噪声方差
R = 0.1          # 观测噪声方差

# 模拟数据
true_position = 5.0
measurements = true_position + np.random.randn(50) * np.sqrt(R)

# 卡尔曼滤波
estimates = []
for z in measurements:
    # 预测
    x_pred = x_est
    P_pred = P_est + Q
    
    # 更新
    K = P_pred / (P_pred + R)
    x_est = x_pred + K * (z - x_pred)
    P_est = (1 - K) * P_pred
    
    estimates.append(x_est)

print(f"最终估计值: {x_est:.3f}, 真实值: {true_position}")

运行这段代码，你会发现估计值很快收敛到真实值附近。这就是卡尔曼滤波的魔力——它能在噪声中「看」到真相。

3.4 知识体系总览

下面这张图总结了本章的核心逻辑，我建议你把它存下来，以后写代码时对照着看：

嗯，这张图把整个流程串起来了。你从左边开始，先做预测，然后拿观测值做更新，更新完的结果又作为下一轮的初始值。周而复始，滤波器就「活」起来了。

好了，这一章的内容就到这里。记住：状态空间模型是骨架，五大公式是血肉，一维实例是让你亲手摸到它的温度。下一章我们会进入更复杂的场景——非线性系统，到时候扩展卡尔曼滤波和无迹卡尔曼滤波会登场。但别急，先把今天的基础打牢。