第4章 KF的Python实现

4. KF的Python实现：一维温度估计案例

好了，终于到了动手写代码的时候了。

前面我们推导了那么多公式，说实话，光看数学确实有点枯燥。我个人习惯是，先跑通一个最简单的例子，再回头理解公式，这样心里更有底。

这一章，我们就用Python从零实现一个卡尔曼滤波器。场景选什么呢？——一维温度估计。说白了，就是用一个温度计去测室温，但温度计有噪声，我们要用KF把真实温度“猜”出来。

4.1 问题定义：我们要解决什么？

想象一下这个场景：

你有一个温度计，每隔1秒测一次室温
温度计有测量噪声，比如±0.5°C的随机误差
房间温度其实也在缓慢变化（过程噪声）
你想实时知道“真实”温度是多少

嗯，这就是一个典型的一维卡尔曼滤波问题。状态量只有一个：温度。控制量没有（或者说为0）。

我当年第一次做这个案例时，犯了个低级错误——把过程噪声设得太小，结果滤波器几乎不相信测量值，温度变化半天都追不上。后来才明白，调参这事，真的得靠手感。

4.2 系统建模：状态方程和观测方程

对于一维温度估计，我们的模型非常简单：

状态方程（预测）：

x(k) = x(k-1) + w(k-1)

其中w是过程噪声，服从N(0, Q)。这里Q就是过程噪声协方差。

观测方程（测量）：

z(k) = x(k) + v(k)

其中v是测量噪声，服从N(0, R)。R是测量噪声协方差。

你看，状态转移矩阵A=1，观测矩阵H=1。简单到不能再简单了。但正是这个简单的例子，能让你看清KF的每一步到底在干什么。

核心参数：

Q：过程噪声协方差——你有多相信模型？Q越大，滤波器越“灵活”，但也越容易受噪声影响
R：测量噪声协方差——你有多相信传感器？R越大，滤波器越“迟钝”，平滑效果越好

4.3 从零手写KF代码

下面就是完整的Python实现。我尽量写得清晰，每一步都对应前面讲过的公式。

import numpy as np
import matplotlib.pyplot as plt

class KalmanFilter1D:
    """一维卡尔曼滤波器 - 温度估计"""
    
    def __init__(self, Q=1e-4, R=0.25):
        # 初始化参数
        self.Q = Q  # 过程噪声协方差
        self.R = R  # 测量噪声协方差
        
        # 状态估计
        self.x = 25.0  # 初始温度估计（随便猜一个）
        self.P = 1.0   # 初始误差协方差（不确定度）
        
    def predict(self):
        """预测步骤"""
        # x(k|k-1) = A * x(k-1|k-1) + B * u(k)
        # 这里A=1, B=0, 所以就是保持不变
        self.x = self.x
        
        # P(k|k-1) = A * P(k-1|k-1) * A^T + Q
        # 这里A=1, 所以就是P + Q
        self.P = self.P + self.Q
        
    def update(self, z):
        """更新步骤"""
        # 卡尔曼增益
        # K = P * H^T / (H * P * H^T + R)
        # 这里H=1, 所以就是P / (P + R)
        K = self.P / (self.P + self.R)
        
        # 状态更新
        # x(k|k) = x(k|k-1) + K * (z - H * x(k|k-1))
        self.x = self.x + K * (z - self.x)
        
        # 误差协方差更新
        # P(k|k) = (I - K * H) * P(k|k-1)
        self.P = (1 - K) * self.P
        
        return self.x

代码就这么短。你看，核心逻辑就两个函数：predict和update。每次来一个新测量值，先predict一下，再update一下，就得到了新的最优估计。

小技巧：初始值x和P怎么设？

如果你对初始状态比较确定，P可以设小一点（比如0.1）。如果不确定，就设大一点（比如1.0或更大）。滤波器会很快收敛的，别担心。

4.4 生成模拟数据并运行

有了滤波器，我们还需要模拟数据来测试。我生成一组“真实温度”加上高斯噪声，模拟温度计的读数。

# 生成模拟数据
np.random.seed(42)  # 固定随机种子，方便复现

n_steps = 100
true_temps = np.zeros(n_steps)
measurements = np.zeros(n_steps)

# 真实温度：先稳定在25°C，然后缓慢上升
for i in range(n_steps):
    if i < 30:
        true_temps[i] = 25.0
    elif i < 70:
        true_temps[i] = 25.0 + (i - 30) * 0.1  # 缓慢升温
    else:
        true_temps[i] = 29.0  # 稳定在29°C

