[단독] AIDT 온디바이스 MUM-T 자율무인잠수정(AUV) 합성 수심측량(Bathy-SLAM) 및 등심선 지형참조항법(TERCOM) 다중 센서 융합 서보 v1.0

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

namespace ZeliDesk.Defense.MUMT.Navigation
{
    /// 

    /// AUV 수심 계측 및 관성 항법 추정 상태
    /// 
    public readonly record struct AuvNavigationPing(
        float EstimatedPosX,        // INS 추정 위치 X (m)
        float EstimatedPosY,        // INS 추정 위치 Y (m)
        float MeasuredDepthM,       // MBES 중앙 빔 계측 수심 (m)
        float AltitudeAboveBottomM, // 해저면 이격 고도 (m)
        float HeadingRad,           // 침로 방위각 (rad)
        float VelocityMps           // 대지 속력 (m/s)
    );

    /// 

    /// 지형 매칭(TERCOM) 위치 보정 출력
    /// 
    public readonly record struct TercomCorrectionResult(
        float CorrectedPosX,        // 보정된 최적 위치 X (m)
        float CorrectedPosY,        // 보정된 최적 위치 Y (m)
        float PositionUncertaintyM, // 매칭 불확실도 반경 (m)
        float CorrelationScore,     // 상관 매칭 품질 계수 [0.0 ~ 1.0]
        bool IsReliableMatch        // 지형 매칭 신뢰 판정 여부
    );

    /// 

    /// 온디바이스 MUM-T 합성 수심측량 및 지형참조항법 서보
    /// 
    public ref struct BathySlamTercomServo
    {
        private const int SearchWindowGrid = 15; // 15x15 탐색 그리드
        private const float GridSpacingM = 10.0f; // 10m 격자
        private float _lastValidScore;

        public BathySlamTercomServo(float initialScore)
        {
            _lastValidScore = initialScore;
        }

        /// 

        /// 계측 수심 시계열과 디지털 수심 지도의 MAD 상관 매칭 위치 보정 연산
        /// 
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public TercomCorrectionResult ComputeTerrainMatch(
            in AuvNavigationPing ping,
            ReadOnlySpan measuredProfile,
            ReadOnlySpan mapGridSlice,
            int gridStride)
        {
            if (measuredProfile.Length == 0 || mapGridSlice.Length == 0)
            {
                return new TercomCorrectionResult(ping.EstimatedPosX, ping.EstimatedPosY, 50.0f, 0.0f, false);
            }

            // 계측 프로파일 평균치 산출
            float sumMeas = 0.0f;
            for (int i = 0; i < measuredProfile.Length; i++) sumMeas += measuredProfile[i];
            float avgMeas = sumMeas / measuredProfile.Length;

            float bestMad = float.MaxValue;
            int bestOffsetX = 0;
            int bestOffsetY = 0;

            int halfWin = SearchWindowGrid / 2;

            // 탐색 윈도우 순회
            for (int dy = -halfWin; dy <= halfWin; dy++)
            {
                for (int dx = -halfWin; dx <= halfWin; dx++)
                {
                    float currentMad = 0.0f;
                    int validCount = 0;

                    for (int k = 0; k < measuredProfile.Length; k++)
                    {
                        int mapIdx = (dy + halfWin) * gridStride + (dx + halfWin) + k;
                        if ((uint)mapIdx < (uint)mapGridSlice.Length)
                        {
                            float mapVal = mapGridSlice[mapIdx];
                            float diff = MathF.Abs((measuredProfile[k] - avgMeas) - mapVal);
                            currentMad += diff;
                            validCount++;
                        }
                    }

                    if (validCount > 0)
                    {
                        float avgMad = currentMad / validCount;
                        if (avgMad < bestMad)
                        {
                            bestMad = avgMad;
                            bestOffsetX = dx;
                            bestOffsetY = dy;
                        }
                    }
                }
            }

            // 최적 매칭 위치 산출
            float corrX = ping.EstimatedPosX + (bestOffsetX * GridSpacingM);
            float corrY = ping.EstimatedPosY + (bestOffsetY * GridSpacingM);

            // 상관 점수 정규화 (MAD 1m 이하 시 우수 매칭)
            float score = Math.Max(0.0f, 1.0f - (bestMad / 5.0f));
            bool isReliable = (score > 0.65f) && (bestMad < 2.0f);
            float uncertainty = isReliable ? 5.0f : 25.0f;

            _lastValidScore = score;

            return new TercomCorrectionResult(
                corrX,
                corrY,
                uncertainty,
                score,
                isReliable
            );
        }

        /// 

        /// 다중 빔 음향 수심 벡터에 대한 AVX-512 SIMD 평균 절대차 일괄 고속 연산
        /// 
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static float ComputeBatchMadSIMD(
            ReadOnlySpan measured,
            ReadOnlySpan mapSlice,
            float avgMeas)
        {
            int i = 0;
            int vectorSize = Vector256.Count;
            var avgVec = Vector256.Create(avgMeas);
            var sumVec = Vector256.Zero;

            for (; i <= measured.Length - vectorSize; i += vectorSize)
            {
                var measVec = Vector256.LoadUnsafe(in measured[i]);
                var mapVec = Vector256.LoadUnsafe(in mapSlice[i]);

                var zeroMeanMeas = Vector256.Subtract(measVec, avgVec);
                var diffVec = Vector256.Subtract(zeroMeanMeas, mapVec);
                var absDiff = Vector256.Abs(diffVec);

                sumVec = Vector256.Add(sumVec, absDiff);
            }

            float totalSum = 0.0f;
            for (int k = 0; k < vectorSize; k++)
            {
                totalSum += sumVec.GetElement(k);
            }

            for (; i < measured.Length; i++)
            {
                totalSum += MathF.Abs((measured[i] - avgMeas) - mapSlice[i]);
            }

            return totalSum / measured.Length;
        }
    }
}