第4章 数据采集与预处理：传感器数据读取、滤波算法、时间戳对齐

好，咱们进入实战环节的第一个硬骨头——数据采集与预处理。

说实话，很多做算法的人容易忽略这一步。他们觉得算法模型才是核心，数据读进来直接用不就完了？我早年也这么想，结果在靶场测试时吃了大亏。数据没处理好，后面什么卡尔曼、什么组合导航，全是白搭。

这一章，咱们就聊聊传感器数据怎么读、怎么滤、怎么对齐。

4.1 传感器数据读取——别让接口成为瓶颈

弹载系统里，最常用的传感器就是IMU（惯性测量单元）和磁力计。IMU一般走SPI接口，磁力计常用I2C。嗯，这里要注意：SPI速度快，但线多；I2C线少，但容易受干扰。

我个人习惯，在弹载场景下优先用SPI。为什么？因为实时性要求高，I2C的时钟同步在强振动环境下偶尔会丢数据。我在某次项目中就遇到过，火箭发动机点火瞬间，I2C总线上的磁力计直接“死机”了，读数卡在上一帧。后来换成SPI，问题解决。

下面是一个典型的IMU数据读取代码片段。我用的是SPI DMA方式，这样CPU不用干等：

// IMU数据读取——SPI DMA方式
void imu_read_data(imu_data_t *data) {
    uint8_t tx_buf[12] = {0};
    uint8_t rx_buf[12] = {0};
    
    // 构造读取命令（以MPU9250为例）
    tx_buf[0] = 0x3B | 0x80;  // 加速度计起始地址，读操作
    // 启动DMA传输
    HAL_SPI_TransmitReceive_DMA(&hspi1, tx_buf, rx_buf, 12);
    // 等待传输完成（实际项目中用信号量或回调）
    while(spi_busy);
    
    // 解析数据（注意字节序）
    data->accel_x = (int16_t)((rx_buf[0] << 8) | rx_buf[1]);
    data->accel_y = (int16_t)((rx_buf[2] << 8) | rx_buf[3]);
    data->accel_z = (int16_t)((rx_buf[4] << 8) | rx_buf[5]);
    // ... 陀螺仪数据类似
}

4.2 滤波算法——均值滤波与中值滤波

传感器原始数据，说白了就是“脏数据”。弹载环境里，振动、温度变化、电磁干扰，哪个都能让数据跳得跟心电图似的。

你想想看，如果直接把这种数据送进导航算法，结果会怎样？发散，必须发散。

4.2.1 均值滤波——简单但有效

均值滤波的原理很简单：取N个采样点的平均值。它能有效抑制高频噪声，但对突发性干扰（比如一个尖峰）的抑制效果一般。

我曾经在测试中发现，均值滤波窗口大小选3~5最合适。窗口太小，滤波效果不明显；窗口太大，信号延迟严重。弹载场景下，延迟是致命的。

// 滑动均值滤波实现
#define FILTER_WINDOW_SIZE 5

typedef struct {
    float buffer[FILTER_WINDOW_SIZE];
    uint8_t index;
    float sum;
} moving_average_t;

float moving_average_filter(moving_average_t *ma, float new_value) {
    // 减去最旧的值
    ma->sum -= ma->buffer[ma->index];
    // 存入新值
    ma->buffer[ma->index] = new_value;
    ma->sum += new_value;
    // 移动索引
    ma->index = (ma->index + 1) % FILTER_WINDOW_SIZE;
    // 返回平均值
    return ma->sum / FILTER_WINDOW_SIZE;
}

4.2.2 中值滤波——专治“野值”

中值滤波就厉害了。它把N个采样值排序，取中间那个。对于脉冲噪声（比如电源波动导致的尖峰），中值滤波几乎是“一刀切”。

我记得有一次，某型导弹在振动台上测试，IMU的Z轴加速度计每隔几十个点就蹦出一个离谱的值。均值滤波根本压不住，换成中值滤波后，数据干净得像教科书一样。

