卡尔曼滤波入门必看：从概率统计到贝叶斯估计的核心基础

打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 卡尔曼滤波使用问题速查

8. 从概率统计到卡尔曼滤波的最后一块拼图

8.1 把全流程串起来

现在把前面所有内容串成一个完整的故事。

初始时刻，我们对目标状态有一个“先验高斯分布”，均值是μ0，协方差是P0。目标移动时，我们根据运动模型做预测，预测结果还是一个高斯分布，但方差变大了，因为模型并不完美，要加上过程噪声Q。这是预测步。

传感器送来一个观测值，我们根据传感器噪声模型知道观测的分布特性。观测和预测之间存在差异，我们用卡尔曼增益作为信任权重，融合两者得到后验高斯分布，方差比预测和观测都小。这是更新步。

然后循环往复。整个过程只有一个核心数学工具：用高斯分布描述状态，用贝叶斯公式做条件概率更新，用协方差矩阵描述不确定性在状态维度和时间维度上的传播。

8.2 下期预告：卡尔曼滤波公式推导

到这里，概率统计基础的前瞻内容就结束。下一篇会正式讲解卡尔曼滤波的完整公式推导，从一维的标量卡尔曼滤波开始，一步步扩展到多维的向量形式，并结合RM里的实际状态方程，把预测步、更新步、增益计算的每个细节掰开揉碎。

8.3 给你留几个自查题

在进入下一讲之前，建议确认自己能不能回答这几个问题：

1. 贝叶斯公式里，先验、似然、后验分别对应卡尔曼滤波中的哪个环节？
2. 为什么协方差矩阵经过线性变换后会变成A P Aᵀ？试着用一维的方差传播公式验证。
3. 高斯分布的哪条性质保证了卡尔曼滤波可以无限递推下去？
4. 如果两个传感器的测量方差分别是R1和R2，最优融合结果的方差是多少？为什么比任何一个单独的方差都小？

如果这几个问题都能不看公式、用自己的话解释清楚，那卡尔曼滤波正式内容对你来说就会顺畅很多。

我个人这几年的体会是：卡尔曼滤波本身不复杂，复杂的是很多教程一上来就堆公式，跳过了概率统计这个地基。地基打牢了，后面不管是标准卡尔曼、扩展卡尔曼还是无迹卡尔曼，就都是在同一个思想框架下换不同的近似手段而已。希望这篇前瞻能帮你把地基打牢，我们下一讲正式进入卡尔曼滤波公式。