卡尔曼滤波入门：从概率统计与高斯分布理解状态估计

做RM电控这几年，最绕不开的算法就是卡尔曼滤波。不管是云台自瞄的陀螺仪数据融合，还是步兵车上的测距模块、弹道解算，甚至底盘里程计定位，到处都能看到它的影子。但很多队员第一次接触卡尔曼滤波，抄了一版代码、调了几个参数，能跑起来就觉得完事了，一旦需要改传感器、换结构，整个滤波就崩得没法看。原因很简单：没搞懂卡尔曼滤波背后的数学逻辑，尤其是概率统计这一层基础。

这个系列我打算从最底层开始，把卡尔曼滤波从头梳理一遍。第一篇不讲公式推导，不贴代码，先把概率统计的基础讲透。因为卡尔曼滤波本质上就是一个在不确定性中做最优估计的框架，它所有的"预测"和"更新"，全部建立在概率分布、期望、方差、协方差、条件概率这些概念之上。搞不懂这些，后面看任何推导都是空中楼阁。这篇文章主要面向RM电控组的新队员，也适合那些想真正理解卡尔曼滤波、而不是只会调参的开发者。我尽量用赛场上的真实场景来讲，让数学概念落在具体的工程直觉上。

1. 为什么RM电控要花一整篇讲概率统计

1.1 云台、弹道、定位：三个逃不开噪声的场景

先回忆一下赛场上最常见的难受场景。云台在快速转动的时候，陀螺仪的yaw轴角速度数值会上下跳动，明明电机匀速转着，串口打印出来的角速度却像锯齿一样，这就是典型的测量噪声。再看测距模块，几米外放一块装甲板，同一位置连续测几十次，返回值不可能完全一样，跳个几厘米是家常便饭。还有弹道解算时用的弹速初值，每颗弹丸的上旋强度不同，落点就跟着抖。

这些场景里，我们最想要的是某个"真实值"——真实的角速度、真实的距离、真实的弹着点。但传感器给我们的永远是"测量值"，而测量值里混入了噪声。更麻烦的是，真实值本身也未必恒定，云台在加速转动时，真实角速度每秒都在变。这就引出一个关键问题：怎么在充满噪声的观测中，尽量准确地估计出那个看不见的真实状态？

只靠一次测量肯定不行，但我们可以用多次测量、用系统的运动规律，把噪声声一点点"磨"掉。这就是滤波算法干的事。卡尔曼滤波之所以强大，是因为它不仅有当前观测，还结合了对系统运动的预测，并且知道预测和观测各自有多可信。这个"有多可信"，在数学上就是方差和协方差，也就是概率统计里最核心的语言。

1.2 卡尔曼滤波到底在做一件什么事

把卡尔曼滤波想成一个老练的射手在调自己的瞄准点。每打一枪，靶纸上的弹着点是一个"观测值"，弹着点受风速、手抖、枪械偏差影响，既散乱又有一定规律。射手心里对靶心位置有一个"预测"，也就是之前打出的平均弹着点附近。如果这一枪落点离预测很近，他就轻微修正；如果落点明显偏离，他会更多参考这个新弹着点，但也不会完全信，因为可能只是偶然误差。

卡尔曼滤波做的事情完全一样。系统运动学告诉我们"下一步状态大概在哪里"，这是预测；传感器告诉我们"状态现在观测到在哪里"，这是更新。最终的最优估计，是预测和观测的加权平均，而权重取决于预测的不确定度和观测的不确定度。如果传感器很准，就多信观测；如果运动模型很准，就多信预测。

这套逻辑里，"不确定度"不是一个笼统的感觉，而是一个可以计算、可以更新、可以传播的数值，也就是方差。所以想理解卡尔曼滤波，第一条命脉就是搞清楚方差如何描述信息质量。这也是为什么所有正经讲卡尔曼滤波的资料，都要从概率统计开始讲起。

1.3 先学概率统计，是在给后面的推导做准备

卡尔曼滤波有五条核心公式，看起来就是矩阵运算，很多人卡住就是因为不知道这些矩阵里的每个元素代表什么。P矩阵为什么叫协方差矩阵？Q和R矩阵的数值为什么影响滤波效果？Kgain（卡尔曼增益）为什么是两个方差的比例？

