（六）卡尔曼滤波（KALMAN）：从理论推导到代码实现的完整闭环

1. 卡尔曼滤波的直观理解

想象你正在玩一个蒙眼走迷宫的游戏。你每走一步都会根据记忆和方向感预测自己的位置（预测值），同时朋友会通过无线电告诉你传感器检测到的位置（测量值）。但这两个信息都不完全准确：你的预测会随着步数增加累积误差，朋友的测量也可能受信号干扰。卡尔曼滤波就像个聪明的裁判，它会根据两者的可信度动态调整权重——当你的方向感靠谱时更相信预测，当传感器信号清晰时更相信测量。

这种动态加权机制的核心在于量化不确定性。我们用协方差矩阵P表示预测的误差范围，R表示测量噪声强度。当P的数值较小时（预测可靠），滤波器会赋予预测更高权重；当R较小时（测量精确），则更信任测量数据。这种"看菜下饭"的调整策略，正是卡尔曼滤波能在导航、自动驾驶、机器人定位等领域大显身手的关键。

2. 数学建模与五大核心方程

2.1 状态空间模型

卡尔曼滤波建立在线性系统的基础上，需要两个关键方程：

状态方程（预测模型）：

x_k = F * x_{k-1} + B * u_k + w_k

其中F是状态转移矩阵（如运动学公式中的速度/加速度关系），B是控制矩阵，u_k是控制输入（如油门/刹车指令），w_k是过程噪声（符合N(0,Q)分布）。

观测方程（测量模型）：

z_k = H * x_k + v_k

H是观测矩阵（如GPS坐标到地图坐标的转换），v_k是观测噪声（符合N(0,R)分布）。在目标追踪中，H可能就是从状态空间（位置+速度）到观测空间（仅位置）的投影矩阵。

2.2 五大黄金公式

预测阶段：

# 状态预测 x_prior = F @ x_last + B @ u # 协方差预测 P_prior = F @ P_last @ F.T + Q

更新阶段：

# 卡尔曼增益计算 K = P_prior @ H.T @ np.linalg.inv(H @ P_prior @ H.T + R) # 状态更新 x_posterior = x_prior + K @ (z - H @ x_prior) # 协方差更新 P_posterior = (I - K @ H) @ P_prior

以无人机定位为例，当GPS信号突然消失（R增大），卡尔曼增益K会自动减小，使系统更依赖惯性导航的预测结果；当GPS信号恢复，则重新增加测量权重。

3. 关键参数初始化技巧

3.1 噪声协方差矩阵

过程噪声Q：

Q = np.diag([0.1, 0.1, 0.01]) # 位置噪声0.1，速度噪声0.01

这表示系统对运动模型的不信任程度。在车辆追踪中，急转弯时应该增大Q值，因为匀速模型假设不再成立。

观测噪声R：

R = np.diag([1.0, 1.0]) # GPS测量误差1米

可以通过传感器标定实验获得。RTKLIB中常见将伪距噪声设为0.3m，载波相位噪声设为0.003m。

3.2 状态转移矩阵F

对于匀速运动模型：

dt = 0.1 # 采样间隔100ms F = np.array([[1, dt, 0, 0], [0, 1, 0, 0], [0, 0, 1, dt], [0, 0, 0, 1]])

当处理高机动目标时，需要引入加速度项，此时F矩阵会扩展为6维（位置+速度+加速度）。

4. 从公式到代码实现

4.1 Python实现核心类

class KalmanFilter: def __init__(self, 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, u=None): self.x = self.F @ self.x if u is not None: self.x += self.B @ u self.P = self.F @ self.P @ self.F.T + self.Q return self.x def update(self, 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 += K @ y self.P = (np.eye(self.P.shape[0]) - K @ self.H) @ self.P

4.2 车辆追踪实例

假设采样周期100ms，车辆初始状态为(0, 20m/s, 0, 10m/s)：

# 初始化参数 dt = 0.1 F = np.array([[1,dt,0,0], [0,1,0,0], [0,0,1,dt], [0,0,0,1]]) H = np.array([[1,0,0,0], [0,0,1,0]]) # 只能观测位置 Q = np.diag([0.1, 0.5, 0.1, 0.5])**2 R = np.diag([1.0, 1.0]) # 测量噪声1米 kf = KalmanFilter(F=F, H=H, Q=Q, R=R) kf.x = np.array([[0], [20], [0], [10]]) # x,vx,y,vy # 模拟运行 for i in range(100): kf.predict() if i % 5 == 0: # 每500ms更新一次GPS z = get_gps_measurement() # 模拟获取带噪声的GPS数据 kf.update(z)

5. 工程实践中的调参技巧

5.1 噪声参数调整

通过新息序列检测判断参数是否合理：

innovations = [] # 记录z - H@x_prior for _ in range(1000): kf.predict() z = get_measurement() innovations.append(z - kf.H @ kf.x) kf.update(z) # 计算新息序列的均值和协方差 mean = np.mean(innovations, axis=0) cov = np.cov(innovations, rowvar=False)

理想情况下，新息序列应符合N(0, S)分布（S=H@P@H.T+R）。若实际协方差远大于S，说明Q可能过小；若呈现明显非零均值，则可能存在模型误差。

5.2 处理非线性系统

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

def ekf_predict(x, P, f, F_jacobian, Q): x_pred = f(x) # 非线性状态转移 F = F_jacobian(x) # 计算雅可比矩阵 P_pred = F @ P @ F.T + Q return x_pred, P_pred def ekf_update(x, P, z, h, H_jacobian, R): y = z - h(x) H = H_jacobian(x) S = H @ P @ H.T + R K = P @ H.T @ np.linalg.inv(S) x_new = x + K @ y P_new = (np.eye(len(x)) - K @ H) @ P return x_new, P_new

6. 与深度学习的结合应用

现代目标追踪算法如SORT、DeepSORT都采用卡尔曼滤波进行运动预测：

# DeepSORT中的卡尔曼配置 class KalmanFilter: def __init__(self): ndim, dt = 4, 1. self.F = np.eye(2 * ndim) # 状态转移矩阵 for i in range(ndim): self.F[i, ndim + i] = dt self.H = np.eye(ndim, 2 * ndim) # 测量矩阵 self.Q = np.eye(2 * ndim) * 0.03 # 过程噪声 self.R = np.eye(ndim) * 10 # 测量噪声

这种组合充分发挥了深度学习的外观特征识别能力和卡尔曼滤波的运动建模优势。当目标被短暂遮挡时，卡尔曼滤波的预测可以提供合理的轨迹延续，大大降低ID切换概率。