企业官网建设流程全解析

最近整理RM电控组的代码库时，又把卡尔曼滤波翻出来看了好几遍。不少新队员第一次跑测速模块，看到示波器上无规律乱跳的读数，第一反应就是写个一阶低通滤波压一压。这个思路本身没有错，可一旦到了比赛场上要算弹道补偿、要卡射速节奏的时候，一个响应慢半拍的滤波器很容易让全队陷入“越调越不稳”的泥潭。带队伍这几年，我一个很深的感受是：电控要学的东西很多，但卡尔曼滤波绝对是最值得花时间彻底搞懂的那一块。它不是玄学，也不是代码库里的黑盒，本质是一套用数学来回答“什么时候该信传感器、什么时候该信模型”的框架。

这篇内容主要面向RM电控组里准备接手云台、测速模块或者裁判系统数据的队员，也会适合那些已经在用低通滤波但总觉得响应和噪声顾此失彼的朋友。我会按照自己实际跑过的流程来写：先从RM里几个典型场景讲清楚卡尔曼滤波到底在解决什么问题，再给出STM32上的一维和二维C代码实现，接着讲Q/R参数怎么调、现场有哪些坑，最后简单聊一聊如何把同一套思路扩展到云台姿态融合上。

1. 思路拆解：卡尔曼滤波在RM机器人的切入场景

1.1 三个典型场景：测速、云台、底盘

电控里的传感器数据，大部分能简化成两类问题：一类是“直接能测，但测不准”，另一类是“不能直接测，只能靠多个信号拼出来”。卡尔曼滤波对这两类问题都有用，但用得对不对，差别很大。我在RM机器人上见过最值得上卡尔曼滤波的场景，基本可以分成三个。

第一个是弹丸初速估计。测速模块通过轮子或光电对管，给出弹丸通过两个检测点的时间差，再用距离除以时间换出速度。问题在于，时间差本身有量化误差，轮子打滑、弹丸姿态变化、摩擦轮温度变化，都会让速度读数跳变。如果直接拿这个瞬时速度去做弹道解算，弹着点会飘得非常厉害。

第二个是云台姿态估计。陀螺仪直接积分会有漂移，加速度计静止时很准、运动时噪声又大得离谱。卡尔曼滤波可以把两者的优势互补，得到更平滑、更可信的姿态角，让云台PID环路的反馈信号不再带着一堆毛刺。

第三个是底盘功率控制。裁判系统给出的实时功率有噪声，电调反馈的电流也不一定准，如果直接拿去闭环，容易把功率环调出低频振荡。用一个状态量去融合“指令”和“反馈”，输出给限功率算法，底盘的爆发力会明显更稳。

这三个场景本质上是同一个数学结构：我有一组状态量，比如速度、角度、功率；状态之间的变化规律我大致知道，这就是模型；我也有若干观察手段，也就是传感器，但两边都带噪声。卡尔曼滤波要做的，就是找一个随时间变化的最优权重，把模型预测和传感器测量合起来。

1.2 核心思想：用不确定度动态分配信任

理解卡尔曼滤波，我推荐用一个“两位专家开会”的类比。你面前有两位专家，一位是模型预测专家，他根据上一帧状态和运动学规律，告诉你“现在弹丸速度应该在15.2 m/s左右”；另一位是传感器测量专家，他根据测速模块实时读数，告诉你“这一发打出来的瞬时速度是16.8 m/s”。两人意见不一致，你到底听谁的？

如果模型专家平时的预测方差很大，传感器专家读数又很稳，你会偏向传感器；反过来，如果传感器读数跳得离谱，你会更相信模型。卡尔曼滤波做的事情非常简单，它把两位专家的“可信程度”用方差量化，每一帧都按方差大小算出最优加权系数。这个系数就是卡尔曼增益K。

这个思路最值钱的地方，在于它不是拍一个固定权重，而是每一帧动态计算。低速打弹时，测速模块的量化误差相对明显，K会自动小一点，滤波结果更偏向模型平滑值；高速连续射击时，测量时间差变小、量化误差占比升高，K继续自适应调整。这比写死一个alpha的低通滤波科学得多，因为低通滤波永远只能用同一个平滑系数去应对不同噪声环境。

1.3 为什么比低通滤波更适合RM电控

很多队员会问，低通滤波简单粗暴，为什么非要上卡尔曼。我的回答是：低通滤波不是不能用，但它本质上是个单参数工具，只能按频率成份做加权，它不知道你的物理模型是什么。云台角速度用一阶低通拉一下，响应变慢了，PID相位裕度就会被吃掉；测速数据拉到很平滑，弹丸真实速度变化时又跟不上。

