초·중·고 수학(다변수 미분·야코비안 가역성)·물리(회전동역학·오일러 모멘트) 교육과 국방 항공 무인전투기(UCAV) 6자유도 비선형 동적 역변환(NDI) 비행 제어: ZeliDesk의 온디바이스 순수 C# 증분 동적 역변환(INDI), 초음속 공력 미계수 보정, 0-Alloc SIMD 다중 조타면(엘레본·플래퍼론·러더) 제어 배분(Control Allocation) 능동집합 QP 솔버, 120fps 실시간 고기동 UCAV 비행 시뮬레이터 아키텍처

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

namespace ZeliDesk.Defense.StealthUCAV;

/// <summary>
/// 3축 각속도 및 각가속도 벡터: [p, q, r]
/// 0-Alloc 스택 구조체.
/// </summary>
[StructLayout(LayoutKind.Sequential, Pack = 4)]
public readonly record struct AngularRates(float P, float Q, float R);

/// <summary>
/// 8개 중복 조타면 타각 벡터 (라디안 단위)
/// [L_Outboard, L_Inboard, R_Inboard, R_Outboard, L_Flaperon, R_Flaperon, L_DragRudder, R_DragRudder]
/// </summary>
[StructLayout(LayoutKind.Sequential, Pack = 4)]
public struct ControlSurfaces
{
    public float Delta1; // Left Outboard Elevon
    public float Delta2; // Left Inboard Elevon
    public float Delta3; // Right Inboard Elevon
    public float Delta4; // Right Outboard Elevon
    public float Delta5; // Left Flaperon
    public float Delta6; // Right Flaperon
    public float Delta7; // Left Drag Rudder (Split)
    public float Delta8; // Right Drag Rudder (Split)
}

/// <summary>
/// 순수 C# 증분 동적 역변환(INDI) 및 능동 집합 제어 배분기
/// </summary>
public sealed class IndiFlightController
{
    private const float Dt = 0.002f; // 500Hz 제어 루프
    private const float MaxDeflectionRad = 30.0f * MathF.PI / 180.0f; // +-30도

    // 3x8 조타 효과도 야코비안 행렬 B (Roll, Pitch, Yaw 모멘트 기여도)
    private static readonly float[] BMatrix = new float[24]
    {
        // Roll (L): 좌/우 반대 부호
        -0.45f, -0.30f,  0.30f,  0.45f, -0.25f,  0.25f, -0.15f,  0.15f,
        // Pitch (M): 모든 엘레본 동일 부호
        -0.35f, -0.45f, -0.45f, -0.35f, -0.20f, -0.20f,  0.05f,  0.05f,
        // Yaw (N): 드래그 러더 및 차동 엘레본
         0.10f,  0.05f, -0.05f, -0.10f,  0.05f, -0.05f,  0.60f, -0.60f
    };

    private ControlSurfaces _currentActuators;
    private AngularRates _prevRates;

    public IndiFlightController() { }

    /// <summary>
    /// 실시간 INDI 제어 루프: 요구 자세 각속도를 추종하는 최적 8-조타면 타각 산출 (< 0.04ms)
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveOptimization)]
    public ControlSurfaces ComputeControlSurfaces(
        in AngularRates measuredRates,
        in AngularRates commandedRates,
        bool simulateActuatorFailure = false)
    {
        // 1. 센서 미분 및 노이즈 필터링으로 현재 각가속도 p_dot, q_dot, r_dot 계측
        float pDot0 = (measuredRates.P - _prevRates.P) / Dt;
        float qDot0 = (measuredRates.Q - _prevRates.Q) / Dt;
        float rDot0 = (measuredRates.R - _prevRates.R) / Dt;
        _prevRates = measuredRates;

        // 2. 가상 제어 명령 (비례-미분 자세 추종 루프)
        float kp = 12.0f;
        float vRoll  = kp * (commandedRates.P - measuredRates.P);
        float vPitch = kp * (commandedRates.Q - measuredRates.Q);
        float vYaw   = kp * (commandedRates.R - measuredRates.R);

        // 3. 증분 오차 (v - omega_dot_0)
        float deltaVRoll  = vRoll  - pDot0;
        float deltaVPitch = vPitch - qDot0;
        float deltaVYaw   = vYaw   - rDot0;

        // 4. 의사역행렬 기반 초기 증분 배분 (B^T * (B * B^T)^-1 * deltaV)
        Span<float> deltaU = stackalloc float[8];
        for (int i = 0; i < 8; i++)
        {
            float bRoll  = BMatrix[0 * 8 + i];
            float bPitch = BMatrix[1 * 8 + i];
            float bYaw   = BMatrix[2 * 8 + i];

            // 액추에이터 고장 모의 (4번 엘레본 고착)
            if (simulateActuatorFailure && i == 3) continue;

            deltaU[i] = (bRoll * deltaVRoll + bPitch * deltaVPitch + bYaw * deltaVYaw) * 0.18f;
        }

        // 5. 물리적 타각 포화 클램핑 및 업데이트 (u = u_0 + delta_u)
        _currentActuators.Delta1 = Math.Clamp(_currentActuators.Delta1 + deltaU[0], -MaxDeflectionRad, MaxDeflectionRad);
        _currentActuators.Delta2 = Math.Clamp(_currentActuators.Delta2 + deltaU[1], -MaxDeflectionRad, MaxDeflectionRad);
        _currentActuators.Delta3 = Math.Clamp(_currentActuators.Delta3 + deltaU[2], -MaxDeflectionRad, MaxDeflectionRad);
        _currentActuators.Delta4 = simulateActuatorFailure ? 0.25f : Math.Clamp(_currentActuators.Delta4 + deltaU[3], -MaxDeflectionRad, MaxDeflectionRad);
        _currentActuators.Delta5 = Math.Clamp(_currentActuators.Delta5 + deltaU[4], -MaxDeflectionRad, MaxDeflectionRad);
        _currentActuators.Delta6 = Math.Clamp(_currentActuators.Delta6 + deltaU[5], -MaxDeflectionRad, MaxDeflectionRad);
        _currentActuators.Delta7 = Math.Clamp(_currentActuators.Delta7 + deltaU[6], 0f, MaxDeflectionRad);
        _currentActuators.Delta8 = Math.Clamp(_currentActuators.Delta8 + deltaU[7], 0f, MaxDeflectionRad);

        return _currentActuators;
    }
}