企业官网建设流程全解析

1. 为什么RM电控要学卡尔曼滤波

1.1 从一场实战惨案说起

先说一个我亲眼在RM赛场看过的场景：某支队伍的自瞄系统在实验室里打得贼准，步兵绕场三圈云台纹丝不动，弹着点跟用尺子量过似的。结果一到正式比赛，换上赛场灯光，目标装甲板在视野里抖成了筛子，云台跟着目标疯狂左右摇摆，子弹全打在装甲板边缘弹飞。下场之后队员围在一起查了一个通宵，发现问题的根源根本不是视觉算法崩了，而是视觉给出来的目标位置数据包含大量噪声，电控侧拿到这些带毛刺的坐标之后直接喂给了PID，云台被噪声带着走，自然没办法稳定输出。

这就是典型的“差一个滤波器”的现场。很多RM队伍把大量精力花在机械结构、视觉识别、电控PID上，却忽略了数据进入控制器之前的预处理环节。实际上，在RoboMaster赛场上，视觉识别出的目标中心坐标、裁判系统给出的装甲板血量、测速模块返回的弹速、陀螺仪积分出来的姿态角，没有任何一个数据是干净无噪声的。而卡尔曼滤波，恰恰就是解决这一类问题最成熟、最系统的工具。

1.2 卡尔曼滤波在RM赛场的三大用途

认真算一笔账，RM电控里面卡尔曼滤波能落地的场景非常多，但最典型的不外乎三个方向。

第一个方向是目标预测。自瞄系统识别出敌方装甲板的位置之后，由于视觉处理帧率有限、通信存在延迟，云台实际执行瞄准时目标早就移动了。这时候卡尔曼滤波可以对目标的运动状态做建模，预测出几十毫秒后目标的位置，把云台提前甩过去，提高命中率。

第二个方向是数据平滑。裁判系统返回的模块血量、弹仓剩余子弹数、底盘功率等数据，在赛场上因为通信干扰会剧烈跳动，直接拿来控制很容易导致误判。卡尔曼滤波可以结合物理模型和观测数据，给出一个相对平滑且可靠的状态估计。

第三个方向是多传感器融合。步兵车的底盘里程计和IMU陀螺仪各有所长，里程计短时间准确但会打滑，陀螺仪长时间稳定但有漂移。卡尔曼滤波可以把两者的优势结合起来，输出一个比任何单一传感器都靠谱的位姿估计。

这三个方向背后，都是同一个数学工具。而这个工具的地基，就是概率统计。

2. 概率统计基础：状态估计的“世界观”

2.1 随机变量与噪声模型：测量为什么总是“不准”

要理解卡尔曼滤波，第一件事是转变观念：不要把一个测量值当成一个确定的数，而要把它当成一个随机变量。

举个例子，你用测速轮给步兵车测速，测出来2.65 m/s，这个数并不是真实速度的完美映射。轮子和地面之间可能有微小滑动，编码器分辨率有限，线材接触电阻会波动，单片机ADC采样有量化误差，等等。所有这些因素叠加在一起，导致测量结果在真实值附近上下浮动。统计学家把这种浮动叫做“噪声”。

如果用数学语言描述，测量值z和真实值x之间的关系可以写成：

z = x + v

其中v就是噪声，通常建模为一个均值为0的高斯随机变量。均值为0意味着噪声的平均效果不偏不倚，不会系统性地把结果往一个方向带；高斯分布则意味着大部分噪声都集中在真实值附近，偏离太远的概率很小。

这就是卡尔曼滤波世界观的第一课：**永远不要精确描述一个测量值，用均值和方差来描述它。**均值告诉你这个数大概在什么位置，方差告诉你这个数有多可信。一个方差很大的测量，即使均值看起来合理，也不能过分相信它。

2.2 高斯分布与协方差：从单个量到多个量

单变量的高斯分布大家都很熟悉，概率密度函数长这样：

f(x) = (1 / sqrt(2π σ²)) · exp(-(x - μ)² / (2σ²))