卡尔曼滤波的优势在于，它可以显式地把物理规律写进状态转移矩阵。弹丸速度的变化率、云台角度的惯性、底盘功率的功率变化趋势，这些都是模型信息。引入模型信息之后，滤波不仅在修噪声，它还在“预测未来”。在RM这种需要极限响应和控制带宽的场合，多一个可用的预测值，往往就意味着云台少一帧丢失、弹道多打中一个移动目标。

不过我也要说一句公道话，卡尔曼滤波不是银弹。如果你的传感器本身线性度差、时序乱、采样周期抖动严重，上什么滤波都是白搭。滤波能做的，是在信号Topology已经稳定的前提下，把估计精度和响应速度同时往前推一步。

2. 从公式到代码：一维和二维卡尔曼的完整落地

2.1 状态空间写法和五个公式

还是拿弹丸测速举例子。我们假设弹丸在很短的一段测量区间里速度变化不大，把它建模成一个带扰动的缓慢变化状态：

x_k = x_{k-1} + w_k

z_k = x_k + v_k

x是真实速度，z是测速模块观测出的速度，w是过程噪声，代表“速度并不严格恒定”这个事实带来的扰动，v是测量噪声，代表测速模块本身的误差。两者的方差分别记为Q和R。

完整卡尔曼滤波每帧要算五个公式，预测两步，更新三步：

预测： x_pred = A * x_prev P_pred = A * P_prev * A^T + Q

更新： K = P_pred * H^T * (H * P_pred * H^T + R)^(-1) x_new = x_pred + K * (z - H * x_pred) P_new = (I - K * H) * P_pred

在一维弹丸速度这个例子里，A=1，H=1，公式会化简得非常清爽：

x_pred = x_prev P_pred = P_prev + Q K = P_pred / (P_pred + R) x_new = x_pred + K * (z - x_pred) P_new = (1 - K) * P_pred

这套化简后的版本特别好理解：K只取决于P和Q、R的相对大小。P大、R小，说明测量可信，K趋近于1；P小、R大，说明模型可信，K趋近于0。整个收敛过程，实际上就是P和K从初始值逐步进入稳态的过程。

2.2 一维版本：STM32上的极简实现

我直接贴一段在RM工程里用过的写法。一维情况下不需要矩阵运算库，就是几个浮点乘除，放在测速模块的中断回调或者5ms周期任务里，单次运行最多耗时几百纳秒，基本不占用CPU时间。

