做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的常见传感器整理一个噪声量级的参考表格。传感器/数据典型噪声来源噪声量级标准差滤波器关注点陀螺仪yaw角速度零偏漂移、电路噪声0.005~0.02 rad/s云台速度环前馈、姿态解算测距模块激光/超声波目标反射面、温度、串口抖动0.01~0.05 m弹道解算、避障光电编码器速度量化误差、低速丢步低速时相对明显底盘速度闭环视觉测距单目/双目像素误差、标定误差0.05~0.2 m自瞄距离估计这张表不是让你背下来而是让你明白一个道理不同传感器、不同工况下噪声的量级完全不同。低速时编码器的速度信号噪声比例会比高速时大很多测距模块在目标倾斜反光时标准差会显著增大。所以在设计滤波器时R矩阵测量噪声不能是一成不变的固定值至少要在不同工况下做区分。这就是概率统计带来的工程思维所有传感器读数都是带噪声的观测样本评估一个传感器好不好不是看它单个读数准不准而是看它的统计特性——均值是否无偏方差是否足够小。我在带新队员时经常让他们做一件事拿一段真实采集的数据算标准差然后和滤波估计出来的误差对比。如果滤波后的残差标准差明显大于传感器原始噪声标准差说明滤波器参数有问题或者模型不对而不是传感器坏了。3. 状态估计的本质条件概率与贝叶斯视角3.1 联合概率、条件概率与独立事件接下来要进入卡尔曼滤波最核心的思维方式了。让我们从条件概率说起。假设两件事A和B。P(A|B)表示已知B发生的情况下A发生的概率。在RM场景里A可以是机器人在位置xB可以是测距模块读数为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 常见问题速查我把带队伍这些年常见的问题整理成一个速查表方便后面排查。现象可能原因排查方向滤波输出发散、冲到极大值系统模型错误、状态方程写错、P初值过小检查状态转移矩阵用仿真数据先验证滤波过于平滑、跟不上快速变化Q过小、R过大增大Q、减小R检查运动模型是否遗漏加速度项滤波抖动明显、贴近原始测量R过小、Q过大减小Q、增大R、检查测量是否有离群点滤波收敛后仍有固定偏差传感器存在系统性偏差检查和标定零偏、修正状态方程P矩阵出现负值或不对称数值计算误差强制对称化、使用高精度数据类型、考虑平方根滤波特别要提醒的是如果滤波输出发散第一反应不要调参数先检查代码。最常见的错误是状态转移矩阵里角度和角速度的耦合关系写错或者把时间步长代入了错误的单位毫秒和秒混用。经验之谈所有涉及时间的地方统一用秒尤其是在把dt传进状态矩阵时这个bug我在好几个队伍里都见过。5.4 后续学习路线和一点扩展思路概率统计打底之后接下来的路线就清晰了。先实现一维卡尔曼滤波理解五条公式的物理含义然后扩展到多维处理好状态向量和P矩阵再往后就是工程里最常用的EKF处理云台自瞄里角度、角速度的非线性关系以及视觉测距、弹道解算这类带非线性观测的问题。还有一个经常被忽略的思路协方差矩阵不只是滤波器内部的计算工具它还能用来评估传感器融合的效果。比如当你融合了视觉测距和编码器里程计后如果P矩阵对角线元素明显小于任何一个单一传感器的方差说明融合确实带来了信息增益。这个量化的验证思路比单纯看感觉变稳了要靠谱得多。我在实际做RM过程中体会最深的一点是卡尔曼滤波不是某个固定公式的套用而是一套在不确定中做最优决策的思维方式。你用它来融合传感器数据也用同样的思路来处理系统误差、模型失配这些工程问题。概率统计这层基础看起来是数学实际上是一种从噪声中提取信息的本能。磨刀不误砍柴工这篇基础过了后面上真算法会快得多。