这些问题不回到概率论，永远只能靠试。你调R和Q感觉是在调两个神秘旋钮，但如果你知道R代表传感器噪声的方差，Q代表运动模型误差的方差，调参就有了方向：测距模块噪声大就调大R，底盘打滑严重就调大Q。更关键的，如果理解了一维高斯分布融合的数学形式，扩展卡尔曼滤波（EKF）和一阶泰勒展开、无迹卡尔曼滤波（UKF）的sigma点选取，都只是在这个基础上做变形。

所以这个系列的前瞻篇，值得认真看。基础不牢，后面全是空中楼阁；基础打牢了，整套算法其实很顺。

2. 绕不开的高斯分布：RM赛场上最常用的概率模型

2.1 从一组IMU数据说起，理解随机变量和概率分布

假设我们把云台固定在一个大转台上，以恒定角速度旋转，大量采集陀螺仪的yaw角速度数据。把这些数据画成直方图，你会发现一个很有意思的形状：中间高、两边低，左右大致对称，像一口倒扣的钟。绝大多数值堆在某个中心值附近，离中心越远，出现次数越少。

这个"中心值"就是真实角速度的估计值，而数据的离散程度体现了噪声的强度。用概率论的视角看，每一次测量都是一个随机变量，它有一个真实的分布。高斯分布（正态分布）就是描述这种"中间集中、两边稀疏"现象的数学工具，数学形式就是那个带e的负二次方的钟形曲线，只需要两个参数：均值μ和方差σ²。

很多新队员一看到高斯分布的概率密度函数就头大，其实工程上根本不用纠结怎么手工算这个式子。你要理解的是两件事：第一，高斯分布完全由均值和方差决定，知道这两个数，就掌握了整个分布的信息；第二，传感器的噪声在大多数情况下可以近似认为服从高斯分布。之所以说"近似"，是因为严格的数学高斯分布要求噪声对称、没有厚尾、没有离群点，而真实传感器偶尔会有跳变，所以工程上我们常说"近似高斯噪声"。这个近似在绝大多数RM场景下是够用的。

2.2 期望和方差：数据的"中心"和"抖动程度"

期望（均值）描述了一组数据的中心位置。在RM里，把一段静止状态下的陀螺仪数据取平均，就能得到一个相对稳定的零偏估计。这就是IMU标定里常用静态均值的原因。

方差和标准差描述的是数据围绕均值的波动程度。标准差越大，数据越散；标准差越小，数据越集中。工程上有一个非常实用的判断技巧：对于一个近似高斯分布的噪声，大约68%的数据落在均值±1σ范围内，95%落在±2σ范围内，99.7%落在±3σ范围内。也就是说，如果你统计出一段IMU角速度数据的标准差是0.01 rad/s，那么大多数测量值落在真实值±0.01 rad/s内，偶尔出现±0.03 rad/s的偏差也是正常的。这个直觉在判断滤波效果的时候特别有用，后面会反复用到。

在实际工作中，我建议每个电控队员都养成一个习惯：拿到一个新的传感器，先花一个下午采集数据，计算均值、标准差，画出直方图看看分布形状。这是所有滤波算法调参的起点。

2.3 为什么高斯分布被卡尔曼滤波看中

卡尔曼滤波对噪声的假设之一就是高斯噪声，这不是随便选的。高斯分布有好几个性质让它成了滤波理论里的"天选之子"。

第一是叠加性。两个高斯随机变量相加，结果仍然是高斯分布，均值和方差都有解析表达式，这让状态传播的计算非常方便。第二是线性变换不变性。一个高斯变量乘以常数、加上常数，结果仍然是高斯分布，这正好对应了卡尔曼滤波里线性系统的假设。第三是对称和单峰的特性，让后验概率的峰值可以直接用来作为状态的最优估计。

这三个性质保证了在系统是线性、噪声是高斯的条件下，"预测分步"和"更新分步"都能保持高斯形式。也就是说，整个估计过程只需要不断更新均值和方差即可，不需要维护一个无限复杂的概率分布函数。这个"只更新均值+方差"的性质，就是卡尔曼滤波计算高效的底层原因。