这里面μ是均值，σ²是方差。方差越小，曲线越瘦高，说明数据越集中，测量越可靠；方差越大，曲线越矮胖，说明数据越分散，测量越不靠谱。

但RM里面几乎没有单变量的估计问题。云台瞄准要同时估计敌方的x、y坐标和运动速度，底盘状态估计要同时关心x坐标、y坐标、朝向角、速度、角速度。这时候就需要多变量高斯分布登场。

多维高斯分布不再用一个数μ来描述中心位置，而是用一个向量μ来描述均值，用一个矩阵Σ来描述各个变量之间的协方差关系。这个矩阵叫做协方差矩阵：

Σ = [[σ₁², σ₁₂], [σ₂₁, σ₂²]]

对角线上的元素是每个变量的方差，非对角线上的元素是变量之间的协方差。协方差描述的是两个变量的相关性：如果敌方装甲板x坐标增大时y坐标倾向于同步增大，协方差就是正的；如果x增大时y倾向于减小，协方差就是负的；如果两者独立变化，协方差就是零。

协方差矩阵在卡尔曼滤波里无处不在。预测步误差变大体现在协方差矩阵变大，观测更新之后状态更确定体现在协方差矩阵缩小。可以说，卡尔曼滤波的整个迭代过程，就是均值向量和协方差矩阵的交替更新。

2.3 条件概率与贝叶斯视角：观测到数据之后该信谁

卡尔曼滤波还有一个绕不开的概率论概念：条件概率和贝叶斯定理。

条件概率描述的是“在已知某个条件的情况下，事件发生的概率”，记作P(A|B)，读作“在B发生的条件下A发生的概率”。贝叶斯定理则把这个关系反过来：

P(A|B) = P(B|A) · P(A) / P(B)

把它翻译到状态估计的语言里：

P(状态 | 观测) = P(观测 | 状态) · P(状态) / P(观测)

左边是“看到观测数据之后，状态取某个值的概率”，叫做后验概率；右边的P(状态)是观测之前对状态分布的猜测，叫做先验概率；P(观测|状态)是给定状态后产生这个观测的概率，叫做似然。

不要被这些术语吓到，用生活类比一下就通了。假设你在宿舍里等队友送电池过来，平时他走那条路大概需要3分钟，这是一开始设定的“先验”。突然听到楼道里传来急促的脚步声，这就是“观测”。听到脚步声之后，你对“他马上就要到”这件事的把握就增大了，这就是“后验”。如果脚步声越来越近，你的把握就越来越大；如果脚步声又远了，你的把握就会回落。

卡尔曼滤波的本质，就是在每一帧里重复做贝叶斯更新：用运动模型做预测得到一个先验估计，用传感器观测计算似然，再把两者按可信度加权融合，得到当前帧的后验估计。权重怎么分配？这就引出了著名的卡尔曼增益。

3. 从贝叶斯到卡尔曼滤波的数学之路

3.1 状态空间模型：把问题“形式化”

在动手写卡尔曼滤波代码之前，先把数学模型搭起来。卡尔曼滤波假设系统可以用一个状态空间模型来描述，包含两个核心方程。

第一个是状态方程，描述状态如何随时间演化：

x_k = A · x_{k-1} + B · u_k + w_k

其中x_k是k时刻的状态向量，A是状态转移矩阵，描述上一时刻的状态如何影响当前时刻；u_k是控制输入，B是控制矩阵；w_k是过程噪声，建模为高斯分布N(0, Q)，Q是过程噪声协方差矩阵。

第二个是观测方程，描述传感器如何观测状态：

z_k = H · x_k + v_k

其中z_k是测量值向量，H是观测矩阵，把状态空间映射到测量空间；v_k是测量噪声，建模为高斯分布N(0, R)，R是测量噪声协方差矩阵。

拿云台瞄准的例子说：如果要估计敌方装甲板的平面位置(x, y)和运动速度(vx, vy)，状态向量就是x = [x, y, vx, vy]^T。如果假设目标做匀速直线运动，状态转移矩阵A就是：

A = [[1, 0, dt, 0], [0, 1, 0, dt], [0, 0, 1, 0], [0, 0, 0, 1]]

