4. 卡尔曼滤波入门：状态空间模型、预测与更新步骤、一维卡尔曼滤波实现
说到定位漂移，很多同学第一反应就是上卡尔曼滤波。嗯，这确实是个经典解法。但我见过不少项目，卡尔曼滤波的参数调得不对，反而让轨迹变得更奇怪了。今天我们就从最基础的一维场景开始，把卡尔曼滤波的底裤扒干净。
4.1 状态空间模型：你在跟踪什么？
卡尔曼滤波的核心，是建立一个数学模型来描述系统怎么变化。这个模型就叫「状态空间模型」。说白了，就是回答两个问题：
- 状态向量：你想知道什么？比如位置、速度。
- 观测向量：你能测量到什么？比如GPS给出的经纬度。
我习惯把状态空间模型写成下面这样：
状态方程： x_k = A * x_{k-1} + B * u_k + w_k
观测方程： z_k = H * x_k + v_k这里：
x_k是k时刻的真实状态（我们永远不知道，只能估计）A是状态转移矩阵，描述上一时刻的状态怎么变到这一时刻B是控制输入矩阵，u_k是外部控制量（比如加速度）w_k是过程噪声，代表模型的不确定性z_k是观测值，H是观测矩阵v_k是观测噪声，代表传感器的误差
关键点：w_k 和 v_k 都假设是高斯白噪声，均值为0，协方差分别为 Q 和 R。这两个协方差矩阵，就是你要调的参数。
举个例子。一维情况下，如果你只关心位置，状态向量就是 x = [位置]。如果你还关心速度，状态向量就是 x = [位置, 速度]^T。我在做室内定位时，通常会把速度和加速度都加进去，效果会好很多。
4.2 预测与更新：卡尔曼滤波的两步走
卡尔曼滤波就两个步骤，循环往复：预测和更新。我当年第一次看公式时觉得好复杂，后来发现其实就是「猜」和「改」。
4.2.1 预测步骤（Time Update）
这一步是根据上一时刻的状态，猜一下当前时刻的状态。公式如下：
1. 状态预测： x̂_k|k-1 = A * x̂_{k-1|k-1} + B * u_k
2. 协方差预测： P_k|k-1 = A * P_{k-1|k-1} * A^T + Q这里：
x̂_k|k-1表示用k-1时刻的信息预测k时刻的状态P_k|k-1是预测状态的协方差，代表这次猜测的不确定性Q是过程噪声协方差，模型越不准，Q就设得越大
我的经验：Q值设得太小，滤波结果会过于相信模型，导致响应慢；设得太大，又容易跟着噪声跑。我一般先设一个中等值，然后看轨迹的平滑程度再微调。
4.2.2 更新步骤（Measurement Update）
拿到新的观测值后，我们要用观测值来修正之前的猜测。公式如下：
1. 卡尔曼增益： K_k = P_k|k-1 * H^T * (H * P_k|k-1 * H^T + R)^{-1}
2. 状态更新： x̂_k|k = x̂_k|k-1 + K_k * (z_k - H * x̂_k|k-1)
3. 协方差更新： P_k|k = (I - K_k * H) * P_k|k-1卡尔曼增益 K_k 是个关键角色。它决定了你更相信预测值还是观测值：
- 如果观测噪声R很小（传感器很准），K_k会变大，更相信观测值
- 如果过程噪声Q很小（模型很准），K_k会变小，更相信预测值
为什么会这样？你想想看，当传感器很准的时候，观测值的权重自然应该大一些。反之，如果模型很准，那就应该多依赖模型。
4.3 一维卡尔曼滤波实现：手写一个最简单的版本
理论说完了，我们来写代码。一维卡尔曼滤波是最简单的场景——只跟踪一个变量，比如一维位置。状态向量就是 x = [位置]，状态转移矩阵 A = [1]，观测矩阵 H = [1]。
下面是我写的一个完整实现：
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.A = 1.0 # 状态转移矩阵
self.H = 1.0 # 观测矩阵
self.Q = Q # 过程噪声协方差
self.R = R # 观测噪声协方差
def predict(self):
# 预测步骤
self.x = self.A * self.x
self.P = self.A * self.P * self.A + self.Q
return self.x, self.P
def update(self, z):
# 更新步骤
# 计算卡尔曼增益
K = self.P * self.H / (self.H * self.P * self.H + self.R)
# 更新状态估计
self.x = self.x + K * (z - self.H * self.x)
# 更新协方差
self.P = (1 - K * self.H) * self.P
return self.x, self.P, K
# 模拟数据：真实位置 + 噪声观测
np.random.seed(42)
true_positions = np.sin(np.linspace(0, 10, 100)) # 真实轨迹
measurements = true_positions + np.random.normal(0, 0.3, 100) # 带噪声的观测
# 运行卡尔曼滤波
kf = KalmanFilter1D(Q=0.01, R=0.1)
filtered_positions = []
for z in measurements:
kf.predict()
x_est, _, _ = kf.update(z)
filtered_positions.append(x_est)
# 绘制结果
plt.figure(figsize=(12, 6))
plt.plot(true_positions, 'g-', label='真实位置', linewidth=2)
plt.plot(measurements, 'r.', label='观测值', alpha=0.5)
plt.plot(filtered_positions, 'b-', label='滤波结果', linewidth=2)
plt.legend()
plt.xlabel('时间步')
plt.ylabel('位置')
plt.title('一维卡尔曼滤波效果')
plt.grid(True)
plt.show()避坑指南：我曾经在项目中直接把Q和R设成0，结果滤波结果完全不收敛。记住，Q和R不能为0，否则协方差矩阵会变成奇异矩阵，卡尔曼增益计算会出问题。
4.4 参数调优：Q和R怎么设？
调参是卡尔曼滤波最头疼的部分。我总结了一个经验表格：
| 参数 | 物理含义 | 设得太大 | 设得太小 |
|---|---|---|---|
| Q（过程噪声） | 模型的不确定性 | 滤波结果抖动大，跟随噪声 | 响应慢，滞后严重 |
| R（观测噪声） | 传感器的误差 | 滤波结果平滑但滞后 | 滤波结果接近原始观测，噪声大 |
我的调参习惯是：先根据传感器的数据手册估算R，然后通过实验调整Q。比如GPS的R值，我一般设为5-10米（取决于信号质量）。Q值则从0.01开始，看轨迹的平滑程度逐步调整。
4.5 本章知识体系
下面这张图总结了一维卡尔曼滤波的核心逻辑：
这张图把整个流程串起来了。你从初始状态开始，先做预测，然后拿观测值做更新，输出结果，再回到预测步骤，周而复始。每次循环，卡尔曼增益都会根据当前的协方差自动调整权重。
核心要点：一维卡尔曼滤波虽然简单，但它是理解多维卡尔曼滤波的基础。把一维的Q、R、P、K这些概念吃透了，后面扩展到二维、三维就水到渠成。
好了，这一章的内容就到这里。代码可以直接复制运行，看看效果。下一章我们会把一维扩展到二维，处理真正的定位数据——到时候你会看到，卡尔曼滤波在GPS漂移抑制上有多大的威力。