你可以对比一下，如果噪声不是高斯的，或者系统不是线性的，整个计算就会爆炸性地复杂化。这就是为什么工程上我们尽量把系统建模成线性，噪声也默认近似高斯。这不只是为了数学好看，更是为了计算可行。

2.4 用标准差理解传感器噪声的工程直觉

下面结合RM的常见传感器，整理一个噪声量级的参考表格。

这张表不是让你背下来，而是让你明白一个道理：不同传感器、不同工况下，噪声的量级完全不同。低速时编码器的速度信号噪声比例会比高速时大很多；测距模块在目标倾斜反光时，标准差会显著增大。所以在设计滤波器时，R矩阵（测量噪声）不能是一成不变的固定值，至少要在不同工况下做区分。

这就是概率统计带来的工程思维：所有传感器读数都是"带噪声的观测样本"，评估一个传感器好不好，不是看它单个读数准不准，而是看它的统计特性——均值是否无偏，方差是否足够小。我在带新队员时经常让他们做一件事：拿一段真实采集的数据，算标准差，然后和滤波估计出来的误差对比。如果滤波后的残差标准差明显大于传感器原始噪声标准差，说明滤波器参数有问题或者模型不对，而不是传感器坏了。

3. 状态估计的本质：条件概率与贝叶斯视角

3.1 联合概率、条件概率与独立事件

接下来要进入卡尔曼滤波最核心的思维方式了。让我们从条件概率说起。

假设两件事A和B。P(A|B)表示"已知B发生的情况下，A发生的概率"。在RM场景里，A可以是"机器人在位置x"，B可以是"测距模块读数为3米"。P(A|B)的含义就是：既然测距模块告诉我距离是3米，那机器人真实位置是x的可能性有多大。这个"反过来的条件概率"，就是状态估计的目标。

联合概率P(A,B)描述的是A和B同时发生的概率。如果两件事互不影响，那么P(A,B)=P(A)P(B)，这就是独立性。在传感器融合里，我们通常假设不同传感器的噪声是相互独立的，比如陀螺仪的误差和测距模块的误差基本无关。这个假设让计算大幅简化。

但要注意，状态变量之间往往不是独立的。比如机器人的位置和速度，如果位置在快速变化，那速度必然很大。后面讲协方差的时候会展开说这个。现在只要记住：条件概率是"已知部分信息后修正不确定性的工具"，而状态估计的本质就是求条件概率P(状态|所有观测)。

3.2 贝叶斯公式：用观测修正先验

P(状态|观测)这个条件概率，方向是从观测到状态，而传感器模型给我们的通常是P(观测|状态)——即"已知真实状态，观测会是什么分布"。从后者推前者，就需要贝叶斯公式。

贝叶斯公式的形式是：后验概率等于似然乘以先验再除以归一化常数。翻译成工程语言就是：先验概率是我在获得这次观测之前，对状态的估计（来自运动模型的预测）；似然是当前的观测对每个可能状态的支持程度（来自传感器模型）；后验概率是综合预测和观测之后，修正得到的状态分布。

卡尔曼滤波的"预测-更新"循环，就是贝叶斯公式的线性高斯版本。预测步骤计算先验：根据运动模型和上一时刻的后验，推算出当前状态的均值和方差。更新步骤把当前观测作为似然乘进来，得到后验：一个融合了预测和观测的高斯分布。后验分布的均值，就是卡尔曼滤波输出的最优状态估计。

这样看，卡尔曼滤波在数学上就不是什么神秘魔法了。它就是一个在线运行的贝叶斯估计器，每次只处理当前一帧观测，递归更新对状态的信念。这也是为什么卡尔曼滤波的初始化可以有偏差，但通过不断更新会逐渐收敛——因为每帧观测都在修正先验，把估计拉向真实的测量。

3.3 卡尔曼滤波的预测-更新循环，就是贝叶斯思想在跑

我把这个过程再落地一点，结合RM里的一维距离估计来走一遍。

假设步兵车在定点巡查，测距模块每秒输出10次距离值。初始化时，我们不知道机器人在哪，先估计一个大概值，方差设得很大，表示"非常不确定"。这是先验。预测步骤：因为机器人底盘静止（或有里程计速度），我们可以根据运动模型预测下一帧距离大概不变或移动一点点，同时方差会略微增大，因为机器人可能有微小滑动（过程噪声）。这是先验的更新。

