2. DVL 速度观测与 INS 速度输出的时间同步对齐

问题定位：两条时间轴为何对不齐

松耦合滤波的观测残差是 z = v_INS(t) − v_DVL(t)，只有当两路速度对应同一物理时刻时，残差才只反映导航误差。实际系统中存在两类错位：

• 频率不一致：INS 机械编排常以 100~200 Hz 输出，DVL 通常 1~10 Hz（底跟踪），两者采样点不重合。
• 时延不一致：DVL 从发射、相关处理到输出存在固有时延 τ_d（典型几十毫秒），且时间戳可能标记在“发射时刻”或“输出时刻”，需统一到同一基准。

若直接用“最近一帧”DVL 与当前 INS 速度相减，残差中会混入 Δv ≈ a·Δt 的运动学项（a 为载体加速度，Δt 为错位时间），这部分会被滤波误当作导航误差去“修正”，造成速度、姿态被错误牵引。

对齐策略：以 INS 时刻为基准做插值/外推

工程上统一做法是：把 DVL 观测“搬运”到 INS 的观测更新时刻 t_k。设 DVL 时间戳序列为 t_d[i]，速度序列为 v_d[i]：

• 内插（首选）：当 t_d[i] ≤ t_k ≤ t_d[i+1] 时，用线性插值 v_d(t_k) = v_d[i] + (v_d[i+1] − v_d[i]) · (t_k − t_d[i]) / (t_d[i+1] − t_d[i])。
• 外推（边界情形）：当 t_k 超出最新 DVL 时间戳但间隔小于门限 T_max（如 0.2 s）时，用最近两点斜率外推；超过门限则丢弃该次观测，避免外推放大误差。
• 时延补偿：若时间戳标在发射时刻，则先做 t_d' = t_d + τ_d 再参与插值；τ_d 可由静态或匀速直线段标定得到。

时间同步误差如何进入残差

可运行实现：时间戳对齐器

下面给出一个不依赖第三方库的对齐器，核心是维护 DVL 历史缓冲，按 INS 时刻查询并做插值/外推，同时输出对齐质量指标（是否外推、残余时间差）。

import bisect

class DVLTimeAligner:
    def __init__(self, t_max_extrap=0.2, tau_d=0.0):
        self.ts = []          # DVL 时间戳（已补偿时延）
        self.vs = []          # 对应速度 [vx, vy, vz]
        self.t_max = t_max_extrap
        self.tau_d = tau_d    # DVL 固有时延，单位 s

    def add(self, t_stamp, v):
        t = t_stamp + self.tau_d
        i = bisect.bisect_left(self.ts, t)
        self.ts.insert(i, t)
        self.vs.insert(i, v)
        # 只保留最近 2 s 数据，防止缓冲无限增长
        while self.ts and self.ts[0] < t - 2.0:
            self.ts.pop(0); self.vs.pop(0)

    def query(self, t_k):
        """返回 (v_aligned, info)；info 含 mode 与 dt_residual"""
        if len(self.ts) == 0:
            return None, {"mode": "empty", "dt": None}
        if len(self.ts) == 1:
            return self.vs[0], {"mode": "hold", "dt": t_k - self.ts[0]}

        # 内插
        if self.ts[0] <= t_k <= self.ts[-1]:
            j = bisect.bisect_right(self.ts, t_k) - 1
            if j == len(self.ts) - 1:
                return self.vs[j], {"mode": "exact", "dt": 0.0}
            t0, t1 = self.ts[j], self.ts[j+1]
            r = (t_k - t0) / (t1 - t0)
            v = [self.vs[j][k] + r*(self.vs[j+1][k]-self.vs[j][k]) for k in range(3)]
            return v, {"mode": "interp", "dt": 0.0}

        # 外推（仅用最近两点）
        if t_k > self.ts[-1]:
            gap = t_k - self.ts[-1]
            if gap > self.t_max:
                return None, {"mode": "reject", "dt": gap}
            t0, t1 = self.ts[-2], self.ts[-1]
            slope = [(self.vs[-1][k]-self.vs[-2][k])/(t1-t0) for k in range(3)]
            v = [self.vs[-1][k] + slope[k]*gap for k in range(3)]
            return v, {"mode": "extrap", "dt": gap}
        return None, {"mode": "reject", "dt": t_k - self.ts[0]}

验证：同步误差对残差的影响

构造一段匀速直线+正弦加速度的运动，INS 200 Hz、DVL 5 Hz，人为给 DVL 时间戳加偏移 Δ，比较“最近邻”与“插值对齐”两种方式下的残差统计。以下为教学示例数据，用于说明趋势而非实测结果。

import math

def run_case(dt_offset, use_interp=True):
    aligner = DVLTimeAligner(t_max_extrap=0.2, tau_d=0.0)
    res = []
    # 生成 10 s 数据
    for n in range(2000):
        t = n * 0.005
        a = 0.5 * math.sin(2*math.pi*0.2*t)      # 加速度 m/s^2
        v_true = 2.0 + 0.5*(-math.cos(2*math.pi*0.2*t))/(2*math.pi*0.2)
        # DVL 每 0.2 s 一帧，时间戳被人为偏移 dt_offset
        if n % 40 == 0:
            aligner.add(t + dt_offset, [v_true, 0.0, 0.0])
        if n % 10 == 0 and n > 40:               # INS 20 Hz 更新
            if use_interp:
                v_d, info = aligner.query(t)
            else:
                v_d = aligner.vs[-1] if aligner.vs else None
                info = {"mode": "nearest"}
            if v_d is not None:
                res.append(v_true - v_d[0])
    rms = math.sqrt(sum(r*r for r in res)/len(res))
    return rms

for off in [0.0, 0.02, 0.05, 0.10]:
    print(f"偏移 {off*1000:.0f} ms | 最近邻 RMS={run_case(off, False):.4f} | 插值 RMS={run_case(off, True):.4f}")

预期趋势：偏移为 0 时两者接近；偏移增大时“最近邻”残差 RMS 随 a·Δ 线性上升，而插值对齐的残差基本不随偏移变化（仅剩插值截断误差）。这正是本例要验证的核心结论——对齐误差会以伪观测形式进入残差，且量级与加速度、错位时间成正比。

工程判据与常见坑