대한민국 해군 P-8A 포세이돈(Poseidon) 해상초계기 다중 소노부이 음향 다자간 위치추정 및 1,000Hz 0-Alloc C# TDOA 쌍곡선 잠수함 3차원 국지화 엔진

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

namespace ZeliDesk.Defense.Navy.AsaLocalization
{
    public readonly record struct SonobuoyNode(
        Vector3 PositionMeters,
        float TimeOfArrivalSec,
        float SignalStrengthDb
    );

    public ref struct SubmarineTargetSolution
    {
        public Vector3 EstimatedPosition;
        public float TargetDepthMeters;
        public float LocalizationErrorMeters;
        public bool ConfirmedSubmarineTrack;
    }

    public static class P8aTdoaLocalizationCore
    {
        private const float AverageWaterSoundSpeedMps = 1492.0f;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ResolveSubmarinePosition(
            ReadOnlySpan buoys, // 최소 4개 부이 필요
            ref SubmarineTargetSolution solution)
        {
            if (buoys.Length < 4)
            {
                solution.ConfirmedSubmarineTrack = false;
                return;
            }

            // 1. 기준 부이 선정 (가장 신호 강도가 높은 부이)
            ref readonly var b0 = ref buoys[0];

            // 2. 가우스-뉴턴 초기 추정점 (부이들의 중심 하방 150m)
            Vector3 targetEst = Vector3.Zero;
            for (int i = 0; i < buoys.Length; i++)
            {
                targetEst += buoys[i].PositionMeters;
            }
            targetEst /= buoys.Length;
            targetEst.Z = -150.0f; // 초기 가정 심도 150m

            // 3. 0-Alloc 3차원 비선형 최소제곱 가우스-뉴턴 반복 (3회 수렴)
            for (int iter = 0; iter < 3; iter++)
            {
                float r0 = Vector3.Distance(targetEst, b0.PositionMeters);
                Vector3 gradAccum = Vector3.Zero;
                float residualAccum = 0.0f;

                for (int i = 1; i < buoys.Length; i++)
                {
                    ref readonly var bi = ref buoys[i];
                    float ri = Vector3.Distance(targetEst, bi.PositionMeters);
                    float measuredDeltaR = (bi.TimeOfArrivalSec - b0.TimeOfArrivalSec) * AverageWaterSoundSpeedMps;
                    float modelDeltaR = ri - r0;
                    float residual = measuredDeltaR - modelDeltaR;

                    Vector3 u_i = (targetEst - bi.PositionMeters) / Math.Max(ri, 1.0f);
                    Vector3 u_0 = (targetEst - b0.PositionMeters) / Math.Max(r0, 1.0f);
                    Vector3 jacobian = u_i - u_0;

                    gradAccum += jacobian * residual;
                    residualAccum += Math.Abs(residual);
                }

                targetEst += gradAccum * 0.25f;
                solution.LocalizationErrorMeters = residualAccum / (buoys.Length - 1);
            }

            solution.EstimatedPosition = targetEst;
            solution.TargetDepthMeters = Math.Abs(targetEst.Z);
            solution.ConfirmedSubmarineTrack = solution.LocalizationErrorMeters < 15.0f;
        }
    }
}