대한민국 육군 천마(Chunma) 지대공 유도무기 Ku-밴드 펄스 도플러 추적 레이더 시선각(LOS) 칼만 필터 & 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.ShortRangeAirDefense
{
    [StructLayout(LayoutKind.Sequential, Pack = 16)]
    public struct ClosGuidanceResult
    {
        public Vector256<float> GuidanceCommand; // [accelCmdX, accelCmdY, accelCmdZ, missDistM, ...]
        public float CrossTrackErrorM;           // 시선선 이탈 거리 (m)
        public float AngularErrorMrad;           // 시선각 이탈 오차 (mrad)
        public float TimeToInterceptSec;         // 잔여 요격 시간 (초)
        public bool IsDirectHitKillConfirmed;    // 근접신관 직격 살상 판정
    }

    public sealed class ChunmaGuidanceEngine
    {
        private const float MissileMaxSpeedMps = 850.0f; // 마하 2.6 (850 m/s)
        private const float MaxGuidanceAccelG = 30.0f;    // 최대 허용 기동 가속도 (30G)
        private const float G0 = 9.80665f;

        private ClosGuidanceResult _result;

        [MethodImpl(MethodImplOptions.AggressiveOptimization)]
        public ref readonly ClosGuidanceResult ComputeGuidanceTick(
            float targetX, float targetY, float targetZ,
            float missileX, float missileY, float missileZ,
            float missileVx, float missileVy, float missileVz, float dt)
        {
            // 1. 표적 거리 및 정규화 시선 벡터 도출
            float rTarget = MathF.Sqrt(targetX * targetX + targetY * targetY + targetZ * targetZ);
            float losX = targetX / MathF.Max(rTarget, 1.0f);
            float losY = targetY / MathF.Max(rTarget, 1.0f);
            float losZ = targetZ / MathF.Max(rTarget, 1.0f);

            // 2. 미사일 거리 및 시선선 투영
            float rMissile = MathF.Sqrt(missileX * missileX + missileY * missileY + missileZ * missileZ);
            float projM = missileX * losX + missileY * losY + missileZ * losZ;

            // 3. 수직 이탈 오차 벡터 epsilon 계산
            float errX = missileX - projM * losX;
            float errY = missileY - projM * losY;
            float errZ = missileZ - projM * losZ;
            float crossTrackDist = MathF.Sqrt(errX * errX + errY * errY + errZ * errZ);

            _result.CrossTrackErrorM = crossTrackDist;
            float angularErrorMrad = (crossTrackDist / MathF.Max(rMissile, 1.0f)) * 1000.0f;
            _result.AngularErrorMrad = angularErrorMrad;

            // 4. CLOS PD 가속도 지령 산출 (비례이득 Kp = 35.0, 미분이득 Kd = 12.0)
            float kp = 35.0f;
            float kd = 12.0f;

            float cmdX = -kp * errX - kd * missileVx;
            float cmdY = -kp * errY - kd * missileVy;
            float cmdZ = -kp * errZ - kd * missileVz + G0; // 중력 보상

            // 5. 최대 G-리미트 클램핑 (30G)
            float cmdMag = MathF.Sqrt(cmdX * cmdX + cmdY * cmdY + cmdZ * cmdZ);
            float maxAccel = MaxGuidanceAccelG * G0;
            if (cmdMag > maxAccel)
            {
                float scale = maxAccel / cmdMag;
                cmdX *= scale;
                cmdY *= scale;
                cmdZ *= scale;
            }

            // 6. 잔여 요격 시간 계산
            float relDist = MathF.Sqrt(
                MathF.Pow(targetX - missileX, 2.0f) + MathF.Pow(targetY - missileY, 2.0f) + MathF.Pow(targetZ - missileZ, 2.0f));
            float tGo = relDist / MissileMaxSpeedMps;
            _result.TimeToInterceptSec = tGo;

            _result.IsDirectHitKillConfirmed = (crossTrackDist <= 0.85f && tGo < 0.15f);

            _result.GuidanceCommand = Vector256.Create(
                cmdX, cmdY, cmdZ, crossTrackDist, 0, 0, 0, 0);

            return ref _result;
        }
    }
}