1. 线性动态系统与卡尔曼滤波初探第一次听说卡尔曼滤波是在研究生时期的机器人课程上。当时教授用了一个特别形象的比喻假设你蒙着眼睛在房间里走路每隔几秒会有人告诉你大概走到了什么位置但每次告诉你的位置都有点误差。卡尔曼滤波就是帮你把这些模糊的信息整合起来最终准确找到自己位置的方法。这种技术在现实中有太多应用场景了。比如自动驾驶汽车需要实时确定自己的精确位置但GPS信号可能有几米的误差车载传感器也会受到各种干扰。这时候卡尔曼滤波就能大显身手它能够融合多源数据给出比单一传感器更可靠的估计结果。线性动态系统(LDS)是卡尔曼滤波的数学模型基础。它描述的是一个系统的状态如何随时间线性变化以及我们如何通过带有噪声的观测来推断这些状态。这个模型有两个关键特点一是状态转移是线性的二是噪声服从高斯分布。这两个特性使得我们可以用数学方法精确求解。我记得第一次实现卡尔曼滤波时最让我惊讶的是它的计算效率。在自动驾驶这样的实时系统中算法需要在毫秒级完成所有计算。卡尔曼滤波的递推特性让它完美适配这种需求——它不需要保存所有历史数据只需要记住上一个时刻的状态估计就能进行下一步预测。2. LDS模型详解2.1 状态空间表示让我们用一个具体的例子来说明LDS模型。假设我们要跟踪一辆汽车的位置和速度。系统的状态变量z可以表示为位置p和速度v的组合z [p, v]ᵀ。在离散时间系统中下一时刻的状态可以表示为# 状态转移方程示例 def state_transition(z_prev, dt): A np.array([[1, dt], [0, 1]]) # 状态转移矩阵 B np.array([0.5*dt**2, dt]) # 控制矩阵(假设有加速度输入) return A z_prev B这个简单的例子展示了LDS的核心思想当前状态是前一状态的线性变换加上可能的控制输入。在实际应用中我们还需要考虑过程噪声ε它代表了模型的不确定性。2.2 观测模型观测模型描述了如何从状态得到测量值。继续用汽车跟踪的例子假设我们只能通过GPS获取位置信息(不能直接测速)那么观测模型可以表示为# 观测方程示例 def observation_model(z): C np.array([[1, 0]]) # 观测矩阵 return C z这里的关键点是观测可能只是状态的一部分而且同样会受到噪声δ的影响。卡尔曼滤波的厉害之处就在于它能够通过这些不完全且有噪声的观测推断出完整的系统状态。3. 卡尔曼滤波的递推过程3.1 预测步骤(Prediction)预测步骤是卡尔曼滤波的第一个关键环节。它利用系统模型和上一时刻的状态估计预测当前时刻的状态分布。数学上这对应于计算状态变量的先验概率分布。在实际编程实现时预测步骤主要做两件事状态预测ẑₖ⁻ Aẑₖ₋₁ Buₖ协方差预测Pₖ⁻ APₖ₋₁Aᵀ Q其中Q是过程噪声的协方差矩阵。我曾在项目中犯过一个错误把Q设得太小导致滤波器过于相信模型预测当模型与实际不符时表现很差。后来明白Q实际上代表了我们对模型不确定性的认知。3.2 更新步骤(Update)更新步骤是卡尔曼滤波的第二个关键环节。当获得新的观测数据后算法会将预测值与实际观测值进行比较然后根据它们的不确定性进行加权平均。具体实现包括以下几个计算计算卡尔曼增益Kₖ Pₖ⁻Cᵀ(CPₖ⁻Cᵀ R)⁻¹状态更新ẑₖ ẑₖ⁻ Kₖ(yₖ - Cẑₖ⁻)协方差更新Pₖ (I - KₖC)Pₖ⁻这里R是观测噪声的协方差矩阵。记得有次调试时发现滤波器响应迟钝后来发现是R值设得太大导致算法过于相信预测而忽略了新的观测数据。4. 实际应用案例分析4.1 自动驾驶中的车辆跟踪在自动驾驶系统中卡尔曼滤波常被用于多传感器融合。假设我们有以下传感器GPS提供全局位置精度约2米更新频率1HzIMU提供加速度和角速度高频(100Hz)但存在漂移轮速计提供车速信息相对准确但无法检测侧滑实现一个简单的车辆跟踪滤波器class VehicleTracker: def __init__(self): # 状态: [x, y, vx, vy] self.state np.zeros(4) self.cov np.eye(4) * 100 # 初始不确定较大 # 状态转移矩阵(假设匀速运动) self.A np.array([[1,0,dt,0], [0,1,0,dt], [0,0,1,0], [0,0,0,1]]) # 过程噪声 self.Q np.diag([0.1,0.1,0.5,0.5]) def predict(self, dt): # 更新状态转移矩阵中的dt self.A[0,2] dt self.A[1,3] dt self.state self.A self.state self.cov self.A self.cov self.A.T self.Q def update_gps(self, x, y): # GPS观测矩阵 H np.array([[1,0,0,0], [0,1,0,0]]) # GPS噪声 R np.diag([4,4]) # 2米标准差 # 计算卡尔曼增益 S H self.cov H.T R K self.cov H.T np.linalg.inv(S) # 更新状态和协方差 innovation np.array([x,y]) - H self.state self.state K innovation self.cov (np.eye(4) - K H) self.cov4.2 参数调优经验卡尔曼滤波器的性能很大程度上取决于Q和R的设定。经过多个项目的实践我总结出以下调参经验过程噪声Q反映你对模型准确性的信心。模型越不准Q应该越大。可以从较小值开始逐步增大直到滤波器响应速度合适。观测噪声R应该与实际传感器的精度匹配。可以通过采集静态数据计算传感器实际噪声特性。初始协方差P₀可以设得大一些表示初始状态不确定性强滤波器会更快收敛。数值稳定性对于嵌入式系统使用平方根滤波等数值稳定的实现方式很重要。
