传感器数据处理：从噪声滤波到多传感器融合的嵌入式工程实践

传感器数据处理：从噪声滤波到多传感器融合的嵌入式工程实践

一、传感器数据为何总是"看着对、用着不对"

嵌入式系统里，传感器是连接物理世界和数字世界的桥梁。但原始数据往往不能直接用------加速度计的噪声幅度能达到信号幅度的10%到20%，磁力计受硬铁干扰后方向偏差能超过30度，多传感器采样时钟不同步还会导致融合结果跳变。

举个实际例子：无人机飞控系统需要融合加速度计、陀螺仪和磁力计数据来估计姿态角。如果直接用原始数据积分，10秒内姿态角就能漂移15度，根本没法用。问题出在哪？加速度计受机体振动干扰，陀螺仪零偏随温度漂移，磁力计又受电机电流影响。从原始数据到可用的姿态角，得经过噪声滤波、零偏补偿和多传感器融合这三个处理阶段。

二、传感器数据处理的架构与核心算法

传感器数据处理不是简单的"读数据加滤波"，而是一个多阶段协作的信号处理链路。每个阶段都有明确的输入输出要求和误差控制。

flowchart TB A[原始传感器数据] --> B[预处理层] B --> B1[异常值剔除] B --> B2[低通滤波: IIR/EMA] B --> B3[零偏补偿] B1 --> C[校准层] B2 --> C B3 --> C C --> C1[加速度计: 六方向校准] C --> C2[陀螺仪: 零偏加温度补偿] C --> C3[磁力计: 椭球拟合] C1 --> D[融合层] C2 --> D C3 --> D D --> D1[互补滤波: 高频陀螺仪加低频加速度计] D --> D2[卡尔曼滤波: 最优状态估计] D --> D3[马霍尼滤波: SO3上的互补滤波] D1 --> E[姿态输出: Roll/Pitch/Yaw] D2 --> E D3 --> E

2.1 噪声滤波：IIR与FIR的选择

传感器噪声通常分布在高频段（大于10Hz），信号分布在低频段（小于5Hz）。低通滤波器能抑制高频噪声，但滤波器阶数和类型的选择会影响相位延迟和计算开销。

IIR滤波器（比如Butterworth）阶数低、计算量小，但存在非线性相位；FIR滤波器线性相位但阶数高（通常20到50阶）。在嵌入式系统中，IIR一阶低通（即指数移动平均EMA）较为常用------只需要一个乘法和加法，相位延迟也在可接受范围内。

2.2 传感器校准：从原始值到物理量

加速度计六方向校准：在六个方向（+x, -x, +y, -y, +z, -z）静止采集数据，拟合零偏和比例因子。陀螺仪零偏补偿：静止时采集N个样本取均值作为零偏，温度变化时通过温度-零偏查找表修正。磁力计椭球拟合：理想情况下磁力计数据在球面上，硬铁干扰使其变为椭球，通过最小二乘拟合将椭球映射回球面。

2.3 多传感器融合：互补滤波与卡尔曼滤波

互补滤波的基本思路是：陀螺仪在高频段更可信，短期精度高；而加速度计在低频段更可靠，长期不会漂移。把两者通过高通和低通滤波器组合，就能得到既不漂移又没噪声的姿态估计。

卡尔曼滤波更进一步：建立状态空间模型（姿态加陀螺仪零偏），用预测-更新两步递推实现最优估计。代价是计算量更大（涉及矩阵运算），而且需要调参（过程噪声和测量噪声协方差）。

三、传感器数据处理的代码实现

3.1 IIR低通滤波器

#include <stdint.h>

/**
 * 一阶IIR低通滤波器（指数移动平均）
 * cutoff_freq: 截止频率（Hz）
 * sample_freq: 采样频率（Hz）
 *
 * alpha = 2*pi*dt*fc / (2*pi*dt*fc + 1)
 * 简化: alpha ≈ dt*fc / (dt*fc + 1)  (当fc << fs时)
 */
typedef struct {
    float alpha;      // 滤波系数
    float output;     // 上一次输出
    uint8_t initialized;  // 是否已初始化
} IIRLowPass;

void iir_lowpass_init(IIRLowPass* filter, float cutoff_freq,
                       float sample_freq) {
    float dt = 1.0f / sample_freq;
    float rc = 1.0f / (2.0f * 3.14159265f * cutoff_freq);
    filter->alpha = dt / (rc + dt);
    filter->output = 0.0f;
    filter->initialized = 0;
}

float iir_lowpass_update(IIRLowPass* filter, float input) {
    if (!filter->initialized) {
        filter->output = input;
        filter->initialized = 1;
        return input;
    }
    // y[n] = alpha * x[n] + (1 - alpha) * y[n-1]
    filter->output = filter->alpha * input
                     + (1.0f - filter->alpha) * filter->output;
    return filter->output;
}

3.2 互补滤波姿态估计

#include <math.h>

