从贝叶斯推断到粒子滤波理解状态估计的核心算法开头怎么从噪声中「猜」出真实状态上一篇我们学习了 Hopfield 网络处理静态模式的联想记忆。今天我们学习卡尔曼滤波——处理动态系统的状态估计。现实问题 GPS 信号有噪声 → 怎么得到准确位置 雷达测量有误差 → 怎么跟踪目标 传感器数据不完整 → 怎么推断完整状态 卡尔曼滤波的答案 结合「预测」和「观测」得到最优估计一、贝叶斯推断基础1.1 贝叶斯公式贝叶斯公式 ═══════════════════════════════════════════════════════════════════ 后验 (似然 × 先验) / 证据 p(x|z) p(z|x) p(x) / p(z) 其中 p(x)先验在看到数据之前的信念 p(z|x)似然数据的生成概率 p(x|z)后验看到数据之后的信念 p(z)证据归一化常数 直觉 先验 新数据 → 后验 → 信念的更新1.2 贝叶斯推断在状态估计中的应用状态估计问题 ═══════════════════════════════════════════════════════════════════ 动态系统 状态 x_t 随时间演化 观测 z_t 带有噪声 目标 从观测序列 {z₁, z₂, ..., z_t} 估计状态 x_t 贝叶斯方法 p(x_t | z_{1:t}) ∝ p(z_t | x_t) p(x_t | z_{1:t-1}) 后验 似然 × 预测二、卡尔曼滤波2.1 系统模型线性高斯系统模型 ═══════════════════════════════════════════════════════════════════ 状态转移预测模型 x_t F x_{t-1} w_t w_t ~ N(0, Q) 观测模型测量模型 z_t H x_t v_t v_t ~ N(0, R) 其中 F状态转移矩阵 H观测矩阵 Q过程噪声协方差 R观测噪声协方差2.2 预测和更新卡尔曼滤波的两个步骤 ═══════════════════════════════════════════════════════════════════ 预测步骤Prediction x̂_{t|t-1} F x̂_{t-1|t-1} P_{t|t-1} F P_{t-1|t-1} Fᵀ Q 更新步骤Update K_t P_{t|t-1} Hᵀ (H P_{t|t-1} Hᵀ R)⁻¹ x̂_{t|t} x̂_{t|t-1} K_t (z_t - H x̂_{t|t-1}) P_{t|t} (I - K_t H) P_{t|t-1} 其中 x̂状态估计 P估计误差协方差 K卡尔曼增益2.3 卡尔曼增益的直觉卡尔曼增益的直觉 ═══════════════════════════════════════════════════════════════════ K P_{预测} Hᵀ (H P_{预测} Hᵀ R)⁻¹ 极端情况 1. R → 0观测非常准确 K → H⁻¹ → 完全相信观测 2. R → ∞观测噪声很大 K → 0 → 完全相信预测 3. P → 0预测非常准确 K → 0 → 完全相信预测 卡尔曼增益自动平衡预测和观测的信任度三、Python 实现卡尔曼滤波3.1 完整代码importnumpyasnpimportmatplotlib.pyplotaspltclassKalmanFilter:卡尔曼滤波器def__init__(self,F,H,Q,R,x0None,P0None): 参数: F: 状态转移矩阵 H: 观测矩阵 Q: 过程噪声协方差 R: 观测噪声协方差 x0: 初始状态估计 P0: 初始估计误差协方差 self.FF self.HH self.QQ self.RR self.dim_xF.shape[0]self.dim_zH.shape[0]# 初始化状态和协方差self.xx0ifx0isnotNoneelsenp.zeros(self.dim_x)self.PP0ifP0isnotNoneelsenp.eye(self.dim_x)# 记录历史self.x_history[]self.P_history[]self.K_history[]defpredict(self):预测步骤# 状态预测self.xself.F self.x# 协方差预测self.Pself.F self.P self.F.Tself.Qreturnself.x.copy()defupdate(self,z):更新步骤# 残差yz-self.H self.x# 残差协方差Sself.H self.P self.H.Tself.R# 卡尔曼增益Kself.P self.H.T np.linalg.inv(S)# 状态更新self.xself.xK y# 协方差更新self.P(np.eye(self.dim_x)-K self.H) self.P# 记录self.K_history.append(K.copy())returnself.x.copy()deffilter(self,observations):对观测序列进行滤波predictions[]updates[]forzinobservations:# 预测predself.predict()predictions.append(pred.copy())# 更新updself.update(z)updates.append(upd.copy())# 记录self.x_history.append(self.x.copy())self.P_history.append(np.diag(self.P).copy())returnnp.array(predictions),np.array(updates)3.2 测试一维追踪# 生成数据np.random.seed(42)n_steps50# 真实状态匀速运动true_statenp.zeros((n_steps,2))true_state[0][0,1]# 初始位置 0速度 1fortinrange(1,n_steps):true_state[t,0]true_state[t-1,0]true_state[t-1,1]# 位置 位置 速度true_state[t,1]true_state[t-1,1]# 速度不变# 观测带噪声的位置observationstrue_state[:,0]np.random.randn(n_steps)*0.5# 定义系统矩阵Fnp.array([[1,1],# 状态转移位置 速度[0,1]])# 速度不变Hnp.array([[1,0]])# 只观测位置Qnp.array([[0.1,0],# 过程噪声[0,0.1]])Rnp.array([[0.25]])# 观测噪声# 创建卡尔曼滤波器kfKalmanFilter(F,H,Q,R)# 滤波predictions,updateskf.filter(observations.reshape(-1,1))# 可视化plt.figure(figsize(12,6))plt.plot(true_state[:,0],g-,labelTrue Position,linewidth2)plt.plot(observations,r.,labelObservations,markersize8)plt.plot(updates[:,0],b-,labelKalman Filter,linewidth2)plt.fill_between(range(n_steps),updates[:,0]-2*np.sqrt(np.array(kf.P_history)[:,0]),updates[:,0]2*np.sqrt(np.array(kf.P_history)[:,0]),alpha0.3,colorblue,labelUncertainty (2σ))plt.xlabel(Time Step)plt.ylabel(Position)plt.title(Kalman Filter for Position Tracking)plt.legend()plt.grid(True)plt.show()3.3 二维追踪# 二维追踪位置 速度np.random.seed(42)n_steps100# 真实轨迹圆周运动tnp.linspace(0,4*np.pi,n_steps)true_x10*np.cos(t)true_y10*np.sin(t)# 观测obs_xtrue_xnp.random.randn(n_steps)*0.5obs_ytrue_ynp.random.randn(n_steps)*0.5# 状态[x, y, vx, vy]Fnp.array([[1,0,1,0],[0,1,0,1],[0,0,1,0],[0,0,0,1]])Hnp.array([[1,0,0,0],[0,1,0,0]])Qnp.eye(4)*0.1Rnp.eye(2)*0.25# 初始化kfKalmanFilter(F,H,Q,R,x0np.array([obs_x[0],obs_y[0],0,0]),P0np.eye(4))# 滤波estimated_positions[]foriinrange(n_steps):kf.predict()kf.update(np.array([obs_x[i],obs_y[i]]))estimated_positions.append(kf.x[:2].copy())estimated_positionsnp.array(estimated_positions)# 可视化plt.figure(figsize(10,10))plt.plot(true_x,true_y,g-,labelTrue Trajectory,linewidth2)plt.plot(obs_x,obs_y,r.,labelObservations,markersize4)plt.plot(estimated_positions[:,0],estimated_positions[:,1],b-,labelKalman Filter,linewidth2)plt.xlabel(X)plt.ylabel(Y)plt.title(2D Trajectory Tracking with Kalman Filter)plt.legend()plt.grid(True)plt.axis(equal)plt.show()四、粒子滤波4.1 为什么需要粒子滤波卡尔曼滤波的局限 ═══════════════════════════════════════════════════════════════════ 卡尔曼滤波假设 1. 线性系统 2. 高斯噪声 现实问题 - 非线性系统机器人运动、目标跟踪 - 非高斯噪声多模态分布 解决方案 粒子滤波Particle Filter - 用一组粒子表示概率分布 - 可以处理任意非线性、非高斯系统4.2 粒子滤波的原理粒子滤波 ═══════════════════════════════════════════════════════════════════ 核心思想 用 N 个粒子 {(x_i, w_i)} 近似后验分布 p(x|z) 步骤 1. 初始化从先验分布采样 N 个粒子 2. 预测对每个粒子从状态转移模型采样 x_i ~ p(x | x_{i, t-1}) 3. 更新根据观测计算权重 w_i p(z | x_i) 4. 归一化权重w_i w_i / Σ w_j 5. 重采样根据权重重新采样粒子 → 高权重的粒子被复制低权重的粒子被淘汰 输出 状态估计 Σ w_i x_i4.3 Python 实现classParticleFilter:粒子滤波器def__init__(self,n_particles,state_dim,motion_model,observation_model,process_noise,observation_noise):self.n_particlesn_particles self.state_dimstate_dim self.motion_modelmotion_model self.observation_modelobservation_model self.process_noiseprocess_noise self.observation_noiseobservation_noise# 初始化粒子self.particlesnp.random.randn(n_particles,state_dim)self.weightsnp.ones(n_particles)/n_particlesdefpredict(self):预测步骤# 添加过程噪声noisenp.random.randn(self.n_particles,self.state_dim)*self.process_noise self.particlesself.motion_model(self.particles)noisedefupdate(self,z):更新步骤# 计算每个粒子的似然foriinrange(self.n_particles):obsself.observation_model(self.particles[i])self.weights[i]np.exp(-0.5*np.sum((obs-z)**2)/self.observation_noise)# 归一化权重self.weights/np.sum(self.weights)defresample(self):重采样indicesnp.random.choice(self.n_particles,self.n_particles,pself.weights)self.particlesself.particles[indices]self.weightsnp.ones(self.n_particles)/self.n_particlesdefestimate(self):估计状态returnnp.average(self.particles,weightsself.weights,axis0)五、工业应用5.1 目标跟踪目标跟踪 ═══════════════════════════════════════════════════════════════════ 雷达跟踪 观测距离、角度 状态位置、速度、加速度 激光雷达跟踪 观测3D 点云 状态6D 位姿 应用 - 自动驾驶 - 无人机 - 军事 tracking5.2 导航系统导航系统 ═══════════════════════════════════════════════════════════════════ GPS/INS 组合导航 GPS绝对位置但有噪声和遮挡 INS相对位姿但有漂移 卡尔曼滤波融合 - GPS 更新位置 - INS 预测状态 → 高精度、高频率的位置估计5.3 传感器融合传感器融合 ═══════════════════════════════════════════════════════════════════ 多传感器数据融合 - 摄像头图像 - 激光雷达点云 - 毫米波雷达距离 卡尔曼滤波 - 统一的状态表示 - 自动处理不同传感器的噪声六、避坑指南坑 1模型不准确 → 滤波发散错误做法模型与实际系统不匹配# ❌ 模型不准确F_wrongnp.array([[1,0.5],# 错误的转移矩阵[0,1]])# 滤波结果偏离真实值正确做法确保模型准确# ✅ 准确的模型F_correctnp.array([[1,1],# 正确的转移矩阵[0,1]])坑 2噪声协方差设置不当 → 滤波效果差错误做法Q 和 R 设置不合理# ❌ 噪声设置不当Qnp.eye(4)*100# 过程噪声太大Rnp.eye(2)*0.01# 观测噪声太小# 滤波器过度信任观测正确做法通过实验或自适应方法确定# ✅ 合理的噪声设置Qnp.eye(4)*0.1# 适中的过程噪声Rnp.eye(2)*0.25# 适中的观测噪声坑 3计算量太大 → 需要优化错误做法粒子数太多# ❌ 粒子数太多pfParticleFilter(n_particles100000,...)# 计算太慢正确做法选择合适的粒子数# ✅ 适中的粒子数pfParticleFilter(n_particles1000,...)# 平衡精度和速度七、本篇总结核心要点回顾贝叶斯推断后验 似然 × 先验卡尔曼滤波预测 更新最优线性估计卡尔曼增益自动平衡预测和观测的信任度粒子滤波用粒子表示分布处理非线性/非高斯系统工业应用目标跟踪、导航、传感器融合下篇预告下一篇是系列终篇——从感知器到深度学习的进化史。我们将回顾 15 个核心算法的关系以及它们在工业中的应用。下一篇你将学到15 个算法的知识图谱深度学习的历史脉络从理论到实践的桥梁下一步学习路径本期互动你对卡尔曼滤波有什么看法你用过卡尔曼滤波吗在什么场景下你觉得卡尔曼滤波和深度学习有什么关系你知道卡尔曼滤波的哪些应用欢迎在评论区留言。系列目录篇标题状态01Haykin 精讲开篇从「只会调参」到「理解神经网络的灵魂」✅ 完成02感知器神经网络的「鼻祖」为什么它能「学会」分类✅ 完成03LMS 算法从最小二乘到随机梯度下降工业自适应滤波的核心✅ 完成04反向传播神经网络为什么能「学习」用 NumPy 手写 BP✅ 完成05核方法为什么 SVM 能处理非线性问题理解「升维」的本质✅ 完成06支持向量机最大间隔的「艺术」为什么它是「小数据之王」✅ 完成07正则化为什么模型越复杂越容易过拟合L1/L2/Dropout✅ 完成08PCA为什么降维能「去噪」从特征值分解到核 PCA✅ 完成09SOM无监督学习的「聚类之王」为什么它能「自组织」✅ 完成10信息论为什么「信息最大化」能学特征从熵到 ICA✅ 完成11玻尔兹曼机深度学习的「前世」从统计力学到 RBM✅ 完成12动态规划强化学习的「数学基础」从 MDP 到值迭代✅ 完成13Hopfield 网络联想记忆的「鼻祖」为什么它能「回忆」✅ 完成14卡尔曼滤波为什么它能「预测」从贝叶斯推断到粒子滤波✅ 当前15Haykin 精讲终篇从感知器到深度学习——一部神经网络的「进化史」⏳ 下一篇点赞收藏转发是我持续更新的动力