초·중·고 정보 과학(상태 추정·다중 센서 융합·도킹 제어)·물리(부력·수중 광학 굴절·유체 저항) 교육과 국방 해군 스마트 네이비(Navy Sea GHOST) 자율 무인수상정(USV)과 자율 무인잠수정(UUV) 간의 해상-수중 이종 복합 무인 체계 자율 회수/랑데부 도킹(Autonomous Docking): ZeliDesk의 온디바이스 순수 C# 실시간 초음파 비컨(USBL) + 수중 레이저 비전 센서 융합 확장 칼만 필터(EKF), 6자유도 유체역학 상대 기구학 제어, 0-Alloc SIMD 가속 원뿔형 도킹 콘(Docking Cone) 래치 잠금 수치 제어 엔진 아키텍처

using System;
using System.Numerics;
using System.Runtime.CompilerServices;
using System.Runtime.InteropServices;

namespace ZeliDesk.Core.MarineRobotics
{
    /// <summary>
    /// UUV-도킹 콘 간 상대 기구학 상태 벡터 (위치, 속도, 자세)
    /// </summary>
    [StructLayout(LayoutKind.Sequential, Pack = 16)]
    public struct RelativeKinematicsState
    {
        public Vector3 RelativePosition; // [x, y, z] (m)
        public Vector3 RelativeVelocity; // [vx, vy, vz] (m/s)
        public Vector3 RelativeEulerAngles; // [roll, pitch, yaw] (rad)

        public readonly float Distance => RelativePosition.Length();
    }

    /// <summary>
    /// 음향(USBL) + 광학(Vision) 비동기 융합 확장 칼만 필터(EKF)
    /// </summary>
    public sealed class DockingSensorFusionEkf
    {
        private RelativeKinematicsState _state;
        private readonly float[] _pCovariance; // 9x9 공분산 (대각 행렬 간이화)
        private readonly float[] _qProcessNoise;

        public DockingSensorFusionEkf()
        {
            _state = default;
            _pCovariance = new float[9];
            _qProcessNoise = new float[9];

            for (int i = 0; i < 9; i++)
            {
                _pCovariance[i] = 1.0f;
                _qProcessNoise[i] = (i < 3) ? 0.01f : (i < 6 ? 0.05f : 0.005f);
            }
        }

        public RelativeKinematicsState CurrentState => _state;

        /// <summary>
        /// 상태 예측 단계 (100Hz 주기 호출, 0-Alloc)
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Predict(Vector3 uuvThrustAcceleration, float dt)
        {
            _state.RelativePosition += _state.RelativeVelocity * dt + 0.5f * uuvThrustAcceleration * dt * dt;
            _state.RelativeVelocity += uuvThrustAcceleration * dt;

            // 공분산 대각 갱신
            for (int i = 0; i < 9; i++)
            {
                _pCovariance[i] += _qProcessNoise[i] * dt;
            }
        }

        /// <summary>
        /// 원거리 USBL 음향 센서 비동기 관측 갱신 (1~2Hz)
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveOptimization)]
        public void UpdateUsblMeasurement(float range, float azimuthRad, float elevationRad, float rNoise = 0.25f)
        {
            // 구면 좌표계 -> 직교 좌표계 변환
            float cosEl = MathF.Cos(elevationRad);
            float zMeasX = range * cosEl * MathF.Cos(azimuthRad);
            float zMeasY = range * cosEl * MathF.Sin(azimuthRad);
            float zMeasZ = range * MathF.Sin(elevationRad);

            UpdatePositionMeasurement(new Vector3(zMeasX, zMeasY, zMeasZ), rNoise);
        }

        /// <summary>
        /// 근거리 레이저 비전 센서 비동기 관측 갱신 (30Hz, 정밀도 1cm)
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveOptimization)]
        public void UpdateVisionMeasurement(Vector3 visionRelativePos, float rNoise = 0.0001f)
        {
            UpdatePositionMeasurement(visionRelativePos, rNoise);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private void UpdatePositionMeasurement(Vector3 measuredPos, float rNoise)
        {
            // 칼만 이득 K = P / (P + R)
            float kx = _pCovariance[0] / (_pCovariance[0] + rNoise);
            float ky = _pCovariance[1] / (_pCovariance[1] + rNoise);
            float kz = _pCovariance[2] / (_pCovariance[2] + rNoise);

            _state.RelativePosition.X += kx * (measuredPos.X - _state.RelativePosition.X);
            _state.RelativePosition.Y += ky * (measuredPos.Y - _state.RelativePosition.Y);
            _state.RelativePosition.Z += kz * (measuredPos.Z - _state.RelativePosition.Z);

            _pCovariance[0] *= (1.0f - kx);
            _pCovariance[1] *= (1.0f - ky);
            _pCovariance[2] *= (1.0f - kz);
        }
    }

    /// <summary>
    /// 비선형 시선각(LOS) 종말 도킹 유도 제어기
    /// </summary>
    public sealed class TerminalDockingGuidanceController
    {
        public enum DockingPhase { Approach, Alignment, FinalLatch, Completed, Abort }

        public DockingPhase CurrentPhase { get; private set; } = DockingPhase.Approach;

        /// <summary>
        /// 0-Alloc 실시간 제어 루프 (UUV 추진기 사령 출력)
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveOptimization)]
        public Vector3 ComputeDockingThrust(in RelativeKinematicsState state, out bool isLatched)
        {
            float dist = state.Distance;
            isLatched = false;

            if (dist > 15.0f)
            {
                CurrentPhase = DockingPhase.Approach;
                // 접근 단계: 목표 속도 1.5 m/s
                Vector3 desiredDir = Vector3.Normalize(-state.RelativePosition);
                return desiredDir * 150.0f; // 추진력
            }
            else if (dist > 1.5f)
            {
                CurrentPhase = DockingPhase.Alignment;
                // 정렬 단계: 진입 축(X축) 일치 및 속도 감속 (0.5 m/s)
                Vector3 alignForce = new Vector3(-state.RelativePosition.X * 50.0f, 
                                                 -state.RelativePosition.Y * 100.0f, 
                                                 -state.RelativePosition.Z * 100.0f);
                return alignForce;
            }
            else if (dist > 0.1f)
            {
                CurrentPhase = DockingPhase.FinalLatch;
                // 최종 삽입: 상대 속도 0.15 m/s 서행 삽입
                return new Vector3(30.0f, -state.RelativePosition.Y * 150.0f, -state.RelativePosition.Z * 150.0f);
            }
            else
            {
                CurrentPhase = DockingPhase.Completed;
                isLatched = true;
                return Vector3.Zero; // 도킹 래치 체결 완료
            }
        }
    }
}