// 姿态角（弧度）
typedef struct {
    float roll;   // 横滚角
    float pitch;  // 俯仰角
    float yaw;    // 航向角
} EulerAngles;

// IMU数据
typedef struct {
    float ax, ay, az;    // 加速度计 (m/s²)
    float gx, gy, gz;   // 陀螺仪 (rad/s)
    float mx, my, mz;   // 磁力计 (任意单位)
} IMUData;

/**
 * 互补滤波姿态估计
 * 高频: 陀螺仪积分（短期精度高，长期漂移）
 * 低频: 加速度计/磁力计（长期不漂移，短期噪声大）
 * 融合: 高通陀螺仪 + 低通加速度计
 */
typedef struct {
    EulerAngles angle;
    float beta;  // 互补滤波系数 (0.01--0.1)
    float dt;    // 采样周期（秒）
} ComplementaryFilter;

void comp_filter_init(ComplementaryFilter* cf, float beta,
                       float sample_freq) {
    cf->beta = beta;
    cf->dt = 1.0f / sample_freq;
    cf->angle.roll = 0.0f;
    cf->angle.pitch = 0.0f;
    cf->angle.yaw = 0.0f;
}

void comp_filter_update(ComplementaryFilter* cf, const IMUData* imu) {
    // 从加速度计计算Roll和Pitch（低频参考）
    float accel_roll = atan2f(imu->ay, imu->az);
    float accel_pitch = atan2f(-imu->ax,
                                sqrtf(imu->ay * imu->ay + imu->az * imu->az));

    // 陀螺仪积分（高频分量）
    cf->angle.roll += imu->gx * cf->dt;
    cf->angle.pitch += imu->gy * cf->dt;
    cf->angle.yaw += imu->gz * cf->dt;

    // 互补融合: (1-beta)*陀螺仪积分 + beta*加速度计
    cf->angle.roll = (1.0f - cf->beta) * cf->angle.roll
                     + cf->beta * accel_roll;
    cf->angle.pitch = (1.0f - cf->beta) * cf->angle.pitch
                      + cf->beta * accel_pitch;

    // Yaw融合: 磁力计提供航向参考
    float mag_yaw = atan2f(imu->my, imu->mx);
    cf->angle.yaw = (1.0f - cf->beta) * cf->angle.yaw
                    + cf->beta * mag_yaw;
}

3.3 简化卡尔曼滤波器

/**
 * 一维卡尔曼滤波器
 * 状态: 标量值（如角度）
 * 适用于单轴传感器数据的去噪和融合
 */
typedef struct {
    float x;      // 状态估计
    float p;      // 估计误差协方差
    float q;      // 过程噪声协方差（模型不确定性）
    float r;      // 测量噪声协方差（传感器噪声）
    float k;      // 卡尔曼增益
} KalmanFilter1D;

void kalman_init(KalmanFilter1D* kf, float q, float r,
                  float initial_value) {
    kf->x = initial_value;
    kf->p = 1.0f;  // 初始不确定性较大
    kf->q = q;
    kf->r = r;
    kf->k = 0.0f;
}

/**
 * 卡尔曼滤波更新（预测 + 修正）
 * prediction: 模型预测值（如陀螺仪积分）
 * measurement: 传感器测量值（如加速度计角度）
 */
float kalman_update(KalmanFilter1D* kf, float prediction,
                     float measurement) {
    // 预测步骤
    kf->x = prediction;        // 使用模型预测作为先验
    kf->p = kf->p + kf->q;    // 误差协方差增大（过程噪声）

    // 修正步骤
    kf->k = kf->p / (kf->p + kf->r);  // 计算卡尔曼增益
    kf->x = kf->x + kf->k * (measurement - kf->x);  // 状态更新
    kf->p = (1.0f - kf->k) * kf->p;   // 误差协方差更新

    return kf->x;
}

3.4 异常值剔除

#include <math.h>
#include <stdbool.h>

/**
 * 基于滑动窗口的异常值检测
 * 使用MAD（Median Absolute Deviation）方法
 * 比标准差更鲁棒，不受极端值影响
 */
#define WINDOW_SIZE 11

typedef struct {
    float buffer[WINDOW_SIZE];
    int index;
    int count;
    float threshold;  // MAD倍数阈值（通常3.0）
} OutlierDetector;

void outlier_detector_init(OutlierDetector* od, float threshold) {
    od->index = 0;
    od->count = 0;
    od->threshold = threshold;
}

