AIDT 온디바이스 국방 MUM-T 유무인 복합 함정간 분산형 AESA 레이더 협력 추적 서보 엔진

using System;
using System.Runtime.CompilerServices;

namespace ZeliDesk.Defense.MUMT.DistributedRadar
{
    public readonly struct CooperativeRadarConfig
    {
        public readonly int NodeCount;
        public readonly double SpeedOfLightMps;
        public readonly double ProcessNoiseQ;
        public readonly double MeasurementNoiseR;
        public readonly double FilterGainK;

        public CooperativeRadarConfig(int nodeCount, double speedOfLightMps,
                                      double processNoiseQ, double measurementNoiseR,
                                      double filterGainK)
        {
            NodeCount = nodeCount;
            SpeedOfLightMps = speedOfLightMps;
            ProcessNoiseQ = processNoiseQ;
            MeasurementNoiseR = measurementNoiseR;
            FilterGainK = filterGainK;
        }
    }

    public ref struct DistributedAesaServo
    {
        private readonly CooperativeRadarConfig _config;
        private double _targetRangeMeters;
        private double _targetVelocityMps;
        private double _rangeResidualIntegral;

        public DistributedAesaServo(in CooperativeRadarConfig config)
        {
            _config = config;
            _targetRangeMeters = 50000.0; // 초기 50km
            _targetVelocityMps = -680.0;  // 마하 2 접근 속도
            _rangeResidualIntegral = 0.0;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void ProcessMultistaticRanges(ReadOnlySpan bistaticRangesMeters, Span fusedStateOutput)
        {
            double dt = 1.0e-4; // 10,000Hz
            double sumRange = 0.0;
            int validCount = 0;

            for (int i = 0; i < bistaticRangesMeters.Length; i++)
            {
                double r = bistaticRangesMeters[i];
                if (r > 1000.0)
                {
                    sumRange += r;
                    validCount++;
                }
            }

            if (validCount > 0)
            {
                double avgMeasuredRange = sumRange / validCount;
                double residual = avgMeasuredRange - _targetRangeMeters;
                _rangeResidualIntegral += residual * dt;

                _targetRangeMeters += (_targetVelocityMps * dt) + (_config.FilterGainK * residual);
                _targetVelocityMps += (_config.FilterGainK * 0.5 * residual / dt);

                for (int j = 0; j < fusedStateOutput.Length; j++)
                {
                    fusedStateOutput[j] = _targetRangeMeters;
                }
            }
        }

        public readonly double FusedTargetRange => _targetRangeMeters;
        public readonly double FusedTargetVelocity => _targetVelocityMps;
    }
}