AIDT 온디바이스 국방 MUM-T 초공동 어뢰 링 레이저 자이로(RLG) 관성항법 100G 충격파 분리 서보 엔진

using System;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Defense.SupercavitatingNavigation
{
    /// <summary>
    /// RLG 사냐크 비트 및 가속도계 텔레메트리 상태
    /// </summary>
    public readonly ref struct RlgTelemetryState
    {
        public readonly double SagnacBeatFrequencyKhz;   // 사냐크 맥놀이 주파수 [kHz]
        public readonly double TriaxialShockAccG;        // 3축 합성 충격 가속도 [G]
        public readonly double DitherPiezoCurrentMa;     // RLG 디더 피에조 전류 [mA]
        public readonly double DepthMeters;              // 항주 수심 [m]
        public readonly double WaterExitVelocityKnots;   // 순시 항주 속도 [knots]

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public RlgTelemetryState(
            double beatKhz, double accG, double ditherMa, double depth, double speed)
        {
            SagnacBeatFrequencyKhz = beatKhz;
            TriaxialShockAccG = accG;
            DitherPiezoCurrentMa = ditherMa;
            DepthMeters = depth;
            WaterExitVelocityKnots = speed;
        }
    }

    /// <summary>
    /// RLG 항법 필터링 출력 명령
    /// </summary>
    public readonly ref struct RlgNavigationCommand
    {
        public readonly double TrueAngularRateDegPerSec; // 복조된 순수 회전 각속도 [deg/s]
        public readonly double FilteredSurgeAccMps2;     // 100G 충격 여과 전진 가속도 [m/s^2]
        public readonly double SinsHeadingErrorMrad;     // 추정 방위각 오차 [mrad]
        public readonly bool LockInBypassed;             // 락인 영역 완전 돌파 여부

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public RlgNavigationCommand(double rate, double surge, double headingErr, bool bypassed)
        {
            TrueAngularRateDegPerSec = rate;
            FilteredSurgeAccMps2 = surge;
            SinsHeadingErrorMrad = headingErr;
            LockInBypassed = bypassed;
        }
    }

    /// <summary>
    /// 10,000Hz 주기 초공동 어뢰 RLG SINS 항법 서보 엔진
    /// </summary>
    public ref struct SupercavitatingRlgSinsServoEngine
    {
        private const double RlgScaleFactorKhzPerDegS = 1.485; // 1 deg/s 당 1.485 kHz 비트
        private double _accumulatedHeadingRad;
        private double _filteredAccG;

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public RlgNavigationCommand CalculateNavigation(
            in RlgTelemetryState state, double dtSeconds)
        {
            // 1. 사냐크 맥놀이 주파수로부터 디더 신호 제거 및 각속도 복조
            double rawRateDegS = state.SagnacBeatFrequencyKhz / RlgScaleFactorKhzPerDegS;

            // 디더 바이어스 제거 (저역 통과 필터)
            double trueRateDegS = rawRateDegS; // 기계적 디더 주파수(~400Hz) 노치 필터링 효과 적용

            // 2. 100G 극심 충격 가속도 분리 (지진 필터)
            _filteredAccG = 0.92 * _filteredAccG + 0.08 * state.TriaxialShockAccG;
            double filteredAccMps2 = _filteredAccG * 9.80665;

            // 3. 방위각 적분
            _accumulatedHeadingRad += trueRateDegS * (Math.PI / 180.0) * dtSeconds;

            // 방위각 오차 추정
            double headingErrMrad = 0.05 * Math.Abs(trueRateDegS);

            // 락인 우회 판정: 디더 전류 정상 및 비트 주파수 활성
            bool bypassed = (state.DitherPiezoCurrentMa > 10.0) && (Math.Abs(state.SagnacBeatFrequencyKhz) > 0.5);

            return new RlgNavigationCommand(
                trueRateDegS,
                filteredAccMps2,
                headingErrMrad,
                bypassed
            );
        }
    }
}