1. 从赛前一次云台抽风说起：RoboMaster电控为什么绕不开卡尔曼滤波
1.1 那段让人抓狂的赛前调试
前年区域赛前一周，我们队的英雄云台在低速跟踪时会周期性抖一下，幅度不大但很致命——打小陀螺的命中率直接掉了三成。拆开看代码，姿态解算用的是最朴素的互补滤波，陀螺仪和加速度计各给一个系数就完事了，参数调了三天，要么响应慢得像喝多了，要么噪声大到电机跟着啸叫。后来把加速度计和陀螺仪的数据丢进一个两状态的卡尔曼滤波器，二十分钟后云台就稳了，那种"原来是这样"的感觉，至今记得很清楚。
这个故事几乎所有打过RoboMaster的队伍都会遇到。云台要跟目标、底盘要估计速度、发射机构要控制摩擦轮转速、超级电容要算剩余能量，每一个环节背后都在跟"带噪声的传感器数据"打交道。而卡尔曼滤波就是处理这类问题最趁手的一把工具。这篇内容我会从中科大战队的实际电控开发出发，把卡尔曼滤波在RM里的落地思路、公式含义、代码实现和调参经验一次讲透，无论你是刚进队的新人还是已经打过一届的老电控，都能从里面找到能直接抄的东西。
1.2 卡尔曼滤波到底替电控解决了什么问题
一句话：它能把"不准的模型预测"和"不准的传感器测量"揉在一起，得到一个比两者都准的估计值。放到RM场景里，陀螺仪短时间准但会漂移，加速度计长期不漂但受震动干扰极大，编码器能测速但有量化误差和延迟，电流传感器受电机换向噪声影响严重。单靠任何一个都做不出稳定的控制，而卡尔曼滤波恰好给出了一个数学上最优的融合方式。
它解决的核心痛点有三个。第一是噪声抑制，原始传感器信号高频抖动大，直接拿去做PID的微分项会把噪声放大成一个咆哮的电机。第二是延迟补偿，很多传感器（比如某些IMU的加速度计、视觉解算的角度）输出频率低或者有固定延迟，用滤波器可以在两帧之间做预测，把控制周期填满。第三是不可观测量估计，比如底盘的实际对地速度很难直接测，但可以通过轮速和IMU加速度融合估计出来，这对打滑抑制和里程计至关重要。
1.3 这篇内容适合谁看，需要什么基础
如果你是刚接手云台电控的新人，建议先把矩阵运算和状态空间表达这两个概念补一下，至少要能看懂"状态向量乘矩阵"是什么意思。有单片机C语言基础、会写PID、会用定时器采IMU，这就够了。我下面讲公式时会尽量用大白话，代码全部是能直接编译进STM32工程的风格。硬件上假设你手里有一块常见的六轴或九轴IMU，MCU有FPU最好，没有的话浮点运算也能跑，只是要注意主频。
2. 卡尔曼滤波的数学思想：把那些吓人的公式讲成人话
2.1 预测和更新：一场反复横跳的自我修正
卡尔曼滤波的整个流程说白了就两个动作在循环：预测和更新。预测是"根据我上一步的估计，推测这一步大概在哪"，更新是"拿传感器实测值来纠正我猜的位置"。你可以把它想象成开车进隧道：进隧道前你知道自己的速度，于是预测出了隧道口的位置（预测）；出来一看路牌发现偏了，就按偏差修正自己的位置估计（更新）。两个动作一轮一轮跑，估计值就越来越贴近真实。
关键在于，卡尔曼滤波不只是修正"位置"这一个值，它同时维护一个叫协方差的东西，代表"我对当前估计有多不确定"。预测时不确定度变大（因为模型有误差），更新时不确定度变小（因为有了新信息）。滤波器会根据"模型的不确定度"和"测量的不确定度"谁更小，来决定更相信谁。这就是它比固定系数的互补滤波高级的地方——系数是动态算出来的，而不是拍脑袋定的。
2.2 五个核心公式逐个掰开揉碎
标准卡尔曼滤波有五个方程，我按RM电控的使用顺序讲。
状态预测：x(k|k-1) = A·x(k-1|k-1) + B·u(k)。意思是这一步的预测状态，等于上一步估计状态经过状态转移矩阵A演化，再加上控制输入u通过矩阵B施加的影响。在云台角度解算里，A通常是一个随时间步长变化的矩阵，x里放角度和陀螺零偏，u就是陀螺仪测到的角速度。
协方差预测：P(k|k-1) = A·P(k-1|k-1)·Aᵀ + Q。这一步是在传播不确定性，Q是过程噪声矩阵，代表你对模型有多不信任。Q越大，说明你认为模型越不准，滤波器就会越依赖传感器。
卡尔曼增益：K(k) = P(k|k-1)·Hᵀ·(H·P(k|k-1)·Hᵀ + R)⁻¹。这个是整个滤波器的灵魂。H是观测矩阵，把状态映射到能被传感器观测的空间；R是观测噪声矩阵。K的本质是一个权重，它告诉滤波器"这次测量值该信几分"。R越大（测量越不可靠），K越小，滤波器越相信自己预测的值。
状态更新：x(k|k) = x(k|k-1) + K(k)·(z(k) - H·x(k|k-1))。括号里那一项叫新息，就是"实测值减去预测的观测量"，也就是预测偏差。用这个偏差乘上增益，去修正状态估计。
协方差更新：P(k|k) = (I - K(k)·H)·P(k|k-1)。修正完状态，也要修正不确定度。这一步保证了滤波器不会一直自信满满，也不会一直摇摆不定。
你不需要在工程里手推这五个公式，但理解它们的物理含义很关键——尤其是Q和R，它们直接决定了滤波器的性格，后面调参会反复用到。
2.3 和互补滤波、低通滤波比，它到底强在哪
很多队伍一开始用的是互补滤波，两个系数一加一减，实现只要几行代码，调起来也直观。它在云台角度这种"陀螺仪积分漂移+加速度计抖动"的经典场景下确实能用，问题是系数固定，遇到不同的运动状态就露馅：静止时希望滤波重一点压噪声，高速旋转时又希望响应快一点别滞后。固定系数只能取折中。
低通滤波更简单，就是压高频，但它天然带来相位滞后，用在控制回路里会吃掉相位裕度，容易引起振荡。卡尔曼滤波的增益是动态的：运动剧烈、预测不准时自动加大增益追传感器；运动平稳时自动减小增益压噪声。这种"自适应"是它最大的优势。
当然它也不是没有代价。计算量比互补滤波大得多，状态维数一高对MCU就是负担；建模不对、Q和R乱设，滤波器照样能发散。所以我的建议是：简单场景用互补滤波够用就别硬上卡尔曼，但在云台、底盘估计、超级电容能量管理这类对精度要求高的环节，卡尔曼值得投入。
3. 云台角度解算：卡尔曼滤波的第一次实战
3.1 传感器选型与坐标系约定，这一步千万别省
先说硬件。常见的六轴IMU（三轴陀螺+三轴加速度）是最低配置，如果还要做绝对航向，就需要加磁力计或视觉。但RM赛场上电机多、电流大，碳纤维车体又是导电的，磁场环境非常脏，磁力计基本是靠不住的，我个人的做法是云台偏航角用陀螺积分为主、视觉或碰撞线做慢速校正，不用磁力计。
坐标系一定要提前约定好并写进注释。我们的约定是：X轴指车头前方，Y轴指左侧，Z轴向上，陀螺仪和加速度计都按右手定则。碳板车身会把IMU的地和车架连在一起，布线时信号线要远离电机线束，最好双绞或加屏蔽，否则你会发现加速度计的数据里全是电机换向的噪声，后面滤波再强也救不回来。IMU安装位置尽量靠近云台旋转中心，减少离心加速度对加速度计的污染。
3.2 状态量怎么选：为什么是角度加零偏
最经典的两状态模型：状态向量x = [角度, 陀螺零偏]ᵀ。为什么要把陀螺零偏也放进状态里估计？因为陀螺仪的零偏会随温度变化慢慢漂，如果当成常数写死，积分出来的角度就会慢慢跑偏。把它当成一个"待估计的缓慢变化量"放进状态，滤波器会在运行过程中自动把它辨识出来，这样角度的长期稳定性会好很多。
状态转移矩阵A就是：[1, -dt; 0, 1]。第一行表示角度 = 上一时刻角度 - 零偏×dt + 陀螺测量×dt，第二行表示零偏保持不变。观测矩阵H取[1, 0]，因为我们只用加速度计解算出的角度作为观测量，零偏是不可直接观测的。这里有个细节：加速度计只有在运动加速度很小的时候，解算出的倾角才可信，所以观测量前面通常要加一个"可信度判断"，运动剧烈时把R调大甚至临时不更新。
3.3 能直接抄进工程的C代码实现
先给一个最通用的单状态版本，方便新手理解核心逻辑：
typedef struct { float x; // 状态估计 float P; // 估计协方差 float Q; // 过程噪声 float R; // 观测噪声 } KF1D; float KF1D_Update(KF1D *kf, float z) { // 1. 预测：不确定度增加 kf->P += kf->Q; // 2. 计算卡尔曼增益 float K = kf->P / (kf->P + kf->R); // 3. 更新状态 kf->x += K * (z - kf->x); // 4. 更新协方差 kf->P *= (1.0f - K); return kf->x; }这段代码用在比如电池电压滤波、单个轮速估计上都非常合适。真正做云台角度，用两状态版本更稳：
typedef struct { float angle; // 角度估计 float bias; // 陀螺零偏估计 float P[2][2]; // 协方差矩阵 float Q_angle; // 角度过程噪声 float Q_bias; // 零偏过程噪声 float R_measure; // 观测噪声 float dt; // 采样周期, 单位秒 } KalmanAngle; float KalmanAngle_Update(KalmanAngle *kf, float newAngle, float newRate) { // ---------- 预测 ---------- float rate = newRate - kf->bias; kf->angle += kf->dt * rate; kf->P[0][0] += kf->dt * (kf->dt * kf->P[1][1] - kf->P[0][1] - kf->P[1][0] + kf->Q_angle); kf->P[0][1] -= kf->dt * kf->P[1][1]; kf->P[1][0] -= kf->dt * kf->P[1][1]; kf->P[1][1] += kf->Q_bias * kf->dt; // ---------- 更新 ---------- float S = kf->P[0][0] + kf->R_measure; // 新息协方差 float K0 = kf->P[0][0] / S; float K1 = kf->P[1][0] / S; float y = newAngle - kf->angle; // 新息 kf->angle += K0 * y; kf->bias += K1 * y; float P00_temp = kf->P[0][0]; float P01_temp = kf->P[0][1]; kf->P[0][0] -= K0 * P00_temp; kf->P[0][1] -= K0 * P01_temp; kf->P[1][0] -= K1 * P00_temp; kf->P[1][1] -= K1 * P01_temp; return kf->angle; }这段的推导思路在业界流传很广，核心就是把零偏当状态估出来。注意dt必须是真实采样周期，如果你的IMU中断有抖动，最好用定时器实测的dt，别用理论值，否则滤波器会周期性偏。
3.4 Q和R怎么调：拿数据和现象说话
参数整定是新手最头疼的地方，我给一套实操方法。
先整R。让云台静止，把加速度计解算出的角度打印出来，看它的波动幅度。波动大说明测量噪声大，R就设大一点，通常从0.01到0.5之间取。判断标准是：静止时滤波输出的角度不能跟着加速度计的抖动跳。
再整Q_angle和Q_bias。Q_angle代表你相信"角度模型的演化有多不确定"，Q_bias代表"零偏变化有多快"。经验上Q_angle从0.001起调，Q_bias从0.003起调。Q_angle设大，滤波器响应快但噪声大；Q_bias设大，零偏跟踪快但可能把真实角度信号也当成漂移给吃掉。
用现象反推。如果角度长期缓慢漂移，说明零偏估计不够，加大Q_bias。如果跟随目标时有明显滞后，加大Q_angle或者减小R。如果输出高频抖动，减小Q_angle或者加大R。我一般会在上位机用曲线把原始陀螺积分、加速度计解算、滤波输出三条线画在一起，一眼就能看出谁的问题。
注意：调参时一定要用真实运动数据回放，不能在静止状态下调好了就上赛场。云台快速旋转时离心加速度会污染加速度计，这时候如果不动态加大R，滤波器会把错误的倾角当真的用，云台会突然摆头。
4. 底盘、超级电容和更多RM场景的卡尔曼应用
4.1 底盘速度估计与打滑抑制
底盘电控里，轮速编码器能测出轮子转速，但在打滑时轮速和实际对地速度完全对不上，麦克纳姆轮全向移动还涉及多个轮子的速度合成。单纯用轮速做里程计，一场比赛下来位置误差能积累到几十厘米。做法是把IMU的加速度积分和轮速融合：轮速作为观测量，加速度作为控制输入，用一个线性卡尔曼在二维（X、Y速度）上估计底盘实际速度。
底盘场景的模型是v(k) = v(k-1) + a·dt，观测量是z = v_odom（由轮速换算），非常简单。难点在于打滑检测：当轮速变化率和加速度计读数严重不一致时，说明在打滑，此时把R临时放大，让滤波器不信里程计，主要靠加速度积分撑过去。实测下来，融合后的速度在急停急转时明显更真实，配合功率控制能减少电机过载。
4.2 超级电容能量状态估计
RM的功率控制离不开超级电容。规则给的是功率上限，超了要扣血，所以每辆车都在用电容把瞬时大功率缓冲掉。电控要做的是实时估计电容的SOC（剩余能量），据此决定放电策略。SOC最直接的办法是安时积分：SOC = SOC₀ - ∫I·dt / 额定容量。但电流传感器有零偏和噪声，积分久了误差就累积了。
这里用一个单状态卡尔曼非常合适：状态是SOC，控制输入是实测电流，观测量是电容端电压（通过开路电压和SOC的关系反推）。电压测量在电容内阻大、电流突变时会有明显偏差，正好用卡尔曼动态权衡。整定的关键是把电流传感器的噪声方差估准，一般做成标定流程，上电时清零并记录一段静置数据反推零偏。
提示：电容电压和SOC的关系是非线性的，做模型时至少用分段线性近似，别当成理想电容用U = Q/C，超级电容在大电流下电压跌落很明显，直接套公式误差很大。
4.3 多模块联调时的接口约定
卡尔曼滤波不是孤立的，它跟控制的接口设计很关键。我们的约定是：滤波器统一跑在1kHz的姿态任务里，输入是IMU原始数据和上一拍控制量，输出是滤波后的角度、角速度、零偏，通过共享内存或者全局结构体给控制层用。控制层不做任何滤波，避免重复滤波引起相位混乱。
另外要注意初始化。滤波器上电时协方差要设成一个较大的初值，让它快速收敛；状态初值可以从传感器各取一次直接赋值。云台上电那几秒钟通常是自检状态，云台不响应遥控，等P[0][0]降到某个阈值以下再放行，这样能避免上电瞬间的乱摆。碳板车体上如果IMU和主控地不干净，初始化时角度可能直接就是一个错的值，所以上电自检里加一条"角度是否在合理范围"的判断很有必要。
5. 常见问题排查与踩坑实录
5.1 三类典型症状：发散、滞后、抖动
发散最常见的原因是符号错误或dt错误。状态转移矩阵里零偏项的符号如果写反，滤波器会越跑越偏，表现为角度单调漂移且无法收敛。dt用错（比如实际1ms写成10ms）也会造成等效增益错乱，最终发散。排查方法很简单：把预测和更新分开打印，看协方差是否单调增大。
滞后是Q设得太小或R设得太大导致的，滤波器过度相信模型，不信任测量。表现为云台跟随目标时"慢半拍"，打快速移动目标时命中率低。解决办法是加大Q_angle或减小R，代价是噪声变大，需要在两者间找平衡。
抖动则相反，R太小或Q太大，滤波器把传感器噪声放大了。表现为静止时电机就有细微的高频啸叫，长时间会把电机和齿轮磨坏。先检查机械和布线，确认不是硬件噪声后再调参数。
5.2 常见问题速查表
| 现象 | 可能原因 | 排查方向 | 处理办法 |
|---|---|---|---|
| 角度缓慢漂移不收敛 | 零偏未被有效估计 | 检查状态里是否含零偏 | 加大Q_bias，检查符号 |
| 快速旋转后角度错乱 | 加速度计被离心力污染 | 看运动时R是否固定 | 动态调大R或禁用该帧观测 |
| 静止时电机高频啸叫 | R太小或Q太大 | 打印滤波输出频谱 | 减小Q或加大R |
| 跟随目标慢半拍 | Q太小或R太大 | 对比滤波与原始信号延迟 | 加大Q_angle |
| 上电瞬间云台乱摆 | 协方差初值太小 | 看收敛速度 | 加大P初值并加自检等待 |
| 电流估计偏差大 | 电流传感器零偏 | 静置标定 | 上电清零并做温补 |
| 底盘速度里程计漂移 | 打滑未检测 | 对比轮速与加速度 | 打滑时临时放大R |
5.3 几条只能靠比赛喂出来的经验
第一条，滤波器不是越复杂越好。有队伍一上来就上扩展卡尔曼处理四元数，结果参数多到调不动，最后还不如老老实实做两状态。把简单的模型调明白，比堆一个调不动的复杂模型强得多。
第二条，一定要做数据回放。赛场上时间紧，不可能在车上慢慢试。我的习惯是把IMU原始数据存到SD卡或者通过无线回传到上位机，离线用Python复现滤波，调参调到满意再烧回去。这样一次上电就能验证，效率高很多。
第三条，注意数值类型和运算顺序。在MCU上做矩阵运算，浮点精度损失比想象中大，尤其是协方差矩阵做减法时可能出现微小的负值，长期会破坏正定性。可以在每次更新后强制让P的对角元素不小于一个极小正数，或者用float的平方根形式（UD分解）实现，稳定性会好很多。
第四条，滤波器只管估计，别让它替你做决策。很多新手喜欢把滤波后的角度直接喂给PID，然后指望它解决所有抖动问题。其实抖动可能来自机械间隙、来自电流环、来自控制周期不匹配，滤波只解决传感器噪声这一环。定位问题时先把滤波旁路，用原始信号跑一遍控制，往往能发现真正的原因在别处。
第五条，从零偏估计的角度说，碳板车身的温升对IMU零偏影响非常明显。比赛打久了电机发热，车体温度上来，陀螺零偏会漂，这时候能不能快速跟上就看Q_bias设得够不够。我个人的经验是把Q_bias设在刚好能在一个控制周期内对零偏变化做出反应的量级，太大反而会把真实角速度吃掉。
最后再分享一个我们队用的小技巧：给滤波器加一个"健康度"监控。每次更新后看新息y的大小，如果连续若干拍y都超过某个阈值（比如三倍标准差），就认为测量异常，临时把R放大或者跳过这次更新。这个机制在对付偶发的IMU数据跳变上非常好用，相当于给滤波器加了一层保险，比赛里少翻好几次车。
