가오리-X(Kaori-X) 무미익 전익기(Tailless Flying Wing) 스텔스 무인 전투기 6자유도 비행제어(FCS) & 공력 제어면 배분 0-Alloc SIMD 슬라이딩 모드 제어(SMC) 온디바이스 C# 엔진

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

namespace ZeliDesk.Defense.Avionics
{
    [StructLayout(LayoutKind.Sequential, Pack = 16)]
    public struct KaoriXFlightState
    {
        public Vector256<float> PosVel;      // [x, y, z, 0, u, v, w, 0]
        public Vector256<float> QuatRate;    // [q0, q1, q2, q3, p, q, r, 0]
        public Vector256<float> ControlSurfaces; // [e1, e2, e3, e4, sdr_l, sdr_r, 0, 0] (rad)
        public float Alpha;                   // 받음각 (rad)
        public float Beta;                    // 사이드슬립 (rad)
        public float DynamicPressure;         // 동압 q (Pa)
        public float Airspeed;                // 대기속도 (m/s)
    }

    public sealed class KaoriXTaillessFcsEngine
    {
        private const float Mass = 4500.0f;           // 공허중량 + 탑재량 (kg)
        private const float Ixx = 12500.0f;           // 롤 관성모멘트 (kg*m^2)
        private const float Iyy = 6800.0f;            // 피치 관성모멘트 (kg*m^2)
        private const float Izz = 18600.0f;           // 요 관성모멘트 (kg*m^2)
        private const float WingSpan = 14.8f;         // 익폭 b (m)
        private const float WingArea = 42.5f;         // 익면적 S (m^2)

        // 슬라이딩 모드 제어 게인
        private static readonly Vector256<float> SmcLambda = Vector256.Create(8.0f, 6.5f, 9.0f, 0f, 0f, 0f, 0f, 0f);
        private static readonly Vector256<float> SmcEta = Vector256.Create(15.0f, 12.0f, 18.0f, 0f, 0f, 0f, 0f, 0f);
        private static readonly Vector256<float> BoundaryLayer = Vector256.Create(0.05f, 0.05f, 0.05f, 1f, 1f, 1f, 1f, 1f);

        private Vector256<float> _integratedError;

        [MethodImpl(MethodImplOptions.AggressiveInlining | MethodImplOptions.AggressiveOptimization)]
        public void StepFcs1000Hz(ref KaoriXFlightState state, in Vector256<float> angularRateCmd, float dt)
        {
            // 1. 공기역학 동압 및 각도 산출
            float u = state.PosVel.GetElement(4);
            float v = state.PosVel.GetElement(5);
            float w = state.PosVel.GetElement(6);
            float vTot = MathF.Sqrt(u * u + v * v + w * w + 1e-6f);
            state.Airspeed = vTot;
            state.Alpha = MathF.Atan2(w, u);
            state.Beta = MathF.Asin(Math.Clamp(v / vTot, -1.0f, 1.0f));

            float rho = 1.225f * MathF.Exp(-state.PosVel.GetElement(2) / 8500.0f); // 고도 기반 밀도
            state.DynamicPressure = 0.5f * rho * vTot * vTot;

            // 2. 각속도 오차 및 슬라이딩 곡면 S 계산: S = e_dot + Lambda * e
            float p = state.QuatRate.GetElement(4);
            float q = state.QuatRate.GetElement(5);
            float r = state.QuatRate.GetElement(6);

            float pCmd = angularRateCmd.GetElement(0);
            float qCmd = angularRateCmd.GetElement(1);
            float rCmd = angularRateCmd.GetElement(2);

            float errP = p - pCmd;
            float errQ = q - qCmd;
            float errR = r - rCmd;

            // 오차 적분 누적 (Windup 제한)
            float intP = Math.Clamp(_integratedError.GetElement(0) + errP * dt, -0.5f, 0.5f);
            float intQ = Math.Clamp(_integratedError.GetElement(1) + errQ * dt, -0.5f, 0.5f);
            float intR = Math.Clamp(_integratedError.GetElement(2) + errR * dt, -0.5f, 0.5f);
            _integratedError = Vector256.Create(intP, intQ, intR, 0f, 0f, 0f, 0f, 0f);

            float sP = errP + 8.0f * intP;
            float sQ = errQ + 6.5f * intQ;
            float sR = errR + 9.0f * intR;

            // 3. 리야푸노프 비선형 토크 명령 (SMC Saturation)
            float satP = Math.Clamp(sP / 0.05f, -1.0f, 1.0f);
            float satQ = Math.Clamp(sQ / 0.05f, -1.0f, 1.0f);
            float satR = Math.Clamp(sR / 0.05f, -1.0f, 1.0f);

            float lCmd = -(Ixx * (8.0f * errP + 15.0f * satP)) + (Izz - Iyy) * q * r;
            float mCmd = -(Iyy * (6.5f * errQ + 12.0f * satQ)) + (Ixx - Izz) * p * r;
            float nCmd = -(Izz * (9.0f * errR + 18.0f * satR)) + (Iyy - Ixx) * p * q;

            // 4. 제어면 배분 (Control Allocation): Elevons + Split Drag Rudders
            // 엘레본 대칭=피치, 비대칭=롤, 스플릿 드래그 러더 차동=요
            float qS = state.DynamicPressure * WingArea;
            float qSb = qS * WingSpan;

            float deltaElevonPitch = Math.Clamp(mCmd / (qSb * 0.035f), -0.45f, 0.45f);
            float deltaElevonRoll  = Math.Clamp(lCmd / (qSb * 0.028f), -0.35f, 0.35f);

            // 요잉 모멘트는 오직 스플릿 드래그 러더(SDR)로 생성 (무미익 전익기 핵심)
            float deltaSdr = Math.Clamp(MathF.Abs(nCmd) / (qSb * 0.020f), 0.0f, 0.60f);
            float sdrLeft = (nCmd < 0.0f) ? deltaSdr : 0.0f;   // 좌측 전개 -> 좌회전 요 모멘트
            float sdrRight = (nCmd > 0.0f) ? deltaSdr : 0.0f;  // 우측 전개 -> 우회전 요 모멘트

            // 타면 각도 할당 (4개 엘레본 + 2개 스플릿 드래그 러더)
            float e1 = Math.Clamp(deltaElevonPitch + deltaElevonRoll, -0.50f, 0.50f);
            float e2 = Math.Clamp(deltaElevonPitch + 0.7f * deltaElevonRoll, -0.50f, 0.50f);
            float e3 = Math.Clamp(deltaElevonPitch - 0.7f * deltaElevonRoll, -0.50f, 0.50f);
            float e4 = Math.Clamp(deltaElevonPitch - deltaElevonRoll, -0.50f, 0.50f);

            state.ControlSurfaces = Vector256.Create(e1, e2, e3, e4, sdrLeft, sdrRight, 0f, 0f);

            // 5. 6자유도 각가속도 적분
            float pDot = lCmd / Ixx;
            float qDot = mCmd / Iyy;
            float rDot = nCmd / Izz;

            float newP = p + pDot * dt;
            float newQ = q + qDot * dt;
            float newR = r + rDot * dt;

            state.QuatRate = Vector256.Create(
                state.QuatRate.GetElement(0),
                state.QuatRate.GetElement(1),
                state.QuatRate.GetElement(2),
                state.QuatRate.GetElement(3),
                newP, newQ, newR, 0f
            );
        }
    }
}