从零实现卡尔曼滤波：Arduino与MPU6050的传感器数据融合实战

1. 卡尔曼滤波：从“称体重”到“数据融合”的通俗理解

大家好，我是老张，一个在嵌入式圈子里摸爬滚打了十多年的老玩家。今天咱们不聊那些虚头巴脑的理论，就从一个最接地气的例子开始，聊聊卡尔曼滤波到底是个啥。我敢保证，就算你之前被那些矩阵公式吓到过，听完我这个比喻，你也能瞬间明白它的精髓。

想象一下，你买了两台体重秤。第一台是你在网上图便宜，花了三十块钱买的。第二台呢，你咬咬牙，在商场花了两百块买了个大品牌的。现在你想知道自己的真实体重，你会怎么做？你可能会先站上便宜的秤，显示63.5公斤。再站上贵的秤，显示60.0公斤。你心里清楚，便宜的秤可能不太准，贵的秤理论上更可靠。但到底信哪个？一个简单的办法是取个平均值，61.75公斤。这个“取平均值”的过程，就是一种最简单的数据融合。

但仔细一想，取平均值好像也不完全对。这相当于你把两台秤放在了完全平等的地位上，各信50%。可现实是，贵的秤质量更好，我们应该更相信它一些。于是，你可能会给贵的秤一个更高的“权重”，比如70%，给便宜的秤30%的权重。这样算出来的体重就是 63.5 * 0.3 + 60.0 * 0.7 = 61.05公斤。这个“权重”，就是卡尔曼滤波里最核心的玩意儿——卡尔曼增益（K）。

卡尔曼滤波干的事儿，本质上就是这个“加权平均”。只不过，它处理的不是两台秤，而是两个“信息来源”：一个是你根据过去经验和物理规律预测出来的状态（比如根据上一秒的速度和加速度，预测下一秒的位置），这叫先验估计；另一个是你的传感器测量到的状态（比如MPU6050测到的角度），这叫测量估计。这两个信息单独看都不完美：预测会受模型简化、外界干扰的影响而有误差；测量会受传感器噪声、温漂的影响而不准。卡尔曼滤波就像一个聪明的裁判，它根据你对预测模型和传感器本身的“信任程度”（这个信任度就是通过噪声统计计算出来的），动态地调整权重K，把预测值和测量值融合起来，得到一个比两者都更靠谱的最优估计。

所以，下次你再看到卡尔曼滤波那五个公式感到头疼时，就回想一下“两台体重秤”的故事。它的目标无比朴实：在充满噪声和不确定性的现实世界里，如何巧妙地结合“理论推测”和“实际观测”，得到最接近真相的那个数。接下来，我们就带着这个理解，用Arduino和MPU6050，亲手把这个聪明的“裁判”实现出来。

2. 实战准备：认识你的硬件伙伴MPU6050

在请出“卡尔曼裁判”之前，我们得先了解场上两位“运动员”的特性。在我们的平衡小车或姿态监测项目里，这两位运动员通常就是MPU6050传感器提供的两种数据：加速度计算出的角度，和陀螺仪积分得到的角度。它们各有各的脾气，也各有各的毛病。

MPU6050是一个集成了三轴加速度计和三轴陀螺仪的芯片，通过I2C接口和我们的Arduino对话。加速度计测量的是各个轴线方向的加速度，包括重力加速度。当我们把模块平放时，利用重力加速度在X、Y、Z轴上的分量，通过一个简单的三角函数（atan2）就能算出模块相对于水平面的倾角。这个方法响应快，在静止或低速时非常准。但是，它有个致命弱点：害怕运动加速度。一旦你的小车加速前进或者突然刹车，额外的运动加速度会混入重力加速度中，导致计算出的角度严重失真。你可以自己试试，快速晃动模块，串口输出的角度值会跳得非常厉害。

陀螺仪则不同，它测量的是绕各个轴旋转的角速度。我们对角速度进行积分（简单说就是角速度乘以时间），就能得到角度变化量，再加上初始角度，就能持续得到当前角度。陀螺仪的优点是对线性运动不敏感，响应迅速。但它的问题在于漂移。任何微小的零点误差（哪怕传感器完全静止，输出也不为零）在积分过程中都会被不断累积放大，时间一长，角度值就会慢慢“飘走”，偏离真实值。