typedef struct { float x; // 状态：弹丸速度估计值 float P; // 状态方差 float Q; // 过程噪声方差 float R; // 测量噪声方差 } Kalman1D_t; void Kalman1D_Init(Kalman1D_t *kf, float init_x, float init_P, float Q, float R) { kf->x = init_x; kf->P = init_P; kf->Q = Q; kf->R = R; } float Kalman1D_Update(Kalman1D_t *kf, float z) { // 预测 float P_pred = kf->P + kf->Q; // 更新 float K = P_pred / (P_pred + kf->R); kf->x = kf->x + K * (z - kf->x); kf->P = (1.0f - K) * P_pred; return kf->x; }

这里有两个特别容易被新手忽略的地方。第一，第一次调用前要合理设置P的初值。P0本质上是“我对初始状态x0有多不确信”。如果完全不确定，就设一个大值，比如100.0，让滤波器在前几帧快速收敛；如果已经知道弹丸速度大概是15 m/s，可以把P0设成0.1这种小值，但后面我会说，大多数场景宁可大一点。

第二，Q和R不是随便拍出来的。它们直接决定滤波的“性格”，Q决定模型跟踪快速变化的灵敏度，R决定平滑程度。这两个参数怎么调，第三章会专门展开。

2.3 二维版本：速度加加速度的矩阵推导

弹丸在被摩擦轮带动的过程中，存在明显的加速段。一维模型在速度快速上升时会有跟踪滞后，表现就是滤波值总是慢半拍。解决办法是把状态向量扩展成“速度+加速度”两个量：

x = [v, a]^T

状态转移方程这样写：

[ v_pred ] [ 1 dt ] [ v_prev ] [ a_pred ] = [ 0 1 ] [ a_prev ]

这个模型的意思是，速度的变化由加速度决定，加速度本身则做缓慢随机游走。P矩阵是2x2的协方差矩阵，Q矩阵也要相应扩展成2x2：

Q = [ Q_v 0 ] [ 0 Q_a ]

观测方程仍然只测速度：z = [1 0] x + v。也就是说，加速度不是直接被测出来的，而是滤波器根据速度的变化趋势“反推”出来的隐藏变量。

二维的好处是能比一维更积极地跟踪速度爬升，代价是要多调一个Q_a参数，而且协方差矩阵交叉项一旦初始化不当，滤波结果可能抖动甚至发散。我的建议是，先把一维跑通，确认数据流和调参逻辑，再上二维。上来就上高维滤波，出了问题你根本分不清是数学错了、代码错了还是参数错了。

2.4 二维版C代码：速度加速度联合估计

下面这段是二维版本的核心更新代码，我保留了全部计算过程，方便你对照公式看。代码里用显式变量写出协方差矩阵的四个元素，没有引入矩阵库，读起来更直观。

typedef struct { float v; // 速度估计 float a; // 加速度估计 float P00, P01; // 协方差矩阵第一行 float P10, P11; // 协方差矩阵第二行 float Q_v, Q_a; // 过程噪声 float R; // 测量噪声 float dt; // 周期 } Kalman2D_t; void Kalman2D_Init(Kalman2D_t *k, float init_v, float init_a, float init_P, float Qv, float Qa, float R, float dt) { k->v = init_v; k->a = init_a; k->P00 = init_P; k->P01 = 0.0f; k->P10 = 0.0f; k->P11 = init_P; k->Q_v = Qv; k->Q_a = Qa; k->R = R; k->dt = dt; } float Kalman2D_Update(Kalman2D_t *k, float z) { float dt = k->dt; // 预测状态 float v_pred = k->v + k->a * dt; float a_pred = k->a; // 预测协方差 P_pred = A * P * A^T + Q float P00_pred = k->P00 + 2.0f * dt * k->P01 + dt * dt * k->P11 + k->Q_v; float P01_pred = k->P01 + dt * k->P11; float P10_pred = k->P10 + dt * k->P11; float P11_pred = k->P11 + k->Q_a; // 更新增益 float S = P00_pred + k->R; float K0 = P00_pred / S; float K1 = P10_pred / S; // 更新状态 float innov = z - v_pred; k->v = v_pred + K0 * innov; k->a = a_pred + K1 * innov; // 更新协方差 P_new = (I - K*H) * P_pred k->P00 = (1.0f - K0) * P00_pred; k->P01 = (1.0f - K0) * P01_pred; k->P10 = P10_pred - K1 * P00_pred; k->P11 = P11_pred - K1 * P01_pred; return k->v; }

注意，协方差矩阵按定义是对称的，所以P01和P10在理论上永远相等。中间如果出现数值漂移导致轻微不对称，不会立刻让算法崩掉，但强行把它俩写成相等也不是好习惯。保持代码按照标准公式算，数值上会让滤波更稳定。

3. 实战调参：Q与R的物理含义和调试流程

3.1 把Q和R当作可解释的物理量

很多同学把Q和R当成两个旋钮在那里乱拧，总觉得拧到一个神秘数值就功德圆满。你得换个角度理解它们。过程噪声Q代表“模型本身有多不可信”。如果你认为弹丸速度在测量区间内几乎恒定，Q应该很小；如果弹丸速度会因为摩擦轮打滑或供弹机构卡顿而剧烈变化，Q就要适当放大。测量噪声R代表“传感器读数有多不可信”，测速模块硬件输出抖动明显时，R要调大。

量纲上，如果状态是速度，单位是m/s，那么R就是速度测量噪声的方差，单位是(m/s)^2。Q则与过程扰动在单个周期内的能量有关。有些人会用(最大加速度*dt)^2这种公式去估算Q，我觉得做精细调参时可以算，但起步阶段没必要太精确。先扫数量级，再微调。

这里有个很直观的观察法。Q很小、R很大的时候，滤波结果非常平滑，但真实速度发生变化时，输出会有明显滞后；Q很大、R很小的时候，滤波结果几乎贴着原始测量走，平滑效果约等于无。最优参数通常落在中间区域，既保留一定平滑度，又不至于在关键时刻反应太慢。

3.2 五步定参流程与判据

我调QM和R一般按下面这个顺序来，大家可以当作模板用。

1. 先确定一个估计范围。拿测速模块来说，测量速度一般在10到30 m/s，把R先粗略定在测量方差的量级上，比如0.25，换算成标准差就是0.5 m/s。然后让Q从0.001到10之间按指数步长扫。
2. 录制一段覆盖低速、中速、高速、急停的数据，把原始测量和滤波结果画在同一张图里。没有上位机的话，用串口把数据导到Excel里画也行。
3. 观察滞后程度。如果滤波结果比真实速度变化慢很多，就增大Q或者减小R。
4. 观察平滑度。如果输出仍然抖得像没滤波一样，就减小Q或者增大R。
5. 重复第三步和第四步，直到你觉得“响应够快，且曲线足够干净”。

这个流程看着笨，但它能帮你建立对Q/R的直觉。等到你对某个传感器的噪声特性足够熟悉，基本上一两次就能定位到量级。

3.3 起步参数参考表

下面这张表是一些常见场景的起步参数，可以直接拿去跑，但不代表你不用再看真实数据。每个机器人传感器硬件不同、安装方式不同，上电噪声也不同，最终参数一定要现场标定。

这张表的价值在于给出数量级参考。很多人调参失败，不是因为灵敏度不够，而是因为初始参数差了三个数量级，导致要么滤波器完全追不上测量，要么干脆发散。

3.4 进阶：用残差做R自适应

固定Q/R能覆盖大部分场景，但RM比赛现场很残酷。测速模块可能因为供电波动变差，云台陀螺仪可能因为振动加剧导致角速度噪声增大。固定参数的滤波器会在一段时间后失配。

解决思路是实时估算测量噪声。计算innovation残差，也就是测量值与预测值的差：

innov = z - H * x_pred

理论上，如果模型和参数都正确，innovation应该是一个均值为零、方差为S = HPH^T + R的随机序列。我们可以用滑动窗口统计innovation的经验方差，反过来修正R。这个思路实现不复杂，效果却很明显。

我写过一版简单的自适应R：维护一个长度为N的循环缓冲区，每次更新后推入当前innovation，定期计算方差，再根据方差与理论S的比值去缩放R。实测下来，连续射击时滤波输出明显更稳，弹道解算的落点散布也小了一圈。这个技巧很值得电控组师弟师妹们去实现，性价比极高。

4. 现场问题与排查技巧实录

4.1 先查时间戳再查数学

卡尔曼滤波的预测步骤，本质是用上一帧状态推导到当前帧。如果某一次测量因为中断延迟晚到了两个周期，滤波器会拿“过期测量”去做更新，整套状态会被带偏到天边去。

我在STM32上遇到过几次“滤波结果发散但数学明明没错”的问题，最后都是时序引起的。排查思路很简单：给每次测量打一个us级时间戳，更新时检查当前测量时间戳和上一帧的时间间隔。如果和预期周期明显不符，要么丢弃这帧数据，要么把状态转移矩阵里的dt换成实际间隔。很多时候滤波炸了，不是数学错了，是时间错了。

4.2 传感器性能漂移与R失配

有一次打弹道测试，连续射击的后半段，解算出来的着弹点总是偏上。排查了很久，最后发现测速模块在连续射击时发热，光电对管响应延迟升高，R变大但代码里还是固定值，滤波器就过度相信了已经变差的测量。

这个案例让我印象很深。从那以后，我对所有滤波代码的要求都是：R可以被动态调整，而不是一个写死的常量。比赛中要警惕传感器性能漂移，卡尔曼滤波不只是一个离线调好的固定算法，更应该被看成一个能感知传感器健康度的在线估计器。

4.3 初始化P0太小导致长时间不收敛

新手最容易犯的错，是上来把P0设成和Q差不多大。比如Q是0.01，就顺手把P0也等于0.01，结果滤波结果头几十帧都拖着一条明显的“温吞”尾巴，怎么等都不收敛。

记住，P0代表先验不确定度，宁可设大十倍也不要设小。P0=1和P0=100在跑一秒数据后通常会收敛到同一个稳态，但P0设小的代价是收敛慢。到了比赛第一发弹就要精准的场景，这几十帧的收敛时间就是致命的。保险起见，初始化时把P0设成一个比Q大两三数量级的数，比如Q=0.01时P0=10，基本不会出错。

4.4 调试手段：把内部状态全部发出来

实战中最有效的排查方法，是把x、P、K、innovation这些内部量全部通过串口发到上位机。很多人只发最终滤波结果，一旦发现结果不对，根本看不出是哪一步出了问题。你把中间量打出来，配合波形可视化工具，就能迅速判断是预测错、更新错还是参数错。

我们组内部定了一个规矩：滤波模块必须提供debug输出接口，至少能打印状态向量和协方差对角元素。这个习惯帮我们省了很多对线时间。你在调Q/R时，光看滤波输出是看不出P在怎么走的，但P的变化能告诉你滤波器是不是已经进入稳态，K的变化能告诉你当前到底是更相信模型还是更相信测量。

4.5 float与double选型

STM32F4这类带FPU的单片机上，float计算是硬件加速的，精度对绝大多数RM场景足够。只有在做低漂移IMU姿态解算，或者需要长时间积分的时候才需要考虑double。

要注意的是，有些芯片的FPU是单精度，double运算是软件模拟，性能差距能到几十倍。我见过有人在全代码里用了double，结果主循环执行时间暴涨，整机控制周期直接拖垮。嵌入式上，能float就float，能用定点就不碰浮点，这是原则。

5. 从单状态滤波走向多传感器融合

5.1 云台姿态估计怎么套用同一套框架

弹丸测速是一维或二维状态估计，云台姿态估计则更容易让人晕，因为涉及角度、角速度和角加速度，还常常有多个传感器。但拆开看，结构还是那套：状态量选偏航角和偏航角速度，陀螺仪角速度作为模型预测依据，视觉或加速度计解算出的角度作为测量。

一个简化模型是：

[ theta_pred ] [ 1 dt ] [ theta_prev ] [ 0 ] [ omega_pred ] = [ 0 1 ] [ omega_prev ] + [ gyro_bias_est ]

陀螺仪的零漂可以作为状态量一并估计，这样滤波器不仅能输出平滑角度，还能在线估计出陀螺仪零偏，这比用固定补尝值靠谱得多。很多RM队伍的云台angle滤波就是从这个模型起步的，效果比互补滤波更有延展性。

如果你要融合IMU里加速度计和陀螺仪的数据，记得先做坐标变换和重力分量补偿，不然测量方程里会平白多出一堆系统误差。我的经验是，先把加速度计在静止时测得的向量和重力向量做对齐，再拿到滤波器里当观测，这样出来的姿态角才不会有明显静差。

5.2 非线性与EKF：什么时候需要升级

标准卡尔曼滤波只适用于线性系统，也就是状态转移和观测都是线性关系。RM里很多真实系统是非线性的，比如云台的旋转矩阵、弹道解算时的空气阻力模型、视觉测距里的透视投影关系。

这类非线性问题，最简单常用的扩展方式是EKF，也就是扩展卡尔曼滤波。做法是先对非线性函数做一阶泰勒展开，用雅可比矩阵替代线性系统中的A和H，然后继续跑标准卡尔曼流程。EKF思路简单，工程上非常常见，但它的线性化误差在小角度、近距离等场景下通常可以接受，一旦状态误差大或者系统强非线性，就可能不收敛。

我自己用得最多的判断标准是：如果系统能在正常工作点附近用线性模型较好地描述，就用标准卡尔曼或EKF；如果机器人要做大范围翻滚、快速掉头这些极端机动，可能就要考虑UKF甚至粒子滤波了。不过对RM电控而言，99%的场景EKF已经完全够用，没必要一上来就追求更高阶算法。

5.3 电控组后续可以扩展的方向

卡尔曼滤波在RM电控里还能扩展出不少方向。比如视觉目标预测，用一个恒速或恒加速模型对敌方机器人的位置做外推，弥补云台转向过程的滞后；再比如电源管理，融合裁判系统功率、电池电压和电调电流，提前预测功率上限，让机器人能在不扣血的条件下打出更高射速。

关于“合集”这两个字，我在标题里也写了，卡尔曼滤波本身不是孤立存在的，它在不同模块里的不同形态，值得电控组以合集的思路去沉淀。测速模块的滤波、云台姿态的融合、功率闭环的状态估计，三套代码可以共用同一个矩阵运算层，参数可以按场景分别标定。形成一个标准化的滤波模板，比每个队员各写各的、彼此不通用要高效得多。

最后再分享一个小技巧。如果你要教新人入门卡尔曼滤波，不要让他直接看多变量矩阵推导。先拉着他在点一份测速模块的数据处理任务，从一维卡尔曼写起，跑起来、画出来、调起来，等他发现一维不够用了，再自然地引入二维状态向量。这样不仅学习曲线平滑，而且他上手后就会自己去找Q/R的物理含义，而不是在那里背公式。我第一次带队员走完这个流程后，后续让他去写云台姿态融合，基本上不用我多解释，他自己就能把模型搭出来，这就是实战路线最大的好处。