视觉识别给出的装甲板像素中心坐标映射到云台坐标系之后就是观测值z = [x_obs, y_obs]^T，观测矩阵H就是把状态向量中的位置分量提取出来：

H = [[1, 0, 0, 0], [0, 1, 0, 0]]

形式化之后，问题就变得很清晰了：每收到一帧新的观测数据，就利用状态方程预测一下目标现在大概在哪，然后用观测值修正一下这个预测，把两者按可信度加权。剩下的问题就是“权重怎么算”，这就要引出卡尔曼滤波的五大核心公式。

3.2 预测步：先验是怎么来的

卡尔曼滤波的每一次迭代分为预测步和更新步。

预测步做的事情，是根据上一帧的后验估计和运动模型，推算当前帧状态的先验估计。公式有两个：

x_pred = A · x_est + B · u

P_pred = A · P_est · A^T + Q

第一个公式很好理解，就是把上一帧的最优估计带入运动模型外推一步。第二个公式稍微复杂一点，它描述的是状态不确定性的传递：上一帧的估计有一个协方差矩阵P_est，经过线性变换A之后，不确定性变成A · P_est · A^T；然后还要加上过程噪声Q，因为运动模型本身不是完美准确的，目标可能会加速、转弯、被其他车挡住，这些模型没有刻画的因素都会增加不确定性。

用我最开始提到的宿舍等队友的例子来类比：你预测他1分钟后到，这个预测有一个方差。如果预测的时间越远，这个方差就应该越大，因为过程中可能发生的意外变数越多。Q就是用来描述这个“过程中变数”的量。Q越大，说明你越不信任运动模型，预测的协方差矩阵会膨胀得越快。

3.3 更新步：后验是怎么融合的

更新步是卡尔曼滤波的精髓，它的任务是把预测值和观测值融合起来。公式有三个：

K = P_pred · H^T · (H · P_pred · H^T + R)^(-1)

x_est = x_pred + K · (z - H · x_pred)

P_est = (I - K · H) · P_pred

其中K就是卡尔曼增益。

没有K的时候，你手里有两条信息：预测出来的状态x_pred（带协方差P_pred）和传感器读到的观测z（带噪声R）。如果观测噪声很小、预测不确定性很大，就应该多信观测；反过来，如果观测噪声很大、预测已经很准，就应该多信预测。卡尔曼增益K就是那个根据两个不确定性的相对大小自动调节信任比例的“调音台”。

可以再看一个简洁的解释角度：两个高斯分布相乘，结果仍然是一个高斯分布。预测分布乘以观测似然分布，得到的新的高斯分布的均值，正好就等于x_est，而且它的协方差P_est比任何参与相乘的分布都要小。更严谨地讲，高斯相乘之后的结果均值落在两个峰之间，偏向方差更小那个峰。卡尔曼增益K的数值大小，决定了融合后的状态在预测和观测之间偏向谁。

拆开来理解这几个更新公式：

z - H · x_pred是“观测残差”，意思是我预测目标应该在某个位置，但传感器说在另一个位置，差了多少。乘以K之后得到一个修正量，加到预测值上，得到融合后的估计。

x_est = x_pred + K · (z - H · x_pred)

这个结构在控制领域非常经典，它本质上是一个带增益的误差修正回路。卡尔曼滤波不是简单地取平均，而是一个自适应的反馈校正过程。每一帧迭代，都在重复“预测-校正”的循环，状态向量和协方差矩阵像滚雪球一样持续更新。

4. 卡尔曼滤波的RM实战场景

4.1 云台瞄准：目标检测数据抖得像心电图怎么办

回到文章开头那个翻车场景。自瞄系统给出的目标中心坐标在皮卡丘模式下飘得厉害，每帧之间可能跳好几个像素。直接把这些坐标发给云台俯仰/偏航电机，PID就会跟着噪声来回抽动，云台不仅瞄不准，还容易发热过载。

