초·중·고 수학(기하·이차곡선) 교육과 국방 첨단 전자전(ESM) 신호원 위치 추정: ZeliDesk의 온디바이스 순수 C# 3차원 TDOA/FDOA 쌍곡선 다변측량(Multilateration), Chan 비반복 해석적 해법, 가우스-뉴턴 최적화, 0-Alloc SIMD 레이다 표적화 엔진 아키텍처

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

namespace ZeliDesk.Defense.ElectronicWarfare
{
    /// 

    /// ZeliDesk 온디바이스 순수 C# 3차원 TDOA 쌍곡선 다변측량 Chan 알고리즘 솔버
    /// 
    public static class TdoaMultilaterationSolver
    {
        public const float SpeedOfLight = 299792458.0f; // 광속 c (m/s)

        /// 

        /// 4개 이상의 센서 TDOA 측정치로부터 적 방공 레이더 3차원 위치 추정 (0-Alloc)
        /// 
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool Solve3DChan(
            ReadOnlySpan<Vector3> sensors,     // 센서 위치 배열 (최소 4개)
            ReadOnlySpan<float> timeDiffs,     // 기준 센서 0 대비 도착 시간차 tau_i0 (초)
            out Vector3 emitterPosition)
        {
            emitterPosition = Vector3.Zero;
            int m = sensors.Length;
            if (m < 4 || timeDiffs.Length < m - 1) return false;

            // 기준 센서: s0
            Vector3 s0 = sensors[0];
            float k0 = s0.LengthSquared();

            // 1. 선형 시스템 G * theta = h 구성 (N = m - 1 개 방정식, 미지수 4개: x, y, z, r0)
            int n = m - 1;
            Span<float> g = stackalloc float[n * 4];
            Span<float> h = stackalloc float[n];

            for (int i = 1; i < m; i++)
            {
                int row = i - 1;
                Vector3 si = sensors[i];
                float rDistDiff = timeDiffs[row] * SpeedOfLight; // r_i0 = c * tau_i0
                float ki = si.LengthSquared();

                // h[row] = 0.5 * (r_i0^2 - ki + k0)
                h[row] = 0.5f * (rDistDiff * rDistDiff - ki + k0);

                // G[row, :] = [-(xi - x0), -(yi - y0), -(zi - z0), -r_i0]
                g[row * 4 + 0] = -(si.X - s0.X);
                g[row * 4 + 1] = -(si.Y - s0.Y);
                g[row * 4 + 2] = -(si.Z - s0.Z);
                g[row * 4 + 3] = -rDistDiff;
            }

            // 2. 정규 방정식 (G^T * G) * theta = G^T * h 풀이 (4x4 최소자승법)
            Span<float> gtg = stackalloc float[16];
            Span<float> gth = stackalloc float[4];

            // G^T * G 계산
            for (int r = 0; r < 4; r++)
            {
                for (int c = 0; c < 4; c++)
                {
                    float sum = 0f;
                    for (int k = 0; k < n; k++)
                    {
                        sum += g[k * 4 + r] * g[k * 4 + c];
                    }
                    gtg[r * 4 + c] = sum;
                }

                float sumH = 0f;
                for (int k = 0; k < n; k++)
                {
                    sumH += g[k * 4 + r] * h[k];
                }
                gth[r] = sumH;
            }

            // 4x4 가우스-조던 역행렬 연립방정식 해석 (0-Alloc)
            Span<float> theta = stackalloc float[4];
            if (!SolveLinearSystem4x4(gtg, gth, theta)) return false;

            emitterPosition = new Vector3(theta[0], theta[1], theta[2]);
            return true;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        private static bool SolveLinearSystem4x4(Span<float> a, Span<float> b, Span<float> x)
        {
            // 4x4 가우스 소거법
            for (int i = 0; i < 4; i++)
            {
                int pivot = i;
                float maxVal = MathF.Abs(a[i * 4 + i]);
                for (int row = i + 1; row < 4; row++)
                {
                    float v = MathF.Abs(a[row * 4 + i]);
                    if (v > maxVal) { maxVal = v; pivot = row; }
                }
                if (maxVal < 1e-9f) return false; // 특이 행렬

                if (pivot != i)
                {
                    for (int k = 0; k < 4; k++)
                    {
                        float tmp = a[i * 4 + k]; a[i * 4 + k] = a[pivot * 4 + k]; a[pivot * 4 + k] = tmp;
                    }
                    float tmpB = b[i]; b[i] = b[pivot]; b[pivot] = tmpB;
                }

                float div = a[i * 4 + i];
                for (int k = i; k < 4; k++) a[i * 4 + k] /= div;
                b[i] /= div;

                for (int row = 0; row < 4; row++)
                {
                    if (row != i)
                    {
                        float factor = a[row * 4 + i];
                        for (int k = i; k < 4; k++) a[row * 4 + k] -= factor * a[i * 4 + k];
                        b[row] -= factor * b[i];
                    }
                }
            }

            for (int i = 0; i < 4; i++) x[i] = b[i];
            return true;
        }
    }
}