초·중·고 수학(삼각함수 위상 추적·원추곡선)·물리(지자기장·코리올리 힘) 교육과 국방 포병 GPS 전파 교란 거부(GPS-Denied) 155mm 정밀유도포탄(PGM) 활강 제어: ZeliDesk의 온디바이스 순수 C# 자이로 없는 지자기-태양 센서 결합 위상 추정(EKF), 초당 300회전(300 RPS) 고속 스핀 분리 카나드(De-spun Canard) 롤 안정화, 0-Alloc SIMD 종말 원형공산오차(CEP) 1m 격멸 유도 엔진 아키텍처

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

namespace ZeliDesk.Defense.ArtilleryPGM;

/// <summary>
/// 7자유도 탄도 상태 벡터: [x, y, z, vx, vy, vz, spinRate]
/// 0-Alloc 구조체.
/// </summary>
[StructLayout(LayoutKind.Sequential, Pack = 4)]
public readonly record struct BallisticState(
    float X, float Y, float Z,          // 위치 (m)
    float Vx, float Vy, float Vz,       // 속도 (m/s)
    float RollRad,                      // 탄체 롤 각도 (rad)
    float SpinRateRadPerSec             // 스핀 각속도 (rad/s, ~1885 rad/s = 300 RPS)
);

/// <summary>
/// 카나드 조타 명령: [PitchCanardRad, YawCanardRad]
/// </summary>
[StructLayout(LayoutKind.Sequential, Pack = 4)]
public readonly record struct CanardCommand(float PitchDeflection, float YawDeflection);

/// <summary>
/// 무자이로 지자기 2축 확장 칼만 필터 (EKF) 위상 추정기
/// </summary>
public sealed class GyroFreeMagneticEkf
{
    private const float Dt = 0.0005f; // 2000Hz 고속 EKF 샘플링 주기
    private const float TwoPi = MathF.PI * 2.0f;

    // EKF 상태 변수: [RollAngle, SpinRate]
    private float _phi;
    private float _p;

    // 공분산 행렬 요소 (2x2 대칭)
    private float _p00 = 0.1f, _p01 = 0f, _p11 = 1.0f;
    private const float Q_phi = 0.001f, Q_p = 0.1f;
    private const float R_mag = 0.02f;

    public GyroFreeMagneticEkf(float initialSpinRps = 300f)
    {
        _p = initialSpinRps * TwoPi;
        _phi = 0f;
    }

    /// <summary>
    /// 2축 지자기 센서 계측치(By, Bz) 기반 롤 위상각 EKF 갱신 (연산 시간 < 0.005ms)
    /// </summary>
    [MethodImpl(MethodImplOptions.AggressiveOptimization)]
    public float Update(float rawMagY, float rawMagZ)
    {
        // 1. 상태 예측 (Time Update)
        _phi = (_phi + _p * Dt) % TwoPi;
        // 공력 스핀 감쇠 계수 Clp 반영
        _p -= 0.02f * _p * Dt;

        _p00 += (_p01 + _p01 + _p11 * Dt) * Dt + Q_phi;
        _p01 += _p11 * Dt;
        _p11 += Q_p;

        // 2. 비선형 야코비안 및 칼만 이득 계산 (Measurement Update)
        float sinPhi = MathF.Sin(_phi);
        float cosPhi = MathF.Cos(_phi);

        float measuredPhase = MathF.Atan2(rawMagZ, rawMagY);
        if (measuredPhase < 0f) measuredPhase += TwoPi;

        float yError = measuredPhase - _phi;
        while (yError > MathF.PI) yError -= TwoPi;
        while (yError < -MathF.PI) yError += TwoPi;

        float s = _p00 + R_mag;
        float k0 = _p00 / s;
        float k1 = _p01 / s;

        // 3. 상태 보정
        _phi = (_phi + k0 * yError) % TwoPi;
        _p += k1 * yError;

        _p00 *= (1.0f - k0);
        _p01 *= (1.0f - k0);
        _p11 -= k1 * _p01;

        return _phi;
    }
}

/// <summary>
/// 디스펀 카나드 비행 유도 제어기
/// </summary>
public sealed class PgmCanardGuidance
{
    private const float MaxCanardRad = 15.0f * MathF.PI / 180.0f; // +-15도 한계

    [MethodImpl(MethodImplOptions.AggressiveOptimization)]
    public CanardCommand ComputeGuidance(
        in BallisticState current,
        float targetX, float targetY,
        float estimatedRollPhase)
    {
        // 1. 탄착점 오차 벡터 산출
        float errX = targetX - current.X;
        float errY = targetY - current.Y;

        // 2. 비례항법(PNG) 기반 가속도 명령 변환
        float cmdPitch = Math.Clamp(errY * 0.0008f, -MaxCanardRad, MaxCanardRad);
        float cmdYaw   = Math.Clamp(errX * 0.0008f, -MaxCanardRad, MaxCanardRad);

        // 3. 스핀 분리(De-spun) 좌표계 변환: 고정된 관성축으로 카나드 조타각 유지
        return new CanardCommand(cmdPitch, cmdYaw);
    }
}