打RM这几年带过几届电控组发现一个特别有意思的规律几乎每个新队员入门卡尔曼滤波都是先打开一篇讲公式推导的博客然后盯着那个长得吓人的状态方程和更新方程发呆半小时最后默默关上网页回去继续用低通滤波。我自己当年也是这样直到后来把概率统计的基础重新捋了一遍再回头看卡尔曼滤波才发现它压根不是什么玄学就是一套非常自然的“用概率语言描述不确定性的递推流程”。这期内容是《中科大RM电控合集》里卡尔曼滤波专题的前瞻篇专门讲概率统计基础。我会把集合里后续要用的数学概念全部用RM赛场上能碰到的实际例子讲清楚比如怎么估计对手机器人的位置、怎么融合裁判系统数据和自瞄数据、怎么理解传感器噪声。这篇更适合那些已经会调车、但是看卡尔曼滤波公式总觉得隔了一层纱的同学也适合刚进电控组、想系统把状态估计这块补起来的新队员。1. 为什么要从概率统计开始讲卡尔曼滤波1.1 从一次失败的“预测”说起先讲个真实案例。有一年分区赛我们工程机器人需要自动对接资源岛用的是视觉识别到的相对位置加惯性测量单元的数据去做云台角度闭环。最开始我们没上卡尔曼滤波只在视觉输出后面跟了一个滑动平均滤波。打测试的时候问题来了视觉识别帧率大约30帧但小陀螺一转起来目标在画面里的位置跳动特别厉害滑动平均窗口开大了滞后明显开小了又滤不干净。后来我换了个思路——把目标在图像平面上的坐标当成一个随机变量它每一次视觉识别得到的值不是“真值”而是“带噪声的观测值”。从这个角度切入问题就变成如何在有噪声的观测序列中实时估计目标真正的位置和速度。要回答这个“如何”必须先建立一套描述噪声和不确定性的数学语言。这套语言就是概率统计。1.2 确定性思维与概率性思维的差别大多数写代码的人天然习惯确定性思维给一个输入就有一个输出传感器读到一个值就认为这是真值。但在物理世界里任何测量都有误差任何模型都有偏差。裁判系统给的弹丸速度不一定准惯性测量单元有零漂视觉识别有像素级噪声就连云台转动响应也存在延迟。概率性思维的核心转变在于不再把目标状态当成一个确定的值而是当成一个概率分布。比如“对面步兵在什么位置”在确定性思维里答案是“(2.3, 1.5)”而在概率性思维里答案是“位置大概率在(2.3, 1.5)附近协方差是某个矩阵”。这个转变看起来只是表述变了实际上整个处理逻辑都变了因为状态是分布所以可以用概率论的框架来做预测和更新。卡尔曼滤波之所以强大不是因为它能算出“下一个时刻目标在哪”而是它能算出“目标在某个位置附近的概率有多大”并且随着观测不断到来这个概率分布会越来越窄、越来越逼近真实位置。这里面的每一步都基于概率统计的运算规则。2. 事件、条件概率与贝叶斯公式卡尔曼滤波的底层逻辑2.1 概率到底是什么意思在RM场景里概率最朴素的理解就是频率如果你做100次同样条件下的测量某件事情发生了87次那这个事件的概率就是0.87。举个例子测距模块在10米处测同一个目标100次得到的结果落在9.9到10.1米之间的次数占了95次那就可以说“测距误差在正负10厘米内的概率约为95%”。这种频率派视角在电控里很好用因为它直接对应标定和测试拿到一个传感器我们先测一堆数据统计它的均值、方差等于把传感器的“性格”摸清楚了。后面做滤波时用的噪声参数本质上都是这么统计出来的。不过卡尔曼滤波用到的概率观念比单纯的频率要深一层。它不仅要描述“测量值的分布”还要描述“在已经有了历史信息和当前测量值的条件下状态的分布”。这就引出了条件概率。2.2 条件概率有了新证据判断就要更新条件概率是指在已知某个条件发生的情况下另一个事件发生的概率记作P(A|B)读作“在B发生的条件下A发生的概率”。它的计算公式是P(A|B) P(AB) / P(B)其中P(AB)是A和B同时发生的概率P(B)是B发生的概率。这个公式在RM里怎么解释举个例子假设你的自瞄视觉模块给出“检测到目标”这个事件B你要判断的目标真实存在这个事件是A。视觉模块有真阳性率目标确实存在且被检测到也有假阳性率没有目标却误报。在没有视觉检测结果之前你对“目标是否存在”有一个先验判断P(A)比如经验告诉你对方基地附近大概率有机器人。当视觉模块报告“检测到目标”后你对“目标是否存在”的判断就应该更新为P(A|B)。这就是条件概率在信息融合里的原始形态。2.3 全概率公式与贝叶斯公式全概率公式解决的是这样一个问题如果一件事可能从多条路径发生那这件事的总概率是多少。公式形式是P(B) Σ P(B|Ai) * P(Ai)其中Ai是一组互斥且完备的“原因”。在传感器融合场景里这个公式也经常出现。比如视觉观测值B可能由三种情况产生目标真实位置在区域1且被观测到、在区域2且被观测到、在区域3且被观测到。那么观测到B的总概率就是把这三种路径的概率加起来。贝叶斯公式则是把条件概率反过来的工具P(A|B) P(B|A) * P(A) / P(B)这里每一项都有明确的物理意义。在状态估计语境下P(A)叫先验概率看到测量之前你基于模型预测对状态的了解。P(B|A)叫似然概率如果状态真值是A那么得到当前测量B的可能性有多大这个由传感器噪声模型决定。P(B)叫证据概率这个测量出现的总体概率通常作为归一化常数。P(A|B)叫后验概率结合预测和测量之后你对状态的最终判断。卡尔曼滤波本质上就是一个递推的贝叶斯估计每一轮预测步骤相当于计算先验每一轮更新步骤相当于引入测量计算后验。看起来卡尔曼滤波公式很复杂核心思想却和这个朴素公式完全同源。注意很多讲卡尔曼滤波的文章直接就上矩阵很少有人停下来把贝叶斯公式和状态估计之间的关系讲透。如果你能把贝叶斯公式用自己的话解释清楚“先验、似然、后验分别是什么”卡尔曼滤波的更新方程基本已经懂了一半。3. 随机变量、期望与方差用数字量化不确定性3.1 随机变量把传感器读数变成数学对象随机变量不是“变量是随机的”这种模糊说法它指的是一个取值带有概率性的变量。在RM里目标位置、目标速度、弹丸飞行时间、陀螺仪漂移都可以建模成随机变量。假设我们用一个近距离红外测距模块测前方障碍物距离。每次读取都会得到一个数值这些数值围绕真实距离上下波动。这个“带波动的读数”就是一个随机变量X。它的取值不是固定的但它的统计规律是稳定的——干了这么多年电控你会发现大部分传感器的噪声都有比较稳定的分布特性这正是可以用概率工具处理的前提。3.2 期望与方差位置在哪有多不确定随机变量有两个最基本的统计量。期望也叫均值是所有可能取值按概率加权平均的结果。对离散变量E[X] Σ xi * P(xi)对连续变量E[X] ∫ x * f(x) dx其中f(x)是概率密度函数。期望在RM里的直观含义是“大量测量后读数平均落在哪里”也就是对真值的一个无偏估计如果传感器没有系统性偏差。方差定义是Var(X) E[(X - E[X])²]它描述随机变量取值偏离均值的程度。方差的开方叫标准差。在RM里方差的直观含义很朴素标准差小的传感器读数稳定标准差大的传感器读数跳动大。比如同一个测距模块对静态目标测100次标准差可能是0.01米对正在高速移动的目标测100次标准差可能变成0.05米。这组数据直接决定了后面卡尔曼滤波里测量噪声矩阵R该取多少。3.3 为什么方差不是越小越好很多队伍在调传感器时容易走入一个极端见到方差就想着怎么压小好像方差越小越厉害。但实际工程里方差小不代表估计就好。举个例子。视觉识别如果连续几帧都稳定地输出同一个位置但是因为图像处理算法本身有系统性偏差比如标定不准导致固定的像素偏移那么它的测量方差看起来很小但均值离真值差了一大截。这时候如果卡尔曼滤波完全信任视觉数据状态估计就会收敛到一个错误的点。这就是为什么卡尔曼滤波不仅需要知道方差还需要给到合理的方差测量方差R太小会过于相信传感器系统性的偏差就会直接进到状态里过程噪声Q太小会过于相信模型目标突然加速减速时滤波器就会反应不过来。做RM自动瞄准最怕的就是R和Q给得不合理一个参数问题能让整个系统表现像“癫痫”一样。4. 随机向量与协方差矩阵进入多变量状态空间4.1 单变量不够用目标位置和速度是联动的到目前为止聊的都是单个随机变量。但卡尔曼滤波里状态几乎总是多维的。最常见的RM状态向量是x [位置, 速度]ᵀ比如你要预测对面步兵的位置光知道它在哪不够还得知道它的速度才能推测下一时刻它在哪。更关键的是位置和速度这两个随机变量不是独立的——位置的变化就是速度所以它们之间天然存在耦合关系。这就需要一个数学工具能同时描述多个随机变量的均值和不确定性还能表达它们之间的相关关系。答案就是随机向量和协方差矩阵。4.2 协方差矩阵的结构与直觉假设有一个n维随机向量X [X1, X2, ..., Xn]ᵀ。它的均值也是一个n维向量μ E[X]它的不确定性由一个n×n矩阵描述叫协方差矩阵P E[(X - μ)(X - μ)ᵀ]展开来看P [[Var(X1), Cov(X1,X2), ..., Cov(X1,Xn)], [Cov(X2,X1), Var(X2), ..., Cov(X2,Xn)], ... [Cov(Xn,X1), Cov(Xn,X2), ..., Var(Xn)]]对角线上的元素是各变量自己的方差非对角线元素是变量之间的协方差。协方差为正说明两个变量倾向于同向变化为负说明反向变化接近0说明两者相关性弱。在RM场景里目标位置和速度的协方差就是典型如果目标当前位置偏右下一时刻位置大概率也会偏右所以位置和速度之间有明显的正相关。卡尔曼滤波里那个不断更新的协方差矩阵P之所以能越算越准就是因为它通过非对角线元素悄悄记录下了“位置和速度之间的耦合关系”。4.3 误差传播预测步骤的数学根源协方差矩阵还有一个重要应用就是描述线性变换对不确定性的影响。假设有两个随机向量X和Y满足线性关系Y A X那么Y的均值是E[Y] A E[X]Y的协方差矩阵是Cov(Y) A Cov(X) Aᵀ这个公式在卡尔曼滤波预测步骤里就是核心。状态转移方程本质上是x_k F x_{k-1} 噪声从概率角度理解我们对上一时刻状态有一个估计它带着不确定性P_{k-1}。经过状态转移矩阵F作用后新的状态均值变成F乘以旧均值新的协方差变成F乘以旧协方差再乘以F的转置最后再加上过程噪声协方差Q。这就是卡尔曼滤波预测步骤中那个P_k F P_{k-1} Fᵀ Q的来历。很多人在调参时对这个公式不理解只知道要“乘一下”。但如果理解了协方差经过线性变换后会变成A P Aᵀ就明白这个公式完全不是拍脑袋定的而是概率论的必然结果。实操心得调试卡尔曼滤波时建议把P矩阵的数值可视化出来看。如果位置方差和速度协方差一直处于一个合理区间说明滤波器比较健康如果P矩阵发散到天文数字那大概率是Q设置太小或者模型有误。5. 高斯分布卡尔曼滤波的“神队友”5.1 为什么到处都是高斯一说到误差分布大家第一反应都是正态分布高斯分布。传感器噪声、测量误差、环境扰动这些在绝大多数情况下都能用高斯分布近似。一元高斯分布的概率密度函数是f(x) 1 / (σ√(2π)) * exp(-(x-μ)² / (2σ²))其中μ是均值σ是标准差。它是一条钟形曲线中间高、两边低绝大多数取值集中在均值附近。多元高斯分布是它的高维推广。对一个n维随机向量X概率密度函数是f(x) 1 / ((2π)^(n/2) |P|^(1/2)) * exp(-0.5 * (x-μ)ᵀ P⁻¹ (x-μ))其中μ是n维均值向量P是协方差矩阵|P|是P的行列式。这个式子看起来吓人但它的核心思想和一元一模一样指数项(x-μ)ᵀ P⁻¹ (x-μ)衡量的是“当前取值离均值有多远”只不过在高维空间里距离需要用协方差矩阵来“加权”。5.2 高斯分布的三大封闭性质卡尔曼滤波选择高斯分布不是因为它是最普遍的选择而是因为高斯分布有三个极其好用的数学性质。这三条性质单独拿出来都不难但合在一起恰好构成了卡尔曼滤波能闭环的根本原因。性质一线性变换封闭如果X服从高斯分布那么Y A X b也服从高斯分布且Y的均值是Aμb协方差是A P Aᵀ。这意味着在卡尔曼滤波里只要状态是高斯分布经过线性状态转移后预测结果依然是高斯分布不需要引入新类型的分布函数。性质二边缘化封闭如果(X1, X2)联合服从多元高斯分布那么单独看X1或X2也是一个高斯分布。在RM里这意味着即使我们把目标位置和速度放在同一个状态向量里联合估计当需要单独拿出位置信息时直接取对应的均值和方差就行分布形式不会变。性质三条件化封闭给定部分变量后另一部分变量的条件分布依然是高斯分布。这条性质最厉害。卡尔曼滤波的更新步骤本质上就是在算“给定测量y的条件下状态x的条件分布”。高斯分布的条件化封闭性保证了这个条件分布还是高斯于是下一轮预测可以继续用高斯分布来算。只要噪声和初始状态都是高斯分布整个过程自始至终都在高斯分布的框架里打转不会越算越复杂。5.3 用一个简单例子理解三大性质假设你要预测对面步兵机器人的位置。初始时刻你对它位置的估计可以用一个二维高斯分布表示位置均值是(2.0, 1.0)协方差矩阵P0 [[0.1, 0], [0, 0.1]]意思是x和y方向各有约0.316米的标准差。目标以匀速直线运动速度v 1.0 m/s沿x轴方向。状态转移之后预测位置均值变成(3.0, 1.0)协方差因为模型没有噪声时会保持较小但实际中因为目标可能急停、加速我们要加一个过程噪声Q进去协方差就会变大。这就是预测步里“不确定性必然增加”的直观来源。等新的视觉观测到了假设视觉测得的y坐标比预测值小了0.2米。卡尔曼滤波会权衡预测的位置有多可信观测的位置有多可信各自的标准差是多少然后给出一个新的高斯分布均值落在两者之间更相信的一方方差比两者都小。这个“方差变小”在直觉上完全合理——两个独立的信息来源互相印证不确定性自然下降。这就是高斯分布三性质在卡尔曼滤波里的整体默契配合。只要搞懂了这三条性质卡尔曼滤波的“预测-更新-再预测-再更新”循环就不再是死记公式而是一个水到渠成的过程。6. 最大似然与最小方差从数据里“逆向”估计状态6.1 最大似然估计的思想前面几节讲的都是“已知状态分布推测观测会怎么分布”。但实际工程里我们面对的是反过来的问题拿到了一些观测数据怎么倒推状态最可能是什么最大似然估计的思路是找出那个使得当前观测数据出现概率最大的状态值。举一个RM里的例子。假设你用裁判系统读到了对方机器人装甲板的反光强度变化间接判断对方在某个位置。你连续观测到5组数据观测1位置x 2.1 m 观测2位置x 2.3 m 观测3位置x 1.9 m 观测4位置x 2.2 m 观测5位置x 2.0 m如果假设这些观测服从高斯分布怎么估计真值最大似然估计会告诉你使这5个数据同时出现概率最大的均值就是它们的算术平均约2.1 m。6.2 两个观测怎么融合方差加权平均最大似然进一步扩展就能解释卡尔曼滤波更新步骤中的数据融合逻辑。假设有两个独立的测量来源一个是视觉测得目标位置z1 2.15 m测量噪声方差R1 0.01另一个是雷达或者裁判系统数据测得位置z2 2.30 m测量噪声方差R2 0.04。两个来源各有各的精度视觉更准、雷达更粗糙。最小方差估计告诉我们最优融合结果不是简单平均而是按方差倒数加权x_hat (z1/R1 z2/R2) / (1/R1 1/R2)代入数字x_hat (2.15 / 0.01 2.30 / 0.04) / (1/0.01 1/0.04) (215 57.5) / (100 25) 272.5 / 125 2.18融合后的位置是2.18 m。可以看到由于视觉更准融合结果偏向视觉多一点但并没有完全忽略雷达的信息。融合后的方差是1 / (1/R1 1/R2) 1 / 125 0.008这个值比R1和R2都小说明融合后的估计比任何一个单独测量都更可靠。这就是多传感器融合在概率框架下的底层原理。6.3 从加权平均到卡尔曼增益把上面的加权平均公式改写一下就能和卡尔曼滤波的更新方程对上了。卡尔曼滤波的更新方程可以写成x_k x_pred K * (z_k - z_pred)其中z_pred是根据预测状态计算出来的“预测观测值”(z_k - z_pred)叫新息K是卡尔曼增益。如果我们只有两个一维观测增益K的取值就是K P_pred / (P_pred R)其中P_pred是预测状态的方差R是测量噪声方差。当预测方差P_pred特别大时对预测不自信K趋近于1几乎完全信任新观测当R特别大时对测量不自信K趋近于0几乎忽略观测。卡尔曼增益本质上就是“预测和测量谁更可信”的权重。理解了这一点卡尔曼滤波更新方程就不再是一堆字母乱飞而是一个很自然的比例分配过程。注意实际卡尔曼滤波是多维的增益是个矩阵而不是数更新方程里多乘了一个观测矩阵H来表达“从状态到观测的映射”。但背后“按方差加权平均”的直觉完全一致。7. 常见问题与排查技巧实录7.1 把概率当固定值用导致先验设死遇到过不少队员在初始化卡尔曼滤波时直接把P0设成一个巨大的数然后不管后续怎么更新P矩阵都不收敛或者干脆飞上天。原因多半是过程噪声Q设得和P0不匹配滤波器认为状态每时每刻都在剧烈变化自然没法有效累积信息。经验做法P0可以根据实际状态的不确定性来估计。比如目标初始位置不确定度是多少米就设对应方差。Q则要反映“模型预测不可靠的程度”一般从比较小的值开始调比如位置过程噪声取0.01量级速度过程噪声取0.1到1量级然后根据滤波轨迹的平滑度和响应速度逐步调整。7.2 协方差矩阵不保持对称正定卡尔曼滤波计算过程中由于浮点误差累积P矩阵可能出现不对称甚至出现负的特征值。这在长时间运行的嵌入式系统上很常见。排查手段有两个一是周期性强制对称化把P更新为(P Pᵀ) / 2二是在更新方程中使用对称正定矩阵分解方式比如用平方根滤波或者UD分解来实现。不过对于大多数RM应用每执行一定周期后做一次对称化处理就够了不用上太复杂的数值算法。7.3 怎么看滤波器有没有“病”调滤波器最怕的是没有直观反馈。建议在调试界面上同时显示三样东西原始观测值、滤波输出值、新息序列观测减预测。如果新息均值明显偏离0说明系统存在系统性偏差可能是传感器标定问题也可能是模型不匹配如果新息方差明显大于R说明R设置过小滤波过于信任观测如果新息方差明显小于R说明R设置过大滤波过于迟钝。7.4 卡尔曼滤波使用问题速查现象可能原因排查思路滤波输出发散数值狂跳Q过大P0过大减小过程噪声Q缩小初始P0滤波响应太慢跟不上目标Q过小R过大增大Q减小R滤波输出太“贴”观测噪声大R过小增大R或检查传感器噪声统计新息序列有恒定偏移传感器存在系统性偏差校正标定或增加状态向量维度如偏差项P矩阵发散成天文数字Q设置不合理或模型错误重新检查状态转移矩阵F、过程噪声Q8. 从概率统计到卡尔曼滤波的最后一块拼图8.1 把全流程串起来现在把前面所有内容串成一个完整的故事。初始时刻我们对目标状态有一个“先验高斯分布”均值是μ0协方差是P0。目标移动时我们根据运动模型做预测预测结果还是一个高斯分布但方差变大了因为模型并不完美要加上过程噪声Q。这是预测步。传感器送来一个观测值我们根据传感器噪声模型知道观测的分布特性。观测和预测之间存在差异我们用卡尔曼增益作为信任权重融合两者得到后验高斯分布方差比预测和观测都小。这是更新步。然后循环往复。整个过程只有一个核心数学工具用高斯分布描述状态用贝叶斯公式做条件概率更新用协方差矩阵描述不确定性在状态维度和时间维度上的传播。8.2 下期预告卡尔曼滤波公式推导到这里概率统计基础的前瞻内容就结束。下一篇会正式讲解卡尔曼滤波的完整公式推导从一维的标量卡尔曼滤波开始一步步扩展到多维的向量形式并结合RM里的实际状态方程把预测步、更新步、增益计算的每个细节掰开揉碎。8.3 给你留几个自查题在进入下一讲之前建议确认自己能不能回答这几个问题贝叶斯公式里先验、似然、后验分别对应卡尔曼滤波中的哪个环节为什么协方差矩阵经过线性变换后会变成A P Aᵀ试着用一维的方差传播公式验证。高斯分布的哪条性质保证了卡尔曼滤波可以无限递推下去如果两个传感器的测量方差分别是R1和R2最优融合结果的方差是多少为什么比任何一个单独的方差都小如果这几个问题都能不看公式、用自己的话解释清楚那卡尔曼滤波正式内容对你来说就会顺畅很多。我个人这几年的体会是卡尔曼滤波本身不复杂复杂的是很多教程一上来就堆公式跳过了概率统计这个地基。地基打牢了后面不管是标准卡尔曼、扩展卡尔曼还是无迹卡尔曼就都是在同一个思想框架下换不同的近似手段而已。希望这篇前瞻能帮你把地基打牢我们下一讲正式进入卡尔曼滤波公式。 
长河君 · 2026/9/16 7:44:45 · 阅读 1.2k 卡尔曼滤波入门必看：从概率统计到贝叶斯估计的核心基础
打RM这几年带过几届电控组发现一个特别有意思的规律几乎每个新队员入门卡尔曼滤波都是先打开一篇讲公式推导的博客然后盯着那个长得吓人的状态方程和更新方程发呆半小时最后默默关上网页回去继续用低通滤波。我自己当年也是这样直到后来把概率统计的基础重新捋了一遍再回头看卡尔曼滤波才发现它压根不是什么玄学就是一套非常自然的“用概率语言描述不确定性的递推流程”。这期内容是《中科大RM电控合集》里卡尔曼滤波专题的前瞻篇专门讲概率统计基础。我会把集合里后续要用的数学概念全部用RM赛场上能碰到的实际例子讲清楚比如怎么估计对手机器人的位置、怎么融合裁判系统数据和自瞄数据、怎么理解传感器噪声。这篇更适合那些已经会调车、但是看卡尔曼滤波公式总觉得隔了一层纱的同学也适合刚进电控组、想系统把状态估计这块补起来的新队员。1. 为什么要从概率统计开始讲卡尔曼滤波1.1 从一次失败的“预测”说起先讲个真实案例。有一年分区赛我们工程机器人需要自动对接资源岛用的是视觉识别到的相对位置加惯性测量单元的数据去做云台角度闭环。最开始我们没上卡尔曼滤波只在视觉输出后面跟了一个滑动平均滤波。打测试的时候问题来了视觉识别帧率大约30帧但小陀螺一转起来目标在画面里的位置跳动特别厉害滑动平均窗口开大了滞后明显开小了又滤不干净。后来我换了个思路——把目标在图像平面上的坐标当成一个随机变量它每一次视觉识别得到的值不是“真值”而是“带噪声的观测值”。从这个角度切入问题就变成如何在有噪声的观测序列中实时估计目标真正的位置和速度。要回答这个“如何”必须先建立一套描述噪声和不确定性的数学语言。这套语言就是概率统计。1.2 确定性思维与概率性思维的差别大多数写代码的人天然习惯确定性思维给一个输入就有一个输出传感器读到一个值就认为这是真值。但在物理世界里任何测量都有误差任何模型都有偏差。裁判系统给的弹丸速度不一定准惯性测量单元有零漂视觉识别有像素级噪声就连云台转动响应也存在延迟。概率性思维的核心转变在于不再把目标状态当成一个确定的值而是当成一个概率分布。比如“对面步兵在什么位置”在确定性思维里答案是“(2.3, 1.5)”而在概率性思维里答案是“位置大概率在(2.3, 1.5)附近协方差是某个矩阵”。这个转变看起来只是表述变了实际上整个处理逻辑都变了因为状态是分布所以可以用概率论的框架来做预测和更新。卡尔曼滤波之所以强大不是因为它能算出“下一个时刻目标在哪”而是它能算出“目标在某个位置附近的概率有多大”并且随着观测不断到来这个概率分布会越来越窄、越来越逼近真实位置。这里面的每一步都基于概率统计的运算规则。2. 事件、条件概率与贝叶斯公式卡尔曼滤波的底层逻辑2.1 概率到底是什么意思在RM场景里概率最朴素的理解就是频率如果你做100次同样条件下的测量某件事情发生了87次那这个事件的概率就是0.87。举个例子测距模块在10米处测同一个目标100次得到的结果落在9.9到10.1米之间的次数占了95次那就可以说“测距误差在正负10厘米内的概率约为95%”。这种频率派视角在电控里很好用因为它直接对应标定和测试拿到一个传感器我们先测一堆数据统计它的均值、方差等于把传感器的“性格”摸清楚了。后面做滤波时用的噪声参数本质上都是这么统计出来的。不过卡尔曼滤波用到的概率观念比单纯的频率要深一层。它不仅要描述“测量值的分布”还要描述“在已经有了历史信息和当前测量值的条件下状态的分布”。这就引出了条件概率。2.2 条件概率有了新证据判断就要更新条件概率是指在已知某个条件发生的情况下另一个事件发生的概率记作P(A|B)读作“在B发生的条件下A发生的概率”。它的计算公式是P(A|B) P(AB) / P(B)其中P(AB)是A和B同时发生的概率P(B)是B发生的概率。这个公式在RM里怎么解释举个例子假设你的自瞄视觉模块给出“检测到目标”这个事件B你要判断的目标真实存在这个事件是A。视觉模块有真阳性率目标确实存在且被检测到也有假阳性率没有目标却误报。在没有视觉检测结果之前你对“目标是否存在”有一个先验判断P(A)比如经验告诉你对方基地附近大概率有机器人。当视觉模块报告“检测到目标”后你对“目标是否存在”的判断就应该更新为P(A|B)。这就是条件概率在信息融合里的原始形态。2.3 全概率公式与贝叶斯公式全概率公式解决的是这样一个问题如果一件事可能从多条路径发生那这件事的总概率是多少。公式形式是P(B) Σ P(B|Ai) * P(Ai)其中Ai是一组互斥且完备的“原因”。在传感器融合场景里这个公式也经常出现。比如视觉观测值B可能由三种情况产生目标真实位置在区域1且被观测到、在区域2且被观测到、在区域3且被观测到。那么观测到B的总概率就是把这三种路径的概率加起来。贝叶斯公式则是把条件概率反过来的工具P(A|B) P(B|A) * P(A) / P(B)这里每一项都有明确的物理意义。在状态估计语境下P(A)叫先验概率看到测量之前你基于模型预测对状态的了解。P(B|A)叫似然概率如果状态真值是A那么得到当前测量B的可能性有多大这个由传感器噪声模型决定。P(B)叫证据概率这个测量出现的总体概率通常作为归一化常数。P(A|B)叫后验概率结合预测和测量之后你对状态的最终判断。卡尔曼滤波本质上就是一个递推的贝叶斯估计每一轮预测步骤相当于计算先验每一轮更新步骤相当于引入测量计算后验。看起来卡尔曼滤波公式很复杂核心思想却和这个朴素公式完全同源。注意很多讲卡尔曼滤波的文章直接就上矩阵很少有人停下来把贝叶斯公式和状态估计之间的关系讲透。如果你能把贝叶斯公式用自己的话解释清楚“先验、似然、后验分别是什么”卡尔曼滤波的更新方程基本已经懂了一半。3. 随机变量、期望与方差用数字量化不确定性3.1 随机变量把传感器读数变成数学对象随机变量不是“变量是随机的”这种模糊说法它指的是一个取值带有概率性的变量。在RM里目标位置、目标速度、弹丸飞行时间、陀螺仪漂移都可以建模成随机变量。假设我们用一个近距离红外测距模块测前方障碍物距离。每次读取都会得到一个数值这些数值围绕真实距离上下波动。这个“带波动的读数”就是一个随机变量X。它的取值不是固定的但它的统计规律是稳定的——干了这么多年电控你会发现大部分传感器的噪声都有比较稳定的分布特性这正是可以用概率工具处理的前提。3.2 期望与方差位置在哪有多不确定随机变量有两个最基本的统计量。期望也叫均值是所有可能取值按概率加权平均的结果。对离散变量E[X] Σ xi * P(xi)对连续变量E[X] ∫ x * f(x) dx其中f(x)是概率密度函数。期望在RM里的直观含义是“大量测量后读数平均落在哪里”也就是对真值的一个无偏估计如果传感器没有系统性偏差。方差定义是Var(X) E[(X - E[X])²]它描述随机变量取值偏离均值的程度。方差的开方叫标准差。在RM里方差的直观含义很朴素标准差小的传感器读数稳定标准差大的传感器读数跳动大。比如同一个测距模块对静态目标测100次标准差可能是0.01米对正在高速移动的目标测100次标准差可能变成0.05米。这组数据直接决定了后面卡尔曼滤波里测量噪声矩阵R该取多少。3.3 为什么方差不是越小越好很多队伍在调传感器时容易走入一个极端见到方差就想着怎么压小好像方差越小越厉害。但实际工程里方差小不代表估计就好。举个例子。视觉识别如果连续几帧都稳定地输出同一个位置但是因为图像处理算法本身有系统性偏差比如标定不准导致固定的像素偏移那么它的测量方差看起来很小但均值离真值差了一大截。这时候如果卡尔曼滤波完全信任视觉数据状态估计就会收敛到一个错误的点。这就是为什么卡尔曼滤波不仅需要知道方差还需要给到合理的方差测量方差R太小会过于相信传感器系统性的偏差就会直接进到状态里过程噪声Q太小会过于相信模型目标突然加速减速时滤波器就会反应不过来。做RM自动瞄准最怕的就是R和Q给得不合理一个参数问题能让整个系统表现像“癫痫”一样。4. 随机向量与协方差矩阵进入多变量状态空间4.1 单变量不够用目标位置和速度是联动的到目前为止聊的都是单个随机变量。但卡尔曼滤波里状态几乎总是多维的。最常见的RM状态向量是x [位置, 速度]ᵀ比如你要预测对面步兵的位置光知道它在哪不够还得知道它的速度才能推测下一时刻它在哪。更关键的是位置和速度这两个随机变量不是独立的——位置的变化就是速度所以它们之间天然存在耦合关系。这就需要一个数学工具能同时描述多个随机变量的均值和不确定性还能表达它们之间的相关关系。答案就是随机向量和协方差矩阵。4.2 协方差矩阵的结构与直觉假设有一个n维随机向量X [X1, X2, ..., Xn]ᵀ。它的均值也是一个n维向量μ E[X]它的不确定性由一个n×n矩阵描述叫协方差矩阵P E[(X - μ)(X - μ)ᵀ]展开来看P [[Var(X1), Cov(X1,X2), ..., Cov(X1,Xn)], [Cov(X2,X1), Var(X2), ..., Cov(X2,Xn)], ... [Cov(Xn,X1), Cov(Xn,X2), ..., Var(Xn)]]对角线上的元素是各变量自己的方差非对角线元素是变量之间的协方差。协方差为正说明两个变量倾向于同向变化为负说明反向变化接近0说明两者相关性弱。在RM场景里目标位置和速度的协方差就是典型如果目标当前位置偏右下一时刻位置大概率也会偏右所以位置和速度之间有明显的正相关。卡尔曼滤波里那个不断更新的协方差矩阵P之所以能越算越准就是因为它通过非对角线元素悄悄记录下了“位置和速度之间的耦合关系”。4.3 误差传播预测步骤的数学根源协方差矩阵还有一个重要应用就是描述线性变换对不确定性的影响。假设有两个随机向量X和Y满足线性关系Y A X那么Y的均值是E[Y] A E[X]Y的协方差矩阵是Cov(Y) A Cov(X) Aᵀ这个公式在卡尔曼滤波预测步骤里就是核心。状态转移方程本质上是x_k F x_{k-1} 噪声从概率角度理解我们对上一时刻状态有一个估计它带着不确定性P_{k-1}。经过状态转移矩阵F作用后新的状态均值变成F乘以旧均值新的协方差变成F乘以旧协方差再乘以F的转置最后再加上过程噪声协方差Q。这就是卡尔曼滤波预测步骤中那个P_k F P_{k-1} Fᵀ Q的来历。很多人在调参时对这个公式不理解只知道要“乘一下”。但如果理解了协方差经过线性变换后会变成A P Aᵀ就明白这个公式完全不是拍脑袋定的而是概率论的必然结果。实操心得调试卡尔曼滤波时建议把P矩阵的数值可视化出来看。如果位置方差和速度协方差一直处于一个合理区间说明滤波器比较健康如果P矩阵发散到天文数字那大概率是Q设置太小或者模型有误。5. 高斯分布卡尔曼滤波的“神队友”5.1 为什么到处都是高斯一说到误差分布大家第一反应都是正态分布高斯分布。传感器噪声、测量误差、环境扰动这些在绝大多数情况下都能用高斯分布近似。一元高斯分布的概率密度函数是f(x) 1 / (σ√(2π)) * exp(-(x-μ)² / (2σ²))其中μ是均值σ是标准差。它是一条钟形曲线中间高、两边低绝大多数取值集中在均值附近。多元高斯分布是它的高维推广。对一个n维随机向量X概率密度函数是f(x) 1 / ((2π)^(n/2) |P|^(1/2)) * exp(-0.5 * (x-μ)ᵀ P⁻¹ (x-μ))其中μ是n维均值向量P是协方差矩阵|P|是P的行列式。这个式子看起来吓人但它的核心思想和一元一模一样指数项(x-μ)ᵀ P⁻¹ (x-μ)衡量的是“当前取值离均值有多远”只不过在高维空间里距离需要用协方差矩阵来“加权”。5.2 高斯分布的三大封闭性质卡尔曼滤波选择高斯分布不是因为它是最普遍的选择而是因为高斯分布有三个极其好用的数学性质。这三条性质单独拿出来都不难但合在一起恰好构成了卡尔曼滤波能闭环的根本原因。性质一线性变换封闭如果X服从高斯分布那么Y A X b也服从高斯分布且Y的均值是Aμb协方差是A P Aᵀ。这意味着在卡尔曼滤波里只要状态是高斯分布经过线性状态转移后预测结果依然是高斯分布不需要引入新类型的分布函数。性质二边缘化封闭如果(X1, X2)联合服从多元高斯分布那么单独看X1或X2也是一个高斯分布。在RM里这意味着即使我们把目标位置和速度放在同一个状态向量里联合估计当需要单独拿出位置信息时直接取对应的均值和方差就行分布形式不会变。性质三条件化封闭给定部分变量后另一部分变量的条件分布依然是高斯分布。这条性质最厉害。卡尔曼滤波的更新步骤本质上就是在算“给定测量y的条件下状态x的条件分布”。高斯分布的条件化封闭性保证了这个条件分布还是高斯于是下一轮预测可以继续用高斯分布来算。只要噪声和初始状态都是高斯分布整个过程自始至终都在高斯分布的框架里打转不会越算越复杂。5.3 用一个简单例子理解三大性质假设你要预测对面步兵机器人的位置。初始时刻你对它位置的估计可以用一个二维高斯分布表示位置均值是(2.0, 1.0)协方差矩阵P0 [[0.1, 0], [0, 0.1]]意思是x和y方向各有约0.316米的标准差。目标以匀速直线运动速度v 1.0 m/s沿x轴方向。状态转移之后预测位置均值变成(3.0, 1.0)协方差因为模型没有噪声时会保持较小但实际中因为目标可能急停、加速我们要加一个过程噪声Q进去协方差就会变大。这就是预测步里“不确定性必然增加”的直观来源。等新的视觉观测到了假设视觉测得的y坐标比预测值小了0.2米。卡尔曼滤波会权衡预测的位置有多可信观测的位置有多可信各自的标准差是多少然后给出一个新的高斯分布均值落在两者之间更相信的一方方差比两者都小。这个“方差变小”在直觉上完全合理——两个独立的信息来源互相印证不确定性自然下降。这就是高斯分布三性质在卡尔曼滤波里的整体默契配合。只要搞懂了这三条性质卡尔曼滤波的“预测-更新-再预测-再更新”循环就不再是死记公式而是一个水到渠成的过程。6. 最大似然与最小方差从数据里“逆向”估计状态6.1 最大似然估计的思想前面几节讲的都是“已知状态分布推测观测会怎么分布”。但实际工程里我们面对的是反过来的问题拿到了一些观测数据怎么倒推状态最可能是什么最大似然估计的思路是找出那个使得当前观测数据出现概率最大的状态值。举一个RM里的例子。假设你用裁判系统读到了对方机器人装甲板的反光强度变化间接判断对方在某个位置。你连续观测到5组数据观测1位置x 2.1 m 观测2位置x 2.3 m 观测3位置x 1.9 m 观测4位置x 2.2 m 观测5位置x 2.0 m如果假设这些观测服从高斯分布怎么估计真值最大似然估计会告诉你使这5个数据同时出现概率最大的均值就是它们的算术平均约2.1 m。6.2 两个观测怎么融合方差加权平均最大似然进一步扩展就能解释卡尔曼滤波更新步骤中的数据融合逻辑。假设有两个独立的测量来源一个是视觉测得目标位置z1 2.15 m测量噪声方差R1 0.01另一个是雷达或者裁判系统数据测得位置z2 2.30 m测量噪声方差R2 0.04。两个来源各有各的精度视觉更准、雷达更粗糙。最小方差估计告诉我们最优融合结果不是简单平均而是按方差倒数加权x_hat (z1/R1 z2/R2) / (1/R1 1/R2)代入数字x_hat (2.15 / 0.01 2.30 / 0.04) / (1/0.01 1/0.04) (215 57.5) / (100 25) 272.5 / 125 2.18融合后的位置是2.18 m。可以看到由于视觉更准融合结果偏向视觉多一点但并没有完全忽略雷达的信息。融合后的方差是1 / (1/R1 1/R2) 1 / 125 0.008这个值比R1和R2都小说明融合后的估计比任何一个单独测量都更可靠。这就是多传感器融合在概率框架下的底层原理。6.3 从加权平均到卡尔曼增益把上面的加权平均公式改写一下就能和卡尔曼滤波的更新方程对上了。卡尔曼滤波的更新方程可以写成x_k x_pred K * (z_k - z_pred)其中z_pred是根据预测状态计算出来的“预测观测值”(z_k - z_pred)叫新息K是卡尔曼增益。如果我们只有两个一维观测增益K的取值就是K P_pred / (P_pred R)其中P_pred是预测状态的方差R是测量噪声方差。当预测方差P_pred特别大时对预测不自信K趋近于1几乎完全信任新观测当R特别大时对测量不自信K趋近于0几乎忽略观测。卡尔曼增益本质上就是“预测和测量谁更可信”的权重。理解了这一点卡尔曼滤波更新方程就不再是一堆字母乱飞而是一个很自然的比例分配过程。注意实际卡尔曼滤波是多维的增益是个矩阵而不是数更新方程里多乘了一个观测矩阵H来表达“从状态到观测的映射”。但背后“按方差加权平均”的直觉完全一致。7. 常见问题与排查技巧实录7.1 把概率当固定值用导致先验设死遇到过不少队员在初始化卡尔曼滤波时直接把P0设成一个巨大的数然后不管后续怎么更新P矩阵都不收敛或者干脆飞上天。原因多半是过程噪声Q设得和P0不匹配滤波器认为状态每时每刻都在剧烈变化自然没法有效累积信息。经验做法P0可以根据实际状态的不确定性来估计。比如目标初始位置不确定度是多少米就设对应方差。Q则要反映“模型预测不可靠的程度”一般从比较小的值开始调比如位置过程噪声取0.01量级速度过程噪声取0.1到1量级然后根据滤波轨迹的平滑度和响应速度逐步调整。7.2 协方差矩阵不保持对称正定卡尔曼滤波计算过程中由于浮点误差累积P矩阵可能出现不对称甚至出现负的特征值。这在长时间运行的嵌入式系统上很常见。排查手段有两个一是周期性强制对称化把P更新为(P Pᵀ) / 2二是在更新方程中使用对称正定矩阵分解方式比如用平方根滤波或者UD分解来实现。不过对于大多数RM应用每执行一定周期后做一次对称化处理就够了不用上太复杂的数值算法。7.3 怎么看滤波器有没有“病”调滤波器最怕的是没有直观反馈。建议在调试界面上同时显示三样东西原始观测值、滤波输出值、新息序列观测减预测。如果新息均值明显偏离0说明系统存在系统性偏差可能是传感器标定问题也可能是模型不匹配如果新息方差明显大于R说明R设置过小滤波过于信任观测如果新息方差明显小于R说明R设置过大滤波过于迟钝。7.4 卡尔曼滤波使用问题速查现象可能原因排查思路滤波输出发散数值狂跳Q过大P0过大减小过程噪声Q缩小初始P0滤波响应太慢跟不上目标Q过小R过大增大Q减小R滤波输出太“贴”观测噪声大R过小增大R或检查传感器噪声统计新息序列有恒定偏移传感器存在系统性偏差校正标定或增加状态向量维度如偏差项P矩阵发散成天文数字Q设置不合理或模型错误重新检查状态转移矩阵F、过程噪声Q8. 从概率统计到卡尔曼滤波的最后一块拼图8.1 把全流程串起来现在把前面所有内容串成一个完整的故事。初始时刻我们对目标状态有一个“先验高斯分布”均值是μ0协方差是P0。目标移动时我们根据运动模型做预测预测结果还是一个高斯分布但方差变大了因为模型并不完美要加上过程噪声Q。这是预测步。传感器送来一个观测值我们根据传感器噪声模型知道观测的分布特性。观测和预测之间存在差异我们用卡尔曼增益作为信任权重融合两者得到后验高斯分布方差比预测和观测都小。这是更新步。然后循环往复。整个过程只有一个核心数学工具用高斯分布描述状态用贝叶斯公式做条件概率更新用协方差矩阵描述不确定性在状态维度和时间维度上的传播。8.2 下期预告卡尔曼滤波公式推导到这里概率统计基础的前瞻内容就结束。下一篇会正式讲解卡尔曼滤波的完整公式推导从一维的标量卡尔曼滤波开始一步步扩展到多维的向量形式并结合RM里的实际状态方程把预测步、更新步、增益计算的每个细节掰开揉碎。8.3 给你留几个自查题在进入下一讲之前建议确认自己能不能回答这几个问题贝叶斯公式里先验、似然、后验分别对应卡尔曼滤波中的哪个环节为什么协方差矩阵经过线性变换后会变成A P Aᵀ试着用一维的方差传播公式验证。高斯分布的哪条性质保证了卡尔曼滤波可以无限递推下去如果两个传感器的测量方差分别是R1和R2最优融合结果的方差是多少为什么比任何一个单独的方差都小如果这几个问题都能不看公式、用自己的话解释清楚那卡尔曼滤波正式内容对你来说就会顺畅很多。我个人这几年的体会是卡尔曼滤波本身不复杂复杂的是很多教程一上来就堆公式跳过了概率统计这个地基。地基打牢了后面不管是标准卡尔曼、扩展卡尔曼还是无迹卡尔曼就都是在同一个思想框架下换不同的近似手段而已。希望这篇前瞻能帮你把地基打牢我们下一讲正式进入卡尔曼滤波公式。