卡尔曼滤波器简明解释

卡尔曼滤波器简明解释：用 Python 把“玄学调参”变成“有迹可循”

昨天晚上十点多，我在公司楼下等外卖，手机导航一会儿飘到河里，一会儿飘到隔壁小区，我当时心里只有一个念头：

“这么抖的数据，手机是怎么判断我到底在哪儿的？”

如果你也好奇过这个问题，那大概率已经和卡尔曼滤波擦肩而过很多次了——定位、惯导、手写识别、目标跟踪…背后都能看到它的影子。

今天就不搞那些“高斯、协方差、贝叶斯”吓人的词了，咱就用一个最简单的一维小例子，把卡尔曼滤波的核心思想讲明白，再用一段 Python 代码跑一跑，看它到底在干嘛。

先搞清楚：卡尔曼滤波到底在干什么？

一句话版本：

卡尔曼滤波就是：在“相信自己预测”和“相信传感器测量”之间，每一刻做一个最合适的折中。

你可以想象有这么一个场景：

• 你在走路，闭着眼，只靠“我大概每秒走 1 米”的感觉在估计位置
• 同时有人每秒给你报一个“带噪声”的位置（比如 GPS），忽高忽低
• 你不可能完全信自己，也不可能完全信他
• 卡尔曼滤波做的，就是每一秒综合一下这两边的“意见”，给出一个相对靠谱的位置估计

所以它其实就干两件事：

1. 预测（Predict）：根据上一时刻的状态，算出“我觉得下一刻大概在哪”
2. 更新 / 纠正（Update / Correct）：拿传感器的新读数来修正刚刚的预测

循环往复：预测 → 更新 → 预测 → 更新…

那在卡尔曼嘴里，“状态”长啥样？ 最简单可以这样定义：

• 位置：x
• 速度：v

那一刻的“状态向量”就是：

