전술 군집 드론 초광대역(UWB) 양방향 거리측정 기반 상대 위치 메쉬 네트워크 서보 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.Swarm.UwbPositioning
{
    public readonly ref struct SwarmPositionState
    {
        public readonly float RelativeDistanceMeters;
        public readonly float RangeResidualMeters;
        public readonly float CollisionAvoidanceThrustN;
        public readonly bool FormationSafe;

        public SwarmPositionState(float dist, float residual, float thrust, bool safe)
        {
            RelativeDistanceMeters = dist;
            RangeResidualMeters = residual;
            CollisionAvoidanceThrustN = thrust;
            FormationSafe = safe;
        }
    }

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

        // 목표 안전 거리 (최소 5.0m 유지, 8.0m 편대 간격)
        private const float MinSafeDistanceMeters = 5.0f;
        private const float TargetFormationDistance = 8.0f;
        private const float SpeedOfLightMps = 299792458.0f;

        // 상대 위치 칼만 필터 상태
        private float _filteredDistance = TargetFormationDistance;
        private float _prevResidual = 0.0f;
        private const float KpDist = 4.5f;
        private const float KdDist = 0.06f;

        [MethodImpl(MethodImplOptions.AggressiveInlining | MethodImplOptions.AggressiveOptimization)]
        public SwarmPositionState UpdateRelativeRanging(
            float tRound1Ns, float tReply1Ns,
            float tRound2Ns, float tReply2Ns,
            float relativeImuAccelMps2)
        {
            // 1. ADS-TWR TOF 연산 (나노초 단위 -> 초 환산)
            float num = (tRound1Ns * tRound2Ns) - (tReply1Ns * tReply2Ns);
            float den = tRound1Ns + tRound2Ns + tReply1Ns + tReply2Ns;
            float tofSeconds = (den > 1.0e-6f) ? (num / den) * 1.0e-9f : 0.0f;

            // 계측된 상대 거리: d = c * TOF
            float rawDistanceMeters = MathF.Max(0.1f, tofSeconds * SpeedOfLightMps);

            // 2. 관성 가속도계 보조 칼만 예측 및 잔차 계산
            float predictedDistance = _filteredDistance + relativeImuAccelMps2 * (0.5f * Dt * Dt);
            float residual = rawDistanceMeters - predictedDistance;

            // 칼만 이득 적용 갱신 (K ~ 0.15)
            _filteredDistance = predictedDistance + (0.15f * residual);

            // 3. 편대 거리 유지 및 충돌 회피 추력 계산 (PD 제어)
            float distanceError = TargetFormationDistance - _filteredDistance;
            float distanceDerivative = (residual - _prevResidual) * SampleRateHz;
            _prevResidual = residual;

            float thrustCorrectionN = (KpDist * distanceError) - (KdDist * distanceDerivative);

            // 근접 충돌 위험 시 지수 비례 반발력 가산
            if (_filteredDistance < MinSafeDistanceMeters)
            {
                float intrusion = MinSafeDistanceMeters - _filteredDistance;
                thrustCorrectionN -= MathF.Exp(intrusion * 1.5f) * 8.0f;
            }

            bool isSafe = _filteredDistance >= MinSafeDistanceMeters;

            return new SwarmPositionState(
                dist: _filteredDistance,
                residual: residual,
                thrust: thrustCorrectionN,
                safe: isSafe
            );
        }
    }
}