AUV 6DOF station-keeping (Fossen)

def _angular_rate_transform(phi, theta):
    """Transform body angular rates [p,q,r] to Euler angle rates [dphi,dtheta,dpsi]."""
    cp, sp = np.cos(phi), np.sin(phi)
    ct = np.cos(theta)
    tt = np.tan(np.clip(theta, -1.4, 1.4))
    sec_t = 1.0 / ct if abs(ct) > 1e-12 else 1e12 * np.sign(ct)

    T = np.array([
        [1.0, sp * tt,  cp * tt],
        [0.0, cp,       -sp],
        [0.0, sp * sec_t, cp * sec_t],
    ])
    return T

def _rotation_matrix(phi, theta, psi):
    """Standard ZYX rotation matrix for NED frame."""
    cp, sp = np.cos(phi), np.sin(phi)
    ct, st = np.cos(theta), np.sin(theta)
    cy, sy = np.cos(psi), np.sin(psi)

    R = np.array([
        [cy * ct, cy * st * sp - sy * cp, cy * st * cp + sy * sp],
        [sy * ct, sy * st * sp + cy * cp, sy * st * cp - cy * sp],
        [-st,     ct * sp,                 ct * cp],
    ])
    return R

def _auv_rhs(t, y):
    """Fossen 6DOF AUV: d_eta/dt = J(eta)*nu, M*d_nu/dt = -C*nu - D*nu - g + tau."""
    eta = y[:6]   # [x, y, z, phi, theta, psi]
    nu = y[6:12]  # [u, v, w, p, q, r]

    phi, theta, psi = eta[3], eta[4], eta[5]
    u, v, w, p, q, r = nu[0], nu[1], nu[2], nu[3], nu[4], nu[5]

    # -- kinematics: d_eta/dt = J(eta) * nu --
    R = _rotation_matrix(phi, theta, psi)
    T_ang = _angular_rate_transform(phi, theta)

    d_pos = R @ nu[:3]
    d_ang = T_ang @ nu[3:]

    # -- Coriolis + centripetal (rigid-body + added-mass cross terms) --
    # Simplified: dominant cross-coupling terms
    c_u = -(_M - _YV_DOT) * v * r + (_M - _ZW_DOT) * w * q
    c_v = (_M - _XU_DOT) * u * r - (_M - _ZW_DOT) * w * p
    c_w = -(_M - _XU_DOT) * u * q + (_M - _YV_DOT) * v * p
    c_p = (_I_R - _I_Q) * q * r
    c_q = (_I_P - _I_R) * p * r
    c_r = (_I_Q - _I_P) * p * q

    # -- damping: linear + quadratic --
    d_u = _XU * u + _XUU * abs(u) * u
    d_v = _YV * v + _YVV * abs(v) * v
    d_w = _ZW * w + _ZWW * abs(w) * w
    d_p = _KP * p
    d_q = _MQ * q
    d_r = _NR * r

    # -- restoring forces (neutrally buoyant, BG offset) --
    cp_r, sp_r = np.cos(phi), np.sin(phi)
    ct_r, st_r = np.cos(theta), np.sin(theta)
    g_vec = np.array([
        -(_W - _B) * st_r,
        (_W - _B) * ct_r * sp_r,
        (_W - _B) * ct_r * cp_r,
        _BG * _B * ct_r * sp_r,
        _BG * _B * st_r,
        0.0,
    ])

    # -- acceleration: M * d_nu/dt = tau + f_hydro - C*nu - g --
    # d_* variables use SNAME sign convention (already negative for drag),
    # so they are ADDED as forces, not subtracted.
    du_dt = (_TAU[0] + d_u - c_u - g_vec[0]) / _M_U
    dv_dt = (_TAU[1] + d_v - c_v - g_vec[1]) / _M_V
    dw_dt = (_TAU[2] + d_w - c_w - g_vec[2]) / _M_W
    dp_dt = (_TAU[3] + d_p - c_p - g_vec[3]) / _I_P
    dq_dt = (_TAU[4] + d_q - c_q - g_vec[4]) / _I_Q
    dr_dt = (_TAU[5] + d_r - c_r - g_vec[5]) / _I_R

    dy = np.empty(12)
    dy[0:3] = d_pos
    dy[3:6] = d_ang
    dy[6] = du_dt
    dy[7] = dv_dt
    dy[8] = dw_dt
    dy[9] = dp_dt
    dy[10] = dq_dt
    dy[11] = dr_dt
    return dy