用卡尔曼滤波解决这个问题的思路是这样的：把目标的平面速度当成隐含状态，通过连续多帧坐标数据估计出目标的位置和运动趋势。这样做的好处是，即使某两帧的坐标因为图像噪声偏离真实位置，滤波器也会认为这个偏离不符合目标的运动趋势，给它一个较低的权重，从而把毛刺平滑掉。

具体实施时，可以先在视觉和电控之间增加一个数据通道，通过串口或CAN总线把目标坐标发送给电控侧，在电控侧（一般是STM32上）实现一个四维状态量的卡尔曼滤波器。目标位置用视觉数据，目标速度则完全靠滤波器自己估计。这样云台收到的坐标就是经过平滑处理的，不再跳变。

如果嫌STM32上手动实现容易出bug，还可以先用MATLAB或Python把算法仿真调好，再移植到单片机。我个人的经验是，在单片机上做纯手写矩阵运算版本，存4x4浮点矩阵，跑一次完整的预测加更新在STM32F4上大约只需要几十微秒，完全不影响控制周期。

4.2 弹道预测与击打时机

RM赛场上弹速有限，子弹飞行需要时间。假设子弹以15 m/s的速度飞行，目标在3米开外横向运动，飞完这3米大约需要0.2秒。如果目标以2 m/s的速度横向移动，0.2秒内就移动了0.4米，这足以让弹着点错开装甲板。

卡尔曼滤波在这里的角色是，根据历史帧的目标位置估计出目标当前的运动速度，从而外推目标在0.2秒之后的位置。外推的位置才是云台实际需要瞄准的点。

实现的时候要额外注意通信延迟。视觉识别到目标，到电控真正发出发射指令，中间经过串口通信、解析、PID响应等环节，总共可能延迟几十毫秒。这个延迟在高速运动的目标面前不可忽略，需要在卡尔曼滤波预测目标位置时额外把通信延迟补偿进去。

补偿方法不复杂：在每次预测时，把预测的时间步长从当前帧间隔改成当前帧间隔加上总延迟时间。也就是把dt改成dt + latency。这样预测出来的是目标“子弹到达时刻”的位置，而不是“当前时刻”的位置，瞄准精度能提升一个量级。

4.3 测速轮与陀螺仪数据融合

底盘里程计和IMU陀螺仪都是RM电控里常用的定位传感器，但各有各的毛病。测速轮（编码器里程计）短期精度很高，但轮子打滑、轮胎磨损会引入累积误差，跑久了位置会漂；陀螺仪的角速度测量短期会漂，但经过积分得到的角度在长时间内相对稳定。

卡尔曼滤波很适合把这两者融合成一套完整位姿估计。状态向量取[tx, ty, yaw, vx, vy, omega]，观测用里程计给的[tx, ty, yaw]和陀螺仪给的omega。里程计在yaw上往往不太准，因为轮式车辆转小弯的时候轮子打滑会破坏轮速和实际转向角的关系；而陀螺仪的yaw积分相对稳定，正好可以弥补这个短板。

融合后的位姿比任何单一传感器都稳，后续的导航、瞄准预测、功率控制都可以在这个基础上进行。我见过不少队伍用这个方案做步兵车的低成本航迹推算，在比赛场地这种规则平面上，跑完整场误差非常可观。

5. 调参经验与常见问题

5.1 噪声矩阵R和Q到底怎么定

这是所有第一次接触卡尔曼滤波的人必问的问题。网上能搜到一堆“根据经验”、“试出来的”这种答案，实际上是有规律可循的。

R代表测量噪声协方差矩阵，它描述的是传感器测量值中含有的噪声水平。最简单的标定方法：把传感器固定住，采集一堆静态测量数据，计算这些数据的方差，那就是R的一个比较合理的估计。比如用视觉测距模块测量一个固定距离的目标，采集200个数据点，求方差，填进R即可。

Q代表过程噪声协方差矩阵，它描述的是运动模型与实际运动之间的偏差。这个一般更难直接用实验标定，因为它反映的是“模型没刻画的东西”，比如目标的突然加速、转弯、被撞击。通常的做法是先设一个较小的初始值，让滤波器的输出与真值对比，不断调整Q直到输出既平滑又不明显滞后。