测量步骤：测距模块返回一个值，比如4.02米。传感器模型告诉我们，如果真实距离是4米，测出4.02米的可能性有多大；如果真实距离是5米，测出4.02米的可能性又有多大。传感器越准，可能性就集中在真实距离附近越窄的区间。贝叶斯更新把预测分布和这个似然分布相乘，得到一个新的高斯分布。这个分布综合了"模型预测我在4.0附近"和"传感器说我在4.02附近"两组信息，方差会变小，均值向更可信的一方靠近。

如果传感器的方差远小于预测方差，结果几乎完全信任传感器；反过来，如果预测方差很小（比如底盘确实很稳的静止状态），而传感器噪声大，结果就会更偏向预测。这个"自动权衡可信度"的过程，就是卡尔曼增益在做的事。理解了这一点，你再看卡尔曼滤波公式里的增益计算，会发现它就是两个方差的一个比例，完全在直觉范围内。

4. 多维状态与协方差矩阵：云台角度和角速度一起估计时怎么办

4.1 从一个云台yaw轴的例子出发

前面举的例子都是一维的，就是只估计一个量。但RM里几乎没有这么简单的情况。拿云台yaw轴来说，做自瞄时需要同时估计角度和角速度。因为云台的控制器不仅需要角度偏差，还需要角速度来预测目标运动、提供前馈。于是状态向量变成了二维：(角度, 角速度)。

问题来了：角度和角速度不是独立的。从数学上讲，角速度本来就是角度的导数，它们在时间上是耦合的。从物理直觉上讲，如果角度正在快速增大，那角速度一定是比较大的正值，这两者之间存在统计上的相关关系。只单独知道角度均值3.0 rad、角速度均值0.5 rad/s，还不足以完整描述状态。你还需要知道，当角度偏离均值时，角速度是倾向于同方向偏离还是反方向偏离，以及这种关联有多强。

这就引出了协方差的概念。协方差描述的是两个随机变量一起变化的趋势。协方差为正，表示一个变量增大时另一个也倾向于增大；协方差为负，表示一个增大另一个倾向于减小；协方差接近零，表示它们的变化基本无关。在云台的例子里，角度和角速度的协方差通常是正的——因为角度变化越快，角速度越大。

4.2 协方差和相关系数：变量之间到底有多"同步"变化

协方差的具体数值会受到变量自身尺度的影响。角度的数量级在弧度（0~6.28），角速度的数量级在rad/s（可能-10到10），直接把它们的协方差拿来比较大小意义不大。更直观的是相关系数，它把协方差除以两个标准差的乘积，归一化到-1到1之间。接近1是强正相关，接近-1是强负相关，接近0是不相关。

在卡尔曼滤波里，我们关心的不是相关系数，而是协方差矩阵P。P矩阵是一个对称方阵，对角线元素是各个状态变量的方差，非对角线元素是状态变量之间的协方差。对于二维状态(角度, 角速度)，P就是两行两列的对称矩阵。左上角是角度的方差，右下角是角速度的方差，左下角和右上角相等，是两者的协方差。

P矩阵在整个滤波过程中不断被更新。预测步骤会让对角线的方差增大（因为运动模型引入了过程噪声，不确定度增加），同时会改变协方差项（因为状态方程把角度和角速度耦合起来了）。更新步骤则会整体压缩P矩阵（因为观测提供了新信息，不确定度降低）。追踪P矩阵的变化，是判断卡尔曼滤波是否正常工作的一个直观方法。我调试时经常把P矩阵对角线元素开根号打印出来，它们就是当前状态估计的标准差，能直接看出滤波对每个状态量有多"有信心"。

4.3 协方差矩阵的物理含义与初值设置思路

初值的设置是一个新手容易纠结的点。P矩阵初始值反映的是最初对状态估计的不确定度。如果初始状态完全未知，可以把P设得大一些，比如角度的初始标准差设为0.1 rad（对应P矩阵元素为0.01），角速度的初始标准差设为1 rad/s（对应P矩阵元素为1）。如果初始状态有把握，比如上电后读取编码器得到准确角度，可以把角度方差设得很小，但角速度方差通常还是设大一点，因为上电瞬间的角速度估计往往不可靠。

