공군 장거리 공대지 순항미사일 천룡(Cheonryong) 지형참조항법(TERCOM) & 고정밀 디지털 지형 표고 모델(DEM) 윤곽 일치 1,000Hz 0-Alloc C# 엔진

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

namespace ZeliDesk.Defense.Avionics
{
    [StructLayout(LayoutKind.Sequential, Pack = 16)]
    public struct NavigationState
    {
        public Vector256<float> PosVel;       // [x, y, z, 0, vx, vy, vz, 0] (m, m/s)
        public float InsErrorX;               // INS 추정 위치 오차 X (m)
        public float InsErrorY;               // INS 추정 위치 오차 Y (m)
        public float CorrelationScore;        // TERCOM 상관 일치 신뢰도 (0~1)
        public bool IsFixAcquired;            // 지형 일치 보정 성공 플래그
    }

    public sealed class TercomTerrainEngine
    {
        private const int ProfileSize = 16;
        private const int SearchWindow = 9;   // -4 ~ +4 격자 탐색
        private const float GridResolutionM = 10.0f;

        private readonly float[] _measuredProfile = new float[ProfileSize];
        private NavigationState _state;

        [MethodImpl(MethodImplOptions.AggressiveOptimization)]
        public ref readonly NavigationState UpdateTercomFix(
            ReadOnlySpan<float> localDemGrid, // 64x64 DEM 고도 맵
            int gridStride,
            float baroAltitude,
            float radarAltitude,
            float dt)
        {
            float groundElev = baroAltitude - radarAltitude;

            // 프로파일 시프트 버퍼 갱신 (0-Alloc)
            for (int i = 0; i < ProfileSize - 1; i++)
            {
                _measuredProfile[i] = _measuredProfile[i + 1];
            }
            _measuredProfile[ProfileSize - 1] = groundElev;

            // 평균 고도 계산 (DC 바이어스 제거)
            float meanMeas = 0.0f;
            for (int i = 0; i < ProfileSize; i++) meanMeas += _measuredProfile[i];
            meanMeas /= ProfileSize;

            // MAD(Mean Absolute Difference) 최적 탐색
            float minMad = float.MaxValue;
            int bestOffsetX = 0;
            int bestOffsetY = 0;

            int centerIdx = 32; // 로컬 DEM 중심

            for (int dy = -4; dy <= 4; dy++)
            {
                for (int dx = -4; dx <= 4; dx++)
                {
                    float mad = 0.0f;
                    for (int k = 0; k < ProfileSize; k++)
                    {
                        int demX = centerIdx + dx + (k - ProfileSize / 2);
                        int demY = centerIdx + dy;
                        float demElev = localDemGrid[demY * gridStride + demX];
                        mad += MathF.Abs((_measuredProfile[k] - meanMeas) - demElev);
                    }
                    mad /= ProfileSize;

                    if (mad < minMad)
                    {
                        minMad = mad;
                        bestOffsetX = dx;
                        bestOffsetY = dy;
                    }
                }
            }

            // 상관 신뢰도 산출 (MAD 오차가 작을수록 높은 신뢰도)
            if (minMad < 12.0f)
            {
                _state.InsErrorX = bestOffsetX * GridResolutionM;
                _state.InsErrorY = bestOffsetY * GridResolutionM;
                _state.CorrelationScore = MathF.Clamp(1.0f - (minMad / 12.0f), 0.0f, 1.0f);
                _state.IsFixAcquired = true;
            }
            else
            {
                _state.CorrelationScore = 0.2f;
                _state.IsFixAcquired = false;
            }

            return ref _state;
        }
    }
}