심해 무인잠수정(AUV) 합성개구소나(SAS) 센티미터급 해저 영상 위상 오차 운동 보상 서보 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.Auv.SasMotion
{
    public readonly ref struct SasMotionState
    {
        public readonly float PhaseErrorRad;
        public readonly float CrossTrackDisplacementMm;
        public readonly float FinCorrectionAngleDeg;
        public readonly bool CoherencePreserved;

        public SasMotionState(float phase, float disp, float fin, bool coherence)
        {
            PhaseErrorRad = phase;
            CrossTrackDisplacementMm = disp;
            FinCorrectionAngleDeg = fin;
            CoherencePreserved = coherence;
        }
    }

    public sealed class DeepSeaSasMotionCompensationServoEngine
    {
        private const float SampleRateHz = 10000.0f;
        private const float Dt = 1.0f / SampleRateHz;

        // 음향 및 파수 상수 (중심 주파수 150 kHz, 음속 1,500 m/s)
        private const float AcousticWavelengthMeters = 0.01f; // 1 cm
        private const float WaveNumberK0 = (2.0f * MathF.PI) / AcousticWavelengthMeters;

        // PI 제어 상태
        private float _phaseIntegral = 0.0f;
        private float _prevPhaseError = 0.0f;
        private const float KpFin = 0.65f;
        private const float KiFin = 15.0f;
        private const float KdFin = 0.003f;

        [MethodImpl(MethodImplOptions.AggressiveInlining | MethodImplOptions.AggressiveOptimization)]
        public SasMotionState CompensateMotionCycle(
            float corrReal, float corrImag,
            float inertialSwayAccelMps2,
            float lookAngleDeg)
        {
            // 1. DPCA 복소 상관 신호로부터 위상 오차 추출: phase = atan2(Q, I)
            float phaseError = MathF.Atan2(corrImag, corrReal);

            // 2. 위상 오차로부터 크로스 트랙 변위(mm) 역산: Delta_r = Delta_phi / (2 * k0)
            float lookRad = lookAngleDeg * (MathF.PI / 180.0f);
            float deltaRMeters = phaseError / (2.0f * WaveNumberK0);
            float crossTrackMm = (deltaRMeters / MathF.Max(MathF.Sin(lookRad), 0.1f)) * 1000.0f;

            // 3. 관성 가속도계 데이터와 긴밀 결합한 PID 제어
            _phaseIntegral += phaseError * Dt;
            _phaseIntegral = Math.Clamp(_phaseIntegral, -0.5f, 0.5f);

            float phaseDerivative = (phaseError - _prevPhaseError) * SampleRateHz;
            _prevPhaseError = phaseError;

            // 관성 피드포워드 가산
            float finAngleDemand = (KpFin * phaseError) + (KiFin * _phaseIntegral) + (KdFin * phaseDerivative) + (inertialSwayAccelMps2 * 0.12f);
            float finalFinAngleDeg = Math.Clamp(finAngleDemand, -12.0f, 12.0f);

            // 결맞음(Coherence) 보존 여부 판정 (위상 오차 0.2 rad 이내)
            bool isCoherent = MathF.Abs(phaseError) <= 0.2f;

            return new SasMotionState(
                phase: phaseError,
                disp: crossTrackMm,
                fin: finalFinAngleDeg,
                coherence: isCoherent
            );
        }
    }
}