记住一个原则：P初值偏大不会导致发散，只是让滤波开始阶段更信任测量、收敛需要更多帧；P初值过小则可能导致滤波对预测过于自信，更新量很微弱，如果预测模型不准，估计会长期偏离真实值。所以实际工程中，宁大勿小。

还要留意P矩阵中的协方差项。在线性卡尔曼里，协方差项会随着预测和更新自然演化，不需要手工精细设定。你只要保证P矩阵是合法的协方差矩阵（对称且半正定），就可以让它自己跑。常见的数值问题，比如P矩阵因为计算误差变得不对称甚至出现负方差，通常需要通过强制对称化或者改用平方根滤波来解决。这个后面写实现篇再展开。

5. 实操心得：从概率统计到调出第一版卡尔曼

5.1 先仿真，后上车的必要流程

很多RM队伍喜欢直接把卡尔曼滤波代码烧到板子上调，我觉得这是效率最低的做法。调参的时候，你分不清是代码写错了、模型建错了，还是参数不合适。更好的流程是先仿真。

用Python或者MATLAB写一个一维的距离估计仿真：生成一组带高斯噪声的数据，真实值设为一个常值或恒速变化，然后实现一个最简单的一维卡尔曼滤波，看看估计结果和真实值之间的误差。改变R和Q，观察估计曲线是更平滑还是更跟随，直到你完全能预测滤波器的行为。

这一步花不了多少时间，但收益极大。它把概率统计里那些抽象概念变成了看得见的曲线。比如，你会亲眼看到R增大时滤波曲线变得平滑但滞后，R减小时曲线变得贴近测量但毛糙；你也会看到Q过小时滤波对运动变化反应迟钝，Q过大时滤波几乎不做平滑。这些直觉，比任何公式都管用。

5.2 记录真实数据来估计噪声方差

仿真跑通了，再回到真车上。采集真实传感器的数据，离线用Python统计方差，作为R矩阵的初值。不要把R随便设成0.01，哪怕只是量级对，也比完全拍脑袋强很多。对于Q矩阵，因为没有"真值"可以统计，通常先设小一点，然后根据滤波效果逐步调大。

我个人的经验是：先固定R为传感器实测方差，Q从很小开始（比如1e-4量级，取决于状态量的尺度），观察滤波输出。如果滤波曲线明显滞后于真实变化，说明Q过小，增大Q；如果滤波输出毛糙、抖动大，说明Q过大或R过小。通过几次来回，基本能锁定一个可用的范围。这个过程其实就是在调试"预测可信度"和"测量可信度"的权重，跟贝叶斯公式里先验和似然的关系是一回事。

5.3 常见问题速查

我把带队伍这些年常见的问题整理成一个速查表，方便后面排查。

特别要提醒的是，如果滤波输出发散，第一反应不要调参数，先检查代码。最常见的错误是状态转移矩阵里角度和角速度的耦合关系写错，或者把时间步长代入了错误的单位（毫秒和秒混用）。经验之谈：所有涉及时间的地方统一用秒，尤其是在把dt传进状态矩阵时，这个bug我在好几个队伍里都见过。

5.4 后续学习路线和一点扩展思路

概率统计打底之后，接下来的路线就清晰了。先实现一维卡尔曼滤波，理解五条公式的物理含义；然后扩展到多维，处理好状态向量和P矩阵；再往后就是工程里最常用的EKF，处理云台自瞄里角度、角速度的非线性关系，以及视觉测距、弹道解算这类带非线性观测的问题。

还有一个经常被忽略的思路：协方差矩阵不只是滤波器内部的计算工具，它还能用来评估传感器融合的效果。比如，当你融合了视觉测距和编码器里程计后，如果P矩阵对角线元素明显小于任何一个单一传感器的方差，说明融合确实带来了信息增益。这个量化的验证思路，比单纯看"感觉变稳了"要靠谱得多。

我在实际做RM过程中体会最深的一点是，卡尔曼滤波不是某个固定公式的套用，而是一套"在不确定中做最优决策"的思维方式。你用它来融合传感器数据，也用同样的思路来处理系统误差、模型失配这些工程问题。概率统计这层基础，看起来是数学，实际上是一种从噪声中提取信息的本能。磨刀不误砍柴工，这篇基础过了，后面上真算法会快得多。