用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_std

1.2 卡尔曼滤波核心思想

卡尔曼滤波本质上是在做两件事：

1. 预测：基于系统模型预测下一个状态
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.state

2.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.state

3.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.state

4.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  # 增加测量噪声