用Python手把手实现一个卡尔曼滤波器（附完整代码），从传感器数据中预测行人位置

卡尔曼滤波算法在自动驾驶和机器人领域有着广泛的应用，它能有效地从带有噪声的传感器数据中提取出真实的状态信息。本文将带你从零开始实现一个完整的线性卡尔曼滤波器，并应用于行人位置预测的实际场景。

1. 卡尔曼滤波基础与核心思想

想象一下你在一个嘈杂的商场里，试图通过手机GPS定位朋友的位置。GPS数据会有误差，而你的朋友也在移动。卡尔曼滤波就像一位聪明的助手，它能结合你对朋友移动速度的估计和GPS的测量值，给出更准确的位置预测。

卡尔曼滤波的核心在于预测-更新两个阶段的循环：

1. 预测阶段：根据系统模型预测当前状态
2. 更新阶段：用新的测量值修正预测

这种递归算法只需要前一时刻的状态估计和当前的测量值，就能持续优化对系统状态的估计，非常适合实时处理传感器数据。

关键数学概念：

• 状态向量（x）：包含我们要估计的所有变量（如位置、速度）
• 状态转移矩阵（F）：描述系统如何从上一状态演化到当前状态
• 观测矩阵（H）：描述如何从状态得到观测值
• 过程噪声协方差（Q）：系统模型的不确定性
• 观测噪声协方差（R）：测量误差的协方差

2. 行人跟踪问题建模

我们以行人跟踪为例，建立一个二维平面上的运动模型。假设我们可以通过激光雷达或摄像头获取行人的位置信息，但这些测量值带有噪声。

状态向量设计：

state_vector = [px, py, vx, vy]  # 位置(x,y)和速度(x,y方向)

状态转移模型（恒定速度模型）：

F = np.array([[1, 0, dt, 0],
              [0, 1, 0, dt],
              [0, 0, 1, 0],
              [0, 0, 0, 1]])

其中dt是时间步长，这个模型假设行人在dt时间内保持匀速运动。

观测模型： 假设我们只能直接测量位置，不能直接测量速度：

H = np.array([[1, 0, 0, 0],
              [0, 1, 0, 0]])

3. Python实现卡尔曼滤波器类

下面我们实现一个完整的卡尔曼滤波器类：

import numpy as np

class KalmanFilter:
    def __init__(self, F, H, Q, R):
        """
        初始化卡尔曼滤波器
        
        参数:
        F -- 状态转移矩阵
        H -- 观测矩阵
        Q -- 过程噪声协方差
        R -- 观测噪声协方差
        """
        self.F = F  # 状态转移矩阵
        self.H = H  # 观测矩阵
        self.Q = Q  # 过程噪声协方差
        self.R = R  # 观测噪声协方差
        self.P = np.eye(F.shape[0])  # 状态协方差矩阵，初始化为单位矩阵
        self.x = np.zeros((F.shape[0], 1))  # 状态向量
        
    def predict(self):
        """预测阶段"""
        self.x = self.F @ self.x
        self.P = self.F @ self.P @ self.F.T + self.Q
        return self.x
    
    def update(self, z):
        """
        更新阶段
        
        参数:
        z -- 观测值
        """
        y = z - self.H @ self.x  # 测量残差
        S = self.H @ self.P @ self.H.T + self.R  # 残差协方差
        K = self.P @ self.H.T @ np.linalg.inv(S)  # 卡尔曼增益
        
        self.x = self.x + K @ y
        I = np.eye(self.P.shape[0])
        self.P = (I - K @ self.H) @ self.P
        
        return self.x

4. 参数调优与实现细节

卡尔曼滤波器的性能很大程度上取决于Q和R矩阵的设置。这些参数需要根据具体应用场景进行调整。

过程噪声协方差Q：

dt = 0.1  # 时间步长
G = np.array([[0.5*dt**2], [0.5*dt**2], [dt], [dt]])  # 噪声传播矩阵
sigma_v = 0.5  # 行人最大加速度(m/s^2)
Q = G @ G.T * sigma_v**2  # 过程噪声协方差

观测噪声协方差R：

position_noise = 0.1  # 位置测量噪声标准差(m)
R = np.array([[position_noise**2, 0],
              [0, position_noise**2]])  # 观测噪声协方差

初始化注意事项：

• 初始状态x可以设为第一个观测值（速度设为0）
• 初始协方差P可以设为一个较大的对角矩阵，表示初始不确定性较高

5. 完整示例：行人轨迹预测

让我们模拟一个行人从(0,0)点出发，以(1,0.5)m/s的速度移动的场景，并加入噪声模拟真实传感器数据：

import matplotlib.pyplot as plt

# 参数设置
dt = 0.1  # 时间步长
total_time = 10  # 总时间(s)
steps = int(total_time / dt)

