用Python从零实现一个卡尔曼滤波器（附完整代码与可视化）

卡尔曼滤波器就像一位经验丰富的导航员，在充满噪声的数据海洋中为你指引方向。想象一下，你正在开发一个无人机追踪系统，GPS信号时强时弱，传感器读数飘忽不定——这正是卡尔曼滤波大显身手的场景。本文将带你用Python从零构建一个完整的卡尔曼滤波器，通过可视化追踪匀速运动的小球，直观理解这个神奇算法的运作机制。

1. 环境准备与问题建模

首先确保你的Python环境已安装以下库：

pip install numpy matplotlib

我们将模拟一个经典的一维匀速运动场景。假设小球以恒定速度移动，但由于空气阻力等因素，实际运动存在微小扰动。同时，我们的观测设备也存在测量误差。这正是卡尔曼滤波要解决的双重噪声问题。

定义状态向量为位置和速度：

state_dim = 2  # [位置, 速度]

系统动力学矩阵A表示状态转移关系。对于匀速运动模型：

dt = 0.1  # 时间步长
A = np.array([[1, dt],
              [0, 1]])  # 状态转移矩阵

2. 噪声与不确定性建模

现实世界存在两种关键噪声：

• 过程噪声（系统扰动）：用Q矩阵表示
• 观测噪声（测量误差）：用R矩阵表示

# 过程噪声协方差
process_noise = 0.01
Q = np.array([[dt**4/4, dt**3/2],
              [dt**3/2, dt**2]]) * process_noise

# 观测噪声协方差
measure_noise = 0.1
R = np.array([[measure_noise]])  # 仅观测位置

提示：噪声参数需要根据实际系统调整，过大会导致响应迟钝，过小则滤波效果不佳。

3. 卡尔曼滤波器核心实现

完整的卡尔曼滤波类实现如下：

class KalmanFilter:
    def __init__(self, A, H, Q, R):
        self.A = A  # 状态转移矩阵
        self.H = H  # 观测矩阵
        self.Q = Q  # 过程噪声
        self.R = R  # 观测噪声
        self.P = np.eye(A.shape[0])  # 误差协方差
        self.x = np.zeros((A.shape[0],1))  # 初始状态
        
    def predict(self):
        self.x = self.A @ self.x
        self.P = self.A @ self.P @ self.A.T + self.Q
        return self.x
    
    def update(self, z):
        K = self.P @ self.H.T @ np.linalg.inv(self.H @ self.P @ self.H.T + self.R)
        self.x = self.x + K @ (z - self.H @ self.x)
        self.P = (np.eye(self.P.shape[0]) - K @ self.H) @ self.P
        return self.x

关键参数说明：

参数	类型	描述
A	ndarray	状态转移矩阵
H	ndarray	观测矩阵
Q	ndarray	过程噪声协方差
R	ndarray	观测噪声协方差
P	ndarray	误差协方差矩阵
x	ndarray	当前状态估计

4. 完整仿真与可视化

让我们生成模拟数据并运行滤波器：

# 生成真实轨迹
true_pos = np.cumsum(0.5 * np.ones(100))  # 匀速运动
true_vel = 0.5 * np.ones(100)
true_states = np.vstack([true_pos, true_vel])

# 添加过程噪声
process_noise = np.random.multivariate_normal(
    mean=[0,0], cov=Q, size=100).T
true_states += process_noise

# 生成带噪声的观测
H = np.array([[1, 0]])  # 仅观测位置
obs_noise = np.random.normal(0, np.sqrt(R[0,0]), 100)
observations = (H @ true_states).flatten() + obs_noise

可视化结果对比：

plt.figure(figsize=(12,6))
plt.plot(true_states[0], label='真实位置', linestyle='--')
plt.scatter(range(100), observations, 
           label='观测值', marker='x', alpha=0.5)
plt.plot(estimated[:,0], label='滤波估计', linewidth=2)
plt.legend()
plt.title('卡尔曼滤波效果对比')
plt.xlabel('时间步')
plt.ylabel('位置')
plt.show()

典型输出结果会显示：

• 真实轨迹（平滑直线）
• 噪声观测（分散的点）
• 滤波结果（紧贴真实轨迹的平滑曲线）

5. 参数调优实战技巧

卡尔曼滤波性能高度依赖参数设置，以下是调优指南：

1. 初始误差协方差P0：

   P0 = np.diag([1.0, 1.0])  # 适中初始值
   

2. 过程噪声Q调优步骤：

   • 从较小值开始（如1e-4）
   • 逐渐增大直到滤波器响应速度合适
   • 观察残差（预测值与观测值差）应呈白噪声

3. 观测噪声R设置原则：

   • 通常取传感器标称精度
   • 可通过样本方差计算：

   R = np.var(sensor_calibration_data)
   

注意：过大的Q会导致滤波器过于信任观测，过小则反应迟钝。

6. 扩展应用：二维空间追踪

将模型扩展到二维空间只需调整矩阵维度：

# 状态向量：[x, x_vel, y, y_vel]
A_2d = np.block([[A, np.zeros((2,2))],
                 [np.zeros((2,2)), A]])

# 观测矩阵 - 仅观测x,y位置
H_2d = np.array([[1,0,0,0],
                [0,0,1,0]])

典型应用场景包括：

• 无人机位置追踪
• 自动驾驶车辆感知
• 体育赛事中的运动员跟踪

7. 常见问题排查

当滤波器表现异常时，检查以下方面：

1. 矩阵维度不匹配：

   • 确保所有矩阵运算维度一致
   • 使用assert A.shape == (n,n)进行验证

2. 非正定协方差矩阵：

   • 添加小量单位矩阵保证数值稳定性

   P = (P + P.T) * 0.5 + 1e-6 * np.eye(P.shape[0])
   

3. 发散问题处理：

   • 检查系统是否满足可观测性条件
   • 尝试增大过程噪声Q

在机器人项目中，我发现当传感器突然失效时，临时增大R矩阵可以防止滤波器过度依赖异常观测值。例如当GPS信号丢失时，可以将R提高10倍，直到信号恢复。