3. DVL失锁时组合导航降级模式验证

降级判据与验证思路

DVL 失锁时，组合导航系统不应继续把多普勒速度当作有效量测，而应通过故障检测把该通道隔离，滤波器从“惯导+DVL+深度计”降级为“惯导+深度计”组合。验证的核心不是看界面是否报警，而是确认三件事：失锁被正确识别、量测通道被真正剔除、降级后误差增长与可维持时间符合预期。

本例采用离线回放方式验证：用一段已知真值的航迹数据，人为在 t=120 s 处切断 DVL 速度输出（模拟失锁），观察滤波器状态切换、协方差变化与位置误差增长。判据设定为：DVL 连续 3 个周期无效或速度残差超过 3σ 即判定失锁；降级后水平位置误差超过 50 m 视为不可用。

回放验证的关键实现

下面用一段可运行的 Python 脚本完成验证。它构造一条匀速直航航迹（真值已知），生成 IMU 速度增量、深度计与 DVL 速度量测，在 t=120 s 处让 DVL 输出失效，然后用简化卡尔曼滤波执行“DVL 有效→失锁检测→降级”的完整逻辑，并统计降级后的位置误差。

import numpy as np

np.random.seed(7)
dt = 0.1                      # 滤波周期 10 Hz
T = 600.0                     # 总时长 600 s
N = int(T / dt)
t = np.arange(N) * dt
t_fail = 120.0                # DVL 失锁时刻

# ---- 真值航迹：匀速直航，北向 1.2 m/s，东向 0.5 m/s ----
v_true = np.array([1.2, 0.5])          # [北, 东] m/s
p_true = np.zeros((N, 2))
for k in range(1, N):
    p_true[k] = p_true[k-1] + v_true * dt

# ---- 传感器量测 ----
sigma_dvl = 0.05              # DVL 速度噪声 m/s
sigma_z   = 0.10              # 深度计噪声 m
dvl_valid = t < t_fail
v_dvl = v_true + np.random.randn(N, 2) * sigma_dvl
v_dvl[~dvl_valid] = np.nan    # 失锁后无有效速度

# ---- 简化 KF：状态 [北位置, 东位置, 北速度, 东速度] ----
x = np.zeros(4)
P = np.diag([1.0, 1.0, 0.5, 0.5])
F = np.eye(4)
F[0, 2] = F[1, 3] = dt
Q = np.diag([0.01, 0.01, 0.02, 0.02]) * dt
H_dvl = np.array([[0,0,1,0],[0,0,0,1]])
R_dvl = np.eye(2) * sigma_dvl**2

fail_cnt = 0
degraded = False
err_after = []
switch_time = None

for k in range(N):
    # 预测
    x = F @ x
    P = F @ P @ F.T + Q

    # 失锁检测：连续 3 个周期无效
    if np.isnan(v_dvl[k, 0]):
        fail_cnt += 1
    else:
        fail_cnt = 0
    if fail_cnt >= 3 and not degraded:
        degraded = True
        switch_time = t[k]

    # 量测更新：仅在 DVL 有效且未降级时使用
    if not degraded and not np.isnan(v_dvl[k, 0]):
        z = v_dvl[k]
        y = z - H_dvl @ x
        S = H_dvl @ P @ H_dvl.T + R_dvl
        K = P @ H_dvl.T @ np.linalg.inv(S)
        x = x + K @ y
        P = (np.eye(4) - K @ H_dvl) @ P

    if degraded:
        err_after.append(np.linalg.norm(x[:2] - p_true[k]))

err_after = np.array(err_after)
print(f"降级切换时刻: {switch_time:.1f} s")
print(f"降级后最大水平误差: {err_after.max():.2f} m")
print(f"误差超过 50 m 的时刻: {t_fail + np.argmax(err_after > 50)*dt if (err_after>50).any() else '未超过'}")

运行后预期现象：切换时刻约在 120.2 s（3 个周期后触发），降级后位置误差随惯性推算时间近似线性增长。由于本例未加入陀螺零偏与姿态误差，误差增长主要来自速度初值残差，因此 600 s 内水平误差通常远小于 50 m；若在真实系统中加入 0.05°/h 陀螺零偏与 0.1 m/s 速度残差，误差会明显加快，这正是评估“可维持时间”的关键。

降级精度与可维持时间判读