调参的时候有个铁律要记住：R越大，说明传感器越不可靠，滤波器越倾向相信预测；Q越大，说明模型越不可靠，滤波器越倾向相信观测。很多人调参调不明白，就是把R和Q的方向搞反了。还有个常用技巧是用试凑法的时候先固定R，只用Q去调平滑程度，这样调试空间降为了一维，会快很多。

5.2 发散、滞后与初始化问题

卡尔曼滤波在实际工程中会遇到的三大典型问题。

第一个是发散。表现为状态估计逐渐偏离真实值，甚至飞上天。常见原因是Q设得太小，模型过度自信，导致滤波器无法用观测数据修正累积的模型误差；或者R设得太小，过度相信单帧观测，被某个异常观测带偏。排查思路是先检查协方差矩阵P的数值是否异常增长，再看观测残差序列的均值是否偏离零。

第二个是滞后。很多队伍滤波之后发现云台动作“慢半拍”，这是因为调参时为了平滑噪声把Q调得太小，滤波器对目标真实运动趋势的响应被压得太慢。滞后和噪声是一对矛盾，压噪声必然会增加滞后，唯一出路是建更好的运动模型。比如匀速模型滞后了就上匀加速模型，匀加速模型还滞后就加一个角速度估计状态。

第三个是初始化。P_est的初始值不要设成零矩阵，否则滤波器一开始就认为自己对状态的估计完全确定，后续预测和更新基本不会修正初始值，导致前几百毫秒的估计非常拉胯。通常的做法是把初始P设成一个较大的对角阵，比如对应位置的方差设成100，表示初始状态极度不确定，让滤波器在开头快速收敛。

5.3 新手必看的几个坑

第一个坑：在回调函数里做矩阵求逆。H·P·H^T + R这一步需要求逆，如果这个矩阵的行列式很小，数值上可能会不稳定。实际工程中这个矩阵一般是正的对称矩阵，用LDUT分解或者直接调用ARM的CMSIS-DSP库做矩阵运算，不要自己写高斯消元法。

第二个坑：数据类型选择。STM32上做矩阵运算，浮点精度尽量用float，双精度会吃掉大量CPU时间，在控制周期紧张的嵌入式环境里不值得。如果对精度有更高要求，可以考虑在关键节点用双精度做中间量，最后再转回float。

第三个坑：采样时间不固定。RM赛场上视觉帧率可能会在30到60帧之间波动，如果滤波器的dt一直写死，预测步的精度会大打折扣。正确做法是每次预测前用定时器或者系统时钟读取实际的时间差作为dt，这样即使帧率波动，滤波器也能维持稳定。

第四个坑：频繁的矩阵运算会让代码变得很长很乱，阅读性差，debug困难。我建议封装成几个独立的函数，至少包括predict()和update()，状态量、协方差矩阵全部用结构体管理，方便二次修改。

6. 从概率视角看状态估计这件事

在RM电控这个领域待得越久，越能感受到卡尔曼滤波不是一个孤立的算法，而是一整套看待数据和不确定性的思维范式。

以前的我很喜欢把传感器返回的每个数值都当作“真值”来用，直到一次调试中，发现视觉给出的目标坐标在短时间内剧烈抖动，当时第一反应是去查视觉算法，后来才意识到任何传感器都存在噪声，真正需要做的是在电控侧对数据做合理建模和滤波。从那以后，我再也不纠结于单个数据点的准确性，而是把每个测量值都看作“均值加噪声”，把系统的状态看作一个概率分布。这种思维转变，比学会卡尔曼滤波本身的公式重要得多。

如果你正在准备RM电控，或者已经在队里负责云台、底盘的控制代码，建议先认真啃一遍概率统计的基础概念，再把卡尔曼滤波的五个公式亲手推到一遍。推公式的过程可能很痛苦，但推完之后再去调参、改模型，思路会清晰很多。还有一个小技巧：动手写代码前，先在Python或者MATLAB里用仿真数据跑一遍算法，把每个中间变量打出来看一眼，理解每一步在做什么，再移植到单片机上，调试效率会翻倍。