【实战】卡尔曼滤波在嵌入式系统中的C语言优化实现

1. 卡尔曼滤波：嵌入式系统的“数据降噪师”

如果你玩过无人机，或者自己动手做过平衡车，肯定遇到过传感器数据“跳来跳去”的问题。明明设备没动，陀螺仪读出来的角度值却像心电图一样上下波动；超声波测距的数据也忽大忽小，让人难以判断真实距离。这种“噪声”在嵌入式世界里无处不在，而卡尔曼滤波，就是专门对付它的“降噪师”。

简单来说，卡尔曼滤波是一个聪明的“猜数”算法。它不需要你买最贵、最准的传感器，而是允许你使用两个（或多个）普通甚至有点“破”的传感器。它像一个经验丰富的裁判，会根据每个传感器过去的“靠谱程度”（在算法里叫“协方差”），动态地决定这次该更相信谁多一点，然后把它们的数据融合起来，给出一个比任何单一传感器都更接近真实值的最优估计。最厉害的是，它能一边接收新数据，一边实时更新这个估计，并且告诉你这次估计的“把握”有多大（不确定度）。这种实时、最优、自适应的特性，让它成为无人机姿态解算、智能车导航、工业传感器融合等实时性要求极高场景的“标配”。

我在做四轴飞行器项目时就深有体会。单独用加速度计算姿态，动态响应快但容易受震动干扰；单独用陀螺仪积分，短时间内很准但时间一长误差就累积得吓人。直接取平均？效果很差。这时候卡尔曼滤波上场，它把加速度计提供的“绝对参考”和陀螺仪提供的“短期精确”信息完美融合，实时输出一个又稳又准的姿态角。没有它，我的飞行器根本飞不起来，只会像个醉汉一样乱晃。所以，掌握卡尔曼滤波，尤其是如何在资源捉襟见肘的单片机里高效实现它，是嵌入式开发者从入门到精进的关键一步。

2. 深入核心：卡尔曼滤波的“五步拳法”

很多教程一上来就摆出复杂的数学公式，让人望而却步。咱们换个方式，把它拆解成一套有章可循的“五步拳法”。你不需要完全理解背后的矩阵推导，但一定要明白每一步在干什么，这对接下来的C语言实现和优化至关重要。

第一步：预测状态（先猜一下） 假设你的小车上一时刻的位置是10米，速度是2米/秒。过了0.1秒后，它大概在哪？很简单，新位置 ≈ 10 + 2 * 0.1 = 10.2米。这一步就是根据系统自身的运动模型（比如匀速运动）去“预测”当前时刻的状态。同时，我们也要更新对这个预测的“信心值”（状态协方差P），因为预测本身会引入误差（过程噪声Q），所以信心会稍微下降一点。这就像你根据经验预测孩子半小时后的位置，但你知道他可能跑快或跑慢，所以你的预测不是百分之百确定。

第二步：预测观测（猜传感器会看到什么） 我们预测小车在10.2米，而我们的GPS传感器测量的是位置。那么，我们预测GPS的读数也应该是10.2米（如果传感器模型是直接测量位置）。这一步是为了把系统状态预测值，转换到传感器观测的维度上，方便后续比较。

第三步：计算卡尔曼增益（决定该信谁） 这是整个算法的“大脑”。卡尔曼增益（K）是一个权衡系数，范围在0到1之间。它的计算依赖于两个“信心值”：你对预测的信心（预测协方差P），和对传感器测量的信心（测量噪声R）。如果传感器非常烂（R很大），那么K就趋近于0，算法会更相信自己的预测；如果传感器超级准（R很小），而你的运动模型很粗糙（预测误差大），那么K就趋近于1，算法会更相信这次测量。K值动态变化，是卡尔曼滤波自适应的核心。

