用Python从零实现卡尔曼滤波：传感器数据融合实战指南
卡尔曼滤波就像一位经验丰富的航海家，在充满噪声的数据海洋中为你指引方向。想象一下，当GPS信号在城市峡谷中漂移，IMU的加速度计因车辆震动而失真时，如何获得可靠的定位？这正是卡尔曼滤波大显身手的时刻。
1. 环境准备与基础概念
1.1 工具链配置
工欲善其事，必先利其器。我们需要以下Python包：
import numpy as np import matplotlib.pyplot as plt from scipy.linalg import inv对于传感器数据模拟，可以创建一个简单的虚拟传感器类：
class VirtualSensor: def __init__(self, true_value, noise_std): self.true_value = true_value self.noise_std = noise_std def read(self): return self.true_value + np.random.randn() * self.noise_std1.2 卡尔曼滤波核心思想
卡尔曼滤波本质上是在做两件事：
- 预测：基于系统模型预测下一个状态
- 更新：用实际测量值修正预测
这种"预测-修正"的循环，就像不断调整瞄准镜的狙击手，每次射击后都根据弹着点修正下一次的瞄准。
2. 一维卡尔曼滤波实现
2.1 基础实现
让我们从最简单的一维情况开始——估计一个恒定值。假设我们要测量室温，温度计有±0.5°C的误差。
class SimpleKalmanFilter: def __init__(self, initial_state, process_variance, measurement_variance): self.state = initial_state self.process_variance = process_variance self.measurement_variance = measurement_variance self.estimate_variance = 1.0 # 初始估计方差 def update(self, measurement): # 预测步骤 predicted_variance = self.estimate_variance + self.process_variance # 更新步骤 kalman_gain = predicted_variance / (predicted_variance + self.measurement_variance) self.state = self.state + kalman_gain * (measurement - self.state) self.estimate_variance = (1 - kalman_gain) * predicted_variance return self.state2.2 实际测试
让我们模拟一个温度测量场景：
true_temp = 25.0 sensor = VirtualSensor(true_temp, 0.5) kf = SimpleKalmanFilter(20.0, 0.01, 0.25) measurements = [] estimates = [] for _ in range(100): z = sensor.read() x = kf.update(z) measurements.append(z) estimates.append(x)绘制结果对比：
plt.figure(figsize=(10,6)) plt.plot(measurements, 'r.', label='Measurements') plt.plot(estimates, 'b-', label='Kalman Filter') plt.axhline(true_temp, color='g', linestyle='--', label='True Value') plt.legend() plt.show()3. 多维卡尔曼滤波：位置与速度追踪
3.1 状态空间模型
现在我们来处理更实际的场景——同时估计位置和速度。状态向量包含位置和速度：
x = [position] [velocity]状态转移矩阵A和观测矩阵H定义为：
dt = 0.1 # 时间步长 A = np.array([[1, dt], [0, 1]]) H = np.array([[1, 0]]) # 只能观测到位置3.2 完整实现
class MultiKalmanFilter: def __init__(self, initial_state, initial_covariance, process_noise, measurement_noise): self.state = initial_state self.covariance = initial_covariance self.Q = process_noise # 过程噪声协方差 self.R = measurement_noise # 测量噪声协方差 def predict(self): self.state = A @ self.state self.covariance = A @ self.covariance @ A.T + self.Q return self.state def update(self, measurement): K = self.covariance @ H.T @ inv(H @ self.covariance @ H.T + self.R) self.state = self.state + K @ (measurement - H @ self.state) self.covariance = (np.eye(2) - K @ H) @ self.covariance return self.state3.3 运动物体跟踪示例
模拟一个匀加速运动的物体：
# 初始化 true_pos = 0 true_vel = 1 accel = 0.1 kf = MultiKalmanFilter( initial_state=np.array([0, 1]).reshape(-1,1), initial_covariance=np.eye(2), process_noise=np.array([[0.01, 0], [0, 0.01]]), measurement_noise=np.array([[0.1]]) ) positions = [] estimates = [] for t in range(100): # 真实运动 true_pos += true_vel * dt true_vel += accel * dt # 模拟测量 z = true_pos + np.random.randn() * 0.5 # 卡尔曼滤波 kf.predict() x = kf.update(z) positions.append(true_pos) estimates.append(x[0,0])4. 传感器数据融合实战
4.1 IMU与GPS融合
现在我们来解决实际问题：融合IMU加速度计和GPS数据。IMU提供高频但会漂移的加速度数据，GPS提供低频但绝对的位置参考。
状态向量扩展为：
x = [position] [velocity] [acceleration]状态转移矩阵需要相应调整：
dt = 0.1 A = np.array([ [1, dt, 0.5*dt**2], [0, 1, dt], [0, 0, 1] ])4.2 实现细节
class SensorFusionKalman: def __init__(self): self.state = np.zeros((3,1)) self.covariance = np.eye(3) self.Q = np.diag([0.01, 0.01, 0.1]) # 过程噪声 self.R_gps = np.array([[1.0]]) # GPS噪声 self.R_imu = np.array([[0.1]]) # IMU噪声 def predict(self, accel_input, dt): # 更新状态转移矩阵 A = np.array([ [1, dt, 0.5*dt**2], [0, 1, dt], [0, 0, 1] ]) B = np.array([[0], [0], [1]]) self.state = A @ self.state + B * accel_input self.covariance = A @ self.covariance @ A.T + self.Q return self.state def update_gps(self, position): H = np.array([[1, 0, 0]]) K = self.covariance @ H.T @ inv(H @ self.covariance @ H.T + self.R_gps) self.state = self.state + K @ (position - H @ self.state) self.covariance = (np.eye(3) - K @ H) @ self.covariance return self.state def update_imu(self, accel): H = np.array([[0, 0, 1]]) K = self.covariance @ H.T @ inv(H @ self.covariance @ H.T + self.R_imu) self.state = self.state + K @ (accel - H @ self.state) self.covariance = (np.eye(3) - K @ H) @ self.covariance return self.state4.3 调参经验
卡尔曼滤波的性能很大程度上取决于Q和R的选择：
过程噪声Q：表示你对模型的信任程度
- Q越大，滤波器对测量的响应越快
- Q越小，滤波器越相信模型预测
测量噪声R：表示你对传感器的信任程度
- R越大，滤波器对测量的权重越低
- R越小，滤波器越相信测量值
一个实用的调试方法是：
# 初始猜测 Q = np.diag([0.1, 0.1, 1.0]) R = np.array([[1.0]]) # 根据实际表现调整 if filter_response_too_slow: Q *= 2 # 增加过程噪声 elif filter_too_jittery: R *= 2 # 增加测量噪声
