第1章：传感器数据预处理——校准与滤波的艺术
各位同学，欢迎来到《低延迟姿态解算算法优化实战》的第一章。
说实话，做姿态解算这么多年，我踩过最大的坑就是——传感器原始数据直接拿来用。你想想看，一个加速度计如果零偏没校准，哪怕只有0.1g的偏差，融合出来的姿态角可能就偏了5度以上。这在四轴飞行器上，基本就是炸机的节奏。
所以，这一章我们老老实实把传感器数据预处理讲透。我个人习惯把预处理分成两大块：校准和滤波。校准解决的是系统误差，滤波解决的是随机噪声。两者缺一不可。
1.1 加速度计校准：零偏与标度因数
加速度计的输出模型，说白了就是：
a_measured = S * a_true + b + n其中S是标度因数矩阵，b是零偏，n是噪声。我们校准的目标就是求出S和b。
零偏校准
零偏校准最简单的方法——把传感器静置在水平面上。理论上，此时三轴输出应该是[0, 0, g]。但实际上，我测过几十个MPU6050，零偏值从-0.05g到+0.12g不等。
具体做法：
- 采集N组静态数据（我一般取500~1000组）
- 对每组数据取平均，得到三轴均值
- 减去理论值[0, 0, g]，剩下的就是零偏
核心公式：
b_x = mean(ax) - 0
b_y = mean(ay) - 0
b_z = mean(az) - g我的经验：采集数据时，传感器要远离金属物体和电机。我曾经把传感器放在电机旁边做校准，结果零偏值飘了3倍——全是电磁干扰惹的祸。
标度因数校准
标度因数校准稍微麻烦一点。你需要让传感器分别绕X、Y、Z轴旋转，采集正反两个方向的数据。
举个例子，绕X轴旋转时：
- 正向：理论上Y轴输出+g，Z轴输出0
- 反向：理论上Y轴输出-g，Z轴输出0
通过正反两次测量，可以解算出标度因数：
S_y = (a_y_positive - a_y_negative) / (2 * g)注意：标度因数校准对旋转台的精度要求很高。如果用手拿着转，误差会很大。我建议用3D打印一个夹具，或者直接用六面体工装。
1.2 陀螺仪校准：零偏稳定性与温漂
陀螺仪比加速度计更「娇气」。它的零偏会随着时间和温度变化，这就是所谓的零偏稳定性和温漂。
零偏稳定性
零偏稳定性描述的是：在恒定温度下，陀螺仪零偏随时间的变化程度。单位是°/h或°/s。
校准方法：
- 传感器静置，采集1小时以上的陀螺仪数据
- 计算每10秒的平均值，得到一组零偏序列
- 计算该序列的标准差，就是零偏稳定性
实际数据（我测的ICM-20948）：
| 轴 | 零偏均值 (°/s) | 零偏稳定性 (°/h) |
|---|---|---|
| X | 0.023 | 8.5 |
| Y | -0.015 | 7.2 |
| Z | 0.031 | 9.1 |
温漂校准
温漂是陀螺仪最大的敌人。我记得有一次做无人机飞控，冬天室外零下10度，陀螺仪零偏直接漂了0.5°/s——这数据根本没法用。
温漂校准的经典做法：
- 把传感器放进温箱，从-40°C到85°C，每5°C采集一次零偏
- 建立温度-零偏的拟合曲线（我一般用3阶多项式）
- 运行时根据当前温度查表或计算补偿值
// 温漂补偿示例（伪代码）
float temp_compensate(float gyro_raw, float temp) {
// 3阶多项式拟合系数
float c0 = 0.021, c1 = 0.0003, c2 = 0.00001, c3 = 0.0000002;
float bias = c0 + c1*temp + c2*temp*temp + c3*temp*temp*temp;
return gyro_raw - bias;
}避坑指南：我曾经偷懒只做了线性拟合，结果在高温段误差反而变大了。后来老老实实用了3阶多项式，效果立竿见影。别省这点计算量，真的值得。
1.3 磁力计校准：硬铁与软铁
磁力计校准是个「玄学」活。硬铁干扰来自永磁体，软铁干扰来自铁磁性材料。两者叠加，会让磁力计的输出变成一个偏移的椭圆。
硬铁校准
硬铁干扰表现为一个固定的偏置。校准方法很简单——让传感器在三维空间内旋转，采集足够多的点，然后找到球心。
// 椭球拟合求球心（简化版）
// 采集N组数据 (mx, my, mz)
// 最小二乘法拟合球体
// 球心坐标即为硬铁偏置软铁校准
软铁干扰更复杂，它会把球体拉伸成椭球。需要求解一个3x3的变换矩阵。
我常用的方法是：
- 先做硬铁校准，减去偏置
- 再做椭球拟合，求出变换矩阵
- 对原始数据应用变换，恢复成标准球体
校准前后对比：
| 指标 | 校准前 | 校准后 |
|---|---|---|
| X轴范围 | -45~55 μT | -50~50 μT |
| Y轴范围 | -40~60 μT | -50~50 μT |
| Z轴范围 | -48~52 μT | -50~50 μT |
| 总场强标准差 | 8.2 μT | 1.1 μT |
注意：磁力计校准对环境要求极高。我建议在空旷的室外做，远离钢筋建筑和高压线。室内校准？基本是白费功夫。
1.4 低通滤波与滑动窗口滤波
校准完了，数据还有随机噪声。这时候就需要滤波上场了。
低通滤波
一阶低通滤波是最常用的，计算量小，效果也不错：
// 一阶低通滤波
float lowpass_filter(float input, float prev_output, float alpha) {
// alpha = 0.1 ~ 0.3 效果较好
return alpha * input + (1 - alpha) * prev_output;
}alpha值怎么选？我一般这样试：
- 加速度计：alpha=0.2（噪声大，需要平滑）
- 陀螺仪：alpha=0.3（响应要快一些）
- 磁力计：alpha=0.1（噪声最大，但响应可以慢）
滑动窗口滤波
滑动窗口滤波更适合去除突发噪声。比如传感器被碰了一下，低通滤波会拖尾，滑动窗口直接把这个异常点剔除了。
// 滑动窗口均值滤波
#define WINDOW_SIZE 10
float buffer[WINDOW_SIZE];
int index = 0;
float sliding_window_filter(float input) {
buffer[index] = input;
index = (index + 1) % WINDOW_SIZE;
float sum = 0;
for (int i = 0; i < WINDOW_SIZE; i++) {
sum += buffer[i];
}
return sum / WINDOW_SIZE;
}我的建议：低通滤波和滑动窗口可以组合使用。先过滑动窗口剔除野点，再过低通滤波平滑噪声。我在做四轴飞控时就是这么干的，效果比单独用任何一种都好。
本章知识体系
下面这张图总结了传感器数据预处理的完整流程：
这张图把整个预处理流程串起来了。从原始数据出发，经过加速度计、陀螺仪、磁力计三路校准，再汇聚到滤波模块，最后输出干净的数据给姿态解算算法。每一步都环环相扣，缺一不可。
好了，第一章的内容就到这里。传感器数据预处理是姿态解算的基石，地基打不牢，上层算法再花哨也是白搭。下一章我们会进入姿态解算的核心——四元数与旋转矩阵，敬请期待。