你看，这不正好对应了我们“体重秤”的例子吗？加速度计就像那台便宜的秤，大部分时候还行，但一有“剧烈运动”就不靠谱；陀螺仪像那台贵的秤，短期精度高，但自己会“慢慢走偏”。如果我们只用其中一个，系统要么在运动时发疯，要么在静止时慢慢“迷路”。所以，我们必须把它们的数据融合起来，用加速度计的长期稳定性去修正陀螺仪的漂移，用陀螺仪的短期精确性去抵抗加速度计在运动时的干扰。而实现这种融合的完美算法，就是卡尔曼滤波。

为了后续的融合，我们首先得学会和MPU6050打交道。你需要准备一块Arduino开发板（UNO、Nano、ESP32等都行），一个MPU6050模块，以及几根杜邦线。接线非常简单，主要就是I2C的那四根线：VCC接5V或3.3V（看模块支持），GND接地，SCL接开发板的I2C时钟线（例如UNO的A5），SDA接I2C数据线（例如UNO的A4）。有的模块带中断引脚和地址选择引脚，我们初步使用可以不用接。

接下来，在Arduino IDE中，你需要安装两个库：Wire.h是I2C通讯库，通常IDE自带；MPU6050.h则需要通过库管理器搜索安装，我常用的是MPU6050_light这个库，它对新手更友好。安装好后，你可以先跑一个最简单的测试程序，读取原始数据并转换成角度，确保硬件连接和通讯正常。这个阶段的目标不是完美，而是先让传感器“动起来”，看到数据在串口监视器里滚动起来，你的信心就有了第一块基石。

3. 卡尔曼滤波的“内功心法”：五大核心公式拆解

硬件通了，数据有了，现在该请出今天的主角——卡尔曼滤波的五个核心公式了。别怕，我们不搞数学证明，只讲每个公式在“干什么”，以及对应到我们的MPU6050例子里，它具体在计算什么。我把它总结为“预测”和“更新”两大步，就像先猜后验证。

第一步：预测（做一道理论计算题）这个阶段，我们不看传感器，只根据上一刻的状态和物理规律，猜一下现在应该是什么样。

1. 状态预测公式：X_hat = A * X + B * U

   • 它在干什么：预测当前时刻的系统状态。X是上一时刻的最优估计状态（比如角度和角速度），A是状态转移矩阵（描述角度和角速度如何随时间变化，对于匀速转动模型，角度=上一时刻角度 + 角速度*时间），B和U是控制输入，在我们这个自主运动的例子里可以先忽略。X_hat就是我们“猜”出来的当前状态，也叫先验估计。
   • MPU6050例子：假设我们只关心一个轴的倾角。那么X可以是一个包含两个元素的向量：[角度; 角速度]。A矩阵就是[[1, dt], [0, 1]]。这个矩阵的意思就是：新角度 = 旧角度 + 角速度 * 时间间隔(dt)；新角速度 = 旧角速度（假设短时间内角速度不变）。这样，我们就用陀螺仪的数据（角速度）预测出了新的角度。

2. 误差协方差预测公式：P_hat = A * P * A^T + Q

   • 它在干什么：预测我们这份“先验估计”的不确定性有多大。P是上一时刻状态估计的误差协方差矩阵（可以理解为我们对估计结果的自信程度），P_hat是预测后的不确定性。Q是过程噪声协方差矩阵，它代表我们物理模型不完美的程度。比如，我们假设角速度恒定，但实际角速度可能在变化，这个建模误差就用Q来描述。
   • MPU6050例子：Q矩阵的大小直接影响了滤波器的“性格”。如果Q设得大，说明我们认为模型很不准，预测值X_hat可信度低，滤波器会更相信测量值。这个矩阵需要我们通过实验或经验来设定，是调参的关键之一。

第二步：更新（用实测数据修正猜测）现在，传感器读数来了，我们要用它来修正刚才的猜测。

3. 卡尔曼增益计算公式：K = P_hat * H^T * (H * P_hat * H^T + R)^-1

   • 它在干什么：计算那个最关键的“权重”K。这个公式看着复杂，但其核心思想是比较“预测的不确定性(P_hat)”和“测量的不确定性(R)”。H是观测矩阵，负责把系统状态（角度和角速度）映射到我们能观测到的量（可能只有角度）。R是测量噪声协方差矩阵，代表传感器有多“吵”。
   • MPU6050例子：R就是加速度计测量角度的噪声方差。如果加速度计此刻非常稳定（R小），而我们的模型预测因为运动不太准（P_hat大），那么计算出的K就会让滤波器更相信测量值。反之，如果传感器受到剧烈震动干扰（R变大），K就会变小，让滤波器更相信自己的预测。H矩阵通常是[1, 0]，因为我们用加速度计只直接测量角度，不直接测角速度。

