3、卡尔曼滤波入门：状态空间模型、预测与更新步骤、一维与二维卡尔曼滤波实现

卡尔曼滤波，说白了就是一套帮你从「有噪声的测量数据」中，猜出「系统真实状态」的数学方法。我最早接触它是在做雷达目标跟踪的时候，那时候觉得这东西太玄乎了。后来自己手推了一遍公式，才发现——嗯，其实没那么复杂。

这一章，我们就从最基础的状态空间模型讲起，一步步拆解预测和更新这两个核心步骤。最后我会带你手写一维和二维的卡尔曼滤波代码。相信我，看完你也能用起来。

3.1 状态空间模型：你在跟踪什么？

先问一个问题：你要跟踪的目标，它的「状态」是什么？

举个例子。如果我在跟踪一辆车的位置，那状态就是它的坐标 (x, y)。但如果车在动，光有位置不够，你还得知道它的速度 (vx, vy)。所以状态向量通常写成：

x = [位置, 速度]

这就是状态空间模型的核心思想——用一组变量来描述系统在任意时刻的完整状态。

状态空间模型由两个方程组成：

状态方程：描述状态如何随时间演化
观测方程：描述我们如何测量到状态

写成数学形式：

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 是观测噪声（也服从高斯分布）

关键点：卡尔曼滤波假设所有噪声都是高斯白噪声，均值为0。这个假设在实际中不一定完全成立，但大多数情况下够用了。

我在项目中遇到过一个问题：状态向量的维度选错了。比如只选了位置没选速度，结果滤波效果很差。后来加上了速度，效果立竿见影。所以我的建议是——状态设计时，尽量把影响系统动态的关键变量都包含进去。

3.2 预测步骤：猜一猜下一时刻

卡尔曼滤波分两步走：先预测，再更新。

预测步骤，说白了就是「根据上一时刻的状态，猜一下当前时刻的状态会是什么」。这个猜测基于系统的物理模型——也就是状态转移矩阵 A。

预测公式有两个：

x_pred = A * x_prev + B * u
P_pred = A * P_prev * A^T + Q

解释一下：

x_pred 是预测的状态
P_pred 是预测的误差协方差矩阵（代表我们对预测的置信度）
Q 是过程噪声协方差矩阵

你可能会问：为什么还要算 P？

因为光有预测值不够，你还得知道这个预测有多「靠谱」。P 越小，说明我们越相信这个预测。而 Q 的存在，是因为模型本身不完美——比如你假设车匀速运动，但实际它可能加速或减速。Q 就是用来吸收这种不确定性的。

我的经验：Q 的调参很关键。Q 设得太小，滤波器会过于相信模型，跟不上真实变化；Q 设得太大，滤波器会过于相信测量，噪声抑制效果变差。我一般从单位矩阵开始调，然后根据实际效果微调。

3.3 更新步骤：用测量修正预测

预测完了，我们拿到一个测量值 z_k。现在要做的是：把预测和测量融合起来，得到更准确的状态估计。

更新公式有三个：

K = P_pred * H^T * (H * P_pred * H^T + R)^(-1)
x_updated = x_pred + K * (z - H * x_pred)
P_updated = (I - K * H) * P_pred

这里 K 就是传说中的卡尔曼增益。它决定了我们更相信预测还是更相信测量。

如果测量噪声 R 很小，K 会变大，滤波器更相信测量
如果预测协方差 P_pred 很小，K 会变小，滤波器更相信预测

说白了，卡尔曼增益就是一个「智能加权系数」。它会根据当前的不确定性，自动调整预测和测量的权重。

注意：更新步骤中，z - H*x_pred 叫做「残差」或「新息」。如果残差突然变得很大，说明要么测量异常，要么模型出了问题。我在做多目标跟踪时，经常用残差来检测目标是否发生了机动（比如突然转弯）。

3.4 一维卡尔曼滤波实现

先来个最简单的——跟踪一个一维位置。比如一个物体在直线上运动，我们测量它的位置。

状态向量：x = [位置, 速度]

状态转移矩阵 A：

A = [[1, dt],
     [0, 1]]

观测矩阵 H：

H = [[1, 0]]

下面是完整的 Python 实现：

import numpy as np

class KalmanFilter1D:
    def __init__(self, dt):
        self.dt = dt
        self.A = np.array([[1, dt],
                           [0, 1]])
        self.H = np.array([[1, 0]])
        self.Q = np.array([[0.1, 0],
                           [0, 0.1]])
        self.R = np.array([[1]])
        self.P = np.eye(2) * 100
        self.x = np.array([[0],
                           [0]])

    def predict(self):
        self.x = self.A @ self.x
        self.P = self.A @ self.P @ self.A.T + self.Q

    def update(self, z):
        y = z - self.H @ self.x
        S = self.H @ self.P @ self.H.T + self.R
        K = self.P @ self.H.T @ np.linalg.inv(S)
        self.x = self.x + K @ y
        self.P = (np.eye(2) - K @ self.H) @ self.P

    def get_state(self):
        return self.x[0, 0], self.x[1, 0]