[

假设每次间隔 Δt 秒，我们有一个非常朴素的物理公式：

• 新位置 ≈ 旧位置 + 速度 × 时间

也就是：

[ x_{k} = x_{k-1} + v_{k-1} \cdot \Delta t ] [ v_{k} \approx v_{k-1} \quad (\text{速度先当常数}) ]

这个就是预测步骤的核心： 我不看新数据，仅靠“运动模型”算出下一刻的大致状态。

然后传感器每一刻给我们一个“测到的位置”：

• 记作 z_k，它 ≈ 真实位置 + 噪声

接下来就是整套戏的灵魂：到底相信谁更多一点？

卡尔曼滤波的“两颗心脏”：P 和 R / Q 是啥鬼？

你会看到教程里到处都是这几个怪字母：

• P：自己对“当前估计”的不确定性（协方差矩阵）
• R：传感器测量的噪声大小（测量越准，R 越小）
• Q：运动模型的不确定性（你对自己“预测模型”的不信任程度）

直白点说：

• P 大：我心虚，我知道自己先前估计不咋准
• R 大：我觉得传感器超吵，测出来的东西水分大
• Q 大：我承认我这个运动模型很粗糙，预测也不太靠谱

卡尔曼滤波每次更新的时候，会算一个“折中权重”：卡尔曼增益 K

• K 大：更信传感器
• K 小：更信自己的预测

就这一个小家伙，把“预测”和“传感器”粘在一起了。

好了，上 Python：写一个最小可跑的卡尔曼滤波

咱不用任何高级库，就用 numpy 手撸一个二维状态（一维位置 + 一维速度）的卡尔曼滤波。

先准备一下环境和“假数据”。

import numpy as np
import matplotlib.pyplot as plt

np.random.seed(42)  # 为了可复现

1. 模拟一个“真实运动 + 带噪声的测量”

假设：

• 小车从 0 米开始
• 速度 1 m/s 匀速
• 每 0.1 秒测一次位置
• 传感器的测量噪声是正态分布 N(0, 1)

dt = 0.1# 时间间隔

# 真实轨迹
steps = 100
true_x = np.zeros(steps)
true_v = np.ones(steps) * 1.0# 恒定速度 1 m/s

for k in range(1, steps):
    true_x[k] = true_x[k-1] + true_v[k-1] * dt

# 测量值（位置测量带噪声）
meas_noise_std = 1.0
meas_z = true_x + np.random.normal(0, meas_noise_std, size=steps)

到这里，我们有了：

• true_x：真实位置（我们“理论上知道”，但算法并不知道）
• meas_z：每一步带噪声的测量

2. 定义卡尔曼模型里的矩阵

# 状态转移矩阵 F
F = np.array([[1, dt],
              [0, 1]])

# 观测矩阵 H（只测量位置）
H = np.array([[1, 0]])

# 过程噪声协方差 Q（运动模型不确定性）
# 这里先随便给个小值，后面可以调
q = 0.01
Q = np.array([[q, 0],
              [0, q]])

# 测量噪声协方差 R（对应上面的 meas_noise_std）
R = np.array([[meas_noise_std**2]])

3. 初始化状态和协方差 P

一开始我们可以很“心虚”：

• 不知道自己在哪（位置方差大）
• 不知道速度多少（速度方差也大）

# 初始状态估计（随便先猜一个）
x_hat = np.array([[0.0],   # 估计的位置
                  [0.0]])  # 估计的速度

# 初始协方差矩阵 P（越大表示越不确定）
P = np.eye(2) * 1000.0# 非常不自信

4. 卡尔曼滤波核心循环：预测 + 更新

xs_hat = []  # 存每一步的状态估计

for k in range(steps):
# ========= 1. 预测 =========
# 状态预测
    x_pred = F @ x_hat           # 2x2 * 2x1 -> 2x1
# 协方差预测
    P_pred = F @ P @ F.T + Q

# ========= 2. 更新 =========
# 当前测量值（位置）
    z_k = np.array([[meas_z[k]]])  # 1x1

# 计算卡尔曼增益 K
    S = H @ P_pred @ H.T + R       # 1x1
    K = P_pred @ H.T @ np.linalg.inv(S)  # 2x1

# 用测量值修正预测
    y = z_k - H @ x_pred           # 残差（测量 - 预测测量）
    x_hat = x_pred + K @ y         # 更新后的最优估计

# 更新协方差
    P = (np.eye(2) - K @ H) @ P_pred

# 保存结果
    xs_hat.append(x_hat.flatten())

xs_hat = np.array(xs_hat)  # shape: [steps, 2]

到这里，卡尔曼滤波完整流程就走完了。你可以看到，整个循环里反复做的就是：

• 用 F 做一次“物理预测”
• 用 K 把这次测量融合进去

我们画个图看效果直观一点。

plt.figure(figsize=(10, 5))
plt.plot(true_x, label='True Position')
plt.scatter(range(steps), meas_z, s=10, alpha=0.4, label='Measurements')
plt.plot(xs_hat[:, 0], label='Kalman Estimated Position')
plt.legend()
plt.xlabel('Step')
plt.ylabel('Position')
plt.title('1D Position Tracking with Kalman Filter')
plt.show()

跑出来你会发现：

• 蓝线（测量）在上蹿下跳
• 橙线（真实位置）平滑是理所当然的
• 绿线（卡尔曼估计）会紧贴着真实轨迹，比测量平滑很多

这就是卡尔曼滤波真正给你的东西：在噪声很大的情况下，给你一个“平滑但又不过分迟钝”的估计。

再扒一下：那一堆矩阵运算到底在算啥？

你如果稍微耐心一点，可以把上面的循环拆开看：

预测部分

x_pred = F @ x_hat
P_pred = F @ P @ F.T + Q

含义：

• x_pred：根据运动模型，推演下一刻的状态
• P_pred：误差协方差也要“跟着时间演化”，并且加上过程噪声 Q

直白理解：时间往前走了，光靠自己脑补的预测，心里应该更没谱一点，所以不确定性会变大。

更新部分

S = H @ P_pred @ H.T + R
K = P_pred @ H.T @ np.linalg.inv(S)
y = z_k - H @ x_pred
x_hat = x_pred + K @ y
P = (np.eye(2) - K @ H) @ P_pred

这几步一起看：

• y：测量和预测差了多少（残差）
• K：这一步到底“要不要听传感器的”，它是一个自动算出来的权重
• x_hat 更新：预测 + K × 残差
• P 更新：融合了新信息后的不确定性会降低

非常关键的一点：整个过程中，从来没有“拍脑袋设个权重 0.3 或 0.7”这种东西，卡尔曼通过 P、Q、R 的组合，自动帮你算好了最优权重 K。

讲讲最常被问的：Q 和 R 怎么调？

现实项目里，80% 卡尔曼问题都死在这俩上。

可以先记一个“感觉上的规则”：

• R 大：觉得传感器噪声很大，不怎么信测量值 → 估计曲线更接近模型预测，更平滑，但可能跟不上真实变化

• Q 大：觉得模型不靠谱（比如速度变化很剧烈） → 估计会更敏感，更容易跟着测量跑，同时也更抖一点

但是别被“调参”吓到，一开始完全可以这样玩：

# 比如先假设测量噪声标准差是经验值
meas_noise_std = 2.0
R = np.array([[meas_noise_std**2]])

# 然后让 Q 比 R 小一个量级开始试
q = 0.1
Q = np.array([[q, 0],
              [0, q]])

然后多画图、多观察：

• 发现估计曲线抖得厉害 → Q 可以小一点，R 大一点
• 发现估计跟真实变化差一截 → Q 大一点，让滤波器更“灵敏”

给个小封装：写成一个可复用的 KalmanFilter 类

如果你以后项目里想用，可以把上面那坨逻辑简单包一下。

import numpy as np

classKalmanFilter1D:
def__init__(self, dt, 
                 process_var=0.01, 
                 meas_var=1.0):
"""
        一维位置 + 速度 的卡尔曼滤波
        dt: 时间间隔
        process_var: 过程噪声方差（Q）
        meas_var: 测量噪声方差（R）
        """
        self.dt = dt

# 状态转移矩阵 F
        self.F = np.array([[1, dt],
                           [0, 1]])

# 观测矩阵 H（只量位置）
        self.H = np.array([[1, 0]])

# 过程噪声 Q
        self.Q = np.array([[process_var, 0],
                           [0, process_var]])

# 测量噪声 R
        self.R = np.array([[meas_var]])

# 初始状态
        self.x_hat = np.zeros((2, 1))
        self.P = np.eye(2) * 1000.0

defpredict(self):
# 预测步骤
        self.x_hat = self.F @ self.x_hat
        self.P = self.F @ self.P @ self.F.T + self.Q
return self.x_hat.copy()

defupdate(self, z):
# z 是当前测量值（标量）
        z = np.array([[z]])
        S = self.H @ self.P @ self.H.T + self.R
        K = self.P @ self.H.T @ np.linalg.inv(S)
        y = z - self.H @ self.x_hat
        self.x_hat = self.x_hat + K @ y
        self.P = (np.eye(2) - K @ self.H) @ self.P
return self.x_hat.copy()

用的时候像这样：

kf = KalmanFilter1D(dt=0.1, process_var=0.01, meas_var=1.0)

est_positions = []

for z in meas_z:
    kf.predict()
    x_hat = kf.update(z)
    est_positions.append(x_hat[0, 0])  # 位置

这东西往真实项目里一塞，就已经能干很多有意思的事了，比如：

• 给游戏角色的移动加“平滑但不延迟”的轨迹
• 手机加速度计 / 陀螺仪数据融合（IMU）
• 简单的目标跟踪（摄像头里追一个点）

行，差不多就到这儿。 如果你现在再看到“卡尔曼滤波”这几个字，心里还是“这不就是玄学嘛”，那就是我这篇写失败了；如果你能把上面那段 Python 敲一遍，画出图，看到绿线紧紧贴着真实轨迹，那你其实已经把这玩意儿学了个七七八八了。