대한민국 공군 E-737 피스아이 조기경보통제기 MESA 레이더 탑사이드 빔조향 및 1,000Hz 0-Alloc C# 저고도 침투표적 MTI 추적 필터

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

namespace ZeliDesk.Defense.AirForce.PeaceEye
{
    public readonly record struct RadarDetection(
        Vector3 PositionMeasurement, // 관측 위치 (x, y, z)
        float DopplerVelocityMps,     // 도플러 시선속도
        float SignalToNoiseDb,        // SNR
        bool IsClutterLikely
    );

    public ref struct MtiTrackState
    {
        public int TrackId;
        public Vector3 EstimatedPosition;
        public Vector3 EstimatedVelocity;
        public float CovarianceNorm;
        public int ConsecutiveHitCount;
        public bool IsConfirmedTarget;
    }

    public static class PeaceEyeMesaCore
    {
        private const float AlphaFilter = 0.85f;
        private const float BetaFilter = 0.55f;
        private const float MinTargetSpeedMps = 15.0f; // 54 km/h 이상 비행체

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void UpdateMtiTrack(
            ref MtiTrackState track,
            ReadOnlySpan detections,
            float dtSeconds)
        {
            // 1. 등속도 모델 상태 예측 (State Prediction)
            Vector3 predPos = track.EstimatedPosition + track.EstimatedVelocity * dtSeconds;
            Vector3 predVel = track.EstimatedVelocity;

            if (detections.IsEmpty)
            {
                track.EstimatedPosition = predPos;
                track.EstimatedVelocity = predVel;
                track.CovarianceNorm += 0.5f * dtSeconds;
                if (track.ConsecutiveHitCount > 0) track.ConsecutiveHitCount--;
                return;
            }

            // 2. 최근접 데이터 연관 (Nearest Neighbor PDAF 단순화)
            float minDistanceSq = 5000.0f * 5000.0f; // 5km 게이트
            int bestIdx = -1;

            for (int i = 0; i < detections.Length; i++)
            {
                ref readonly var det = ref detections[i];
                if (det.IsClutterLikely && det.SignalToNoiseDb < 12.0f) continue;

                // 클러터 제거: 항공기 자체 플랫폼 속도 대비 도플러 속도 검사
                if (MathF.Abs(det.DopplerVelocityMps) < MinTargetSpeedMps) continue;

                float distSq = Vector3.DistanceSquared(predPos, det.PositionMeasurement);
                if (distSq < minDistanceSq)
                {
                    minDistanceSq = distSq;
                    bestIdx = i;
                }
            }

            // 3. 필터 갱신 (Measurement Update)
            if (bestIdx >= 0)
            {
                ref readonly var validDet = ref detections[bestIdx];
                Vector3 residual = validDet.PositionMeasurement - predPos;

                track.EstimatedPosition = predPos + AlphaFilter * residual;
                track.EstimatedVelocity = predVel + (BetaFilter / dtSeconds) * residual;
                track.CovarianceNorm = MathF.Max(track.CovarianceNorm * 0.8f, 0.1f);
                track.ConsecutiveHitCount++;

                if (track.ConsecutiveHitCount >= 3)
                {
                    track.IsConfirmedTarget = true;
                }
            }
            else
            {
                track.EstimatedPosition = predPos;
                track.EstimatedVelocity = predVel;
                track.CovarianceNorm += 0.3f * dtSeconds;
            }
        }
    }
}