【工程师学算法】工程常用算法（二）—— 卡尔曼滤波(Kalman Filter)实战：从传感器融合到状态估计-平芜编程栈

1. 卡尔曼滤波：从理论到实战

第一次接触卡尔曼滤波是在调试无人机飞控系统时。当时遇到一个棘手问题：GPS定位数据更新频率低（约10Hz），而IMU（惯性测量单元）数据虽然高频（100Hz）但存在累积误差。单独使用任一种传感器都无法满足飞行稳定性要求。这时导师建议我试试卡尔曼滤波——这个诞生于1960年代却至今仍在自动驾驶、机器人导航等领域广泛使用的算法。

卡尔曼滤波本质上是一种最优估计算法，它通过融合多源传感器数据，在系统存在不确定性的情况下，给出状态的最优估计。就像经验丰富的船长在浓雾中航行时，会综合雷达、声呐和航海图的信息来判断船只位置。卡尔曼滤波的独特优势在于：

实时性：只需前一时刻的估计值和当前测量值即可迭代计算
自适应性：根据传感器可靠性动态调整权重（更信任误差小的传感器）
多维处理：天然支持多变量系统的状态估计

2. 核心原理：五步拆解卡尔曼滤波

2.1 状态预测：建立系统模型

假设我们要跟踪一辆匀速运动的小车。状态向量可以表示为位置和速度：

state = np.array([[position], [velocity]]) # 形状(2,1)

状态转移矩阵F描述物理规律：

F = np.array([[1, dt], [0, 1]]) # dt为时间间隔

预测方程为：

predicted_state = F @ current_state

2.2 不确定性传播：误差协方差预测

每个估计都有不确定性，用协方差矩阵P表示。过程噪声Q表示模型不完美带来的误差：

Q = np.diag([0.1, 0.01]) # 通常通过实验调试 predicted_P = F @ current_P @ F.T + Q

2.3 卡尔曼增益计算：动态权重调节

这是算法的核心魔法。以GPS（R=1）和IMU（R=0.1）为例：

H = np.eye(2) # 观测矩阵 K = predicted_P @ H.T @ np.linalg.inv(H @ predicted_P @ H.T + R)

R越小（传感器越可靠），K越大，测量值权重越高。

2.4 状态更新：融合预测与测量

measurement = np.array([[gps_position], [imu_velocity]]) updated_state = predicted_state + K @ (measurement - H @ predicted_state)

2.5 协方差更新：误差修正

I = np.eye(2) updated_P = (I - K @ H) @ predicted_P

3. 实战：无人机位置估计

3.1 传感器配置

GPS：精度1m，频率10Hz
IMU：加速度计误差0.1m/s²，陀螺仪漂移1°/s

3.2 Python实现

class KalmanFilter: def __init__(self, dt): self.dt = dt self.F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]]) # 状态转移矩阵 self.H = np.eye(4) # 观测矩阵 self.Q = np.diag([0.1, 0.01, 0.1, 0.01]) # 过程噪声 self.R_gps = np.diag([1, 0.5]) # GPS噪声 self.R_imu = np.diag([0.1, 0.05]) # IMU噪声 self.P = np.eye(4) # 初始协方差 self.state = np.zeros((4,1)) # [x, vx, y, vy] def predict(self): self.state = self.F @ self.state self.P = self.F @ self.P @ self.F.T + self.Q return self.state def update(self, z, sensor_type): R = self.R_gps if sensor_type == 'gps' else self.R_imu K = self.P @ self.H.T @ np.linalg.inv(self.H @ self.P @ self.H.T + R) self.state = self.state + K @ (z - self.H @ self.state) self.P = (np.eye(4) - K @ self.H) @ self.P

3.3 调参经验

Q矩阵：过小会导致滤波器反应迟钝，过大会使输出抖动。建议从对角线元素0.01开始调试
R矩阵：应与传感器实际误差匹配。可通过静态测试测量传感器噪声方差
初始状态：错误的初值会导致收敛慢，可用首次测量值初始化

4. 工程中的常见陷阱与解决方案

4.1 非线性系统处理

当系统存在非线性时（如无人机姿态估计），需使用扩展卡尔曼滤波(EKF)：

# 以角度估计为例 def jacobian_F(x): theta = x[2,0] return np.array([ [1, 0, -v*np.sin(theta)*dt], [0, 1, v*np.cos(theta)*dt], [0, 0, 1]])

4.2 传感器异步问题

不同传感器数据到达时间不同步的解决方案：

def handle_sensor_data(data, timestamp): while buffer.has_older_data(timestamp): old_data = buffer.pop() kf.predict() kf.update(old_data) buffer.push(data)