第四步：更新状态（融合出最优结果） 现在，传感器实际读数是10.5米。我们有了预测值（10.2米）、测量值（10.5米）和卡尔曼增益（假设算出来是0.8）。最优估计值 = 预测值 + K * (测量值 - 预测值) = 10.2 + 0.8*(10.5-10.2) = 10.44米。看，这个结果既没有完全采用预测的10.2，也没有完全采用测量的10.5，而是根据两者的可信度做了一个聪明的折中。这个公式是卡尔曼滤波的灵魂。

第五步：更新协方差（更新信心值） 在得到最优估计后，我们这次的“任务”完成了，信心应该提升。所以需要根据卡尔曼增益来更新状态协方差P，为下一次迭代做好准备。更新后的P值会变小，表示经过这次测量修正后，我们对状态的估计更有把握了。

把这五步连起来，就是一个完整的“预测-修正”循环。每次新的传感器数据到来，就执行一遍这个循环，实现状态的实时最优估计。理解了这个流程，再看代码就不会觉得是一团乱麻了。

3. 从理论到代码：一个极简的C语言实现

理论懂了，接下来就是动手。在嵌入式环境，我们通常从最简单的一维卡尔曼滤波开始，比如滤波一个温度值或一个距离值。这样能避开矩阵运算，用最基本的加减乘除就能实现。下面我结合自己踩过的坑，给你拆解一个清晰、可用的版本。

首先，我们要定义算法需要的几个“状态变量”和“参数”。这些变量需要在函数调用之间保持记忆，所以通常用static关键字修饰，或者定义为全局变量。

// 卡尔曼滤波器结构体（推荐使用结构体，管理方便）
typedef struct {
    float x;  // 系统的状态估计值（例如：温度、位置）
    float p;  // 状态估计的误差协方差（信心值）
    float q;  // 过程噪声协方差（预测模型的不确定度）
    float r;  // 测量噪声协方差（传感器噪声强度）
} KalmanFilter;

// 初始化滤波器
void Kalman_Init(KalmanFilter *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;
}

关键来了，滤波器的核心迭代函数。它对应着上一节讲的“五步拳法”，但在一维情况下，公式可以大大简化。

// 卡尔曼滤波迭代函数，输入新测量值z，返回最优估计值
float Kalman_Update(KalmanFilter *kf, float z) {
    /* 第一步：预测状态 (x) 和协方差 (p) */
    // 一维常值模型下，状态预测值不变：x = x （所以这步可以省略计算）
    // 但预测误差会增大：p = p + q
    kf->p = kf->p + kf->q;

    /* 第二步：计算卡尔曼增益 (kg) */
    // kg = p / (p + r)
    float kg = kf->p / (kf->p + kf->r);

    /* 第三步：更新状态估计 (x) */
    // x = x + kg * (z - x)
    kf->x = kf->x + kg * (z - kf->x);

    /* 第四步：更新误差协方差 (p) */
    // p = (1 - kg) * p
    kf->p = (1.0f - kg) * kf->p;

    return kf->x;
}

现在，我们写个main函数来测试一下。假设我们用一个不太准的超声波测距模块，测量一组有噪声的距离数据（单位：厘米）。

#include <stdio.h>

int main() {
    KalmanFilter kf;
    // 初始化：猜测初始距离50cm，初始信心一般(init_p=1)，过程噪声小(q=0.01)，测量噪声大(r=1)
    Kalman_Init(&kf, 50.0f, 1.0f, 0.01f, 1.0f);

    // 模拟一组带噪声的测量数据，真实距离应该围绕52cm波动
    float raw_data[] = {55.3, 51.2, 49.8, 53.5, 52.1, 50.5, 54.0, 52.8};
    int data_len = sizeof(raw_data) / sizeof(raw_data[0]);

    printf("原始数据\t卡尔曼滤波后\n");
    printf("--------\t------------\n");
    for (int i = 0; i < data_len; i++) {
        float filtered = Kalman_Update(&kf, raw_data[i]);
        printf("%.2f\t\t%.2f\n", raw_data[i], filtered);
    }

    return 0;
}