4. 状态更新公式：X = X_hat + K * (Z - H * X_hat)

   • 它在干什么：这就是最终的“数据融合”！X是融合后的最优估计（后验估计）。Z是传感器当前的测量值（加速度计算出的角度）。(Z - H * X_hat)被称为新息，是测量值和预测值之间的差异。我们用卡尔曼增益K作为权重，将这个差异值补偿到预测值X_hat上，得到更优的结果。
   • MPU6050例子：这就是整个算法的灵魂。陀螺仪积分预测了一个角度X_hat，加速度计测量了一个角度Z。两者一减，得到偏差。然后用K决定这个偏差有多少需要被采纳，去修正预测值。最终输出的X，就是那个既平滑（滤除了加速度计抖动）又准确（修正了陀螺仪漂移）的完美角度。

5. 误差协方差更新公式：P = (I - K * H) * P_hat

   • 它在干什么：更新我们对这次最优估计的“自信程度”。因为我们已经用测量值进行了修正，所以估计的不确定性P应该比预测的不确定性P_hat要减小。I是单位矩阵。
   • MPU6050例子：这一步为下一次迭代做准备。更新后的P会在下一个周期代入公式2，如此循环往复，实现动态的实时滤波。

这五个公式，构成了一个完整的“预测-修正”循环。在Arduino的loop()函数里，我们每次读取传感器数据，就执行一遍这一套流程，输出融合后的最优角度。理解了这个流程，代码就不再是黑盒了。

4. 关键一步：如何测量噪声矩阵Q和R

很多教程讲到卡尔曼滤波，都会轻描淡写地说一句：“Q和R矩阵需要根据经验设定或测量得到。”然后就没下文了。这恰恰是新手从理论迈向实践的最大门槛。今天，我们就用MPU6050，实实在在地把这两个矩阵“测”出来。这是让你的卡尔曼滤波器从“能跑”到“跑得好”的关键。

测量噪声协方差矩阵 RR代表了传感器的测量噪声有多大。对于MPU6050，我们通常只用加速度计来直接计算角度，所以R在这里可以简化为一个数值，即角度测量值的方差。怎么测？很简单，把MPU6050绝对静止地放在桌面上，采集几百个由加速度计计算出来的角度值。理论上，真实角度不变，所有的波动都来自传感器噪声。计算这组数据的方差，就得到了R。

下面是一段实测代码的核心思路：

// 假设采集N个静止状态下的角度样本 float angle_samples[N]; float angle_sum = 0; // 1. 采集数据并求均值 for(int i=0; i<N; i++){ mympu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); // 使用atan2计算倾角angle（以Y轴为例） accy = ay / AcceRatio; accz = az / AcceRatio; angle_samples[i] = atan2(accy, accz) * 180 / PI; angle_sum += angle_samples[i]; delay(10); // 适当延时 } float angle_mean = angle_sum / N; // 2. 计算方差R float R = 0; for(int i=0; i<N; i++){ R += (angle_samples[i] - angle_mean) * (angle_samples[i] - angle_mean); } R = R / N; Serial.print("Measurement Noise Variance R = "); Serial.println(R);

把模块静止放置，运行这段代码，串口就会输出一个方差值。这个值就是你的R。我实测过一些模块，这个值通常在0.1到1.0（度^2）之间。数值越小，说明你的加速度计在静止时越稳定。

过程噪声协方差矩阵 QQ代表了我们的物理模型不准确的程度。在我们的模型里，我们假设角速度在短时间内是恒定的。但实际上，角速度可能变化，这个变化就是模型误差，也就是“过程噪声”。严格测量Q比较复杂，因为它和系统动态特性有关。一个非常实用且有效的工程方法是：把它当作一个可调节的参数。

Q矩阵通常是一个2x2的矩阵（因为我们状态是[角度; 角速度]）。我们可以先将其设为一个对角矩阵，例如Q = [[Q_angle, 0], [0, Q_gyro]]。Q_angle代表了我们对角度预测的信任程度，Q_gyro代表了对角速度预测的信任程度。

