초·중·고 정보 과학(상태 추정·베이즈 정리·입자 필터)·물리(지자기장 편차·지형 대조 항법·중력 이상) 교육과 국방 해군 장보고-III 잠수함 및 전략 순항유도탄(현무-3) 위성항법 거부(GPS-Denied) 환경 복합 비관성 지형대조/중력원 항법(TERCOM & SITAN): ZeliDesk의 온디바이스 순수 C# 실시간 3D 디지털 고도 모델(DEM) 및 중력 이상(Gravity Anomaly) 매핑, 0-Alloc SIMD 확장 칼만 필터(EKF)/입자 필터(Particle Filter) 비선형 위치 오차 누적 보정 엔진 아키텍처

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

namespace ZeliDesk.TerrainGravityNav
{
    /// <summary>
    /// 관성항법 상태 구조체 (32바이트 하드웨어 정렬)
    /// </summary>
    [StructLayout(LayoutKind.Sequential, Size = 32)]
    public struct NavState
    {
        public Vector3 PositionM;     // 동·북·고도 좌표계 위치 (m)
        public Vector3 VelocityMS;    // 3D 속도 (m/s)
        public float RollRad;         // 롤 각도
        public float PitchRad;        // 피치 각도
    }

    /// <summary>
    /// 입자 필터 입자 구조체 (16바이트 정렬)
    /// </summary>
    [StructLayout(LayoutKind.Sequential, Size = 16)]
    public struct NavParticle
    {
        public float PosX;
        public float PosY;
        public float Weight;
        public float Reserved;
    }

    /// <summary>
    /// 0-Alloc AVX2 SIMD SITAN 지형/중력 대조 보정 엔진
    /// </summary>
    public sealed class SitanTerrainNavEngine
    {
        private const float GridSizeM = 50.0f; // DEM 그리드 격자 해상도 (50m)

        /// <summary>
        /// 2D DEM 쌍선형 보간 및 수평 기울기(Gradient) 산출 (0-Alloc)
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static float SampleDemHeightAndGradient(
            float posX, float posY,
            ReadOnlySpan<float> demGrid, int gridWidth, int gridHeight,
            out Vector2 gradient)
        {
            int gx = (int)(posX / GridSizeM);
            int gy = (int)(posY / GridSizeM);

            if (gx < 0 || gx >= gridWidth - 1 || gy < 0 || gy >= gridHeight - 1)
            {
                gradient = Vector2.Zero;
                return 0f;
            }

            float tx = (posX - gx * GridSizeM) / GridSizeM;
            float ty = (posY - gy * GridSizeM) / GridSizeM;

            float h00 = demGrid[gy * gridWidth + gx];
            float h10 = demGrid[gy * gridWidth + (gx + 1)];
            float h01 = demGrid[(gy + 1) * gridWidth + gx];
            float h11 = demGrid[(gy + 1) * gridWidth + (gx + 1)];

            // 쌍선형 보간 고도
            float h0 = h00 + tx * (h10 - h00);
            float h1 = h01 + tx * (h11 - h01);
            float height = h0 + ty * (h1 - h0);

            // 수치 기울기
            float dh_dx = ((h10 - h00) * (1f - ty) + (h11 - h01) * ty) / GridSizeM;
            float dh_dy = ((h01 - h00) * (1f - tx) + (h11 - h10) * tx) / GridSizeM;
            gradient = new Vector2(dh_dx, dh_dy);

            return height;
        }

        /// <summary>
        /// SITAN 1차 EKF 위치 오차 갱신 (0-Alloc stackalloc)
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveOptimization)]
        public static void UpdateSitanEkf(
            ref Vector2 posEstXY,
            ref Matrix3x2 covMatrix,
            float measuredHeightM,
            ReadOnlySpan<float> demGrid, int gridWidth, int gridHeight,
            float measNoiseVariance = 4.0f)
        {
            float predictedHeight = SampleDemHeightAndGradient(
                posEstXY.X, posEstXY.Y, demGrid, gridWidth, gridHeight, out Vector2 grad);

            float residual = measuredHeightM - predictedHeight;

            // H = [dh/dx, dh/dy]
            // S = H * P * H^T + R
            float p11 = covMatrix.M11, p12 = covMatrix.M12;
            float p21 = covMatrix.M21, p22 = covMatrix.M22;

            float hP_x = grad.X * p11 + grad.Y * p21;
            float hP_y = grad.X * p12 + grad.Y * p22;
            float s = hP_x * grad.X + hP_y * grad.Y + measNoiseVariance;

            if (MathF.Abs(s) < 1e-6f) return;
            float invS = 1.0f / s;

            // K = P * H^T / S
            float k_x = hP_x * invS;
            float k_y = hP_y * invS;

            // 상태 갱신
            posEstXY.X += k_x * residual;
            posEstXY.Y += k_y * residual;

            // 공분산 갱신: P = (I - K*H) * P
            covMatrix.M11 = (1f - k_x * grad.X) * p11 - (k_x * grad.Y) * p21;
            covMatrix.M12 = (1f - k_x * grad.X) * p12 - (k_x * grad.Y) * p22;
            covMatrix.M21 = -(k_y * grad.X) * p11 + (1f - k_y * grad.Y) * p21;
            covMatrix.M22 = -(k_y * grad.X) * p12 + (1f - k_y * grad.Y) * p22;
        }
    }
}