4. 传感器数据预处理基础
水下多传感器融合系统的输入往往来自不同厂商、不同采样率、不同时钟源的设备：DVL 可能以 5 Hz 输出速度，IMU 以 100 Hz 输出角速度和加速度，深度计以 1 Hz 输出压力换算深度，磁力计或 USBL 则以更低频率给出航向或位置修正。这些数据在进入融合算法之前，必须先经过一套稳定的预处理流程，否则滤波器会把传感器故障、时间错位和坐标混用当成真实运动，导致估计发散。本节围绕缺失值处理、异常值检测与剔除、时间对齐、坐标系统一四个环节展开，并给出一个可复用的清洗流程设计。
一、缺失值：先判断“为什么缺”，再决定“怎么补”
水下数据的缺失通常有三类来源：通信丢包（如水声链路中断导致 USBL 修正帧丢失）、设备间歇性无回波（如 DVL 在离底高度过大时底跟踪失效）、以及采集程序写入失败。处理前应先统计每个通道的缺失率与连续缺失长度，因为短缺口和长缺口应采用不同策略。
对于 IMU、DVL 这类高频通道，若连续缺失不超过 2～3 个采样周期，可用线性插值或前值保持填补；若连续缺失超过阈值（例如 0.5 s），更合理的做法是标记为无效并让融合器跳过该观测，而不是强行插值制造虚假信息。深度计变化缓慢，短缺口用线性插值通常安全。USBL 位置修正属于低频绝对观测，缺失时应直接跳过，绝不能用插值“补”出一个位置点，否则会引入错误的绝对约束。
import numpy as np
import pandas as pd
def fill_short_gaps(series, max_gap=3, method="linear"):
"""仅填补连续缺失长度 <= max_gap 的缺口，长缺口保留 NaN。"""
s = series.copy()
is_na = s.isna()
# 标记每段连续缺失的长度
grp = (is_na != is_na.shift()).cumsum()
gap_len = is_na.groupby(grp).transform("sum")
short = is_na & (gap_len <= max_gap)
filled = s.interpolate(method=method, limit_direction="both")
s[short] = filled[short]
return s这段代码的关键在于“只补短缺口”，长缺口保留 NaN 交给下游判断，避免把设备失效伪装成正常数据。
二、异常值：用物理约束和统计规则双重把关
水下异常值常见于 DVL 底跟踪跳变、深度计受波浪冲击的瞬时尖峰、磁力计受船体磁场干扰的航向突变。单纯用 3σ 统计规则容易把真实机动误判为异常，因此建议“物理阈值 + 统计检测”组合使用：先用物理范围做硬性剔除（如深度为负、DVL 速度超过载体最大航速、姿态角超出量程），再用滑动窗口的中位数绝对偏差（MAD）检测残余尖峰。
def mad_outlier_mask(x, window=15, k=3.5):
med = x.rolling(window, center=True, min_periods=5).median()
mad = (x - med).abs().rolling(window, center=True, min_periods=5).median()
return (x - med).abs() > k * 1.4826 * mad
# 示例：DVL 前向速度，物理上限 2.0 m/s
vx = dvl["vx"]
mask_phys = vx.abs() > 2.0
mask_stat = mad_outlier_mask(vx)
dvl.loc[mask_phys | mask_stat, "vx"] = np.nan注意异常值剔除后应置为 NaN 而非直接删除整行，这样后续缺失值策略可以统一处理，也保留了时间戳的连续性。对于连续多个点同时被判异常的情况，应视为传感器故障而非单点噪声，需要记录事件并触发降级策略。
三、时间对齐：统一到同一时间基准
多传感器融合的前提是所有观测都带有可比的时间戳。实际系统常见两个问题：一是各设备使用自身时钟，存在固定偏移和漂移；二是采样时刻不对齐，例如 IMU 在 t=0.010 s、0.020 s 采样，而 DVL 在 t=0.013 s 输出。推荐做法是先做时钟同步（如 PPS 或软件时间同步），再把所有数据重采样到统一的高频时间网格上。
对姿态、速度等连续量，可用线性插值对齐到 IMU 时间网格；对 USBL 位置修正这类稀疏观测，应保留原始时间戳，在融合时按“最近邻 + 最大时间差门限”匹配，超过门限（例如 0.2 s）则丢弃，避免用过时观测更新状态。下图展示了本节推荐的完整清洗链路与各环节的输入输出关系。
四、坐标系统一：先约定，再变换
水下载体通常涉及四个坐标系：传感器自身安装系、载体（机体）系、当地导航系（北-东-地或北-东-下）、以及 USBL 等外部参考系。融合前必须明确每个坐标系的轴向定义和旋转顺序，并在全文与代码中保持一致。例如约定载体系为前-右-下（FRD），导航系为北-东-地（NED），则 DVL 输出的速度需先经安装外参旋转到载体系，再经姿态旋转到导航系。
def rot_body_from_sensor(v_sensor, R_bs):
"""R_bs: 传感器系到载体系的旋转矩阵 (3x3)"""
return R_bs @ v_sensor
def rot_nav_from_body(v_body, R_nb):
"""R_nb: 载体系到导航系的旋转矩阵 (3x3)"""
return R_nb @ v_body外参旋转矩阵应通过安装测量或标定获得，并作为配置项固定下来。若发现融合结果出现系统性偏差（例如航向匀速漂移、速度方向恒定偏转），应优先检查外参符号和旋转顺序是否与约定一致，而不是急于调整滤波器噪声参数。
五、清洗流程的组织与验证
把上述四个环节组织成一条固定顺序的流水线：先做缺失值标记与短缺口填补，再做异常值检测（异常点同样置为无效），然后统一时间基准，最后做坐标变换。顺序不能随意调换——若先做坐标变换再检测异常，物理阈值将失去意义；若先做时间对齐再处理缺失，插值会跨越本应保留的长缺口。
验证清洗效果时，可观察三个指标：各通道有效数据比例、异常事件次数与持续时间、以及对齐后相邻观测的时间差分布。以一段 60 s 的示例数据为例（以下为合理假设的教学示例，非实测结果）：DVL 原始缺失率约 4%，经短缺口填补后有效比例提升到 98%；检测到 3 次连续异常，均对应底跟踪失效事件；时间对齐后 95% 的观测与 IMU 网格时间差小于 5 ms。若这些指标明显偏离预期，应回到对应环节检查阈值设置，而不是直接进入融合调参。