一个经典的起始设置方法是：

• Q_angle：设为一个非常小的值，比如0.001。这表示我们相信角速度积分预测角度的模型是相当准确的。
• Q_gyro：这个值相对更重要。它反映了角速度的变化率。你可以通过观察陀螺仪静止时的输出波动来估算。采集静止时的角速度数据，计算其方差，这个值可以作为Q_gyro的参考起点。通常比R小一个数量级左右。

在实际调试中，你可以遵循这个原则：如果融合后的角度输出反应迟钝，跟不上实际运动，就适当增大Q（让滤波器更相信新的测量值）；如果输出对震动过于敏感，噪声大，就适当减小Q（让滤波器更相信自己的预测）。Q和R的比值决定了滤波器的“性格”。Q/R大，滤波器响应快但噪声大；Q/R小，滤波器平滑但延迟大。你需要根据实际应用（是竞速无人机还是平稳摄影云台）来找到最佳平衡点。

5. 手把手代码实现：Arduino上的卡尔曼滤波器

理论懂了，参数会测了，现在我们把所有东西组装起来，写一个完整的、可运行的Arduino卡尔曼滤波程序。我会逐段解释，你完全可以复制粘贴，然后根据自己的MPU6050模块稍作调整。

首先，是初始化和变量定义。我们这里实现一个一维（单轴）的卡尔曼滤波器，状态量是角度和角速度。

#include <Wire.h> #include <MPU6050.h> // 使用你安装的MPU6050库 MPU6050 mpu; // 卡尔曼滤波变量 float Q_angle = 0.001; // 过程噪声协方差：角度 float Q_gyro = 0.003; // 过程噪声协方差：角速度 float R_angle = 0.5; // 测量噪声协方差：加速度计测角方差，需实测调整 float x_angle = 0; // 状态向量：角度 float x_gyro = 0; // 状态向量：角速度 float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0; // 误差协方差矩阵P float dt = 0.005; // 采样时间间隔，单位秒（对应200Hz） float K_0, K_1; // 卡尔曼增益（二维向量，此处拆成两个标量） // MPU6050相关 float AccelRatio = 16384.0; // 加速度计量程为±2g时的比例因子 float GyroRatio = 131.0; // 陀螺仪量程为±250°/s时的比例因子 float accAngle, gyroRate; // 加速度计角度，陀螺仪角速度 unsigned long lastTime = 0;

接下来是setup()函数，初始化I2C通讯、MPU6050传感器，并进行简单的校准（计算零点偏移）。