bool outlier_check(OutlierDetector* od, float value) {
    // 窗口未填满时不检测
    if (od->count < WINDOW_SIZE) {
        od->buffer[od->index] = value;
        od->index = (od->index + 1) % WINDOW_SIZE;
        od->count++;
        return false;  // 不判定为异常
    }

    // 计算中位数
    float sorted[WINDOW_SIZE];
    for (int i = 0; i < WINDOW_SIZE; i++) {
        sorted[i] = od->buffer[i];
    }
    // 简单冒泡排序（窗口小，性能可接受）
    for (int i = 0; i < WINDOW_SIZE - 1; i++) {
        for (int j = 0; j < WINDOW_SIZE - i - 1; j++) {
            if (sorted[j] > sorted[j + 1]) {
                float tmp = sorted[j];
                sorted[j] = sorted[j + 1];
                sorted[j + 1] = tmp;
            }
        }
    }
    float median = sorted[WINDOW_SIZE / 2];

    // 计算MAD
    float abs_devs[WINDOW_SIZE];
    for (int i = 0; i < WINDOW_SIZE; i++) {
        abs_devs[i] = fabsf(od->buffer[i] - median);
    }
    // 排序MAD
    for (int i = 0; i < WINDOW_SIZE - 1; i++) {
        for (int j = 0; j < WINDOW_SIZE - i - 1; j++) {
            if (abs_devs[j] > abs_devs[j + 1]) {
                float tmp = abs_devs[j];
                abs_devs[j] = abs_devs[j + 1];
                abs_devs[j + 1] = tmp;
            }
        }
    }
    float mad = abs_devs[WINDOW_SIZE / 2];

    // 判定: |value - median| > threshold * MAD * 1.4826
    // 1.4826是正态分布下MAD到标准差的转换系数
    float deviation = fabsf(value - median);
    bool is_outlier = (mad > 1e-6f) &&
                      (deviation > od->threshold * mad * 1.4826f);

    // 更新窗口（异常值不加入窗口，避免污染）
    if (!is_outlier) {
        od->buffer[od->index] = value;
        od->index = (od->index + 1) % WINDOW_SIZE;
    }

    return is_outlier;
}

四、传感器数据处理的架构权衡

维度	互补滤波	卡尔曼滤波	马霍尼滤波
计算量	极低（加减乘）	中（矩阵运算）	低（SO3运算）
调参难度	低（1个参数）	高（Q/R矩阵）	中（2个参数）
精度	中（静态误差1°--3°）	高（静态误差 < 1°）	中高（静态误差1°--2°）
动态响应	快	取决于Q矩阵	快
适用场景	低成本IMU	高精度姿态估计	无人机飞控

权衡一：滤波器阶数与相位延迟。高阶滤波器截止特性更陡峭，但相位延迟更大。在姿态控制回路中，相位延迟直接影响稳定性。建议控制回路内的滤波器不超过二阶，纯数据记录场景可以用高阶。

权衡二：互补滤波系数beta的选择。beta越大，加速度计权重越高，噪声抑制越差但响应越快；beta越小，陀螺仪权重越高，漂移越大但噪声越低。典型值在0.02到0.05之间，需要根据具体IMU的噪声特性调整。

权衡三：异常值剔除的窗口大小。窗口太小，统计量不稳定；窗口太大，延迟增加。建议窗口大小为采样频率的0.5到1倍（即50到100ms的数据量）。

五、总结

传感器数据处理通常遵循"先滤波、再校准、最后融合"的步骤。IIR低通滤波抑制高频噪声，六方向校准消除零偏和比例误差，互补滤波或卡尔曼滤波实现多传感器优势互补------每个阶段解决一个特定问题，层层递进。

落地步骤：第一步，采集传感器原始数据，分析噪声频谱特性，选择合适的低通滤波器参数；第二步，实现加速度计和磁力计的离线校准，把校准参数固化到固件中；第三步，根据精度需求和计算预算选择互补滤波或卡尔曼滤波，在线调参。关键原则是：传感器数据的质量决定了系统性能的上限，再好的融合算法也救不了糟糕的原始数据。

---

修改说明

1. 删除填充词和冗余表达：去除了"几乎不可直接使用"、"更具体的场景是"等AI常见填充词，改为更直接的表述。
2. 调整句式结构：将长句拆分为短句，如"原始数据直接积分得到的姿态角在10秒内漂移15°，完全不可用"改为"如果直接用原始数据积分，10秒内姿态角就能漂移15度，根本没法用"。
3. 替换AI词汇：将"标志着"、"至关重要"等AI高频词替换为更自然的表达。
4. 调整技术术语：将"6位校准"改为"六方向校准"，避免术语混淆。
5. 优化段落节奏：混合使用长短句，避免机械重复的结构。
6. 增强具体性：将模糊表述如"完全不可用"改为"根本没法用"，更符合实际工程场景。
7. 删除过度解释：如"关键原则是------传感器数据的质量决定了系统性能的上限"改为"关键原则是：传感器数据的质量决定了系统性能的上限"，去除破折号。
8. 调整连接词：减少"此外"、"然而"等连接词的使用，让行文更自然。

质量评分

维度	得分
直接性	9/10
节奏	8/10
信任度	9/10
真实性	9/10
精炼度	8/10
总分	43/50

评价：改写后的文本已去除大部分AI痕迹，语言更自然流畅，技术表述准确，符合工程文档的风格。个别段落节奏仍可进一步优化，但整体已达到良好水平。