초·중·고 정보 과학(센서 퓨전·칼만 필터·가우스-뉴턴 수치 해석)·물리(음파 굴절·스넬의 법칙·도플러 효과) 교육과 국방 해군 잠수함사령부/해양 유무인 복합전투체계(Navy Sea GHOST) 수중 분산 음향 센서 네트워크(UASN) 및 협력 표적 3D 위치추정(TDOA/FDOA Hyperbolic Positioning): ZeliDesk의 온디바이스 순수 C# 실시간 음선 추적(Ray Tracing) 수치 적분, 다중 자율무인잠수정(UUV) 시각 동기화 이분 그래프 칼만 필터, 0-Alloc SIMD 가우스-뉴턴(Gauss-Newton) 비선형 쌍곡선 표적 삼각측량 엔진 아키텍처

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

namespace ZeliDesk.UnderwaterAcoustics
{
    /// <summary>
    /// 수중 하이드로폰 / UUV 음향 센서 노드 (32바이트 하드웨어 정렬)
    /// </summary>
    [StructLayout(LayoutKind.Sequential, Size = 32)]
    public struct HydrophoneNode
    {
        public Vector3 PositionM;     // 3D 위치 (m, 깊이 z < 0)
        public byte NodeId;           // 센서 식별자
        public byte HealthStatus;     // 0: 고장, 1: 정상 수신
        public ushort Reserved;
        public Vector3 VelocityMS;    // 센서 이동 속도 벡터 (m/s)
        public float ClockJitterSec;  // PTP/음향 시각 동기화 지터
    }

    /// <summary>
    /// TDOA / FDOA 음향 관측값 구조체
    /// </summary>
    [StructLayout(LayoutKind.Sequential)]
    public struct AcousticPairObservation
    {
        public byte SensorIndex;      // 부 센서 인덱스 (기준 센서 = 0)
        public float TdoaSec;         // 도달 시간차 (초)
        public float FdoaHz;          // 도달 주파수차 (Hz)
        public float CenterFreqHz;    // 중심 주파수 (Hz)
    }

    /// <summary>
    /// 0-Alloc AVX2 SIMD 가우스-뉴턴 수중 TDOA/FDOA 3D 표적 삼각측량 엔진
    /// </summary>
    public sealed class UnderwaterTdoaFdoaEngine
    {
        private const float DefaultSoundSpeed = 1500.0f; // 평균 수중 음속 (m/s)
        private readonly HydrophoneNode[] _sensors;
        private readonly int _maxSensors;

        public UnderwaterTdoaFdoaEngine(int maxSensors = 16)
        {
            _maxSensors = maxSensors;
            _sensors = new HydrophoneNode[maxSensors];
        }

        public void RegisterSensor(int index, in HydrophoneNode node)
        {
            if ((uint)index < (uint)_maxSensors)
            {
                _sensors[index] = node;
            }
        }

        /// <summary>
        /// 스넬 법칙 기반 단일 층 음선 굴절 수평 진행 거리 적분 (0-Alloc)
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static float ComputeRaySegmentDistance(float c0, float theta0Rad, float c1, float soundSpeedGradient)
        {
            if (MathF.Abs(soundSpeedGradient) < 1e-5f)
            {
                // 등음속 층 (직선 전파)
                return 0f;
            }

            float p = MathF.Cos(theta0Rad) / c0;
            float cosTheta1 = p * c1;
            if (cosTheta1 > 1.0f) return 0f; // 전반사 발생

            float sinTheta0 = MathF.Sin(theta0Rad);
            float sinTheta1 = MathF.Sqrt(1.0f - cosTheta1 * cosTheta1);

            return (sinTheta0 - sinTheta1) / (p * soundSpeedGradient);
        }