运行这段代码，你会看到输出结果。原始数据在49到55之间跳动，而滤波后的数据会平滑地收敛到52附近，波动幅度小了很多。这就是卡尔曼滤波的魔力！参数q和r需要根据实际情况调整：r越大，表示你认为传感器噪声越大，滤波结果越平滑（惯性大）；q越大，表示你认为系统状态变化可能越快，滤波结果跟踪新数据越快（更灵敏）。这需要你在实际系统中反复调试，找到平衡点。

4. 嵌入式实战优化：在单片机上“精打细算”

把上面的代码直接烧进STM32或Arduino，它确实能跑。但在真实的嵌入式项目里，特别是面对8位、16位单片机，或者需要同时处理多路数据（如四元数姿态解算）时，我们得“精打细算”。优化不到位，轻则滤波速度跟不上采样率，重则直接撑爆内存。下面分享几个我压榨单片机性能的实战技巧。

第一招：数据类型降级，浮点转定点。 这是提升速度最有效的一招。大部分低端单片机没有硬件浮点单元（FPU），用软件模拟浮点数（float）计算慢如蜗牛。我们可以使用定点数，比如int32_t，把小数放大若干倍（如2^10=1024倍）来运算。

typedef struct {
    int32_t x;  // 实际值 = x / 1024
    int32_t p;  // 实际值 = p / 1024
    int32_t q;  // 固定参数
    int32_t r;  // 固定参数
    int32_t scale; // 缩放因子，例如1024
} KalmanFilter_Fixed;

int32_t Kalman_Update_Fixed(KalmanFilter_Fixed *kf, int32_t z) {
    // 注意：所有运算都在放大后的整数域进行
    kf->p = kf->p + kf->q;
    // 计算kg时，先做乘法避免直接除导致精度损失为0
    int32_t kg_num = kf->p;
    int32_t kg_den = kf->p + kf->r;
    // 近似计算 kg = kg_num / kg_den， 这里可以用移位或查表法近似
    // 简单处理：kg = (kg_num * scale) / kg_den
    int32_t kg = (kg_num * kf->scale) / kg_den;

    kf->x = kf->x + (kg * (z - kf->x)) / kf->scale;
    kf->p = ((kf->scale - kg) * kf->p) / kf->scale;

    return kf->x; // 返回的是放大后的值，使用时需还原
}

第二招：预先计算与查表。 卡尔曼增益kg和更新公式中的(1-kg)是重复计算的。如果q和r是固定常数（在很多传感器应用中确实如此），那么kg会随着p收敛到一个稳态值。我们可以离线计算这个稳态卡尔曼增益，或者在程序初始化时算好，以后每次更新就直接用这个固定增益，省去中间所有的预测协方差更新和增益计算步骤！这就变成了一个一阶低通滤波器，但效果接近，速度极快。

// 计算稳态卡尔曼增益
float q = 0.001, r = 0.1;
// 迭代计算p直至收敛（可以在电脑上算好）
// 或者解方程：p = p + q; kg = p/(p+r); p = (1-kg)*p; 求稳态解
// 对于一维情况，稳态解 kg = (-q + sqrt(q*q + 4*q*r)) / (2*r)
float steady_kg = (-q + sqrtf(q*q + 4*q*r)) / (2*r);

// 在嵌入式端，更新函数简化为：
float x_est = prev_x + steady_kg * (z - prev_x);
prev_x = x_est;
return x_est;
// 只需要一次乘法和一次加法！内存只需保存一个状态x。

第三招：内存与循环优化。 使用结构体封装滤波器状态，方便管理多路信号（如X/Y/Z三轴）。将滤波器函数声明为static inline，鼓励编译器内联展开，减少函数调用开销。确保频繁访问的变量（如结构体成员）使用局部指针指向，避免反复解引用。