# 添加测量噪声（标准差0.5°C）
measurements = true_temps + np.random.normal(0, 0.5, n_steps)

# 运行卡尔曼滤波
kf = KalmanFilter1D(Q=1e-4, R=0.25)
estimates = np.zeros(n_steps)

for i in range(n_steps):
    kf.predict()
    estimates[i] = kf.update(measurements[i])

# 计算误差
mse_before = np.mean((measurements - true_temps) ** 2)
mse_after = np.mean((estimates - true_temps) ** 2)

print(f"原始测量均方误差: {mse_before:.4f}")
print(f"卡尔曼滤波均方误差: {mse_after:.4f}")

运行结果会告诉你，滤波后的误差明显小于原始测量误差。这就是KF的价值所在。

4.5 结果可视化

光看数字不够直观，我们画个图看看。

plt.figure(figsize=(12, 6))
plt.plot(true_temps, 'g-', label='真实温度', linewidth=2)
plt.plot(measurements, 'r.', label='测量值（含噪声）', alpha=0.5)
plt.plot(estimates, 'b-', label='卡尔曼估计', linewidth=2)
plt.legend()
plt.xlabel('时间步')
plt.ylabel('温度 (°C)')
plt.title('一维卡尔曼滤波 - 温度估计')
plt.grid(True, alpha=0.3)
plt.show()

你会看到：

红色测量点上下跳动，噪声很明显
蓝色估计线平滑很多，紧贴着绿色真实值
在温度变化时（第30步附近），估计值会有一点延迟，但很快追上

我曾经踩过的坑：

刚开始调参时，我把Q设成了0。结果温度变化时，滤波器完全追不上，因为模型认为温度不会变。后来我把Q调到1e-3，效果就好多了。记住：Q不能为0，否则滤波器会“死掉”。

4.6 参数调优：Q和R的博弈

Q和R的比值，决定了滤波器的行为。我整理了一个表格，方便你理解：

Q/R比值	滤波器行为	适用场景
Q很小，R很大	非常平滑，但响应慢	温度稳定、传感器噪声大
Q很大，R很小	响应快，但容易受噪声影响	温度变化快、传感器精度高
Q和R适中	平衡平滑和响应	大多数通用场景

说白了，这就是一个权衡。你想想看，如果传感器很准（R小），你就多信测量值；如果模型很准（Q小），你就多信预测值。卡尔曼增益K就是自动做这个权衡的。

4.7 完整代码汇总

为了方便你直接运行，我把完整代码放在下面。你复制到Python环境里就能跑。

import numpy as np
import matplotlib.pyplot as plt

class KalmanFilter1D:
    def __init__(self, Q=1e-4, R=0.25):
        self.Q = Q
        self.R = R
        self.x = 25.0
        self.P = 1.0
        
    def predict(self):
        self.P = self.P + self.Q
        
    def update(self, z):
        K = self.P / (self.P + self.R)
        self.x = self.x + K * (z - self.x)
        self.P = (1 - K) * self.P
        return self.x

# 生成数据
np.random.seed(42)
n_steps = 100
true_temps = np.zeros(n_steps)
for i in range(n_steps):
    if i < 30:
        true_temps[i] = 25.0
    elif i < 70:
        true_temps[i] = 25.0 + (i - 30) * 0.1
    else:
        true_temps[i] = 29.0
measurements = true_temps + np.random.normal(0, 0.5, n_steps)

# 运行滤波
kf = KalmanFilter1D(Q=1e-4, R=0.25)
estimates = np.zeros(n_steps)
for i in range(n_steps):
    kf.predict()
    estimates[i] = kf.update(measurements[i])

# 绘图
plt.figure(figsize=(12, 6))
plt.plot(true_temps, 'g-', label='真实温度', linewidth=2)
plt.plot(measurements, 'r.', label='测量值', alpha=0.5)
plt.plot(estimates, 'b-', label='卡尔曼估计', linewidth=2)
plt.legend()
plt.xlabel('时间步')
plt.ylabel('温度 (°C)')
plt.title('一维卡尔曼滤波 - 温度估计')
plt.grid(True, alpha=0.3)
plt.show()

print(f"原始MSE: {np.mean((measurements - true_temps)**2):.4f}")
print(f"滤波MSE: {np.mean((estimates - true_temps)**2):.4f}")

4.8 本章知识结构图

下面这张图，帮你把这一章的核心逻辑串起来。从问题定义到代码实现，每一步都清晰可见。

嗯，这一章的内容就到这里。代码不长，但麻雀虽小五脏俱全。你把这个一维例子跑通了，后面扩展到多维就水到渠成了。