使用起来很简单：

kf = KalmanFilter1D(dt=0.1)
for z in measurements:
    kf.predict()
    kf.update(z)
    pos, vel = kf.get_state()
    print(f"位置: {pos:.2f}, 速度: {vel:.2f}")

注意：初始 P 设得很大（比如 100），表示我们一开始对状态估计很不确定。随着滤波进行，P 会逐渐收敛到稳定值。

3.5 二维卡尔曼滤波实现

一维搞定了，二维其实只是把矩阵维度扩展一下。比如跟踪一个在平面上运动的物体：

状态向量：x = [x, y, vx, vy]

状态转移矩阵 A：

A = [[1, 0, dt, 0],
     [0, 1, 0, dt],
     [0, 0, 1, 0],
     [0, 0, 0, 1]]

观测矩阵 H（只观测位置）：

H = [[1, 0, 0, 0],
     [0, 1, 0, 0]]

代码实现：

class KalmanFilter2D:
    def __init__(self, dt):
        self.dt = dt
        self.A = np.array([[1, 0, dt, 0],
                           [0, 1, 0, dt],
                           [0, 0, 1, 0],
                           [0, 0, 0, 1]])
        self.H = np.array([[1, 0, 0, 0],
                           [0, 1, 0, 0]])
        self.Q = np.eye(4) * 0.1
        self.R = np.eye(2) * 1
        self.P = np.eye(4) * 100
        self.x = np.zeros((4, 1))

    def predict(self):
        self.x = self.A @ self.x
        self.P = self.A @ self.P @ self.A.T + self.Q

    def update(self, z):
        y = z - self.H @ self.x
        S = self.H @ self.P @ self.H.T + self.R
        K = self.P @ self.H.T @ np.linalg.inv(S)
        self.x = self.x + K @ y
        self.P = (np.eye(4) - K @ self.H) @ self.P

    def get_state(self):
        return self.x[0, 0], self.x[1, 0], self.x[2, 0], self.x[3, 0]

二维和一维的区别，说白了就是矩阵变大了。核心逻辑完全一样。

避坑指南：我曾经在二维滤波中把 Q 和 R 的维度搞反了，结果滤波结果发散。检查了半天才发现是矩阵维度不匹配。所以写代码时，建议先打印一下各矩阵的 shape，确认无误再跑。

3.6 核心逻辑流程图

下面我用一张 SVG 图来展示卡尔曼滤波的完整流程。你可以把它当作「速查地图」：

这张图把卡尔曼滤波的「预测-更新-输出-再预测」循环展示得很清楚。你想想看，每次迭代，我们都在做同一件事：先用模型猜一下，再用测量修正一下。循环往复，状态估计就会越来越准。

3.7 调参心得与常见坑

最后分享几个我踩过的坑：

Q 和 R 的比值：Q/R 的比值决定了滤波器的「响应速度」。比值越大，滤波器对测量变化越敏感，但噪声也越大。我一般先设 Q=R，然后根据实际效果调整。
初始 P 的影响：P₀ 设得越大，滤波器初期越依赖测量。如果初始状态估计比较准，P₀ 可以设小一点。
数值稳定性：当 P 矩阵变得很小时，求逆可能会出问题。我习惯用 np.linalg.solve 代替 np.linalg.inv，数值上更稳定。
非高斯噪声：如果噪声不是高斯分布（比如有野值点），卡尔曼滤波效果会变差。这时候可以考虑用扩展卡尔曼滤波（EKF）或无迹卡尔曼滤波（UKF）。

曾经有一次，我在做无人机跟踪时，测量数据里混入了几个野值点。卡尔曼滤波直接炸了。后来我加了一个简单的残差检测——如果残差超过 3 倍标准差，就跳过这次更新。问题就解决了。

好了，这一章的内容就到这里。卡尔曼滤波的核心就是「预测 + 更新」两个步骤，配上状态空间模型这个框架。代码实现也不复杂，关键是把矩阵维度搞对，把 Q 和 R 调好。下一章我们会把卡尔曼滤波用到多目标跟踪中，到时候你会看到它真正的威力。

📚 课程目录

3、卡尔曼滤波入门：状态空间模型、预测与更新步骤、一维与二维卡尔曼滤波实现

3.1 状态空间模型：你在跟踪什么？

3.2 预测步骤：猜一猜下一时刻

3.3 更新步骤：用测量修正预测

3.4 一维卡尔曼滤波实现

3.5 二维卡尔曼滤波实现

3.6 核心逻辑流程图

3.7 调参心得与常见坑