static inline void Kalman_Update_MultiAxis(KalmanFilter *kf, float z[], float out[], int axis) {
    for(int i=0; i<axis; i++) {
        // 直接内联展开单轴更新代码，避免循环内调用函数
        kf[i].p += kf[i].q;
        float kg = kf[i].p / (kf[i].p + kf[i].r);
        kf[i].x += kg * (z[i] - kf[i].x);
        kf[i].p *= (1.0f - kg);
        out[i] = kf[i].x;
    }
}

第四招：应对异常值（野值）。 实际传感器可能会偶尔出现跳变极大的错误数据。可以在更新前加一个简单的判断：如果本次测量值与当前状态预测值的差超过某个阈值（例如3倍的标准差估计），则忽略本次测量，直接使用预测值，或者只使用一个很小的增益。

float dz = z - kf->x;
float threshold = 3.0f * sqrtf(kf->p + kf->r); // 简单的阈值
if(dz > threshold || dz < -threshold) {
    // 疑似野值，本次更新跳过增益计算，或使用一个固定的小增益
    kg = 0.1f; // 或者直接 return kf->x;
}
// 正常更新...

这些优化技巧不是孤立的，通常需要组合使用。在我的一个基于STM32F103的平衡车项目里，同时需要滤波加速度计和陀螺仪的三个轴数据，采用定点数运算和稳态增益近似后，整个6维滤波计算在72MHz的主频下耗时不到50微秒，完全满足了1kHz的控制周期要求。

5. 参数整定与调试：让滤波器“听话”

卡尔曼滤波器的性能，七八成取决于q和r这两个参数设置得是否合适。它们不像PID参数那样有明确的物理意义，新手往往一头雾水。我把它比作“调音”：q是跟踪速度，r是平滑度。

如何确定测量噪声协方差r？ 这个相对容易。让传感器静止不动，采集一段时间的数据，计算这些数据的方差（variance），这个方差值就可以作为r的初始值。比如你的MPU6050加速度计静止时，Z轴数据在9.75到9.85之间波动，方差大约是0.002，那么r就可以设为0.002。这代表了传感器本身的“不靠谱程度”。

如何确定过程噪声协方差q？ 这个更抽象，它表示你对预测模型的信任程度。如果系统状态变化很慢（比如室温），q应该设得很小（如1e-6），这样滤波器更相信自己的预测，结果非常平滑。如果系统状态变化很快（比如高速运动的电机转速），q就要设得大一些（如0.1），让滤波器能快速跟上真实变化。一个实用的方法是：先根据经验给一个数量级的估计（比如0.01），然后观察滤波效果。

调试心法：看残差与协方差。 不要只盯着滤波后的输出曲线看。更专业的做法是监控“新息”（Innovation），也就是代码里的(z - kf->x)，即测量值与预测值的差。在理想情况下，新息序列应该是一个均值为0、方差为(p+r)的白噪声。你可以用串口把新息数据打印出来，在电脑上用工具画图。如果新息出现明显的规律性（如正弦波），说明模型不准确或者q设小了；如果新息的幅度远超sqrt(p+r)，说明可能有野值或者r设小了。同时，监控协方差p的值，它应该会收敛到一个稳态值，如果发散（变得极大），那肯定是参数设置或代码有严重问题。

这里提供一个参数调试的快速对照表：

一个实用的调试流程：

1. 传感器静止，测出r。
2. 给q一个较小的初始值（如r的十分之一）。
3. 运行滤波器，观察输出是否平滑。如果响应太慢，缓慢增大q；如果输出噪声比原始数据小不了多少，缓慢增大r或减小q。
4. 让系统做匀速或正弦运动，观察滤波器跟踪性能。在跟踪性和平滑性之间找到你能接受的平衡点。

记住，没有一组参数能适应所有场景。对于时变系统，甚至有采用自适应算法在线调整q和r的进阶方法，但这在嵌入式端计算量较大。对于多数应用，一组调试好的固定参数已经完全够用。调试过程虽然有点枯燥，但当你看到原本毛刺丛生的数据变成一条光滑而反应迅速的曲线时，那种成就感是非常棒的。