4、KF的一维实战：用Python实现一维位置估计、温度滤波案例

好，咱们今天来点真家伙。

前面讲了那么多理论，什么状态空间、协方差矩阵、卡尔曼增益……说实话，光看公式容易晕。我当年第一次接触卡尔曼滤波时，也是盯着那五个公式看了半天，感觉每个字都认识，但连起来就不知道在干嘛。

后来我发现，最好的学习方法就是——动手写代码。

这一章，我们就用Python实现两个最经典的一维KF案例：位置估计和温度滤波。代码我会逐行解释，你跟着敲一遍，保证能打通任督二脉。

4.1 一维KF的核心公式回顾

在写代码之前，先快速回顾一下一维情况下的卡尔曼滤波公式。一维的好处是——所有矩阵都退化成标量，理解起来直观得多。

步骤	公式	说明
预测	x̂k|k-1 = x̂k-1|k-1 + uk	状态预测（假设匀速模型）
预测协方差	Pk|k-1 = Pk-1|k-1 + Q	预测不确定性增加
卡尔曼增益	Kk = Pk|k-1 / (Pk|k-1 + R)	权衡预测和测量
更新	x̂k|k = x̂k|k-1 + Kk(zk - x̂k|k-1)	融合测量值
更新协方差	Pk|k = (1 - Kk) · Pk|k-1	不确定性降低

嗯，就这五个公式。说白了，就是先猜一下，再拿测量值修正一下。循环往复，越估越准。

4.2 案例一：一维位置估计

先来个简单的——估计一个物体在一维直线上的位置。

假设我们有一个小车，它在直线上运动。我们每隔一秒测量一次它的位置，但测量有噪声。我们希望用KF来平滑这些测量值，得到更准确的位置估计。

4.2.1 问题建模

• 状态量：位置 x（标量）
• 控制量：无（假设匀速，但过程有噪声）
• 测量量：位置 z（带噪声）
• 过程噪声方差 Q：0.1（模型不确定性）
• 测量噪声方差 R：1.0（传感器精度）

这里Q和R怎么选？我个人的习惯是：先根据传感器手册给个初值，然后调参。R一般可以从传感器数据方差算出来，Q就靠经验了。我曾经在一个项目中，Q设得太小，导致滤波器反应迟钝，跟踪不上快速移动的目标——嗯，那叫一个惨。

4.2.2 Python实现

import numpy as np
import matplotlib.pyplot as plt

# 一维卡尔曼滤波器类
class KalmanFilter1D:
    def __init__(self, Q=0.1, R=1.0):
        # 初始状态
        self.x = 0.0      # 状态估计
        self.P = 1.0      # 状态协方差
        self.Q = Q        # 过程噪声方差
        self.R = R        # 测量噪声方差

    def predict(self):
        # 预测步骤：状态不变（匀速模型），协方差增加
        self.P = self.P + self.Q
        # 注意：这里x不变，因为没有控制输入

    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)
true_position = 5.0                     # 真实位置
measurements = true_position + np.random.randn(50) * 2.0  # 带噪声的测量

# 运行KF
kf = KalmanFilter1D(Q=0.1, R=4.0)      # R=4.0 因为测量噪声标准差为2
estimates = []
for z in measurements:
    kf.predict()
    est = kf.update(z)
    estimates.append(est)

# 打印前10个结果对比
print("时间 | 测量值 | 估计值 | 真实值")
print("-" * 40)
for i in range(10):
    print(f"{i+1:4d} | {measurements[i]:6.2f} | {estimates[i]:6.2f} | {true_position:6.2f}")

4.2.3 结果分析

运行上面的代码，你会看到：

• 刚开始时，估计值可能偏差较大（因为初始状态设为了0）
• 但很快，估计值就会收敛到真实位置附近
• 相比原始测量值，估计值的波动小得多

为什么会这样？因为卡尔曼增益K在动态调整。刚开始P比较大，K接近1，滤波器更相信测量值；随着P减小，K也减小，滤波器开始更相信预测值。说白了，就是自适应地做加权平均。

4.3 案例二：温度滤波

位置估计搞定了，咱们再来个更贴近生活的例子——温度滤波。

假设你有一个温度传感器，每隔一秒采集一次室温。但传感器有噪声，读数跳来跳去。你想得到一个平滑的温度曲线。

4.3.1 问题建模

• 状态量：温度 T（标量）
• 过程模型：温度变化缓慢，可以认为基本不变（加上小噪声）
• 测量模型：直接测量温度，但有噪声

这里有个小技巧：Q设小一点，R设大一点。因为室温变化很慢，过程噪声应该很小；而传感器噪声可能比较大。我做过一个智能家居项目，传感器放在窗边，风吹一下读数就跳2度——那时候R就得设大些，不然滤波器根本压不住噪声。

4.3.2 Python实现

import numpy as np
import matplotlib.pyplot as plt

# 生成模拟温度数据
np.random.seed(123)
time = np.arange(0, 100, 1)
true_temp = 25.0 + 2.0 * np.sin(0.1 * time)  # 真实温度缓慢变化
measurements = true_temp + np.random.randn(100) * 1.5  # 带噪声的测量

# 初始化KF
kf_temp = KalmanFilter1D(Q=0.01, R=2.25)  # R=2.25 因为噪声标准差1.5

# 运行滤波
temp_estimates = []
for z in measurements:
    kf_temp.predict()
    est = kf_temp.update(z)
    temp_estimates.append(est)

# 打印部分结果
print("时间(s) | 测量温度 | 滤波温度 | 真实温度")
print("-" * 50)
for i in range(0, 100, 10):
    print(f"{time[i]:6.1f} | {measurements[i]:8.2f} | {temp_estimates[i]:8.2f} | {true_temp[i]:8.2f}")

4.3.3 效果对比

运行这段代码，你会发现：

• 原始测量值像心电图一样上下乱跳
• 滤波后的温度曲线平滑多了
• 而且能跟上真实温度的变化趋势

这就是KF的厉害之处——既能滤除噪声，又不滞后太多。相比之下，简单的滑动平均滤波虽然也能平滑，但会有明显的滞后。KF通过自适应调整增益，做到了平滑和响应速度的平衡。

4.4 参数调优经验

调参是KF实战中最头疼的部分。我总结了几条经验：

参数	调大	调小	我的建议
Q（过程噪声）	响应快，但噪声大	平滑，但滞后大	先设小，再逐步增大
R（测量噪声）	更相信预测，平滑	更相信测量，响应快	从传感器方差估算
P0（初始协方差）	收敛快，但初期波动大	收敛慢，但初期稳定	设大一点，让滤波器自适应

说白了，调参就是在平滑和响应速度之间找平衡。没有万能参数，只有适合你场景的参数。

4.5 本章小结

这一章我们做了两件事：

• 用一维KF实现了位置估计，理解了预测-更新的循环
• 用一维KF实现了温度滤波，体会了参数调优的重要性

代码都很短，但麻雀虽小五脏俱全。你想想看，从一维到多维，从标量到矩阵，核心思想其实是一样的——用测量值修正预测值，用不确定性指导融合权重。

下一章我们会进入多维KF的世界，到时候矩阵运算就来了。但别怕，有了这一章的基础，你已经有底气了。

---

公众号:蓝海资料掘金营，微信deep3321