第一步：数据预处理与滤波
说实话，我见过太多人在轨迹优化上栽跟头，最后发现根本不是算法的问题——原始数据本身就是一团糟。你想想看，拿一堆带毛刺、有跳点、甚至趋势都没对齐的数据去跑优化，结果能好才怪。
我个人习惯把预处理分成三个台阶：先管好采集规范，再干掉异常值，最后做滤波。每一步都有坑，咱们一个一个说。
核心原则：预处理不是可有可无的「清洁工」，它是整个优化流程的地基。地基歪了，上面盖多高的楼都没用。
数据采集规范——源头决定上限
我在项目里遇到过最离谱的事：客户给了三个月的轨迹数据，结果一问，采样率一会儿 50Hz 一会儿 100Hz，时间戳还有重复的。你说这怎么处理？
所以第一步，先把采集规范定死：
- 采样率统一：所有传感器必须用同一个时钟源，或者事后重采样到固定频率。我个人习惯用 100Hz，够用又不至于数据量爆炸。
- 时间戳对齐：多传感器融合时，时间戳偏差超过 1ms 就会引入相位误差。我一般用插值法把各通道对齐到同一时间轴。
- 量纲归一：位置用米，速度用米/秒，角度用弧度。别混着用，否则滤波器的参数根本调不通。
小技巧：采集时顺手记一份日志，把传感器型号、采样率、环境温度都写进去。排查问题时，这些信息能救命。
异常值检测与剔除——别让一个坏点毁掉整条曲线
异常值这东西，说白了就是「一眼假」的数据点。比如无人机定位突然跳了 10 米，或者机械臂关节角度瞬间变了 30 度。这种点不干掉，后面的滤波和优化全白干。
我常用的方法有三种：
| 方法 | 适用场景 | 缺点 |
|---|---|---|
| 3σ 准则 | 数据近似正态分布 | 对非高斯分布效果差 |
| IQR 四分位距 | 有离群点的稳健估计 | 计算稍复杂 |
| 局部离群因子 (LOF) | 密度不均匀的数据 | 参数敏感 |
我个人最常用的是 IQR 法，因为它对数据分布不敏感，而且实现简单。具体做法：
- 计算数据的 Q1（25% 分位）和 Q3（75% 分位）
- IQR = Q3 - Q1
- 正常范围：[Q1 - 1.5×IQR, Q3 + 1.5×IQR]
- 超出范围的点直接剔除，用前后点的均值插补
def remove_outliers_iqr(data, factor=1.5):
q1 = np.percentile(data, 25)
q3 = np.percentile(data, 75)
iqr = q3 - q1
lower = q1 - factor * iqr
upper = q3 + factor * iqr
mask = (data >= lower) & (data <= upper)
return data[mask]注意：剔除异常值后一定要做插补，否则时间轴会断掉。我一般用线性插值，简单够用。如果数据变化剧烈，可以用样条插值。
低通滤波——把高频噪声滤干净
传感器数据里最烦人的就是高频抖动。比如 IMU 的加速度计，静止时都能读出 ±0.1g 的噪声。这时候就需要低通滤波。
我常用的两种低通滤波：
移动平均滤波
这是最朴素的方法，说白了就是取窗口内的平均值。窗口越大，曲线越平滑，但延迟也越大。
def moving_average(data, window_size=5):
return np.convolve(data, np.ones(window_size)/window_size, mode='same')我在项目中遇到过一个问题：窗口选 3 太平滑不够，选 10 又太滞后。后来我用了自适应窗口——根据当前数据的方差动态调整窗口大小。方差大就加大窗口，方差小就缩小窗口。效果还不错。
卡尔曼滤波
卡尔曼滤波比移动平均高级不少。它不光滤波，还能做状态估计和预测。说白了，它知道系统的运动模型，然后根据模型预测和观测值做加权平均。
我刚开始用卡尔曼滤波时也犯过傻——直接把所有参数设成 1，结果滤波效果还不如移动平均。后来才明白，调参才是关键：
- 过程噪声协方差 Q：代表你对模型的信任程度。Q 越小，越相信模型；Q 越大，越相信观测。
- 观测噪声协方差 R：代表传感器的噪声水平。R 可以从传感器手册里查，也可以自己标定。
经验之谈：如果你不确定 Q 和 R 怎么设，先跑一次离线标定。把传感器静止时的数据录下来，方差就是 R；然后根据运动模型算一下理论误差，那就是 Q 的起点。
高通滤波——把趋势项去掉
低通滤波搞定了高频噪声，但数据里可能还有低频趋势项。比如无人机长时间飞行，GPS 定位会慢慢漂移；或者机械臂长时间运行，关节编码器会有温度漂移。
趋势项不干掉，优化算法会误以为这是真实运动，结果就是轨迹越优化越偏。
高通滤波的做法很简单：先低通滤波提取趋势，然后用原始数据减去趋势。
def highpass_filter(data, window_size=100):
trend = moving_average(data, window_size)
return data - trend窗口大小怎么选？我一般看趋势项的周期。比如 GPS 漂移的周期是几分钟，那窗口就设成几分钟的数据量。你想想看，窗口太小会把真实运动也滤掉，窗口太大又滤不干净趋势。
避坑指南：高通滤波会引入相位偏移。如果你后续要做时间对齐或者多传感器融合，一定要做零相位滤波（比如用 scipy 的 filtfilt 函数）。
本章知识体系
下面这张图把数据预处理的流程串起来了。你可以把它当作操作手册，每一步做完检查一下，再往下走。
嗯，到这里数据预处理就做完了。你拿到的应该是一条干净、平滑、没有趋势漂移的轨迹。别小看这一步，我见过太多人急着跑优化算法，结果花在调参上的时间比预处理还多——得不偿失。