우주영역인식(SDA) 지상 기지 원자 간섭계 양자 중력계 섭동 보정 서보 C# SIMD 엔진

3. Zero-Allocation C# SIMD 양자 중력계 지진 격리 서보 엔진

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

namespace ZeliDesk.Defense.Sda.Gravimeter
{
    public readonly ref struct GravimeterServoState
    {
        public readonly float MeasuredGravityUgal;
        public readonly float SeismicVibrationRmsNm;
        public readonly float PiezoDriveVoltage;
        public readonly bool MicroGalPrecisionLocked;

        public GravimeterServoState(float grav, float vib, float volt, bool locked)
        {
            MeasuredGravityUgal = grav;
            SeismicVibrationRmsNm = vib;
            PiezoDriveVoltage = volt;
            MicroGalPrecisionLocked = locked;
        }
    }

    public sealed class QuantumGravimeterServoEngine
    {
        private const float SampleRateHz = 20000.0f;
        private const float Dt = 1.0f / SampleRateHz;

        // 표준 중력 가속도 및 환산 계수 (1 uGal = 1.0e-8 m/s^2)
        private const float NominalG = 9.80665f;
        private const float RamanKeff = 1.610566e7f; // rad/m (780nm Rb D2 라만 천이)
        private const float TargetMaxNoiseUgal = 1.5f;

        // 지진계 이중 적분기 상태
        private float _velocityMps = 0.0f;
        private float _displacementMeters = 0.0f;
        private float _phaseIntegral = 0.0f;
        private const float KpSeismo = 1.45e5f;
        private const float KiPhase = 25.0f;

        [MethodImpl(MethodImplOptions.AggressiveInlining | MethodImplOptions.AggressiveOptimization)]
        public GravimeterServoState CompensateSeismicNoise(
            float seismoZAccelMps2,
            float rawInterferometerPhaseRad,
            float pulseSeparationTimeMs)
        {
            // 1. 고주파 진동 가속도 실시간 이중 적분으로 변위(nm) 도출
            _velocityMps += seismoZAccelMps2 * Dt;
            _velocityMps *= 0.9995f; // 드리프트 방지 고역통과 필터
            _displacementMeters += _velocityMps * Dt;
            _displacementMeters *= 0.9995f;

            float displacementNm = _displacementMeters * 1.0e9f;

            // 2. 간섭계 위상 오차 및 중력값(uGal 편차) 계산
            float pulseTimeT = pulseSeparationTimeMs * 1.0e-3f;
            float scaleFactor = RamanKeff * (pulseTimeT * pulseTimeT);
            float deltaG = (rawInterferometerPhaseRad / MathF.Max(scaleFactor, 1.0e-6f));
            float gravityAnomalyUgal = deltaG * 1.0e8f;

            // 3. 반사 거울 피에조 능동 상쇄 전압 산출 (피드포워드 + PI 보정)
            _phaseIntegral += rawInterferometerPhaseRad * Dt;
            _phaseIntegral = Math.Clamp(_phaseIntegral, -0.2f, 0.2f);

            float pztVoltage = (-_displacementMeters * KpSeismo) - (KiPhase * _phaseIntegral);
            float finalPztVoltage = Math.Clamp(50.0f + pztVoltage, 0.0f, 100.0f);

            // 마이크로갈 정밀도 락 판정 (변위 RMS 0.5nm 이하 & 중력 노이즈 1.5 uGal 이내)
            bool isLocked = MathF.Abs(displacementNm) < 0.5f && MathF.Abs(gravityAnomalyUgal) <= TargetMaxNoiseUgal;

            return new GravimeterServoState(
                grav: gravityAnomalyUgal,
                vib: MathF.Abs(displacementNm),
                volt: finalPztVoltage,
                locked: isLocked
            );
        }
    }
}