前言：
在之前的文章中，我们提到了互补滤波，它通过固定比例（比如 0.98 和 0.02）来融合陀螺仪和加速度计的数据。
但这有个致命缺陷：环境是会变的！当你的无人机平稳飞行时，加速度计很准；但当无人机暴力加速或疯狂震动时，加速度计的数据全变成了垃圾噪声。如果死守着固定的融合比例，解算出的姿态就会疯狂发抖。
卡尔曼滤波的伟大之处在于：它能根据当前的噪声情况，动态计算出一个“最优信任比例”！
一、 核心思想：预测与纠正（打工人的自我修养）
想象你是一个闭着眼睛走路的打工人，你想知道自己当前的确切位置（最优估计值）。你获取位置有两个途径：
预测（经验）：你凭感觉，根据刚才跨出的步伐长度，猜测自己往前走了 1 米。但这有误差（比如步子迈大了，这叫过程噪声 $Q$）。
测量（仪器）：你睁开眼睛看了一眼旁边的路标，路标告诉你走了 1.2 米。但这也有误差（比如你看眼花了，这叫测量噪声 $R$）。
卡尔曼滤波就是在这两者之间做一个权衡：
如果我知道自己的步子一向很稳（$Q$ 很小），我就多相信“预测”；如果我知道自己是个高度近视，看路标经常看错（$R$ 很大），那我就少相信“测量”。
二、 卡尔曼的 5 步黄金法则（一维标量版）
对于简单的单轴姿态解算（比如只算一个 Pitch 角），我们可以把吓人的矩阵全部退化成普通的标量（一维变量）。卡尔曼滤波永远在循环执行以下 5 步：
第一阶段：预测（Predict）—— 闭着眼睛往前走
预测当前状态：$X_{predict} = X_{last} + Gyro \cdot dt$
(根据上一时刻的角度，加上陀螺仪积分，推算出当前角度)
预测误差协方差：$P_{predict} = P_{last} + Q$
(我凭感觉走了一步，心里的“不确定性 $P$”肯定增加了，增加的量就是过程噪声 $Q$)
第二阶段：纠正（Update）—— 睁开眼睛看路标
3.计算卡尔曼增益（核心！）：$K = \frac{P_{predict}}{P_{predict} + R}$
（这是一个信任比例。如果测量噪声 $R$ 很大，$K$ 就趋近于 0；如果 $R$ 很小，$K$ 就趋近于 1）
4.计算最优估计值：$X_{best} = X_{predict} + K \cdot (Acc\_Angle - X_{predict})$
（用测量值修正预测值。如果 $K=0$，完全相信预测；如果 $K=1$，完全相信测量）
5.更新误差协方差：$P_{new} = (1 - K) \cdot P_{predict}$
（睁开眼睛确认了位置后，我心里的“不确定性 $P$”变小了，保存下来留给下一次循环）
三、 极简 C 语言实现
不要去网上抄那些几百行的多维矩阵代码了，对于 90% 的电赛姿态融合需求，下面这个一维卡尔曼滤波器足够你在赛场上横着走：
// 定义卡尔曼结构体 typedef struct { float Q_angle; // 过程噪声协方差（陀螺仪漂移噪声） float R_measure;// 测量噪声协方差（加速度计高频噪声） float angle; // 最优估计角度 float P; // 误差协方差 float K; // 卡尔曼增益 } Kalman_t; // 初始化 void Kalman_Init(Kalman_t *klm) { klm->Q_angle = 0.001f; // 经验值：越小越相信陀螺仪 klm->R_measure = 0.03f; // 经验值：越大越不相信加速度计 klm->angle = 0.0f; klm->P = 1.0f; } // 核心滤波循环 // newAngle: 加速度计算出的角度 // newRate: 陀螺仪测出的角速度 // dt: 采样周期 float Kalman_GetAngle(Kalman_t *klm, float newAngle, float newRate, float dt) { // 1. 预测当前角度 klm->angle += newRate * dt; // 2. 预测不确定性 klm->P += klm->Q_angle; // 3. 计算卡尔曼增益 klm->K = klm->P / (klm->P + klm->R_measure); // 4. 计算最优角度 klm->angle += klm->K * (newAngle - klm->angle); // 5. 更新不确定性 klm->P = (1 - klm->K) * klm->P; return klm->angle; }四、 调参玄学：$Q$ 和 $R$ 到底怎么调？
在工程实战中，$Q$ 和 $R$ 其实是调参魔法：
调大 $R$：意味着告诉系统“传感器震动很厉害，别信加速度计”。输出波形会变得极其平滑，但相位滞后会变大（反应慢）。
调大 $Q$：意味着告诉系统“陀螺仪温漂很严重，别信陀螺仪积分”。输出波形会紧紧跟随加速度计，变得反应灵敏，但伴随毛刺抖动。
寻找 $Q$ 和 $R$ 的最佳比例，就是寻找平滑与灵敏的完美平衡点！
五、 总结
卡尔曼滤波并不神秘，它本质上就是一个通过动态改变权重（卡尔曼增益）来实现最优融合的算法。只要理解了它的 5 步循环思想，你就能将其应用到电池 SOC 估算、GPS 轨迹平滑等任何存在噪声的系统中。
