卡尔曼滤波前置课：概率统计、高斯分布与协方差矩阵在电控中的应用

很多朋友问中科大RM电控合集进了卡尔曼滤波这个专题之后为什么第一讲不讲滤波公式、不推状态方程反而先讲概率统计。这个问题问得很实在。卡尔曼滤波整套数学框架的本质就是用概率的语言描述我们对系统状态知道多少、不知道多少再按照贝叶斯规则把这些信息合到一起。如果概率统计的底子不牢矩阵公式背得再熟也是空中楼阁到了赛场上真机一跑噪声分布、初值设定、Q阵R阵调参处处都会卡壳。这篇我就把卡尔曼滤波真正需要用的概率统计基础掰开揉碎讲清楚全程结合RoboMaster电控的实际场景适合刚接触滤波、或者懂公式但不知道怎么对应物理意义的同学。1. 为什么卡尔曼滤波之前必须先补概率统计1.1 从RM赛场的预估击杀说起RoboMaster赛场上步兵机器人云台要打中远处的移动能量机关或者追踪敌方机器人的时候控制程序面临一个很现实的问题目标的位置不是直接可测的而是带有噪声的观测。摄像头测到的像素坐标有延迟、有波动陀螺仪测角速度有漂移、有高频噪声弹道还要考虑重力下坠和空气阻力。你如果直接用当前观测值去瞄准那些噪声会被放大器放大云台会疯狂抖动弹丸落点散得没法看。所以大家都会做一个预测不只看当前这一帧还把前面几帧的信息都用上推测目标下一时刻最可能在哪个位置。这个预测用到的原理本质上就是概率统计。因为目标的位置在我们脑子里不是一个确切的点而是一个分布——一个它可能在这里也可能在那里的分布。传感器给了一个观测这个分布就缩窄一点系统自己往前推一拍这个分布又会变宽一点。卡尔曼滤波做的事情就是不断重复预测让分布变宽、观测让分布变窄的过程时时刻刻输出那个分布的中心和最可能的范围。1.2 卡尔曼滤波不是预测未来而是融合不确定性很多人一听到滤波预测就以为是算出一个未来值。其实卡尔曼滤波的输出从来不是一个拍脑袋的未来点而是一个高斯分布均值代表最可能的状态协方差代表我们对这个状态的信任程度。如果把这个信任程度丢掉只把算法当成一个平滑器那就会错过它最重要的能力——帮你回答我现在到底有多确定。在比赛里这个能力特别有价值。比如云台在快速旋转的时候视觉识别偶尔丢帧你要是只靠一个预测值硬顶可能目标已经跑出视野了你还按旧速度瞄。但如果你知道协方差越来越大说明不确定性越来越大此时就应该切回匀速模型继续推或者触发搜索策略。这就是概率统计的语言带来的工程判断力跟单纯背卡尔曼公式完全是两码事。1.3 这个系列的前置路线图我打算把卡尔曼滤波这个主题拆成三篇第一篇是概率统计基础也就是现在这篇扫清随机变量、分布、期望方差、协方差、贝叶斯公式这些概念第二篇讲卡尔曼滤波的核心推导从一维到多维对应预测步和更新步第三篇讲工程落地包括Q阵R阵调参、传感器噪声估计、以及扩展卡尔曼滤波EKF在自瞄中的应用。这一篇把概率统计讲透后面的公式就不再是死记硬背而是每个符号都能对应到云台上的某个具体物理量。2. 随机变量、概率分布与期望方差先把描述不确定性的语言学会2.1 随机变量从目标在哪儿说起数学上随机变量是在一个随机试验中可能取不同值的变量它不是一个固定的数而是取值服从某种规律的数。听起来抽象放到RM里就很好懂目标装甲板中心的yaw角是一条随机变量因为每一帧测量出来都会有点不一样弹丸从枪管里飞出去之后弹着点的x坐标也是一条随机变量因为每次发射都受轻微扰动。关键的一点是随机变量的随机不代表瞎碰它是有分布规律的。我们研究随机变量本质上就是研究它的分布。比赛中你不可能知道下一帧测量值具体是多少但你可以知道它有百分之多少的概率落在某个范围内。这就是概率统计给工程师的底气。2.2 概率分布与概率密度函数对于离散型随机变量比如目标类别识别结果是敌方步兵还是英雄我们能列一个概率表。对连续型随机变量比如角度、距离、速度就需要用概率密度函数PDF来描述。PDF有一个直观解释在某一点附近取值的概率密度有多大曲线越高说明随机变量落在该点附近的可能性越大总面积等于1。大多数情况下PDF曲线看起来像一座山。这座山的位置和形状就是我们关心的全部信息。卡尔曼滤波里状态变量被建模成连续型随机变量而且用高斯分布去近似描述就是为了用山的峰顶和坡度来代表状态估计的均值和不确定性。后文会专门讲高斯的性质这里先建立分布是一座山的直觉。2.3 期望与方差状态的中心和不确定度期望就是随机变量的平均值对连续分布来说就是按照概率密度加权求积分。放在云台上你对目标yaw角的期望就是平均来看目标角是多少也就是卡尔曼滤波输出的那条估计曲线。方差描述的是取值围绕期望的分散程度方差越小说明我们对状态的把握越大方差越大说明越没谱。这里有个新手容易忽略的点期望和方差是两个互相独立的维度。一个系统的状态可能期望很稳定但方差很大也可能期望剧烈变化但每个时刻的方差很小。比如陀螺仪短期内没有漂移期望稳定但高频噪声的方差大而目标处于快速转向时角速度期望在变但视觉观测噪声方差可能是恒定的。卡尔曼滤波的核心就是把这两个维度同时维护起来。2.4 RM中常用的均匀分布与高斯分布在建模传感器噪声的时候最常见的是两种分布均匀分布和高斯分布。均匀分布表示在某个区间内等概率取值。比如视觉识别坐标系里像素量化误差可以粗略建模成均匀分布。均匀分布的方差和区间长度的平方成正比如果量化步长是1个像素那方差大约是1/12。这在估算最小噪声下限的时候很有用。高斯分布也叫正态分布绝大多数传感器噪声在工程上都可以近似看作高斯分布。它有两个参数——均值μ和方差σ²就对应我们前面说的期望和不确定度。高斯分布用起来方便是因为它的数学性质好线性变换后仍然是高斯分布两个独立高斯随机变量相加仍是高斯分布。这两条性质简直是为卡尔曼滤波量身定做的后面第五节专门讲。3. 协方差矩阵与多元高斯分布一维单点不够用电控需要多维状态3.1 为什么一维描述不够用如果你只关心目标当前的角度用一个一维高斯分布就够了。但是卡尔曼滤波在电控里往往同时估计角度和角速度甚至角度、角速度、加速度一起估计。这个时候问题来了角度和角速度不是独立的。目标在做匀加速运动的时候角度和角速度之间有明确的耦合关系在观测到角度突然跳变的时候角速度的估计也会受影响。所以你不能简单把它们拆开各搞一个一维高斯你必须描述角度和角速度这两个随机变量之间的相关关系。这就是协方差矩阵出现的动机。它不只记录每个变量自己的方差还记录变量与变量之间的协方差正好对应了状态估计中的你知道角度偏了也顺便知道角速度很可能也偏了这一层信息。3.2 协方差与协方差矩阵两个随机变量X和Y的协方差定义是它们各自减去期望之后乘积的期望。协方差为正说明两者倾向于一起变大或一起变小协方差为负说明一个变大的时候另一个倾向变小协方差为0说明它们不相关。对n个状态变量来说两两之间的方差异可以排成一个n×n矩阵对角线是各自的方差非对角线是两两之间的协方差。这个矩阵就叫协方差矩阵。卡尔曼滤波里的P矩阵就是这个协方差矩阵它完整描述了状态向量这个多元随机变量的不确定性和内部相关性。RM中一个典型的三维状态向量可能是 [yaw, yaw_rate, yaw_accel]它的协方差矩阵就是3×3。矩阵数值不是随便填的它应该体现物理规律yaw_rate越大yaw的变化就越剧烈两者之间的协方差在预测步中就会被状态转移矩阵推着变大。后面我们会看到卡尔曼滤波里P矩阵每一步都在被两个方程更新一个是状态转移导致的协方差传播一个是观测更新导致的协方差收缩。3.3 多元高斯分布的形状与几何意义多元高斯分布是在n维空间里定义的高斯分布。当n2时它的概率密度函数的等值线是一个椭圆椭圆中心就是均值向量椭圆的长短轴方向和长度由协方差矩阵的两个特征向量和特征值决定。协方差矩阵越接近单位矩阵的倍数椭圆越像圆表示各分量独立且方差相近如果椭圆很扁说明两个分量有强相关关系比如角度大时角速度大它们沿某条直线分布得更集中。这个几何直觉非常有用。在RM调试的时候如果你能可视化出状态分布的椭圆就能一眼看出估计到底哪里靠谱哪里不靠谱。比如yaw方向的方差比pitch方向的方差大很多椭圆会在yaw方向拉长说明现在对yaw的估计不够确定可能是视觉横向分辨率不够高也可能是运动模型没配好。3.4 一个RM中的实际例子云台角度与角速度的联合分布举一个实际建模的例子。目标在图像平面上从右往左运动我们在某个时刻估计得到yaw0.2 radyaw_rate-1.5 rad/s。假设两者的协方差是负的这意味着什么如果这个时刻目标实际yaw比0.2大那么它的角速度很可能比-1.5更负也就是目标在更靠右的位置上移动更快。这种负相关关系其实反映了目标运动的连续性一个高速目标不可能突然停在某个位置。卡尔曼滤波在更新步计算增益K的时候其实是根据协方差矩阵里的相关性做合理补偿的。如果你只测到了yaw没有直接测到yaw_rate但因为协方差矩阵已经知道yaw和yaw_rate的相关性它就能在更新yaw的同时顺带修正yaw_rate的估计。这个用相关关系互相修正的能力正是多维卡尔曼比多个一维低通滤波器强得多的地方。很多电控新手发现用卡尔曼比用一阶低通好了不少原因就在这个协方差矩阵上。4. 条件概率与贝叶斯公式卡尔曼滤波的推理逻辑线4.1 条件概率已知陀螺仪读数目标在哪条件概率描述的是在已知某个信息的前提下另一个随机变量的分布会变成什么样。比如云台控制里传感器已经读到了目标中心在像素坐标系下的坐标是(320, 240)在这个条件下目标在场地坐标系中的yaw角就趋向于某一个小范围这就是一个条件分布。我们在工程上几乎离不开条件概率因为处理数据本质上是根据观测结果推测真值。没有观测之前你对目标yaw角有一个先验分布比如说大概在云台前方30度到50度之间拿到观测之后这个分布就会被条件化变成一个更集中在某一点附近的后验分布。卡尔曼滤波的更新步实际上就是在做这个条件化。4.2 贝叶斯公式的记忆方法贝叶斯公式把我们关心的后验概率和先验、似然、证据联系起来后验 ∝ 似然 × 先验凡是学过概率统计的人都会背这一句但在电控里要理解它实际操作的含义。先验是在没看这帧观测之前你认为状态在哪似然是如果状态真的是某个值那么当前观测有多可能出现在这里后验就是两方信息融合后你对状态的最终估计。我见过很多同学把贝叶斯公式背得滚瓜烂熟但不知道它跟卡尔曼滤波有半毛钱关系。其实卡尔曼滤波就是贝叶斯公式在连续高斯情况下的解析解。先验来自预测步上一步的均值经过状态转移之后加上过程噪声后得到的分布。似然来自观测模型假设当前观测服从真实状态加观测噪声的分布。两者相乘、归一化得到的还是一个高斯分布其均值就是当前最优估计方差就是P阵更新后的结果。4.3 先验、似然、后验在RM里的对应落实到自瞄代码上先验、似然、后验可以对应到非常具体的模块先验上一帧卡尔曼输出的后验作为这一帧的初值。经过运动学模型比如匀速或匀加速模型推进外推出一个预测状态和预测协方差。似然视觉识别这一帧给出的目标像素坐标以及这个测量的噪声特性。如果目标是对方的机器人视觉输出还会附带一个置信度置信度低时可以把R阵调大这在概率意义上就是降低似然的权重。后验卡尔曼增益K把预测值和观测值做加权平均。如果观测噪声小、过程噪声大K就偏向观测反过来K就偏向预测。这个K不是拍脑袋定的它完全由协方差矩阵推导出来也就是由你给Q和R的信任程度决定。4.4 为什么贝叶斯思想是卡尔曼滤波的核心贝叶斯思想最大的价值是它提供了一个统一的融合框架。在RM电控里你可能会同时用视觉、陀螺仪、编码器三种信息观测同一个状态。三种传感器有各自不同的噪声水平。你可以在观测更新步里把它们当作多次独立的观测一个一个喂进去也可以把它们的观测拼成一个更大的观测向量一次性更新。不管怎么处理贝叶斯框架都能给你一个按噪声水平加权的融合结果。这种框架感在比赛里非常宝贵。比如能量机关的瞄准云台yaw角可以从灰度图像上提取也可以从视觉特征匹配的结果里提取如果再加上云台陀螺仪提供的云台自身角度三种信息互相印证。你不需要手写一堆if else来判断谁更可信只需要建立好各自的观测噪声R卡尔曼会自动把高噪声的信息权重降低。5. 高斯分布的线性变换与两个重要性质卡尔曼滤波怎么预测和更新5.1 线性变换后仍为高斯高斯分布有一个被广泛使用的性质如果X服从高斯分布Y AX BA是矩阵B是向量的话Y也服从高斯分布均值是AE[X]B协方差是A Cov(X) Aᵀ。这个性质是卡尔曼滤波预测步的理论基础。预测步里状态方程是 x_k F x_{k-1} B u_{k-1} w其中F是状态转移矩阵w是过程噪声通常建模成高斯分布。上一步的x_{k-1}已经是高斯分布经过F的线性变换后还是高斯再加上高斯噪声w仍然是高斯。于是你就可以既更新均值又更新协方差一步算出新的状态分布。如果F是非线性的那就是后话要用EKF去线性化。5.2 两个独立高斯相加仍为高斯第二个重要性质如果两个独立的高斯随机变量相加结果仍然服从高斯分布均值相加方差相加。这个性质在更新步中体现得淋漓尽致。观测方程 z H x v其中v是观测噪声也建模成高斯。如果x的先验分布是高斯v也是高斯那预测的观测值 Hx和高斯噪声v相加得到的观测分布也是高斯。而在融合时卡尔曼滤波本质上是把预测给观测值的分布和实际观测值的分布这两座高斯函数相乘。高斯函数相乘的结果仍然是高斯函数。这就保证了你不会越算越没形状最后还能解析地写出均值和协方差。这也是为什么卡尔曼滤波只对高斯噪声才有最优性的原因——换一个非高斯分布这个乘完还是同族分布的性质就破了。5.3 从性质看卡尔曼预测步和更新步的设计动机理解了这两条性质卡尔曼滤波两大步骤的形状就自然出来了。预测步里P_{k|k-1} F P_{k-1|k-1} Fᵀ Q就是在做线性变换后加噪声先是协方差被F从左边右边两边夹一下这是线性变换导致的协方差传播然后加上过程噪声的协方差Q表示模型本身就有不确定性信息量只会变少协方差只会变大。更新步里先算观测预测残差 y z - Hx再算残差协方差 S H P Hᵀ R这里面H P Hᵀ是把先验不确定性映射到观测空间加R是观测自身噪声再增加不确定性。最后卡尔曼增益 K P Hᵀ S⁻¹就是先验和观测两个分布融合时的权重比例。增益方程完全由协方差决定这就是为什么P阵的初值和Q/R阵设定直接决定了滤波性能。6. 从概率统计到电控实车噪声建模与参数标定的实操心得6.1 传感器噪声怎么测静态数据采集法不管理论多漂亮上了真机第一件事就是标定噪声参数。以IMU的yaw角速度为例把机器人放在地面上静止以100Hz或200Hz的记录频率采集一分钟数据这段数据理论上均值应该接近0但实际会围绕0上下波动。把它做直方图你会看到一个近似高斯形状的分布它的标准差就是静态噪声的粗略估计。视觉识别噪声比较麻烦因为目标真实位置很难知道。一个实用做法是架一个标定板或放一个固定的发光标记让自瞄算法连续识别这个固定目标提取识别到的yaw和pitch角。因为目标没动这些数值的波动就直接反映了视觉识别在静止场景下的噪声。记录几百帧后计算均值和标准差就能作为R阵的参考初值。6.2 系统噪声Q和观测噪声R的经验设定Q矩阵描述的是你对运动模型的信任程度。运动模型越粗糙Q就得设越大。比如你用匀速模型去跟踪正在剧烈加减速的敌方机器人每拍都会产生模型误差这个误差就应该体现在Q里。如果你把Q设得很小滤波器会太信任模型跟随性变差目标急转弯之后估计会明显滞后。反过来Q设太大滤波器会太信任每一帧观测噪声没法被有效滤除输出会很抖。一个还不错的起步套路是先确定R。用静态测量估出观测噪声方差比如视觉yaw的测量噪声方差是0.01 rad²。然后让系统做正弦运动或阶跃运动调节Q让输出在不要太抖和延迟不要太大之间平衡。一般以R为基准Q从小到大扫几轮记录跟踪的均方根误差和超调量。RM比赛时间紧张通常不会去做特别精细的最优化经验做法是让Q约为R的0.1到10倍范围内试凑。但你要明白Q和R并不是单纯的数值大小关系它们放大了很多倍输出可能不变因为卡尔曼增益K本质上取决于两者的比值。6.3 常见误区正态性假设不满足时的表现卡尔曼滤波要求过程噪声和观测噪声都是高斯白噪声。但实际上比赛环境里很多噪声不满足这个假设。比如视觉识别在目标被遮挡时可能突然出现一个极大的离群值这根本不符合高斯分布。遇到这种情况不把离群值处理掉就喂给卡尔曼会导致滤波结果被严重带偏。一个简单的处理是设置观测置信度门限当测量残差超过设定阈值若干倍标准差的时候把该帧观测的R放大或者干脆跳过更新步。这在概率统计上就是放弃极端似然的做法用后验的平滑性来换取鲁棒性。我在电控项目里还会给R加一个动态系数当自瞄算法给出低置信度时R乘以一个放大倍数让观测的权重下降。6.4 小技巧用单位球和无量纲化带来调试便利最后分享一个小技巧。卡尔曼滤波里不同状态量可能单位差异巨大比如角度是rad角速度是rad/s加速度是rad/s²。如果直接堆进状态向量协方差矩阵里的数值可能差好几个数量级调P初值的时候会非常难受。这时候可以对状态做无量纲化或者使用合适的单位球半径来归一化。比如角度不用rad而用角度归一化到[-1.1]之间角速度也除一个最大可能值变成当前角速度占最大角速度的比例。这样处理后P矩阵对角线初值可以统一设为0.01到0.1的数量级Q阵和R阵的调节也更容易找到规律。另外调试时建议把滤波后的均值和估计方差实时打印出去在客户端显示出来观察方差在急转弯时的变化。这会让你直观感受到预测时不确定性变大、更新时不确定性变小的过程对理解卡尔曼滤波非常有帮助。概率统计这部分是把卡尔曼滤波从调参玄学变成可视化的数学问题的关键。你不需要在理论上走得很深但一定要把随机变量、分布、协方差、贝叶斯公式这几个概念跟云台、视觉、IMU数据一一对应起来。下一篇我会在这个基础上展开卡尔曼滤波的具体推导从一维例子开始一步步写出预测和更新方程再把多维矩阵公式拆开看。如果有想提前聊的可以留言告诉我你们在自瞄里最想解决的问题我挑典型场景放到后面的例子里。