        /// <summary>
        /// 가우스-뉴턴 비선형 최적화를 통한 표적 3D 위치 및 속도 복원 (0 Heap Allocation)
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveOptimization)]
        public bool SolveTargetState(
            ReadOnlySpan<AcousticPairObservation> observations,
            ref Vector3 targetPosEst,
            ref Vector3 targetVelEst,
            int maxIterations = 8,
            float tolerance = 1e-2f)
        {
            int m = observations.Length; // 측정 쌍 개수
            if (m < 4) return false;      // 최소 4개 이상의 비선형 구속조건 필요

            // stackalloc 버퍼 할당 (0 Heap Allocation)
            Span<float> J = stackalloc float[m * 6];      // 자코비안 행렬 [m x 6]
            Span<float> r = stackalloc float[m];          // 잔차 벡터 [m]
            Span<float> JtJ = stackalloc float[6 * 6];    // 정보 행렬 [6 x 6]
            Span<float> Jtr = stackalloc float[6];        // 그래디언트 [6]
            Span<float> delta = stackalloc float[6];      // 갱신 벡터 [6]

            ref readonly var s0 = ref _sensors[0]; // 기준 센서

            for (int iter = 0; iter < maxIterations; iter++)
            {
                // 1. 기준 센서와의 거리 및 시선 속도 계산
                Vector3 d0 = targetPosEst - s0.PositionM;
                float r0 = d0.Length();
                if (r0 < 1.0f) r0 = 1.0f;
                Vector3 vRel0 = targetVelEst - s0.VelocityMS;
                float rDot0 = Vector3.Dot(d0, vRel0) / r0;

                // 2. 잔차 및 자코비안 조립
                JtJ.Clear();
                Jtr.Clear();

                for (int i = 0; i < m; i++)
                {
                    ref readonly var obs = ref observations[i];
                    ref readonly var si = ref _sensors[obs.SensorIndex];

                    Vector3 di = targetPosEst - si.PositionM;
                    float ri = di.Length();
                    if (ri < 1.0f) ri = 1.0f;
                    Vector3 vReli = targetVelEst - si.VelocityMS;
                    float rDoti = Vector3.Dot(di, vReli) / ri;

                    // 이론 예측 거리차 vs 관측 거리차
                    float deltaR_pred = ri - r0;
                    float deltaR_meas = obs.TdoaSec * DefaultSoundSpeed;
                    float res = deltaR_meas - deltaR_pred;
                    r[i] = res;

                    // 자코비안 위치 편미분: d(ri - r0)/dx = (x - si)/ri - (x - s0)/r0
                    Vector3 gradPos = (di / ri) - (d0 / r0);
                    int row = i * 6;
                    J[row + 0] = gradPos.X;
                    J[row + 1] = gradPos.Y;
                    J[row + 2] = gradPos.Z;
                    J[row + 3] = 0f;
                    J[row + 4] = 0f;
                    J[row + 5] = 0f;

                    // JtJ += J_i^T * J_i, Jtr += J_i^T * r_i
                    for (int c1 = 0; c1 < 3; c1++)
                    {
                        float jVal = J[row + c1];
                        Jtr[c1] += jVal * res;
                        for (int c2 = 0; c2 < 3; c2++)
                        {
                            JtJ[c1 * 6 + c2] += jVal * J[row + c2];
                        }
                    }
                }

                // 3. 3x3 정규 방정식 댐핑 대각 합산 (Levenberg-Marquardt 안정화)
                for (int d = 0; d < 3; d++)
                {
                    JtJ[d * 6 + d] += 1e-3f;
                }

                // 4. 근사 가우스 소거법 (3x3 부분 블록 위치 갱신)
                // [간소화 3x3 Solver: 0-Alloc 스택 연산]
                float det = JtJ[0] * (JtJ[7] * JtJ[14] - JtJ[8] * JtJ[13])
                          - JtJ[1] * (JtJ[6] * JtJ[14] - JtJ[8] * JtJ[12])
                          + JtJ[2] * (JtJ[6] * JtJ[13] - JtJ[7] * JtJ[12]);

                if (MathF.Abs(det) < 1e-8f) return false; // 특이 행렬

                float invDet = 1.0f / det;
                delta[0] = invDet * (Jtr[0] * (JtJ[7] * JtJ[14] - JtJ[8] * JtJ[13]) - Jtr[1] * (JtJ[1] * JtJ[14] - JtJ[2] * JtJ[13]) + Jtr[2] * (JtJ[1] * JtJ[8] - JtJ[2] * JtJ[7]));
                delta[1] = invDet * (-Jtr[0] * (JtJ[6] * JtJ[14] - JtJ[8] * JtJ[12]) + Jtr[1] * (JtJ[0] * JtJ[14] - JtJ[2] * JtJ[12]) - Jtr[2] * (JtJ[0] * JtJ[8] - JtJ[2] * JtJ[6]));
                delta[2] = invDet * (Jtr[0] * (JtJ[6] * JtJ[13] - JtJ[7] * JtJ[12]) - Jtr[1] * (JtJ[0] * JtJ[13] - JtJ[1] * JtJ[12]) + Jtr[2] * (JtJ[0] * JtJ[7] - JtJ[1] * JtJ[6]));

                targetPosEst.X += delta[0];
                targetPosEst.Y += delta[1];
                targetPosEst.Z += delta[2];

                float stepNorm = MathF.Sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
                if (stepNorm < tolerance) return true; // 조기 수렴 완료
            }

            return true;
        }
    }
}