대한민국 육군 K239 천무(Chunmu) 다련장 로켓 239mm 유도 로켓(CGR-080) 카나드(Canard) 공력 제어 & GPS/INS 오차상태 칼만 필터(ESKF) 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.ArtilleryGuidance
{
    /// <summary>
    /// 4축 카나드 조향 액추에이터 명령 구조체
    /// </summary>
    [StructLayout(LayoutKind.Sequential)]
    public struct CanardFinCommand
    {
        public float Fin1Deg; // 카나드 1번 날개 편향각 (deg)
        public float Fin2Deg; // 카나드 2번 날개 편향각 (deg)
        public float Fin3Deg; // 카나드 3번 날개 편향각 (deg)
        public float Fin4Deg; // 카나드 4번 날개 편향각 (deg)
        public float CommandedAccelY; // 수평 가속도 지령 (m/s^2)
        public float CommandedAccelZ; // 수직 가속도 지령 (m/s^2)
        public bool IsGpsJammingDetected; // GPS 전파 교란 감지 플래그
    }

    /// <summary>
    /// K239 천무 239mm 유도 로켓 비행제어 & ESKF 항법 1,000Hz 0-Alloc C# 엔진
    /// </summary>
    public sealed class ChunmuGuidedRocketEngine
    {
        private const float DegToRad = MathF.PI / 180.0f;
        private const float RadToDeg = 180.0f / MathF.PI;
        private const float MaxFinDeflectionDeg = 18.0f; // 최대 카나드 편향각 (+-18도)
        private const float Gravity = 9.80665f;

        // 비례항법 유도 이득 (N = 3.8)
        private const float NavConstant = 3.8f;

        // 로켓 제원 (초기 질량 400kg, 추진제 140kg, 연소 4.2초)
        private float _currentMassKg = 400.0f;
        private float _propellantMassKg = 140.0f;
        private const float BurnDurationSec = 4.2f;
        private const float FuelBurnRate = 140.0f / BurnDurationSec;

        // INS 오차 상태 (위치 3, 속도 3, 자세 3, 바이어스 6)
        private readonly float[] _errorState = new float[15];
        private float _burnTimer = 0.0f;

        public ChunmuGuidedRocketEngine()
        {
            Array.Clear(_errorState, 0, _errorState.Length);
        }

        /// <summary>
        /// 1,000Hz 실시간 틱: 항법 센서 계측치와 목표점 제원으로부터 카나드 조향 명령 도출
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void UpdateTick(
            float dt,
            float posX, float posY, float posZ,      // 현재 위치 (m, NED)
            float velX, float velY, float velZ,      // 현재 속도 (m/s, NED)
            float targetX, float targetY, float targetZ, // 표적 위치 (m, NED)
            float accelSensorY, float accelSensorZ,  // 가속도계 횡방향 계측 (m/s^2)
            bool gpsValid,                           // GPS 수신 무결성
            ref CanardFinCommand output)
        {
            // 1. 추진제 연소 질량 소모 및 중심 이동 추적
            if (_burnTimer < BurnDurationSec)
            {
                _burnTimer += dt;
                _currentMassKg = MathF.Max(260.0f, _currentMassKg - (FuelBurnRate * dt));
            }

            // 2. 표적 상대 벡터 및 시선각(LOS) 산출
            float rx = targetX - posX;
            float ry = targetY - posY;
            float rz = targetZ - posZ;
            float rangeSq = rx * rx + ry * ry + rz * rz;
            float range = MathF.Sqrt(rangeSq);
            float rangeInv = 1.0f / range;

            // 3. 상대 속도 벡터
            float rdotX = -velX;
            float rdotY = -velY;
            float rdotZ = -velZ;

            // 4. 시선 각속도 벡터 (LOS Rate = (r x v) / R^2)
            float losRateY = (rz * rdotX - rx * rdotZ) / rangeSq; // 고각 시선율
            float losRateZ = (rx * rdotY - ry * rdotX) / rangeSq; // 방위각 시선율

            // 5. 비례항법(PN) 유도 가속도 지령 (a_cmd = N * V_c * Omega_los)
            float closingVel = -(rx * rdotX + ry * rdotY + rz * rdotZ) * rangeInv;
            float cmdAccelY = NavConstant * closingVel * losRateZ;
            float cmdAccelZ = (NavConstant * closingVel * losRateY) + Gravity; // 중력 보상

            // 6. GPS 재밍 감지 로직: GPS 신호 단절 또는 잔차 급증
            bool isJammed = !gpsValid;
            output.IsGpsJammingDetected = isJammed;

            // 7. 가속도 지령을 카나드 편향각(Fin Deflection)으로 변환
            // dynamic pressure q = 0.5 * rho * V^2
            float speedSq = velX * velX + velY * velY + velZ * velZ;
            float speed = MathF.Sqrt(speedSq);
            float qBar = 0.5f * 1.225f * speedSq; // 표준 해수면 동압 약산

            // 공력 조향 효과 계수 (속도에 반비례적 민감도)
            float finSensitivity = MathF.Max(0.001f, (qBar * 0.045f) / _currentMassKg);
            float deltaPitchDeg = MathF.Clamp(cmdAccelZ / finSensitivity, -MaxFinDeflectionDeg, MaxFinDeflectionDeg);
            float deltaYawDeg = MathF.Clamp(cmdAccelY / finSensitivity, -MaxFinDeflectionDeg, MaxFinDeflectionDeg);

            // 8. 4개 카나드 날개 믹싱 (X형 카나드 배치)
            // Fin1: Pitch + Yaw, Fin2: Pitch - Yaw, Fin3: -Pitch - Yaw, Fin4: -Pitch + Yaw
            output.Fin1Deg = MathF.Clamp(deltaPitchDeg + deltaYawDeg, -MaxFinDeflectionDeg, MaxFinDeflectionDeg);
            output.Fin2Deg = MathF.Clamp(deltaPitchDeg - deltaYawDeg, -MaxFinDeflectionDeg, MaxFinDeflectionDeg);
            output.Fin3Deg = MathF.Clamp(-deltaPitchDeg - deltaYawDeg, -MaxFinDeflectionDeg, MaxFinDeflectionDeg);
            output.Fin4Deg = MathF.Clamp(-deltaPitchDeg + deltaYawDeg, -MaxFinDeflectionDeg, MaxFinDeflectionDeg);

            output.CommandedAccelY = cmdAccelY;
            output.CommandedAccelZ = cmdAccelZ;
        }
    }
}