1. DVL/INS 松耦合滤波状态空间建模与实现

为什么选 15 维误差状态，而不是直接估导航量

松耦合的核心是：INS 独立做机械编排，输出位置、速度、姿态；DVL 只提供速度观测。滤波器不直接估计“绝对速度/位置”，而是估计INS 机械编排的误差，再用误差去修正 INS 输出。这样做的原因是：INS 误差在小失准角、短时间条件下近似线性，卡尔曼滤波的线性假设成立；而直接估计绝对导航量会引入大范围非线性与可观性耦合。

本例采用“位置误差 + 速度误差 + 姿态误差 + 加速度计零偏 + 陀螺零偏”的 15 维误差状态：

δx = [ δp(3) , δv(3) , φ(3) , b_a(3) , b_g(3) ]ᵀ

各状态物理含义：δp 为导航系（n 系，东北天）位置误差；δv 为 n 系速度误差；φ 为 n 系下的小失准角；b_a、b_g 为载体系（b 系）下加速度计与陀螺零偏。误差状态定义为“真实值 = INS 解算值 − 误差”，因此修正时用减法。

连续时间误差方程与离散化

忽略地球自转与曲率的高阶项（短时、局部作业的常用简化），误差传播方程可写为：

δṗ = δv
δv̇ = −[f^n ×] φ + C_b^n · b_a + w_v
φ̇  = −C_b^n · b_g + w_φ
ḃ_a = w_ba ,  ḃ_g = w_bg

其中 f^n 为比力在 n 系下的投影，[·×] 为反对称矩阵，C_b^n 为姿态矩阵。系统矩阵 F 由上述方程对 δx 求偏导得到，噪声矩阵 G 把过程噪声映射到状态。离散化采用一阶近似：

Φ ≈ I + F·Δt ,   Q_d ≈ G·Q_c·Gᵀ·Δt

过程噪声强度按传感器指标设定：加速度计噪声密度 σ_a、陀螺噪声密度 σ_g，以及零偏随机游走 σ_ba、σ_bg。它们决定 Q_d 的量级，直接影响滤波器对 INS 的信任程度。

观测方程：DVL 只约束速度

DVL 测得的是载体系速度 v_b^DVL，需用 INS 姿态投影到 n 系：v_n^DVL = C_b^n · v_b^DVL。观测残差为 INS 速度减 DVL 投影速度：

z = v_n^INS − C_b^n · v_b^DVL = H·δx + n_DVL
H = [ 0(3×3)  I(3×3)  0(3×3)  0(3×3)  0(3×3) ]

H 只有速度块为单位阵，说明 DVL 观测直接可观的是速度误差；姿态误差通过 δv 方程中的 −[f^n×]φ 项间接可观，零偏则通过更长时间的速度/姿态误差积累间接可观。这正是松耦合可观性结构的本质，也是本例必须把 H 写清楚的原因。

基础滤波循环实现

下面给出可直接运行的 Python 实现，包含状态转移矩阵构造、过程噪声离散化、观测更新与误差反馈。示例参数为教学假设值，运行结果为按给定输入直接推出的预期现象，不代表实测。

import numpy as np

def skew(v):
    return np.array([[0, -v[2], v[1]],
                     [v[2], 0, -v[0]],
                     [-v[1], v[0], 0]])

def build_F(f_n, C_bn, dt):
    """15 维误差状态转移矩阵，一阶离散化"""
    F = np.zeros((15, 15))
    I3 = np.eye(3)
    F[0:3, 3:6] = I3                      # δṗ = δv
    F[3:6, 6:9] = -skew(f_n)              # δv̇ 中姿态误差项
    F[3:6, 9:12] = C_bn                   # 加速度计零偏
    F[6:9, 12:15] = -C_bn                 # 陀螺零偏
    return np.eye(15) + F * dt

def build_Qd(C_bn, dt, sa=0.02, sg=0.002, sba=1e-4, sbg=1e-5):
    """过程噪声：速度/姿态由传感器噪声密度驱动，零偏为随机游走"""
    G = np.zeros((15, 12))
    G[3:6, 0:3] = C_bn
    G[6:9, 3:6] = C_bn
    G[9:12, 6:9] = np.eye(3)
    G[12:15, 9:12] = np.eye(3)
    q = np.diag([sa**2]*3 + [sg**2]*3 + [sba**2]*3 + [sbg**2]*3)
    return G @ q @ G.T * dt

def kf_step(dx, P, f_n, C_bn, v_ins, v_dvl_b, R, dt):
    Phi = build_F(f_n, C_bn, dt)
    Qd = build_Qd(C_bn, dt)
    # 预测
    dx = Phi @ dx
    P = Phi @ P @ Phi.T + Qd
    # 观测：DVL 载体系速度投影到 n 系
    v_dvl_n = C_bn @ v_dvl_b
    H = np.zeros((3, 15)); H[0:3, 3:6] = np.eye(3)
    z = v_ins - v_dvl_n
    S = H @ P @ H.T + R
    K = P @ H.T @ np.linalg.inv(S)
    # 更新
    dx = dx + K @ (z - H @ dx)
    P = (np.eye(15) - K @ H) @ P
    return dx, P, z

# ---- 教学示例：单步验证 ----
dt = 0.05
C_bn = np.eye(3)                       # 假设载体系与导航系对齐
f_n = np.array([0.0, 0.0, 9.81])       # 静止时比力近似为重力反作用
P = np.diag([1.0]*3 + [0.25]*3 + [0.01]*3 + [1e-4]*3 + [1e-6]*3)
dx = np.zeros(15)
R = np.diag([0.01**2]*3)               # DVL 速度噪声方差 (m/s)^2
v_ins = np.array([0.10, -0.05, 0.02])  # INS 速度含误差
v_dvl_b = np.array([0.0, 0.0, 0.0])    # DVL 静止输出

dx, P, z = kf_step(dx, P, f_n, C_bn, v_ins, v_dvl_b, R, dt)
print("残差 z =", np.round(z, 4))
print("δv 估计 =", np.round(dx[3:6], 4))
print("P_vv 对角 =", np.round(np.diag(P)[3:6], 5))

预期现象：残差 z 等于 INS 速度与 DVL 投影速度之差，即 [0.10, −0.05, 0.02]；由于 H 只作用于速度块，δv 估计会向残差方向靠拢，P 的速度对角元在更新后下降。若把 R 调大，δv 估计更保守、P 下降更慢；把 R 调小则更信任 DVL。这正是后续调参的抓手。