MUM-T 유무인복합 극초음속 비행체 분산 센서 융합 무향 칼만 필터(UKF) 플라즈마 블랙아웃 극복 서보 아키텍처 (.NET 9 C# SIMD)

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

namespace ZeliDesk.Defense.Hypersonic
{
    /// <summary>
    /// 극초음속 비행체 플라즈마 블랙아웃 극복 분산 무향 칼만 필터(UKF) 서보
    /// GC 제로 할당 ref struct 및 AVX-256 SIMD 가속화
    /// </summary>
    public ref struct HypersonicSensorFusionUkfServo
    {
        private readonly ReadOnlySpan<float> _priorStates;      // 사전 상태 추정 x_prior
        private readonly ReadOnlySpan<float> _measurements;     // 통합 센서 측정치 z
        private readonly ReadOnlySpan<float> _predictedMeas;    // 예측 측정치 z_pred
        private readonly Span<float> _posteriorStates;          // 사후 보정 상태 x_post
        private readonly Span<float> _stateResiduals;           // 상태 잔차

        private readonly float _kalmanGain;     // 튜닝된 등가 칼만 게인 K
        private readonly float _maxCorrection;  // 최대 허용 단일 스텝 보정량

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public HypersonicSensorFusionUkfServo(
            ReadOnlySpan<float> priorStates,
            ReadOnlySpan<float> measurements,
            ReadOnlySpan<float> predictedMeas,
            Span<float> posteriorStates,
            Span<float> stateResiduals,
            float kalmanGain = 0.42f,
            float maxCorrection = 15.0f)
        {
            _priorStates = priorStates;
            _measurements = measurements;
            _predictedMeas = predictedMeas;
            _posteriorStates = posteriorStates;
            _stateResiduals = stateResiduals;
            _kalmanGain = kalmanGain;
            _maxCorrection = maxCorrection;
        }

        /// <summary>
        /// 8채널 상태 변수(위치, 속도, 자세각)의 UKF 측정 갱신을 SIMD 병렬 연산
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveOptimization)]
        public void ExecuteMeasurementUpdate()
        {
            int length = _priorStates.Length;
            int simdLength = length - (length % 8);

            Vector256<float> vGain = Vector256.Create(_kalmanGain);
            Vector256<float> vMaxCorr = Vector256.Create(_maxCorrection);
            Vector256<float> vMinCorr = Vector256.Create(-_maxCorrection);

            int i = 0;
            if (Avx2.IsSupported)
            {
                for (; i < simdLength; i += 8)
                {
                    Vector256<float> vPrior = Vector256.Create(MemoryMarshal.Cast<float, Vector256<float>>(_priorStates.Slice(i, 8))[0]);
                    Vector256<float> vZ = Vector256.Create(MemoryMarshal.Cast<float, Vector256<float>>(_measurements.Slice(i, 8))[0]);
                    Vector256<float> vZpred = Vector256.Create(MemoryMarshal.Cast<float, Vector256<float>>(_predictedMeas.Slice(i, 8))[0]);

                    // 잔차: Inn = z - z_pred
                    Vector256<float> vInn = Avx.Subtract(vZ, vZpred);

                    // 보정값: Corr = Gain * Inn
                    Vector256<float> vRawCorr = Avx.Multiply(vGain, vInn);
                    Vector256<float> vClampedCorr = Avx.Min(vMaxCorr, Avx.Max(vMinCorr, vRawCorr));

                    // 사후 상태: Post = Prior + ClampedCorr
                    Vector256<float> vPost = Avx.Add(vPrior, vClampedCorr);

                    vInn.CopyTo(_stateResiduals.Slice(i, 8));
                    vPost.CopyTo(_posteriorStates.Slice(i, 8));
                }
            }

            for (; i < length; i++)
            {
                float inn = _measurements[i] - _predictedMeas[i];
                float corr = Math.Clamp(_kalmanGain * inn, -_maxCorrection, _maxCorrection);
                _stateResiduals[i] = inn;
                _posteriorStates[i] = _priorStates[i] + corr;
            }
        }
    }
}