第二讲：一维卡尔曼滤波实战

1. 从理论到代码：我的第一行卡尔曼滤波

说实话，我第一次接触卡尔曼滤波时，被那些矩阵公式搞得头晕。后来我发现，从一维入手，事情就简单多了。

一维卡尔曼滤波，说白了就是处理单个变量的状态估计。比如测量一个房间的温度，传感器读数总是跳来跳去，我们想知道真实温度到底是多少。这就是最经典的案例。

我当年在做一个环境监测项目时，就遇到过类似问题。传感器每秒钟采集一次温度，但数据噪声大得离谱。嗯，那时候我就意识到，光靠原始数据是不行的。

2. 一维卡尔曼滤波的核心公式

先别急着写代码。我们得把公式理清楚。一维情况下，所有矩阵都退化成标量，理解起来容易得多。

整个流程分两步：预测和更新。

预测阶段：

• 状态预测：x̂_k|k-1 = x̂_k-1|k-1（温度基本不变，所以直接拿上一刻的值）
• 误差协方差预测：P_k|k-1 = P_k-1|k-1 + Q（Q是过程噪声）

更新阶段：

• 卡尔曼增益：K_k = P_k|k-1 / (P_k|k-1 + R)（R是测量噪声）
• 状态更新：x̂_k|k = x̂_k|k-1 + K_k (z_k - x̂_k|k-1)
• 误差协方差更新：P_k|k = (1 - K_k) P_k|k-1

你想想看，卡尔曼增益K就是核心。它决定了你更相信测量值，还是更相信预测值。K越大，越相信测量；K越小，越相信预测。

3. Python实现：从零开始写一维卡尔曼滤波

好了，我们直接上代码。我会把每一步都拆开讲清楚。

import numpy as np
import matplotlib.pyplot as plt

class OneDKalmanFilter:
    def __init__(self, initial_x, initial_P, Q, R):
        self.x = initial_x      # 初始状态估计
        self.P = initial_P      # 初始误差协方差
        self.Q = Q              # 过程噪声协方差
        self.R = R              # 测量噪声协方差
        
    def predict(self):
        # 一维情况下，状态转移矩阵为1
        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

这段代码看起来简单吧？但就是这十几行，解决了无数工程问题。我建议你亲手敲一遍，不要复制粘贴。敲代码的过程，就是理解算法的过程。

4. 温度估计案例：模拟真实场景

我们来模拟一个真实的温度测量场景。假设房间真实温度是25°C，传感器有噪声，我们看看卡尔曼滤波怎么工作。

# 模拟真实温度
true_temperature = 25.0
num_steps = 100

# 生成带噪声的测量值
np.random.seed(42)
measurements = true_temperature + np.random.randn(num_steps) * 2.0

# 初始化滤波器
kf = OneDKalmanFilter(
    initial_x=20.0,    # 初始猜测
    initial_P=10.0,    # 初始不确定性
    Q=0.01,            # 过程噪声（温度变化很小）
    R=4.0              # 测量噪声（传感器方差）
)

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

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

运行这段代码，你会看到：测量值像疯了一样上下跳动，但滤波后的曲线平滑地逼近真实温度。这就是卡尔曼滤波的魔力。

5. 参数调优：Q和R的艺术

参数调优是卡尔曼滤波中最让人头疼的部分。我踩过不少坑，这里分享一些实战经验。

参数	含义	调大效果	调小效果
Q（过程噪声）	系统状态变化的剧烈程度	滤波器响应更快，但更易受噪声影响	滤波器更平滑，但可能跟不上真实变化
R（测量噪声）	传感器测量的不确定度	更相信预测值，滤波更平滑	更相信测量值，响应更快
P₀（初始协方差）	初始状态的不确定性	收敛速度慢，但更稳定	收敛速度快，但可能震荡

调参口诀：

• 如果滤波结果太毛躁，说明Q太大或R太小
• 如果滤波结果滞后严重，说明Q太小或R太大
• 如果初始阶段震荡厉害，试试把P₀调大

6. 实战技巧：让滤波器更鲁棒

在实际项目中，光有基础卡尔曼滤波是不够的。这里分享几个我常用的技巧。

技巧一：自适应调节Q和R

固定参数往往不够灵活。我习惯根据残差（测量值与预测值的差）动态调整R。残差大时，说明测量可能异常，增大R；残差小时，减小R。

def adaptive_update(self, z):
    residual = z - self.x
    # 根据残差动态调整R
    adaptive_R = self.R * (1 + abs(residual) / 2.0)
    K = self.P / (self.P + adaptive_R)
    self.x = self.x + K * residual
    self.P = (1 - K) * self.P
    return self.x

技巧二：初始化策略

初始状态怎么设？我一般取前几个测量值的平均值。初始P₀设大一些，让滤波器自己快速收敛。

技巧三：多尺度滤波

如果温度变化有快有慢，可以试试两个滤波器并行运行。一个用大Q跟踪快速变化，一个小Q跟踪慢速趋势。最后加权融合。

7. 完整代码与测试

最后，我把完整代码整理一下。你可以直接运行测试。

import numpy as np
import matplotlib.pyplot as plt

class OneDKalmanFilter:
    def __init__(self, initial_x, initial_P, Q, R):
        self.x = initial_x
        self.P = initial_P
        self.Q = Q
        self.R = R
        
    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

# 测试不同参数组合
def test_kalman(Q, R, title):
    true_temp = 25.0
    np.random.seed(42)
    measurements = true_temp + np.random.randn(100) * 2.0
    
    kf = OneDKalmanFilter(20.0, 10.0, Q, R)
    estimates = [kf.update(z) for z in measurements]
    
    mse = np.mean((np.array(estimates) - true_temp)**2)
    print(f"{title}: MSE = {mse:.4f}")

test_kalman(0.01, 4.0, "默认参数")
test_kalman(0.1, 4.0, "大Q")
test_kalman(0.01, 1.0, "小R")
test_kalman(0.1, 1.0, "大Q小R")

运行这段测试代码，你会看到不同参数组合下的均方误差（MSE）。默认参数通常表现最好，但具体场景需要具体调整。

下一讲，我们会把一维扩展到二维，处理位置和速度的联合估计。到时候你会发现，矩阵形式虽然看起来复杂，但本质和一维完全一样。