# 真实轨迹（匀速直线运动）
true_trajectory = np.zeros((steps, 2))
true_velocity = np.array([1.0, 0.5])  # m/s

for t in range(1, steps):
    true_trajectory[t] = true_trajectory[t-1] + true_velocity * dt

# 生成带噪声的观测
np.random.seed(42)
noise_std = 0.5  # 观测噪声标准差
observations = true_trajectory + np.random.normal(0, noise_std, (steps, 2))

# 初始化卡尔曼滤波器
F = np.array([[1, 0, dt, 0],
              [0, 1, 0, dt],
              [0, 0, 1, 0],
              [0, 0, 0, 1]])

H = np.array([[1, 0, 0, 0],
              [0, 1, 0, 0]])

# 过程噪声和观测噪声
sigma_v = 0.5  # 最大加速度(m/s^2)
G = np.array([[0.5*dt**2], [0.5*dt**2], [dt], [dt]])
Q = G @ G.T * sigma_v**2

R = np.array([[noise_std**2, 0],
              [0, noise_std**2]])

kf = KalmanFilter(F, H, Q, R)

# 初始状态设为第一个观测值，速度设为0
initial_state = np.array([[observations[0,0]], 
                         [observations[0,1]], 
                         [0], 
                         [0]])
kf.x = initial_state

# 存储滤波结果
filtered_trajectory = np.zeros((steps, 2))

for t in range(steps):
    # 预测
    kf.predict()
    
    # 更新
    z = observations[t].reshape(2,1)
    kf.update(z)
    
    # 存储结果
    filtered_trajectory[t] = kf.x[:2].flatten()

# 可视化结果
plt.figure(figsize=(10,6))
plt.plot(true_trajectory[:,0], true_trajectory[:,1], 'g-', label='真实轨迹')
plt.plot(observations[:,0], observations[:,1], 'rx', label='观测值', markersize=4)
plt.plot(filtered_trajectory[:,0], filtered_trajectory[:,1], 'b-', label='滤波结果')
plt.legend()
plt.xlabel('X位置 (m)')
plt.ylabel('Y位置 (m)')
plt.title('卡尔曼滤波行人轨迹预测')
plt.grid(True)
plt.show()

6. 性能评估与调优技巧

从可视化结果可以看到，卡尔曼滤波能有效平滑噪声观测值，得到接近真实轨迹的估计。为了量化性能，我们可以计算均方根误差(RMSE)：

def rmse(predictions, targets):
    return np.sqrt(((predictions - targets) ** 2).mean())

obs_error = rmse(observations, true_trajectory)
filter_error = rmse(filtered_trajectory, true_trajectory)

print(f"观测误差RMSE: {obs_error:.3f} m")
print(f"滤波后误差RMSE: {filter_error:.3f} m")

调优技巧：

1. Q矩阵调整：如果滤波器响应太慢（跟不上真实变化），增大Q；如果结果波动太大，减小Q
2. R矩阵调整：根据传感器实际精度设置，可通过传感器规格或实验测量获得
3. 初始状态设置：不确定时可设较大的初始协方差P
4. 非线性系统：对于转弯等非线性运动，考虑使用扩展卡尔曼滤波(EKF)或无迹卡尔曼滤波(UKF)

常见问题排查：

• 发散问题：检查矩阵是否正定，数值是否稳定
• 性能不佳：验证系统模型是否合理，参数是否合适
• 实时性问题：优化矩阵运算，考虑使用预计算

7. 实际应用中的扩展与优化

在实际自动驾驶系统中，卡尔曼滤波的应用会更加复杂：

多传感器融合：

# 伪代码示例：融合激光雷达和摄像头数据
def update_with_multiple_sensors(kf, lidar_z, camera_z):
    # 激光雷达更新
    kf.update(lidar_z)
    
    # 摄像头更新（可能有不同的H矩阵）
    H_cam = get_camera_H_matrix()
    y = camera_z - H_cam @ kf.x
    S = H_cam @ kf.P @ H_cam.T + R_cam
    K = kf.P @ H_cam.T @ np.linalg.inv(S)
    
    kf.x = kf.x + K @ y
    I = np.eye(kf.P.shape[0])
    kf.P = (I - K @ H_cam) @ kf.P

自适应滤波：

• 根据运动状态动态调整Q矩阵（如检测到转弯时增加过程噪声）
• 根据传感器置信度动态调整R矩阵

工程优化：

• 使用矩阵对称性优化计算
• 预计算不变部分
• 并行化处理多个跟踪目标

卡尔曼滤波在自动驾驶中不仅用于行人跟踪，还可应用于：

• 车辆自身定位
• 其他车辆轨迹预测
• 交通标志位置估计
• 传感器标定与校准