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

各位同学，欢迎来到定位漂移鲁棒性解决方案的第三讲。

今天咱们聊点硬核的——卡尔曼滤波。说实话，这玩意儿在定位领域，就像炒菜离不开盐一样。我当年刚入行时，被各种噪声搞得焦头烂额，直到真正理解了卡尔曼滤波，才算是开了窍。你想想看，GPS信号飘忽不定，传感器数据时好时坏，怎么才能得到一个靠谱的位置？卡尔曼滤波就是干这个的。

3.1 状态空间模型：把问题装进数学框子里

先别急着看公式，咱们先聊聊“状态空间模型”是个啥。

说白了，就是把一个动态系统，用一组数学方程描述出来。比如你现在拿着手机走路，你的位置、速度就是“状态”。这些状态会随时间变化，对吧？

状态空间模型通常包含两个核心方程：

状态方程：描述状态怎么随时间演变。比如“当前位置 = 上一时刻位置 + 速度 × 时间差”。
观测方程：描述我们怎么测量到状态。比如“GPS报的位置 ≈ 真实位置 + 噪声”。

我习惯用数学语言来表达，这样更严谨：

状态方程：x_k = A * x_{k-1} + B * u_k + w_k
观测方程：z_k = H * x_k + v_k

这里：

x_k 是k时刻的真实状态（比如位置）
A 是状态转移矩阵（怎么从上一时刻变到这一时刻）
u_k 是控制输入（比如你主动加速）
w_k 是过程噪声（模型不完美带来的误差）
z_k 是观测值（比如GPS读数）
H 是观测矩阵（怎么从状态得到观测）
v_k 是观测噪声（传感器本身的误差）

嗯，这里要注意：w_k 和 v_k 通常假设为高斯白噪声。我在项目中遇到过，如果噪声不是高斯分布，卡尔曼滤波的效果会大打折扣。这个坑后面咱们再细聊。

3.2 预测与更新：卡尔曼滤波的两步走

卡尔曼滤波的核心思想，说白了就是“预测 + 修正”的循环。你想想看，这和我们日常做判断是不是很像？

比如你预估朋友5分钟后到地铁站（预测），然后你收到他发来的定位消息（观测），你会结合自己的预估和实际观测，得出一个更靠谱的判断（更新）。

卡尔曼滤波也是这个逻辑，分两步：

3.2.1 预测步骤（Predict）

这一步，我们用状态方程，根据上一时刻的最优估计，来预测当前时刻的状态和误差。

预测状态：x_pred = A * x_est + B * u
预测误差协方差：P_pred = A * P_est * A^T + Q

这里的 Q 是过程噪声协方差矩阵。Q 越大，说明我们对模型的信任度越低，更倾向于相信观测数据。

3.2.2 更新步骤（Update）

拿到观测值后，我们用它来修正预测结果。

卡尔曼增益：K = P_pred * H^T * (H * P_pred * H^T + R)^{-1}
更新状态：x_est = x_pred + K * (z - H * x_pred)
更新误差协方差：P_est = (I - K * H) * P_pred

这里 R 是观测噪声协方差矩阵。R 越大，说明我们对传感器越不信任，更倾向于相信模型预测。

核心理解：卡尔曼增益 K 就像一个“信任调节器”。当观测噪声小（R小）时，K变大，我们更相信观测；当过程噪声小（Q小）时，K变小，我们更相信预测。

我曾经在一个无人机定位项目里，就是因为没调好 Q 和 R 的比值，导致滤波结果还不如原始数据。后来花了整整两天调试参数，才找到合适的平衡点。所以，参数调优真的很重要。

3.3 一维卡尔曼滤波：从最简单的情况开始

咱们先别急着搞复杂的多维情况。一维卡尔曼滤波，说白了就是只考虑一个变量，比如一维空间中的位置。

假设你在一根直线上移动，我们想估计你的位置。状态向量就是位置 x，速度 v 我们暂时忽略。

状态方程简化为：

x_k = x_{k-1} + w_k

观测方程简化为：

z_k = x_k + v_k

你看，A=1，H=1，B=0。是不是简单多了？

3.4 Python实现：手把手写一个一维卡尔曼滤波器

好了，理论讲完了，咱们来点实际的。我带你用Python实现一个一维卡尔曼滤波器。

import numpy as np
import matplotlib.pyplot as plt

class KalmanFilter1D:
    def __init__(self, Q=0.01, R=0.1):
        # 初始状态
        self.x = 0.0  # 位置估计
        self.P = 1.0  # 误差协方差
        
        # 噪声协方差
        self.Q = Q    # 过程噪声
        self.R = R    # 观测噪声
        
    def predict(self):
        # 预测步骤
        self.P = self.P + self.Q
        # 状态不变（因为A=1）
        return self.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) * 0.5  # 带噪声的观测

# 运行卡尔曼滤波
kf = KalmanFilter1D(Q=0.01, R=0.25)
estimates = []

for z in measurements:
    kf.predict()
    est = kf.update(z)
    estimates.append(est)

# 可视化
plt.figure(figsize=(10, 6))
plt.plot(measurements, 'ro', markersize=4, label='观测值')
plt.plot(estimates, 'b-', linewidth=2, label='滤波估计')
plt.axhline(y=true_position, color='g', linestyle='--', label='真实位置')
plt.legend()
plt.xlabel('时间步')
plt.ylabel('位置')
plt.title('一维卡尔曼滤波效果')
plt.grid(True)
plt.show()

小提示：运行这段代码，你会看到滤波后的估计值（蓝线）比原始观测值（红点）平滑得多，而且更接近真实位置（绿线）。这就是卡尔曼滤波的魅力——去噪、平滑、逼近真实。

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

在实际项目中，Q和R的取值直接影响滤波效果。我总结了一个经验法则：

场景	Q值	R值	效果
传感器精度高，模型不准	大	小	更相信观测，响应快但可能抖动
传感器噪声大，模型准确	小	大	更相信模型，平滑但可能有延迟
两者都一般	适中	适中	折中方案，需要反复调试

避坑指南：我曾经在一个项目中，直接把Q设成0，以为模型完美。结果滤波结果完全跟不上真实变化，因为模型不可能100%准确。记住，Q不能为0，要给模型留点“容错空间”。

3.6 本章小结

好了，咱们今天聊了不少。从状态空间模型，到预测与更新的两步走，再到一维卡尔曼滤波的Python实现。你想想看，是不是感觉卡尔曼滤波没那么神秘了？

说白了，它就是一个聪明的“加权平均器”——在模型预测和实际观测之间，找到一个最优的平衡点。这个平衡点，就是卡尔曼增益K。

下一章，咱们会把这个一维的扩展到二维、三维，真正用在定位场景中。到时候你会发现，今天的这些基础，就是搭建高楼的地基。

可可精品资料库

📚 课程目录

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

3.1 状态空间模型：把问题装进数学框子里

3.2 预测与更新：卡尔曼滤波的两步走

3.2.1 预测步骤（Predict）

3.2.2 更新步骤（Update）

3.3 一维卡尔曼滤波：从最简单的情况开始

3.4 Python实现：手把手写一个一维卡尔曼滤波器

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

3.6 本章小结