// 中值滤波实现（N=5，快速排序法）
#define MEDIAN_WINDOW_SIZE 5

float median_filter(float *window) {
    // 复制数据（避免破坏原始顺序）
    float temp[MEDIAN_WINDOW_SIZE];
    for(int i = 0; i < MEDIAN_WINDOW_SIZE; i++) {
        temp[i] = window[i];
    }
    // 简单的冒泡排序（窗口小，够用）
    for(int i = 0; i < MEDIAN_WINDOW_SIZE - 1; i++) {
        for(int j = 0; j < MEDIAN_WINDOW_SIZE - 1 - i; j++) {
            if(temp[j] > temp[j+1]) {
                float t = temp[j];
                temp[j] = temp[j+1];
                temp[j+1] = t;
            }
        }
    }
    // 返回中间值
    return temp[MEDIAN_WINDOW_SIZE / 2];
}

4.3 时间戳对齐——多传感器融合的命门

好，数据读进来了，也滤干净了。但还有个要命的问题：时间戳对齐。

为什么？因为IMU、磁力计、气压计，它们的采样时刻不一样。IMU可能1kHz，磁力计100Hz，气压计50Hz。如果你不管时间差，直接把数据拼在一起，那融合出来的姿态就是“时空错乱”的。

说白了，时间戳对齐就是给每个数据打上精确的时间标签，然后统一插值到同一个时间点上。

4.3.1 硬件时间戳 vs 软件时间戳

我个人强烈建议用硬件时间戳。也就是在传感器数据就绪中断发生时，由定时器捕获单元直接记录当前时间。这样延迟最小，精度最高。

如果硬件不支持，那就用软件时间戳。在中断服务函数里读取系统滴答计时器（SysTick）的值。注意：中断响应延迟会导致时间戳偏大，但一般几十微秒的误差在弹载场景下可以接受。

// 软件时间戳实现
volatile uint32_t system_tick = 0;  // 1ms递增

void SysTick_Handler(void) {
    system_tick++;
}

// 在传感器中断中记录时间戳
void IMU_DataReady_Handler(void) {
    uint32_t timestamp = system_tick;  // 记录当前时间
    imu_data_t data;
    imu_read_data(&data);
    // 将数据和时间戳打包存入环形缓冲区
    imu_buffer_push(timestamp, data);
}

4.3.2 时间插值——把不同频率的数据对齐

假设IMU数据是1kHz，磁力计是100Hz。我们要把磁力计数据插值到每个IMU时刻上。

最常用的方法是线性插值。简单、计算量小，在弹载MCU上跑得动。

// 线性插值：已知t0时刻值v0，t1时刻值v1，求t时刻的值
float linear_interpolate(float t0, float v0, float t1, float v1, float t) {
    if(t1 == t0) return v0;  // 防止除零
    float ratio = (t - t0) / (t1 - t0);
    return v0 + ratio * (v1 - v0);
}

// 使用示例：将磁力计数据插值到IMU时间戳
// 假设 mag_t0, mag_v0 是上一个磁力计数据
// mag_t1, mag_v1 是当前磁力计数据
// imu_t 是当前IMU时间戳
float mag_interpolated = linear_interpolate(mag_t0, mag_v0, mag_t1, mag_v1, imu_t);

4.4 小结

这一章的内容，说白了就是三个字：读、滤、对齐。

• 读：用SPI DMA，别让CPU干等。开启FIFO，减少中断次数。
• 滤：中值滤波去野值，均值滤波平噪声。先中值后均值，顺序别搞反。
• 对齐：硬件时间戳优先，软件时间戳保底。线性插值把不同频率的数据拉到同一时间线上。

这些看起来都是基础功夫，但基础不牢，地动山摇。我见过太多项目，算法模型调得漂漂亮亮，一上硬件就崩，最后发现是数据预处理没做好。

下一章，咱们聊聊传感器标定——怎么把IMU的零偏、刻度因子这些“出厂缺陷”给校准掉。到时候见。