void setup() { Serial.begin(115200); Wire.begin(); mpu.initialize(); // 简单校准：采集一段时间数据求平均作为零点偏移 long accXSum = 0, accYSum = 0, accZSum = 0; long gyroXSum = 0, gyroYSum = 0, gyroZSum = 0; int calibCount = 500; for (int i = 0; i < calibCount; i++) { mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); accXSum += ax; accYSum += ay; accZSum += az; gyroXSum += gx; gyroYSum += gy; gyroZSum += gz; delay(2); } // 注意：这里简化处理，实际库函数可能自带校准或需更复杂处理 // 本例旨在说明流程，高级校准可参考库文档 }

核心中的核心，是卡尔曼滤波的预测和更新函数。我们把它写成一个函数kalmanUpdate(float newAngle, float newRate)，其中newAngle是加速度计测量的新角度，newRate是陀螺仪测量的新角速度。

float kalmanUpdate(float newAngle, float newRate) { // --- 第一步：预测 --- // 1. 根据角速度更新角度预测：x_angle = x_angle + dt * x_gyro x_angle += dt * x_gyro; // 2. 角速度预测：假设短时间内角速度不变，所以x_gyro = newRate (这里在状态更新步骤处理) // 3. 更新误差协方差矩阵P的先验估计: P = P + Q P_00 += dt * (dt*P_11 - P_01 - P_10 + Q_angle); P_01 -= dt * P_11; P_10 -= dt * P_11; P_11 += Q_gyro * dt; // --- 第二步：更新 --- // 4. 计算新息 (测量值与预测值的差) float y = newAngle - x_angle; // 5. 计算新息协方差 S = H*P*H^T + R, 此处H=[1,0]，所以S = P_00 + R float S = P_00 + R_angle; // 6. 计算卡尔曼增益 K = P * H^T / S K_0 = P_00 / S; K_1 = P_10 / S; // 7. 更新状态估计 (后验估计) x_angle += K_0 * y; x_gyro += K_1 * y; // 用角度偏差修正角速度估计！ // 8. 更新误差协方差 P = (I - K*H) * P float P_00_temp = P_00; float P_01_temp = P_01; P_00 -= K_0 * P_00_temp; P_01 -= K_0 * P_01_temp; P_10 -= K_1 * P_00_temp; P_11 -= K_1 * P_01_temp; return x_angle; // 返回最优估计角度 }

最后，在主循环loop()中，我们以固定的时间间隔读取传感器数据，调用这个卡尔曼函数，并输出结果。

void loop() { unsigned long now = millis(); dt = (now - lastTime) / 1000.0; // 计算实际时间差 if (dt > 0.02) dt = 0.02; // 限制最大dt，防止异常值 lastTime = now; // 读取MPU6050原始数据 int16_t ax, ay, az, gx, gy, gz; mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); // 计算加速度计角度 (以Y轴为例，计算俯仰角) float accY = ay / AccelRatio; float accZ = az / AccelRatio; accAngle = atan2(accY, accZ) * 180 / PI; // 使用atan2更安全 // 计算陀螺仪角速度 (以Y轴对应的角速度X为例，注意轴对应关系) gyroRate = gx / GyroRatio; // 单位：度/秒 // 调用卡尔曼滤波，输入测量角度和角速度 float kalmanAngle = kalmanUpdate(accAngle, gyroRate); // 输出结果到串口绘图器，方便观察 Serial.print(accAngle); Serial.print(","); // 原始加速度计角度，噪声大 Serial.print(x_gyro * dt); Serial.print(","); // 纯陀螺仪积分角度，会漂移 Serial.println(kalmanAngle); // 卡尔曼融合后的角度，平滑且准确 // 控制循环频率，使dt接近设定值 delay(5); }

把这段代码上传到你的Arduino，打开串口绘图器（工具->串口绘图器），你会看到三条曲线。快速晃动模块，你会发现黄色的原始加速度计角度曲线抖动剧烈，蓝色的陀螺仪积分角度曲线相对平滑但慢慢漂移，而红色的卡尔曼滤波输出曲线，既能快速跟踪真实角度变化，又保持了极好的平滑性，几乎不受瞬时抖动影响。这一刻，你会真切感受到数学和算法带来的美妙。

6. 调试技巧与常见问题排坑

代码跑起来了，但效果可能不如预期？别急，调参和排错是嵌入式开发的常态。根据我多年的踩坑经验，下面这些技巧和常见问题能帮你快速定位。

效果不理想？先调整这两个关键参数！

1. R_angle（测量噪声方差）：如果你发现卡尔曼输出曲线仍然包含很多高频抖动，说明滤波器过于相信加速度计了。尝试增大R_angle。这相当于告诉滤波器：“加速度计数据很吵，别太信它。” 效果是输出会更平滑，但可能会引入一点延迟。
2. Q_gyro（过程噪声方差-角速度）：如果你发现卡尔曼输出反应迟钝，跟不上你快速转动模块的速度，或者无法修正陀螺仪的漂移，说明滤波器过于相信自己的预测模型了。尝试增大Q_gyro。这相当于告诉滤波器：“我们的角速度恒定模型不太准，世界变化很快，多看看传感器数据。” 效果是响应会变快，但可能对噪声更敏感。

调试黄金法则：隔离测试

• 先看原始数据：在加入卡尔曼滤波之前，确保你能正确读取并转换加速度计和陀螺仪的原始数据。分别绘制出加速度计计算的角度和陀螺仪积分的角度，确认它们各自的行为符合预期（一个噪声大，一个会漂移）。
• 再调卡尔曼：在原始数据正确的基础上，再启用卡尔曼滤波。通过串口绘图器同时输出原始角度和滤波后角度，直观地对比调整Q和R的效果。
• 固定时间间隔dt是关键：卡尔曼滤波理论假设采样时间是恒定的。务必确保你的loop循环周期稳定。使用millis()计算真实的时间差dt并代入公式，或者使用定时器中断来严格定时采样，这是保证滤波器性能稳定的基础。我见过太多问题都是因为dt不稳定导致的。

常见问题与解决方案

• 问题：角度输出总是向一个方向缓慢漂移。
  • 检查：陀螺仪的零点偏移（零偏）是否校准。即使进行了初始校准，温度变化也可能引起零漂。可以在loop中加入简单的动态零偏估计（例如，在检测到系统静止时，缓慢更新零偏值）。
• 问题：快速运动时，卡尔曼角度出现很大跳跃或滞后。
  • 检查：dt是否准确且稳定。循环中是否有其他耗时操作（如Serial.print）影响了采样率？尝试提高采样率（减小delay），或使用更快的MCU（如ESP32）。
  • 检查：Q_gyro是否太小？尝试增大它。
• 问题：模块静止时，卡尔曼输出仍有微小波动。
  • 检查：R_angle是否足够大？尝试增大它。另外，检查加速度计角度计算是否使用了atan2函数，它比atan更能处理全角度范围，避免在±90度附近出现跳变。
• 问题：初始化时角度跳变。
  • 解决：在setup中，不要用第一次读取的加速度计角度作为卡尔曼滤波的初始状态x_angle。应该连续读取多次取平均，或者让滤波器在开始的几百毫秒内以一个较大的R_angle运行，使其快速收敛到真实角度。

记住，调参没有银弹。最好的方法就是一边动手实验，一边观察曲线，理解每个参数改变对滤波器“性格”的影响。这个过程本身，就是对卡尔曼滤波思想最深刻的学习。

7. 进阶与扩展：从一维到多维，从角度到姿态

恭喜你，已经成功实现了一个一维的卡尔曼滤波器，并能稳定地输出一个轴上的融合后角度。这对于平衡小车（只需要俯仰角）或者简单的倾斜检测项目来说，已经足够了。但如果你想做四轴飞行器、全向机器人或者复杂的姿态跟踪，就需要处理三维空间中的姿态，也就是同时得到俯仰角（Pitch）、横滚角（Roll）和偏航角（Yaw）。这就进入了多维卡尔曼滤波，或者更常用的互补滤波的领域。

为什么需要多维？在三维空间中，三个角度的变化是耦合的。绕X轴的旋转（横滚）会影响Y轴和Z轴加速度计的读数。此时，用一个一维卡尔曼滤波器独立处理每个轴，效果会变差。我们需要建立包含三个角度和三个角速度的6维状态向量，并使用更复杂的状态转移矩阵A和观测矩阵H。观测值也不再是简单的atan2计算，而是需要从加速度计和磁力计（如果有）的读数中，通过更复杂的算法（如重力场向量匹配）解算出的姿态。

从卡尔曼到扩展卡尔曼（EKF）当系统模型（状态转移方程）或观测模型不是简单的线性关系时，标准的卡尔曼滤波就失效了。在姿态估计中，用四元数表示姿态时，运动方程是非线性的。这时就需要扩展卡尔曼滤波（EKF）。EKF的核心思想是在当前估计点附近，对非线性模型进行一阶泰勒展开，将其线性化，然后再套用标准卡尔曼滤波的公式。实现EKF难度陡增，但很多开源飞控（如ArduPilot, Betaflight）的核心姿态算法正是基于EKF。

给新手的实用建议：先掌握互补滤波在进军多维EKF之前，我强烈建议你先实践互补滤波。它可以看作是卡尔曼滤波在特定条件下的一个简化特例，思想同样直观：将高频特性好的传感器（陀螺仪）和低频特性好的传感器（加速度计）通过一个高通滤波器和一个低通滤波器结合起来。代码实现比卡尔曼滤波简单很多，效果对于大多数应用来说已经非常出色。

一个经典的一阶互补滤波角度更新公式如下：angle = (0.98) * (angle + gyroRate * dt) + (0.02) * accAngle其中，0.98和0.02是滤波系数，它们的和必须为1。这个公式是不是很像我们之前说的“加权平均”？它本质上就是一个固定增益的、极其简化的卡尔曼滤波器。你可以从这个公式入手，理解传感器融合的本质，然后再回过头来看卡尔曼滤波的动态增益调整，会有更深的体会。

无论是卡尔曼还是互补滤波，最终目的都是让机器更好地感知世界。当你看到自己制作的小车稳稳立住，或者无人机平稳飞行时，你就会明白，这些看似复杂的公式和代码，最终都化为了优雅而稳定的物理运动。这，就是嵌入式开发的魅力所在。