From 5f76e5b067ba93d3705cb4137499947c71d9de75 Mon Sep 17 00:00:00 2001 From: Tian jianyong <11429339@qq.com> Date: Thu, 22 May 2025 21:20:31 +0800 Subject: [PATCH] =?UTF-8?q?=E9=87=87=E7=94=A8=E6=BB=91=E5=8A=A8=E7=AA=97?= =?UTF-8?q?=E5=8F=A3=E5=92=8C=E9=A2=84=E7=83=AD=E5=B8=A7=E6=9C=BA=E5=88=B6?= =?UTF-8?q?=E5=AE=8C=E5=96=84=E4=BA=86=E6=AF=AB=E7=B1=B3=E6=B3=A2=E5=AF=BC?= =?UTF-8?q?=E5=BC=95=E7=B3=BB=E7=BB=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../data/missiles/composite/cg_001.toml | 18 +- .../data/missiles/ir_imaging/itg_001.toml | 14 +- ThreatSource/data/missiles/mmw/mmw_001.toml | 14 +- .../Guidance/InfraredImagingGuidanceSystem.cs | 503 ++++++++---------- .../Guidance/MillimeterWaveGuidanceSystem.cs | 391 +++++++------- .../src/MIssile/CompositeGuidedMissile.cs | 2 +- .../InfraredImagingTerminalGuidedMissile.cs | 2 +- .../MillimeterWaveTerminalGuidedMissile.cs | 2 +- 8 files changed, 449 insertions(+), 497 deletions(-) diff --git a/ThreatSource/data/missiles/composite/cg_001.toml b/ThreatSource/data/missiles/composite/cg_001.toml index 87227bb..13f5475 100644 --- a/ThreatSource/data/missiles/composite/cg_001.toml +++ b/ThreatSource/data/missiles/composite/cg_001.toml @@ -14,9 +14,9 @@ MaxAcceleration = 100.0 ProportionalNavigationCoefficient = 4.0 LaunchAcceleration = 100.0 MaxEngineBurnTime = 2.5 -CruiseTime = 5.0 +CruiseTime = 4.0 Mass = 25.0 -ExplosionRadius = 5.5 +ExplosionRadius = 5.0 HitProbability = 0.9 SelfDestructHeight = 0.0 InfraredRadiationIntensity = 96.0 # 红外辐射强度 (瓦特/球面度) @@ -69,31 +69,23 @@ FieldOfViewAngle = 45.0 # 视场角 (度) TargetRecognitionProbability = 0.95 # 目标识别概率 WaveFrequency = 9.4e10 # 波频率 (赫兹) PulseDuration = 1.0e-6 # 脉冲持续时间 (秒) - SearchBeamWidth = 5.0 # 搜索波束宽度 (度) -TrackBeamWidth = 3.0 # 跟踪波束宽度 (度) -LockBeamWidth = 2.0 # 锁定波束宽度 (度) - +TrackBeamWidth = 4.0 # 跟踪波束宽度 (度) +LockBeamWidth = 3.0 # 锁定波束宽度 (度) ScanAngularSpeedDeg = 360.0 # 扫描角速度 (度/秒) ScanRadiusGrowthRateDeg = 22.5 # 扫描半径增长率 (度) - RecognitionSNRThreshold = -25.0 # 识别信噪比阈值 (分贝) LockSNRThreshold = -10.0 # 锁定信噪比阈值 (分贝) - TargetLostTolerance = 0.2 # 目标丢失容忍时间 (秒) -LockConfirmationTime = 0.3 # 锁定确认时间 (秒) - +LockConfirmationTime = 0.3 # 锁定确认时间 (秒) PulseRepetitionFrequency = 1.0e-4 # 脉冲重复间隔 (秒) TransmitPower = 0.3 # 发射功率 (瓦特) - DopplerVelocityResolution = 1.0 # 多普勒速度分辨率 (米/秒) MaxMeasurableVelocity = 1000.0 # 最大可测量速度 (米/秒) - AntennaGainDB = 23.0 # 天线增益 (分贝) NoiseFigureDB = 7.0 # 噪声系数 (分贝) SystemLossDB = 6.0 # 系统损耗 (分贝) MonopulseSensitivity = 1.0 # 单脉冲灵敏度 - YawControlEffectiveness = 120.0 # 偏航控制有效性 (度/秒^2 或类似单位) PitchControlEffectiveness = 150.0 # 俯仰控制有效性 (度/秒^2 或类似单位) JammingResistanceThreshold = 1.0e-5 # 干扰抗性阈值 (瓦特) \ No newline at end of file diff --git a/ThreatSource/data/missiles/ir_imaging/itg_001.toml b/ThreatSource/data/missiles/ir_imaging/itg_001.toml index bab81e3..009cc69 100644 --- a/ThreatSource/data/missiles/ir_imaging/itg_001.toml +++ b/ThreatSource/data/missiles/ir_imaging/itg_001.toml @@ -14,24 +14,24 @@ MaxAcceleration = 100.0 ProportionalNavigationCoefficient = 4.0 LaunchAcceleration = 100.0 MaxEngineBurnTime = 2.5 -CruiseTime = 5.0 +CruiseTime = 4.0 Mass = 25.0 -ExplosionRadius = 5.5 +ExplosionRadius = 5.0 HitProbability = 0.9 SelfDestructHeight = 0.0 InfraredRadiationIntensity = 96.0 # 红外辐射强度 (瓦特/球面度) UltravioletRadiationIntensity = 100.0 # 紫外辐射强度 (瓦特/球面度) [InfraredImagingGuidanceConfig] -MaxDetectionRange = 1000.0 # 最大探测距离 (米), JSON中为1000，对应C#默认为1000.0 -SearchFieldOfView = 12.0 # 搜索视场角 (度), 对应C#默认为12.0度 -TrackFieldOfView = 6.0 # 跟踪视场角 (度), 对应C#默认为3.0度 +MaxDetectionRange = 1000.0 # 最大探测距离 (米), 默认为1000.0 +SearchFieldOfView = 12.0 # 搜索视场角 (度), 默认为12.0度 +TrackFieldOfView = 6.0 # 跟踪视场角 (度), 默认为3.0度 ImageWidth = 640 # 图像宽度 (像素) ImageHeight = 480 # 图像高度 (像素) -BackgroundIntensity = 1.0e-4 # 背景辐射强度 (瓦特/球面度), JSON中为1e-4, C#默认为0.01 +BackgroundIntensity = 1.0e-4 # 背景辐射强度 (瓦特/球面度), 默认为1e-4 SearchRecognitionProbability = 0.6 # 搜索模式目标识别概率阈值 TrackRecognitionProbability = 0.8 # 跟踪模式目标识别概率阈值 TargetLostTolerance = 0.3 # 目标丢失容忍时间 (秒) LockConfirmationTime = 0.5 # 锁定确认时间 (秒) JammingResistanceThreshold = 1.0e-5 # 干扰抗性阈值 (瓦特) -Wavelength = 3.0 # 波长 (微米), C#中属性名为Wavelength, JSON中为waveLength \ No newline at end of file +Wavelength = 3.0 # 波长 (微米) \ No newline at end of file diff --git a/ThreatSource/data/missiles/mmw/mmw_001.toml b/ThreatSource/data/missiles/mmw/mmw_001.toml index ff00ce9..f2352f3 100644 --- a/ThreatSource/data/missiles/mmw/mmw_001.toml +++ b/ThreatSource/data/missiles/mmw/mmw_001.toml @@ -28,31 +28,23 @@ FieldOfViewAngle = 45.0 # 视场角 (度) TargetRecognitionProbability = 0.95 # 目标识别概率 WaveFrequency = 9.4e10 # 波频率 (赫兹, JSON中为94e9) PulseDuration = 1.0e-6 # 脉冲持续时间 (秒) - SearchBeamWidth = 5.0 # 搜索波束宽度 (度) -TrackBeamWidth = 3.0 # 跟踪波束宽度 (度) -LockBeamWidth = 2.0 # 锁定波束宽度 (度) - +TrackBeamWidth = 4.0 # 跟踪波束宽度 (度) +LockBeamWidth = 3.0 # 锁定波束宽度 (度) ScanAngularSpeedDeg = 360.0 # 扫描角速度 (度/秒) ScanRadiusGrowthRateDeg = 22.5 # 扫描半径增长率 (度) - RecognitionSNRThreshold = -25.0 # 识别信噪比阈值 (分贝) LockSNRThreshold = -10.0 # 锁定信噪比阈值 (分贝) - TargetLostTolerance = 0.2 # 目标丢失容忍时间 (秒) LockConfirmationTime = 0.3 # 锁定确认时间 (秒) - -PulseRepetitionFrequency = 1.0e-4 # 脉冲重复频率 (秒, JSON中为1e-4，通常PRT单位是秒，PRF是Hz。这里JSON的注释可能不准确，按数值和C#模型属性名推断，这里应为PulseRepetitionTime，即脉冲重复间隔) +PulseRepetitionFrequency = 1.0e-4 # 脉冲重复频率 (秒) TransmitPower = 0.3 # 发射功率 (瓦特) - DopplerVelocityResolution = 1.0 # 多普勒速度分辨率 (米/秒) MaxMeasurableVelocity = 1000.0 # 最大可测量速度 (米/秒) - AntennaGainDB = 23.0 # 天线增益 (分贝) NoiseFigureDB = 7.0 # 噪声系数 (分贝) SystemLossDB = 6.0 # 系统损耗 (分贝) MonopulseSensitivity = 1.0 # 单脉冲灵敏度 (单位取决于具体实现，JSON中为1) - YawControlEffectiveness = 120.0 # 偏航控制有效性 (度/秒^2 或类似单位) PitchControlEffectiveness = 150.0 # 俯仰控制有效性 (度/秒^2 或类似单位) JammingResistanceThreshold = 1.0e-5 # 干扰抗性阈值 (瓦特) \ No newline at end of file diff --git a/ThreatSource/src/Guidance/InfraredImagingGuidanceSystem.cs b/ThreatSource/src/Guidance/InfraredImagingGuidanceSystem.cs index b4ce3b6..6f54906 100644 --- a/ThreatSource/src/Guidance/InfraredImagingGuidanceSystem.cs +++ b/ThreatSource/src/Guidance/InfraredImagingGuidanceSystem.cs @@ -95,17 +95,17 @@ namespace ThreatSource.Guidance /// /// 当前正在跟踪或已锁定的目标ID /// - private string? _currentlyTrackedTargetId; + private string? currentlyTrackedTargetId; /// /// Search模式下当前关注的候选目标ID /// - private string? _searchModeFocusCandidateId; + private string? searchModeFocusCandidateId; /// /// 当前活动模式 (搜索焦点或跟踪稳定) 的识别历史队列 /// - private Queue _activeRecognitionHistory = new(); + private readonly Queue activeRecognitionHistory = new(); /// /// 识别历史窗口大小 (搜索和跟踪阶段共用) @@ -120,7 +120,7 @@ namespace ThreatSource.Guidance /// /// 锁定模式下目标丢失计时器 /// - private double _lockModeTargetLostTimer = 0; + private double lockModeTargetLostTimer = 0; /// /// 初始化红外成像制导系统的新实例 @@ -222,7 +222,7 @@ namespace ThreatSource.Guidance public override void Deactivate() { base.Deactivate(); - LastTargetPosition = null; // Add reset to null + LastTargetPosition = null; HasTarget = false; HasGuidance = false; GuidanceAcceleration = Vector3D.Zero; @@ -241,91 +241,78 @@ namespace ThreatSource.Guidance /// public override void Update(double deltaTime) { + bool tryDetectSucceeded = false; + base.Update(deltaTime); - bool targetFoundAndIdentifiedThisFrame = false; - Vector3D currentTargetPosition = Vector3D.Zero; // 初始化 if (IsBlockingJammed) { - targetFoundAndIdentifiedThisFrame = false; Debug.WriteLine($"[IR IMAGING] 系统被阻塞干扰，无法探测目标。当前模式: {currentMode}"); } else { - if (TryDetectAndIdentifyTarget(KState.Position, KState.Velocity, deltaTime, out currentTargetPosition)) + if (TryDetectAndIdentifyTarget(KState.Position, KState.Velocity, deltaTime, out Vector3D detectedTargetPosition)) { - targetFoundAndIdentifiedThisFrame = true; - // 目标成功探测/跟踪/锁定 + tryDetectSucceeded = true; + Vector3D? currentTargetVelocity = null; - if(LastTargetPosition != null && deltaTime > 0) - { - currentTargetVelocity = (currentTargetPosition - LastTargetPosition.Value) / deltaTime; // 使用 .Value + if (LastTargetPosition != null && deltaTime > 0) { + currentTargetVelocity = (detectedTargetPosition - LastTargetPosition.Value) / deltaTime; } - - LastTargetPosition = currentTargetPosition; + LastTargetPosition = detectedTargetPosition; GuidanceAcceleration = MotionAlgorithm.CalculateProportionalNavigation( - ProportionalNavigationCoefficient, - KState.Position, - KState.Velocity, - currentTargetPosition, - currentTargetVelocity ?? Vector3D.Zero + ProportionalNavigationCoefficient, KState.Position, KState.Velocity, + detectedTargetPosition, currentTargetVelocity ?? Vector3D.Zero ); - - if (GuidanceAcceleration.Magnitude() > MaxAcceleration) - { + + if (GuidanceAcceleration.Magnitude() > MaxAcceleration) { GuidanceAcceleration = GuidanceAcceleration.Normalize() * MaxAcceleration; } HasGuidance = true; - if (currentMode == WorkMode.Lock) - { - _lockModeTargetLostTimer = 0; // 目标在锁定模式下重新确认，重置计时器 + if (currentMode == WorkMode.Lock) { + lockModeTargetLostTimer = 0; } } - else // TryDetectAndIdentifyTarget 返回 false (目标未找到/不稳定) + else { - targetFoundAndIdentifiedThisFrame = false; - // 不更新 LastTargetPosition Debug.WriteLine($"[IR IMAGING] TryDetectAndIdentifyTarget 返回 false。当前模式: {currentMode}"); } } - // 处理目标未识别/丢失的后果 - if (!targetFoundAndIdentifiedThisFrame) + if (!tryDetectSucceeded) { GuidanceAcceleration = Vector3D.Zero; HasGuidance = false; - // 模式切换逻辑 - if (currentMode == WorkMode.Track) - { + if (currentMode == WorkMode.Track) { Debug.WriteLine($"[IR IMAGING] 目标在 Track 模式下丢失/不稳定 (或受干扰)，立即切换回搜索模式。"); SwitchToSearchMode(); } - else if (currentMode == WorkMode.Lock) - { - if (IsBlockingJammed) - { - Debug.WriteLine($"[IR IMAGING] 因阻塞干扰，在 Lock 模式下丢失目标，立即切换回搜索模式。"); - SwitchToSearchMode(); - } - else - { - _lockModeTargetLostTimer += deltaTime; - Debug.WriteLine($"[IR IMAGING] 目标在 Lock 模式下丢失。计时器: {_lockModeTargetLostTimer:F2}s / {config.TargetLostTolerance:F2}s"); - if (_lockModeTargetLostTimer >= config.TargetLostTolerance) - { - Debug.WriteLine($"[IR IMAGING] Lock 模式目标丢失超过容限，切换回搜索模式。"); - SwitchToSearchMode(); - } - // else 容限未到，保持在 Lock 模式，但 HasGuidance 已为 false - } + else if (currentMode == WorkMode.Lock) { + HandleLockModeTargetLoss(deltaTime); + } + } + } + + private void HandleLockModeTargetLoss(double deltaTime) + { + if (IsBlockingJammed) + { + Debug.WriteLine($"[IR IMAGING] 因阻塞干扰，在 Lock 模式下丢失目标，立即切换回搜索模式。"); + SwitchToSearchMode(); + } + else + { + lockModeTargetLostTimer += deltaTime; + Debug.WriteLine($"[IR IMAGING] 目标在 Lock 模式下丢失。计时器: {lockModeTargetLostTimer:F2}s / {config.TargetLostTolerance:F2}s"); + if (lockModeTargetLostTimer >= config.TargetLostTolerance) + { + Debug.WriteLine($"[IR IMAGING] Lock 模式目标丢失超过容限，切换回搜索模式。"); + SwitchToSearchMode(); } - // 如果在Search模式下目标丢失，通常意味着本帧未找到任何满足条件的目标， - // TryDetectAndIdentifyTarget内部逻辑会处理是否重置焦点等，此处无需额外操作 } - // 如果 targetFoundAndIdentifiedThisFrame 为 true，则制导参数已设置，模式状态由 TryDetectAndIdentifyTarget 内部逻辑（如切换到Track/Lock）或 Update 稍早部分（重置Lock模式计时器）处理 } /// @@ -341,12 +328,11 @@ namespace ThreatSource.Guidance { currentMode = WorkMode.Search; HasTarget = false; - // targetLostTimer = 0; // 已移除 LastTargetPosition = null; - _currentlyTrackedTargetId = null; - _searchModeFocusCandidateId = null; - _activeRecognitionHistory.Clear(); - _lockModeTargetLostTimer = 0; // 清理状态 + currentlyTrackedTargetId = null; + searchModeFocusCandidateId = null; + activeRecognitionHistory.Clear(); + lockModeTargetLostTimer = 0; // 创建图像生成器 imageGenerator = new InfraredImageGenerator( @@ -371,13 +357,11 @@ namespace ThreatSource.Guidance public void SwitchToTrackMode() { currentMode = WorkMode.Track; - lockConfirmationTimer = 0; // 重置锁定确认计时器 - // _lockModeTargetLostTimer 在切换到Track时不需要重置，因为它特定于Lock模式， - // 并且在从Lock切换到Search，再可能到Track时，应由SwitchToSearchMode清理。 + lockConfirmationTimer = 0; // 清理搜索阶段的状态 - _searchModeFocusCandidateId = null; - _activeRecognitionHistory.Clear(); // Track模式应从零开始积累历史 + searchModeFocusCandidateId = null; + activeRecognitionHistory.Clear(); // 创建图像生成器 (通常视场角会变化) imageGenerator = new InfraredImageGenerator( @@ -402,7 +386,7 @@ namespace ThreatSource.Guidance public void SwitchToLockMode() { currentMode = WorkMode.Lock; - _lockModeTargetLostTimer = 0; // 进入锁定模式时重置计时器 + lockModeTargetLostTimer = 0; Debug.WriteLine("切换到锁定模式 - 目标类型确认"); } @@ -425,14 +409,12 @@ namespace ThreatSource.Guidance private bool TryDetectAndIdentifyTarget(Vector3D missilePosition, Vector3D missileVelocity, double deltaTime, out Vector3D targetPosition) { targetPosition = Vector3D.Zero; - double minDistance = double.MaxValue; bool foundThisFrame = false; - bool currentFrameDirectRecognitionSuccess = false; // 新增：用于记录当前帧的直接识别/探测成功状态 + bool isRecognizedThisFrame = false; double searchFovRadians = config.SearchFieldOfView * Math.PI / 180.0; double trackFovRadians = config.TrackFieldOfView * Math.PI / 180.0; - double currentFovRadians = currentMode == WorkMode.Search ? searchFovRadians : trackFovRadians; - + foreach (var element in SimulationManager.GetEntitiesByType()) { if (element is BaseEquipment currentEntity) @@ -440,224 +422,182 @@ namespace ThreatSource.Guidance Vector3D toTarget = currentEntity.KState.Position - missilePosition; double distanceToCurrentEntity = toTarget.Magnitude(); - // 调用新的统一烟幕评估方法 - var smokeImpact = EvaluateSmokeImpact(missilePosition, currentEntity); - bool isGeometricallyObscuredBySmoke = smokeImpact.IsGeometricallyObscured; - double liveTransmittanceForImage = smokeImpact.OverallTransmittance; // 重命名以避免与旧变量名冲突 + var (isGeometricallyObscured, overallTransmittance) = EvaluateSmokeImpact(missilePosition, currentEntity); - // 根据当前模式调整FOV检查，并将烟幕几何遮挡作为主要过滤条件之一 - bool passesBasicChecks = false; - if (currentMode == WorkMode.Search) - { - passesBasicChecks = distanceToCurrentEntity <= config.MaxDetectionRange && - Math.Acos(Vector3D.DotProduct(toTarget.Normalize(), missileVelocity.Normalize())) <= (searchFovRadians / 2) && - !isGeometricallyObscuredBySmoke; - } - else if (currentMode == WorkMode.Track || currentMode == WorkMode.Lock) - { // Track 和 Lock 模式使用 TrackFieldOfView - passesBasicChecks = distanceToCurrentEntity <= config.MaxDetectionRange && - Math.Acos(Vector3D.DotProduct(toTarget.Normalize(), missileVelocity.Normalize())) <= (trackFovRadians / 2) && - !isGeometricallyObscuredBySmoke; - } + bool isWithinFovAndRange; + double relevantFovRadians = currentMode == WorkMode.Search ? searchFovRadians : trackFovRadians; - if (passesBasicChecks) + isWithinFovAndRange = distanceToCurrentEntity <= config.MaxDetectionRange && + Math.Acos(Vector3D.DotProduct(toTarget.Normalize(), missileVelocity.Normalize())) <= (relevantFovRadians / 2); + + if (!isWithinFovAndRange || isGeometricallyObscured) { - switch (currentMode) + // 如果是 Search 模式下的焦点目标，但它不再满足基本条件，则清除焦点 + if (currentMode == WorkMode.Search && currentEntity.Id == searchModeFocusCandidateId) { - case WorkMode.Search: - // 几何和主要烟幕检查已在上方 passesBasicChecks 中完成 - // if (distanceToCurrentEntity > config.MaxDetectionRange || ... || CheckIfTargetObscuredBySmoke(...)) - // 上述条件已由 passesBasicChecks 处理 - - if (currentEntity.Id == _searchModeFocusCandidateId && !passesBasicChecks) - { - // 如果是焦点目标，但它不再满足基本条件 (例如，进入了烟幕或飞出FOV) - _searchModeFocusCandidateId = null; - _activeRecognitionHistory.Clear(); - continue; // 跳过此不合格的焦点实体 - } - - if (!passesBasicChecks) { continue; } // 如果任何实体不通过基本检查，直接跳过 - - // 确定或确认焦点目标 - if (_searchModeFocusCandidateId == null) - { - _searchModeFocusCandidateId = currentEntity.Id; - _activeRecognitionHistory.Clear(); - } - else if (currentEntity.Id != _searchModeFocusCandidateId) - { - continue; - } - - // 此处意味着 currentEntity.Id == _searchModeFocusCandidateId，处理焦点目标 - // double liveSmokeTransmittance = CalculateLiveSmokeTransmittance(missilePosition, currentEntity.KState.Position); - var image = imageGenerator.GenerateImage(currentEntity, missilePosition, liveTransmittanceForImage, SimulationManager); - var searchResult = targetRecognizer.RecognizeTarget(image, currentEntity); - - bool currentRecognitionSuccessInSearch = (searchResult.Type == targetType && searchResult.Confidence >= config.SearchRecognitionProbability); - currentFrameDirectRecognitionSuccess = currentRecognitionSuccessInSearch; // 赋值给提升作用域的变量 - _activeRecognitionHistory.Enqueue(currentRecognitionSuccessInSearch); - while (_activeRecognitionHistory.Count > COMMON_RECOGNITION_WINDOW_SIZE) - { - _activeRecognitionHistory.Dequeue(); - } - - // 只有当窗口填满时才做决策 - if (_activeRecognitionHistory.Count == COMMON_RECOGNITION_WINDOW_SIZE) - { - double successRate = (double)_activeRecognitionHistory.Count(s => s) / COMMON_RECOGNITION_WINDOW_SIZE; - if (successRate >= COMMON_RECOGNITION_SUCCESS_RATE_THRESHOLD) - { - Vector3D toTargetForTrackCheck = currentEntity.KState.Position - missilePosition; - double angleToTargetForTrackCheck = Math.Acos(Vector3D.DotProduct(toTargetForTrackCheck.Normalize(), missileVelocity.Normalize())); - - if (angleToTargetForTrackCheck <= (trackFovRadians / 2)) - { - _currentlyTrackedTargetId = _searchModeFocusCandidateId; - targetPosition = currentEntity.KState.Position; - minDistance = distanceToCurrentEntity; - foundThisFrame = true; - Debug.WriteLine($"[IR IMAGING SEARCH] 目标 {_searchModeFocusCandidateId} 滑动窗口达标 (率: {successRate:P2}) 且在Track FOV内. 切换到Track模式。"); - SwitchToTrackMode(); - break; // 跳出 foreach, 完成搜索并切换 - } - else - { - _searchModeFocusCandidateId = null; - _activeRecognitionHistory.Clear(); - foundThisFrame = false; // 仅对当前焦点而言，未通过最终检查 - Debug.WriteLine($"[IR IMAGING SEARCH] 目标 {currentEntity.Id} 滑动窗口达标，但不满足Track FOV. 清除焦点，继续搜索。"); - } - } - else - { - _searchModeFocusCandidateId = null; - _activeRecognitionHistory.Clear(); - foundThisFrame = false; // 明确当前焦点未通过滑动窗口 - Debug.WriteLine($"[IR IMAGING SEARCH] 目标 {currentEntity.Id} 滑动窗口未达标 (率: {successRate:P2}). 清除焦点，继续搜索。"); - } - } - else // 窗口未满，foundThisFrame取决于本帧的直接识别结果，但不做模式切换 - { - foundThisFrame = currentRecognitionSuccessInSearch; - if (currentRecognitionSuccessInSearch) targetPosition = currentEntity.KState.Position; // 如果本帧识别到，更新临时位置 - Debug.WriteLine($"[IR IMAGING SEARCH] 目标 {_searchModeFocusCandidateId} 正在收集数据，窗口 ({_activeRecognitionHistory.Count}/{COMMON_RECOGNITION_WINDOW_SIZE}). 本帧识别: {currentRecognitionSuccessInSearch}"); - } - break; // Search case 的 break - - case WorkMode.Track: - if (_currentlyTrackedTargetId != null && currentEntity.Id == _currentlyTrackedTargetId) - { - bool actualRecognitionSuccessInTrack = false; // 重命名，避免与search模式的变量混淆 - if (!passesBasicChecks) - { - actualRecognitionSuccessInTrack = false; - Debug.WriteLine($"[IR IMAGING TRACK] 目标 {_currentlyTrackedTargetId} 未通过基本检查. 本帧失败。"); - } - else - { - var image_track = imageGenerator.GenerateImage(currentEntity, missilePosition, liveTransmittanceForImage, SimulationManager); - var trackResult = targetRecognizer.RecognizeTarget(image_track, currentEntity); - bool recognitionSucceededThisFrame = trackResult.Type == targetType && trackResult.Confidence >= config.TrackRecognitionProbability; - - if (recognitionSucceededThisFrame) - { - actualRecognitionSuccessInTrack = true; - Debug.WriteLine($"[IR IMAGING TRACK] 目标 {_currentlyTrackedTargetId} 本帧识别成功 (置信度 {trackResult.Confidence:P2})。"); - } - else - { - actualRecognitionSuccessInTrack = false; - Debug.WriteLine($"[IR IMAGING TRACK] 目标 {_currentlyTrackedTargetId} 本帧识别失败 (置信度: {trackResult.Confidence:P2})。"); - } - } - currentFrameDirectRecognitionSuccess = actualRecognitionSuccessInTrack; // 赋值给提升作用域的变量 - - _activeRecognitionHistory.Enqueue(actualRecognitionSuccessInTrack); // 使用 track 模式下的直接识别结果 - while (_activeRecognitionHistory.Count > COMMON_RECOGNITION_WINDOW_SIZE) - { - _activeRecognitionHistory.Dequeue(); - } - - if (_activeRecognitionHistory.Count == COMMON_RECOGNITION_WINDOW_SIZE) - { - double trackSuccessRate = (double)_activeRecognitionHistory.Count(s => s) / COMMON_RECOGNITION_WINDOW_SIZE; - if (trackSuccessRate >= COMMON_RECOGNITION_SUCCESS_RATE_THRESHOLD) - { - foundThisFrame = true; - if (actualRecognitionSuccessInTrack) - { - targetPosition = currentEntity.KState.Position; - minDistance = distanceToCurrentEntity; - } - else if (LastTargetPosition.HasValue) // 滑动窗口稳定，但本帧识别失败，使用上一位置 - { - targetPosition = LastTargetPosition.Value; - // minDistance 不更新，因为这是基于旧位置的 - Debug.WriteLine($"[IR IMAGING TRACK] 滑动窗口稳定，但本帧识别失败。使用LastTargetPosition: {targetPosition}"); - } - Debug.WriteLine($"[IR IMAGING TRACK] 目标 {_currentlyTrackedTargetId} 滑动窗口稳定 (成功率 {trackSuccessRate:P2})。foundThisFrame = true。"); - } - else - { - foundThisFrame = false; - Debug.WriteLine($"[IR IMAGING TRACK] 目标 {_currentlyTrackedTargetId} 滑动窗口不稳定 (成功率 {trackSuccessRate:P2})。foundThisFrame = false。"); - } - } - else // 滑动窗口未满 - { - foundThisFrame = passesBasicChecks; - if (passesBasicChecks) - { - if (actualRecognitionSuccessInTrack) // 如果本帧具体识别也成功了 - { - targetPosition = currentEntity.KState.Position; - minDistance = distanceToCurrentEntity; - } - else if (LastTargetPosition.HasValue) // 基本检查通过，本帧识别失败，使用上一位置 - { - targetPosition = LastTargetPosition.Value; - // minDistance 不更新 - Debug.WriteLine($"[IR IMAGING TRACK] 滑动窗口未满，本帧识别失败。使用LastTargetPosition: {targetPosition}"); - } - } - Debug.WriteLine($"[IR IMAGING TRACK] 目标 {_currentlyTrackedTargetId} 正在收集数据，窗口 ({_activeRecognitionHistory.Count}/{COMMON_RECOGNITION_WINDOW_SIZE}). 本帧passesBasicChecks:{passesBasicChecks}, 本帧识别:{actualRecognitionSuccessInTrack}. foundThisFrame set to {foundThisFrame}"); - } - break; - } - break; - - case WorkMode.Lock: - if (_currentlyTrackedTargetId != null && currentEntity.Id == _currentlyTrackedTargetId) - { - if (!passesBasicChecks) - { - foundThisFrame = false; - currentFrameDirectRecognitionSuccess = false; - Debug.WriteLine($"[IR IMAGING LOCK] 锁定目标 {_currentlyTrackedTargetId} 未通过基本检查 (FOV, 范围, 烟幕几何遮挡)。"); - } - else - { - targetPosition = currentEntity.KState.Position; - minDistance = distanceToCurrentEntity; - foundThisFrame = true; - currentFrameDirectRecognitionSuccess = true; // Lock模式下通过基础检查即认为本帧直接探测成功 - } - break; - } - break; + searchModeFocusCandidateId = null; + activeRecognitionHistory.Clear(); + Debug.WriteLine($"[IR IMAGING SEARCH] 焦点目标 {currentEntity.Id} 不再满足基本条件，清除焦点。"); } + continue; // 不满足基本FOV、距离或被烟幕遮挡，处理下一个实体 + } + + switch (currentMode) + { + case WorkMode.Search: + // 处理焦点 + if (searchModeFocusCandidateId == null) + { + searchModeFocusCandidateId = currentEntity.Id; + activeRecognitionHistory.Clear(); + Debug.WriteLine($"[IR IMAGING SEARCH] 选取新的焦点目标: {searchModeFocusCandidateId}"); + } + else if (currentEntity.Id != searchModeFocusCandidateId) + { + continue; // 不是当前选定的焦点目标，跳过 + } + + // 当前是焦点目标，且通过了基本检查 + var image = imageGenerator.GenerateImage(currentEntity, missilePosition, overallTransmittance, SimulationManager); + var searchResult = targetRecognizer.RecognizeTarget(image, currentEntity); + + isRecognizedThisFrame = (searchResult.Type == targetType && searchResult.Confidence >= config.SearchRecognitionProbability); + activeRecognitionHistory.Enqueue(isRecognizedThisFrame); + while (activeRecognitionHistory.Count > COMMON_RECOGNITION_WINDOW_SIZE) + { + activeRecognitionHistory.Dequeue(); + } + + if (activeRecognitionHistory.Count == COMMON_RECOGNITION_WINDOW_SIZE) + { + double successRate = (double)activeRecognitionHistory.Count(s => s) / COMMON_RECOGNITION_WINDOW_SIZE; + if (successRate >= COMMON_RECOGNITION_SUCCESS_RATE_THRESHOLD) + { + // 检查是否也在Track模式的FOV内 (因为search FOV可能比track FOV宽) + double angleToTargetForTrackCheck = Math.Acos(Vector3D.DotProduct(toTarget.Normalize(), missileVelocity.Normalize())); + if (angleToTargetForTrackCheck <= (trackFovRadians / 2)) + { + currentlyTrackedTargetId = searchModeFocusCandidateId; + targetPosition = currentEntity.KState.Position; + foundThisFrame = true; + Debug.WriteLine($"[IR IMAGING SEARCH] 目标 {searchModeFocusCandidateId} 滑动窗口达标 (率: {successRate:P2}) 且在Track FOV内. 切换到Track模式。"); + SwitchToTrackMode(); + } + else + { + searchModeFocusCandidateId = null; // 不在Track FOV内，放弃此焦点 + activeRecognitionHistory.Clear(); + foundThisFrame = false; + Debug.WriteLine($"[IR IMAGING SEARCH] 目标 {currentEntity.Id} 滑动窗口达标，但不满足切换到Track模式的FOV条件. 清除焦点。"); + } + } + else + { + searchModeFocusCandidateId = null; // 滑动窗口未达标，放弃此焦点 + activeRecognitionHistory.Clear(); + foundThisFrame = false; + Debug.WriteLine($"[IR IMAGING SEARCH] 目标 {currentEntity.Id} 滑动窗口未达标 (率: {successRate:P2}). 清除焦点。"); + } + } + else // 窗口未满 + { + foundThisFrame = isRecognizedThisFrame; // foundThisFrame反映本帧直接识别结果 + if (isRecognizedThisFrame) targetPosition = currentEntity.KState.Position; + Debug.WriteLine($"[IR IMAGING SEARCH] 目标 {searchModeFocusCandidateId} 正在收集数据，窗口 ({activeRecognitionHistory.Count}/{COMMON_RECOGNITION_WINDOW_SIZE}). 本帧识别: {isRecognizedThisFrame}"); + } + break; + + case WorkMode.Track: + if (currentlyTrackedTargetId == null || currentEntity.Id != currentlyTrackedTargetId) + { + continue; // 不是当前正在跟踪的目标 + } + + // 当前是跟踪目标，且已通过外层基本检查 + var image_track = imageGenerator.GenerateImage(currentEntity, missilePosition, overallTransmittance, SimulationManager); + var trackResult = targetRecognizer.RecognizeTarget(image_track, currentEntity); + isRecognizedThisFrame = trackResult.Type == targetType && trackResult.Confidence >= config.TrackRecognitionProbability; + + if (isRecognizedThisFrame) + { + Debug.WriteLine($"[IR IMAGING TRACK] 目标 {currentlyTrackedTargetId} 本帧识别成功 (置信度 {trackResult.Confidence:P2})。"); + } + else + { + Debug.WriteLine($"[IR IMAGING TRACK] 目标 {currentlyTrackedTargetId} 本帧识别失败 (置信度: {trackResult.Confidence:P2})。"); + } + + activeRecognitionHistory.Enqueue(isRecognizedThisFrame); + while (activeRecognitionHistory.Count > COMMON_RECOGNITION_WINDOW_SIZE) + { + activeRecognitionHistory.Dequeue(); + } + + if (activeRecognitionHistory.Count == COMMON_RECOGNITION_WINDOW_SIZE) + { + double trackSuccessRate = (double)activeRecognitionHistory.Count(s => s) / COMMON_RECOGNITION_WINDOW_SIZE; + if (trackSuccessRate >= COMMON_RECOGNITION_SUCCESS_RATE_THRESHOLD) + { + foundThisFrame = true; + if (isRecognizedThisFrame) + { + targetPosition = currentEntity.KState.Position; + } + else if (LastTargetPosition.HasValue) + { + targetPosition = LastTargetPosition.Value; + Debug.WriteLine($"[IR IMAGING TRACK] 滑动窗口稳定，但本帧识别失败。使用LastTargetPosition: {targetPosition}"); + } + Debug.WriteLine($"[IR IMAGING TRACK] 目标 {currentlyTrackedTargetId} 滑动窗口稳定 (成功率 {trackSuccessRate:P2})。foundThisFrame = true。"); + } + else + { + foundThisFrame = false; // 滑动窗口不稳定 + Debug.WriteLine($"[IR IMAGING TRACK] 目标 {currentlyTrackedTargetId} 滑动窗口不稳定 (成功率 {trackSuccessRate:P2})。foundThisFrame = false。"); + } + } + else // 滑动窗口未满 + { + foundThisFrame = isRecognizedThisFrame; + if (isRecognizedThisFrame) + { + targetPosition = currentEntity.KState.Position; + } + else if (LastTargetPosition.HasValue) + { + targetPosition = LastTargetPosition.Value; + Debug.WriteLine($"[IR IMAGING TRACK] 滑动窗口未满，本帧识别失败。使用LastTargetPosition: {targetPosition}"); + } + Debug.WriteLine($"[IR IMAGING TRACK] 目标 {currentlyTrackedTargetId} 正在收集数据，窗口 ({activeRecognitionHistory.Count}/{COMMON_RECOGNITION_WINDOW_SIZE}). 本帧识别:{isRecognizedThisFrame}. foundThisFrame set to {foundThisFrame}"); + } + break; + + case WorkMode.Lock: + if (currentlyTrackedTargetId == null || currentEntity.Id != currentlyTrackedTargetId) + { + continue; + } + targetPosition = currentEntity.KState.Position; + foundThisFrame = true; + isRecognizedThisFrame = true; + Debug.WriteLine($"[IR IMAGING LOCK] 锁定目标 {currentlyTrackedTargetId} 通过基本检查。"); + break; + } + + // 如果在Search模式下成功切换到Track，或者在Track/Lock模式下处理了目标（无论成功与否），则不再继续遍历此帧中的其他实体 + if (foundThisFrame || currentMode != WorkMode.Search || (currentMode == WorkMode.Search && currentlyTrackedTargetId != null) ) + { + break; } } } if (currentMode == WorkMode.Track) { - if (foundThisFrame && currentFrameDirectRecognitionSuccess) + if (foundThisFrame && isRecognizedThisFrame) { lockConfirmationTimer += deltaTime; - Debug.WriteLine($"[IR IMAGING TRACK] 目标 {_currentlyTrackedTargetId} 跟踪中，累积锁定确认时间: {lockConfirmationTimer:F2}s / {config.LockConfirmationTime:F2}s"); + Debug.WriteLine($"[IR IMAGING TRACK] 目标 {currentlyTrackedTargetId} 跟踪中，累积锁定确认时间: {lockConfirmationTimer:F2}s / {config.LockConfirmationTime:F2}s"); if (lockConfirmationTimer >= config.LockConfirmationTime) { SwitchToLockMode(); @@ -666,22 +606,19 @@ namespace ThreatSource.Guidance else { lockConfirmationTimer = 0; - Debug.WriteLine($"[IR IMAGING TRACK] 目标 {_currentlyTrackedTargetId} 跟踪条件不满足 (foundThisFrame:{foundThisFrame} 或本帧直接识别失败:{currentFrameDirectRecognitionSuccess})，重置锁定确认计时器。"); + Debug.WriteLine($"[IR IMAGING TRACK] 目标 {currentlyTrackedTargetId} 跟踪条件不满足 (foundThisFrame:{foundThisFrame} 或本帧直接识别失败:{isRecognizedThisFrame})，重置锁定确认计时器。"); } } else if (currentMode == WorkMode.Lock) { if (!foundThisFrame) { - // 在Lock模式下，如果 TryDetectAndIdentifyTarget 返回 false (即 foundThisFrame 为 false), - // _currentlyTrackedTargetId 暂时不清除，而是由 Update 方法中的 _lockModeTargetLostTimer 机制来决定何时因超时而真正丢失目标并切换模式。 - // 此时 _currentlyTrackedTargetId 仍然保留，以便在容限时间内如果目标重现，可以继续锁定。 - Debug.WriteLine($"[IR IMAGING LOCK] 锁定目标 {_currentlyTrackedTargetId} 在本帧未被找到。等待容限超时..."); + Debug.WriteLine($"[IR IMAGING LOCK] 锁定目标 {currentlyTrackedTargetId} 在本帧未被找到。等待容限超时..."); } } - HasTarget = foundThisFrame; // HasTarget 反映的是本帧是否成功探测/识别/跟踪到目标 - return foundThisFrame; // 返回本帧的探测/跟踪状态 + HasTarget = foundThisFrame; + return foundThisFrame; } private (bool IsGeometricallyObscured, double OverallTransmittance) EvaluateSmokeImpact(Vector3D observerPosition, BaseEquipment target) diff --git a/ThreatSource/src/Guidance/MillimeterWaveGuidanceSystem.cs b/ThreatSource/src/Guidance/MillimeterWaveGuidanceSystem.cs index c82782d..6b1d492 100644 --- a/ThreatSource/src/Guidance/MillimeterWaveGuidanceSystem.cs +++ b/ThreatSource/src/Guidance/MillimeterWaveGuidanceSystem.cs @@ -66,15 +66,6 @@ namespace ThreatSource.Guidance /// private Vector3D? LastTargetVelocity { get; set; } - /// - /// 目标丢失计时器 - /// - /// - /// 记录目标丢失的时间 - /// 用于切换到搜索模式 - /// - private double targetLostTimer = 0; - /// /// 锁定确认计时器 /// @@ -94,6 +85,26 @@ namespace ThreatSource.Guidance /// private WorkMode currentMode = WorkMode.Search; + /// + /// 当前正在跟踪或已锁定的目标ID (由Search模式切换时设置) + /// + private string? currentlyTrackedTargetId; + + /// + /// 当前活动模式 (Search焦点、Track、Lock) 的探测历史队列 + /// + private readonly Queue activeDetectionHistory = new(); + + /// + /// Search到Track模式切换的滑动窗口大小 + /// + private const int COMMON_DETECTION_WINDOW_SIZE = 10; + + /// + /// Search到Track模式切换的成功率阈值 + /// + private const double COMMON_DETECTION_SUCCESS_RATE_THRESHOLD = 0.8; + /// /// 是否有目标 /// @@ -129,6 +140,11 @@ namespace ThreatSource.Guidance /// private readonly SwerlingRcsModel swerlingRcsModel; + private int _framesInTrackMode = 0; + private const int MIN_FRAMES_FOR_TRACK_STABILITY_CHECK = 5; + + private double lastDetailSnrForDecision; // 新增类成员存储SNR供决策 + /// /// 初始化毫米波制导系统的新实例 /// @@ -169,12 +185,15 @@ namespace ThreatSource.Guidance currentMode = WorkMode.Search; HasTarget = false; HasGuidance = false; - targetLostTimer = 0; lockConfirmationTimer = 0; currentScanAngle = 0; currentScanRadius = config.SearchBeamWidth * Math.PI / 720.0; // 从半个波束宽度的一半开始 LastTargetPosition = null; LastTargetVelocity = null; + + activeDetectionHistory.Clear(); + currentlyTrackedTargetId = null; + Trace.TraceInformation($"切换到搜索模式，波束宽度: {config.SearchBeamWidth}度"); } @@ -186,6 +205,10 @@ namespace ThreatSource.Guidance currentMode = WorkMode.Track; lockConfirmationTimer = 0; HasGuidance = true; + + activeDetectionHistory.Clear(); + _framesInTrackMode = 0; // 重置计数器 + Trace.TraceInformation($"切换到跟踪模式，波束宽度: {config.TrackBeamWidth}度"); } @@ -207,7 +230,6 @@ namespace ThreatSource.Guidance if (!IsActive) { IsActive = true; - // 在这里订阅事件，确保只订阅一次 SimulationManager.SubscribeToEvent(HandleJammingEvent); } base.Activate(); @@ -243,7 +265,6 @@ namespace ThreatSource.Guidance if (parameters.Type == JammingType.MillimeterWave) { Debug.WriteLine($"[MMW_GUIDANCE] 受到毫米波干扰，功率：{parameters.Power} W。切换到搜索模式。", "Jamming"); - // 干扰时切换到搜索模式 if (currentMode != WorkMode.Search) { SwitchToSearchMode(); @@ -252,7 +273,6 @@ namespace ThreatSource.Guidance else if (parameters.Type == JammingType.SmokeGrenade) { Debug.WriteLine($"[MMW_GUIDANCE] 受到烟幕干扰影响。Jammer ID: {parameters.JammerId}", "Jamming"); - // 烟幕主要通过影响SNR体现，不直接触发模式切换 } } @@ -267,11 +287,9 @@ namespace ThreatSource.Guidance if (parameters.Type == JammingType.MillimeterWave) { Debug.WriteLine($"[MMW_GUIDANCE] 毫米波干扰已清除。", "Jamming"); - // 干扰清除后是否需要切换模式？取决于当前是否有目标。Update 会处理。 } else if (parameters.Type == JammingType.SmokeGrenade) { - // _currentSmokeTransmittance = 1.0; // Field removed, no longer needed here Debug.WriteLine($"[MMW_GUIDANCE] 烟幕干扰解除。Jammer ID: {parameters.JammerId}", "Jamming"); } } @@ -283,17 +301,13 @@ namespace ThreatSource.Guidance /// 如果应该处理则返回 true，否则返回 false protected override bool ShouldHandleJamming(JammingParameters parameters) { - // 首先调用基类检查是否支持该干扰类型 if (!base.ShouldHandleJamming(parameters)) { return false; } - // 对毫米波干扰添加波长检查 if (parameters.Type == JammingType.MillimeterWave) { - // 检查波长是否与工作波长匹配 (单位: 毫米 mm) - // 计算配置波长 (单位: mm), 直接使用光速值 3e8 m/s const double speedOfLight = 3e8; double configWavelength_mm = speedOfLight / config.WaveFrequency * 1000.0; // m/s / Hz * 1000 = mm @@ -304,7 +318,6 @@ namespace ThreatSource.Guidance } } - // 如果是支持的非毫米波干扰 (例如烟幕) 或通过了毫米波检查，则返回 true return true; } @@ -467,69 +480,52 @@ namespace ThreatSource.Guidance /// public override void Update(double deltaTime) { - base.Update(deltaTime); // 更新基类状态和干扰状态 + base.Update(deltaTime); - // 在执行核心逻辑前检查阻塞性干扰状态 if (IsBlockingJammed) { - // 如果被阻塞性干扰，确保处于搜索模式并且没有制导 - if (currentMode != WorkMode.Search) - { - SwitchToSearchMode(); - } - HasGuidance = false; + if (currentMode != WorkMode.Search) SwitchToSearchMode(); + this.HasTarget = false; GuidanceAcceleration = Vector3D.Zero; + HasGuidance = false; return; } - // 更新扫描参数 UpdateConicalScan(deltaTime); - - if (TryDetectAndTrackTarget(KState.Position, KState.Velocity, deltaTime, out Vector3D currentTargetPosition)) + + TryDetectAndTrackTarget(KState.Position, KState.Velocity, deltaTime, out Vector3D currentTargetPositionFromDetector); + + HasGuidance = HasTarget; + + if (HasGuidance) { - targetLostTimer = 0; Vector3D? currentTargetVelocity = null; - if (LastTargetPosition != null && deltaTime > 0) + if (LastTargetPosition.HasValue && deltaTime > 0 && currentTargetPositionFromDetector != Vector3D.Zero) { - currentTargetVelocity = (currentTargetPosition - LastTargetPosition) / deltaTime; + currentTargetVelocity = (currentTargetPositionFromDetector - LastTargetPosition.Value) / deltaTime; } - - LastTargetPosition = currentTargetPosition; - LastTargetVelocity = currentTargetVelocity; + LastTargetVelocity = currentTargetVelocity; GuidanceAcceleration = MotionAlgorithm.CalculateProportionalNavigation( ProportionalNavigationCoefficient, KState.Position, KState.Velocity, - currentTargetPosition, - currentTargetVelocity ?? Vector3D.Zero + currentTargetPositionFromDetector, + LastTargetVelocity ?? Vector3D.Zero ); if (GuidanceAcceleration.Magnitude() > MaxAcceleration) { GuidanceAcceleration = GuidanceAcceleration.Normalize() * MaxAcceleration; } - Debug.WriteLine($"制导加速度: {GuidanceAcceleration}"); - HasGuidance = true; } - else + else { - if (currentMode != WorkMode.Search) - { - targetLostTimer += deltaTime; - if (targetLostTimer >= config.TargetLostTolerance) - { - HasGuidance = false; - Trace.TraceInformation($"目标丢失 {targetLostTimer:F3}秒，切换回搜索模式"); - SwitchToSearchMode(); - } - } - else - { - HasGuidance = false; - } + GuidanceAcceleration = Vector3D.Zero; + LastTargetVelocity = null; } + HasGuidance = HasTarget; } /// @@ -551,178 +547,209 @@ namespace ThreatSource.Guidance private bool TryDetectAndTrackTarget(Vector3D missilePosition, Vector3D missileVelocity, double deltaTime, out Vector3D targetPosition) { targetPosition = Vector3D.Zero; - double minDistance = double.MaxValue; - bool foundTarget = false; - double currentSNR_dB = double.MinValue; foreach (var element in SimulationManager.GetEntitiesByType()) { - if (element is BaseEquipment target && target.Id != MissileId && target.IsActive) + if (!(element is BaseEquipment target && target.Id != MissileId && target.IsActive)) { - Vector3D toTarget = target.KState.Position - missilePosition; - double distance = toTarget.Magnitude(); + continue; // 卫语句: 跳过自身或无效目标 + } - if (distance <= config.MaxDetectionRange && distance > 0) + Vector3D toTarget = target.KState.Position - missilePosition; + double distance = toTarget.Magnitude(); + + // 卫语句: 超出最大探测距离或距离无效 + if (!(distance <= config.MaxDetectionRange && distance > 0)) + { + if ((currentMode == WorkMode.Track || currentMode == WorkMode.Lock) && target.Id == currentlyTrackedTargetId) { - double liveSmokeTransmittance = CalculateLiveSmokeTransmittance(missilePosition, target.KState.Position); - - // --- 开始RCS获取逻辑 --- - double rcsDbSm = RcsPattern.DEFAULT_RCS_DBSM; - double rcsLinear; - - Vector3D targetForward = Vector3D.Zero; - Vector3D targetRight = Vector3D.Zero; - Vector3D targetUp = Vector3D.Zero; - Vector3D observerVector = Vector3D.Zero; - bool isTargetOrientationValid = false; - MotionStateType motionState = MotionStateType.Static; + Debug.WriteLine($"[MMW {currentMode.ToString().ToUpper()}] 当前跟踪目标 {currentlyTrackedTargetId} 超出最大探测距离 ({distance:F0}m > {config.MaxDetectionRange:F0}m) 或距离无效。"); + } + else + { + continue; + } + } - if (target.KState != null) + double liveSmokeTransmittance = CalculateLiveSmokeTransmittance(missilePosition, target.KState.Position); + double rcsDbSm = RcsPattern.DEFAULT_RCS_DBSM; + double rcsLinear; + Vector3D targetForward = Vector3D.Zero; + Vector3D targetRight = Vector3D.Zero; + Vector3D targetUp = Vector3D.Zero; + bool isTargetOrientationValid = false; + MotionStateType motionState = MotionStateType.Static; + + if (target.KState != null) + { + targetForward = target.KState.Orientation.ToVector().Normalize(); + if (targetForward.MagnitudeSquared() > 1e-10) + { + Vector3D worldUpRef = Vector3D.UnitY; + if (Math.Abs(Vector3D.DotProduct(targetForward, worldUpRef)) > 0.99) worldUpRef = Vector3D.UnitZ; + targetRight = Vector3D.CrossProduct(targetForward, worldUpRef).Normalize(); + if (targetRight.MagnitudeSquared() > 1e-10) { - targetForward = target.KState.Orientation.ToVector().Normalize(); - if (targetForward.MagnitudeSquared() > 1e-10) // 确保从Orientation得到的Forward有效 - { - Vector3D worldUpRef = Vector3D.UnitY; - if (Math.Abs(Vector3D.DotProduct(targetForward, worldUpRef)) > 0.99) // 目标几乎垂直运动 - { - worldUpRef = Vector3D.UnitZ; - } - targetRight = Vector3D.CrossProduct(targetForward, worldUpRef).Normalize(); - if (targetRight.MagnitudeSquared() > 1e-10) // 确保Right向量有效 - { - targetUp = Vector3D.CrossProduct(targetRight, targetForward).Normalize(); - if (targetUp.MagnitudeSquared() > 1e-10) // 确保Up向量有效 - { - isTargetOrientationValid = true; - } - } - } - if(target.KState.Speed >0.1) - { - motionState = MotionStateType.ConstantVelocity; - } + targetUp = Vector3D.CrossProduct(targetRight, targetForward).Normalize(); + if (targetUp.MagnitudeSquared() > 1e-10) isTargetOrientationValid = true; + } + } + if (target.KState.Speed > 0.1) motionState = MotionStateType.ConstantVelocity; + } + if (!isTargetOrientationValid) Debug.WriteLine($"[MMW_GUIDANCE] Target {target.Id}: 方向无效或 KState 为 null. 使用默认RCS."); + + if (target.Properties.RcsPattern != null) + { + if (isTargetOrientationValid) rcsDbSm = target.Properties.RcsPattern.GetRcsValueDbSm(missilePosition - target.KState!.Position, targetForward, targetUp, targetRight); + } + else Debug.WriteLine($"[MMW_GUIDANCE] Target {target.Id}: 没有 RcsPattern 对象. 使用默认RCS."); + + rcsLinear = Math.Pow(10, rcsDbSm / 10.0); + Debug.WriteLine($"[MMW_GUIDANCE] 目标 {target.Id}: 原始RCS: {rcsLinear:F2} dBsm."); + bool isNewScanPeriodForRcs = currentMode == WorkMode.Track || currentMode == WorkMode.Lock; + rcsLinear = swerlingRcsModel.GetRealtimeRcs(target.Id, target.Properties.Type, motionState, rcsLinear, isNewScanPeriodForRcs); + Debug.WriteLine($"[MMW_GUIDANCE] 目标 {target.Id}: 实时RCS: {rcsLinear:F2} dBsm."); + double localSnrDb = CalculateSNR(distance, rcsLinear, liveSmokeTransmittance); + + switch (currentMode) + { + case WorkMode.Search: + if (IsTargetInBeam(missileVelocity, toTarget) && localSnrDb >= config.RecognitionSNRThreshold) + { + Debug.WriteLine($"[MMW SEARCH] TryDetect发现目标: {target.Id}, SNR: {localSnrDb:F2} dB."); + targetPosition = target.KState!.Position; + UpdateStateAndPotentiallySwitchMode(true, localSnrDb, target.Id, targetPosition, deltaTime); + return true; + } + break; + + case WorkMode.Track: + if (currentlyTrackedTargetId == null || target.Id != currentlyTrackedTargetId) + { + continue; } - if (!isTargetOrientationValid) - { - Debug.WriteLine($"[MMW_GUIDANCE] Target {target.Id}: 方向无效或 KState 为 null. 使用默认RCS ({RcsPattern.DEFAULT_RCS_DBSM} dBsm) 进行线性转换."); - } + bool currentFrameSuccessForTrack = IsTargetInBeam(missileVelocity, toTarget) && localSnrDb >= config.RecognitionSNRThreshold; + activeDetectionHistory.Enqueue(currentFrameSuccessForTrack); + while (activeDetectionHistory.Count > COMMON_DETECTION_WINDOW_SIZE) activeDetectionHistory.Dequeue(); + + Debug.WriteLine($"[MMW TRACK] TryDetect目标 {currentlyTrackedTargetId} 本帧探测: {currentFrameSuccessForTrack}, SNR: {localSnrDb:F2} dB. 窗口 ({activeDetectionHistory.Count}/{COMMON_DETECTION_WINDOW_SIZE}), 预热帧: {_framesInTrackMode}/{MIN_FRAMES_FOR_TRACK_STABILITY_CHECK}."); - if (target.Properties.RcsPattern != null) + bool trackOverallSuccess; + if (_framesInTrackMode < MIN_FRAMES_FOR_TRACK_STABILITY_CHECK) { - if (isTargetOrientationValid) - { - observerVector = missilePosition - target.KState!.Position; // 从目标指向导弹 (观察者) - Debug.WriteLine($"[RCS获取] Target {target.Id}: observerVector: {observerVector}, targetForward: {targetForward}, targetUp: {targetUp}, targetRight: {targetRight}"); - rcsDbSm = target.Properties.RcsPattern.GetRcsValueDbSm(observerVector, targetForward, targetUp, targetRight); - } + _framesInTrackMode++; + trackOverallSuccess = true; } else { - Debug.WriteLine($"[MMW_GUIDANCE] Target {target.Id}: 没有 RcsPattern 对象. 使用默认RCS ({RcsPattern.DEFAULT_RCS_DBSM} dBsm) 进行线性转换."); + if (activeDetectionHistory.Count == COMMON_DETECTION_WINDOW_SIZE) + { + double successRate = (double)activeDetectionHistory.Count(s => s) / COMMON_DETECTION_WINDOW_SIZE; + trackOverallSuccess = successRate >= COMMON_DETECTION_SUCCESS_RATE_THRESHOLD; + if(trackOverallSuccess) Debug.WriteLine($"[MMW TRACK] TryDetect目标 {currentlyTrackedTargetId} 滑动窗口稳定 (率: {successRate:P2})."); + else Debug.WriteLine($"[MMW TRACK] TryDetect目标 {currentlyTrackedTargetId} 滑动窗口不稳定 (率: {successRate:P2})."); + } + else + { + trackOverallSuccess = currentFrameSuccessForTrack; + Debug.WriteLine($"[MMW TRACK] TryDetect目标 {currentlyTrackedTargetId} 预热期后窗口未满. trackOverallSuccess = currentFrameSuccessForTrack ({currentFrameSuccessForTrack})."); + } } + + targetPosition = trackOverallSuccess ? + (currentFrameSuccessForTrack ? target.KState!.Position : (LastTargetPosition ?? Vector3D.Zero)) : + Vector3D.Zero; + UpdateStateAndPotentiallySwitchMode(trackOverallSuccess, localSnrDb, currentlyTrackedTargetId, targetPosition, deltaTime); + return trackOverallSuccess; - // 单位转换 (dBsm -> 平方米) - rcsLinear = Math.Pow(10, rcsDbSm / 10.0); - Debug.WriteLine($"[RCS获取] Target {target.Id}: RCS_dBsm: {rcsDbSm}, RCS_Linear: {rcsLinear}, "); - - // 采用Swerling模型计算RCS - rcsLinear = swerlingRcsModel.GetRealtimeRcs(target.Id, target.Properties.Type, motionState, rcsLinear, false); - Debug.WriteLine($"[RCS获取-Swerling模型] Target {target.Id}: RCS_Linear: {rcsLinear} "); - - double snr_dB = CalculateSNR(distance, rcsLinear, liveSmokeTransmittance); - - if (currentMode == WorkMode.Search) + case WorkMode.Lock: + if (currentlyTrackedTargetId == null || target.Id != currentlyTrackedTargetId) { - if (IsTargetInBeam(missileVelocity, toTarget) && snr_dB >= config.RecognitionSNRThreshold) - { - if (distance < minDistance) - { - targetPosition = target.KState!.Position; - minDistance = distance; - foundTarget = true; - currentSNR_dB = snr_dB; - } - } + continue; } - else if (currentMode == WorkMode.Track || currentMode == WorkMode.Lock) - { - bool isPotentiallyTrackedTarget = true; - if (LastTargetPosition != null) - { - double proximityThreshold = (target.Properties.Length > 0) ? target.Properties.Length * 5.0 : 100.0; - isPotentiallyTrackedTarget = Vector3D.Distance(target.KState!.Position, LastTargetPosition.Value) < proximityThreshold; - } - if (IsTargetInBeam(missileVelocity, toTarget) && isPotentiallyTrackedTarget) - { - double requiredSNR_dB = (currentMode == WorkMode.Track) ? - config.RecognitionSNRThreshold : config.LockSNRThreshold; - - if (snr_dB >= requiredSNR_dB) - { - targetPosition = target.KState!.Position; - foundTarget = true; - currentSNR_dB = snr_dB; - break; - } - } + bool currentFrameSuccessForLock = IsTargetInBeam(missileVelocity, toTarget) && localSnrDb >= config.LockSNRThreshold; + activeDetectionHistory.Enqueue(currentFrameSuccessForLock); + while (activeDetectionHistory.Count > COMMON_DETECTION_WINDOW_SIZE) activeDetectionHistory.Dequeue(); + + Debug.WriteLine($"[MMW LOCK] TryDetect目标 {currentlyTrackedTargetId} 本帧探测: {currentFrameSuccessForLock}, SNR: {localSnrDb:F2} dB. 窗口 ({activeDetectionHistory.Count}/{COMMON_DETECTION_WINDOW_SIZE})."); + + bool lockOverallSuccess; + if (activeDetectionHistory.Count == COMMON_DETECTION_WINDOW_SIZE) + { + double successRate = (double)activeDetectionHistory.Count(s => s) / COMMON_DETECTION_WINDOW_SIZE; + lockOverallSuccess = successRate >= COMMON_DETECTION_SUCCESS_RATE_THRESHOLD; + if(lockOverallSuccess) Debug.WriteLine($"[MMW LOCK] TryDetect目标 {currentlyTrackedTargetId} 滑动窗口稳定 (率: {successRate:P2})."); + else Debug.WriteLine($"[MMW LOCK] TryDetect目标 {currentlyTrackedTargetId} 滑动窗口不稳定 (率: {successRate:P2})."); } - } + else + { + lockOverallSuccess = currentFrameSuccessForLock; + Debug.WriteLine($"[MMW LOCK] TryDetect目标 {currentlyTrackedTargetId} 窗口未满. lockOverallSuccess = currentFrameSuccessForLock ({currentFrameSuccessForLock})."); + } + + targetPosition = lockOverallSuccess ? + (currentFrameSuccessForLock ? target.KState!.Position : (LastTargetPosition ?? Vector3D.Zero)) : + Vector3D.Zero; + UpdateStateAndPotentiallySwitchMode(lockOverallSuccess, localSnrDb, currentlyTrackedTargetId, targetPosition, deltaTime); + return lockOverallSuccess; } } - UpdateSystemState(foundTarget, currentSNR_dB, deltaTime); - HasTarget = foundTarget; - return foundTarget; + Debug.WriteLine($"[MMW {currentMode.ToString().ToUpper()}] TryDetect循环结束，未在switch中返回。"); + UpdateStateAndPotentiallySwitchMode(false, double.MinValue, currentlyTrackedTargetId, Vector3D.Zero, deltaTime); + targetPosition = Vector3D.Zero; + return false; } - /// - /// 更新系统状态 - /// - private void UpdateSystemState(bool hasTarget, double snr, double deltaTime) + private void UpdateStateAndPotentiallySwitchMode(bool frameSuccess, double snr, string? newOrCurrentTargetId, Vector3D newOrCurrentPosition, double deltaTime) { - if (!hasTarget) + HasTarget = frameSuccess; + lastDetailSnrForDecision = snr; + + if (frameSuccess && newOrCurrentPosition != Vector3D.Zero) { - targetLostTimer += deltaTime; - if (targetLostTimer >= config.TargetLostTolerance) + LastTargetPosition = newOrCurrentPosition; + } + + if (!frameSuccess) + { + if (currentMode == WorkMode.Track || currentMode == WorkMode.Lock) { + Debug.WriteLine($"[MMW {currentMode.ToString().ToUpper()}] UpdateState判定目标丢失/不稳定 (frameSuccess=false)，切换到搜索模式。"); SwitchToSearchMode(); } return; } - targetLostTimer = 0; - switch (currentMode) { case WorkMode.Search: - if (snr >= config.RecognitionSNRThreshold) + if (newOrCurrentTargetId != null) { + currentlyTrackedTargetId = newOrCurrentTargetId; + Debug.WriteLine($"[MMW SEARCH] UpdateState确认目标 {newOrCurrentTargetId}，准备切换到Track。"); SwitchToTrackMode(); } + else + { + Debug.WriteLine("[MMW SEARCH] Warning: UpdateState收到Search成功但无TargetId。"); + } break; case WorkMode.Track: - if (snr >= config.LockSNRThreshold) + lockConfirmationTimer += deltaTime; + Debug.WriteLine($"[MMW TRACK] UpdateState目标 {currentlyTrackedTargetId} 跟踪稳定. Lock确认计时: {lockConfirmationTimer:F2}s / {config.LockConfirmationTime:F2}s. SNR决策值: {snr:F2}dB"); + if (lockConfirmationTimer >= config.LockConfirmationTime) { - lockConfirmationTimer += deltaTime; - if (lockConfirmationTimer >= config.LockConfirmationTime) - { - SwitchToLockMode(); - } - } - else - { - lockConfirmationTimer = 0; + Debug.WriteLine($"[MMW TRACK] UpdateState达到锁定持续时间. 切换到Lock模式."); + SwitchToLockMode(); } break; case WorkMode.Lock: - if (snr < config.LockSNRThreshold) - { - SwitchToTrackMode(); - } + Debug.WriteLine($"[MMW LOCK] UpdateState目标 {currentlyTrackedTargetId} 锁定稳定. SNR决策值: {snr:F2}dB"); break; } } @@ -877,3 +904,7 @@ namespace ThreatSource.Guidance } } + + + + diff --git a/ThreatSource/src/MIssile/CompositeGuidedMissile.cs b/ThreatSource/src/MIssile/CompositeGuidedMissile.cs index 341a057..dd32b17 100644 --- a/ThreatSource/src/MIssile/CompositeGuidedMissile.cs +++ b/ThreatSource/src/MIssile/CompositeGuidedMissile.cs @@ -431,7 +431,7 @@ namespace ThreatSource.Missile /// protected virtual void UpdateCruiseStage(double deltaTime) { - if (FlightTime >= Properties.CruiseTime) + if (FlightTime >= Properties.CruiseTime + Properties.MaxEngineBurnTime) { currentFlightStage = CompositeFlightStage.Terminal; } diff --git a/ThreatSource/src/MIssile/InfraredImagingTerminalGuidedMissile.cs b/ThreatSource/src/MIssile/InfraredImagingTerminalGuidedMissile.cs index 286b3c8..85e8be2 100644 --- a/ThreatSource/src/MIssile/InfraredImagingTerminalGuidedMissile.cs +++ b/ThreatSource/src/MIssile/InfraredImagingTerminalGuidedMissile.cs @@ -168,7 +168,7 @@ namespace ThreatSource.Missile private void UpdateCruiseStage(double deltaTime) { // 如果巡航时间达到，切换到末制导搜索阶段 - if (FlightTime > Properties.CruiseTime) + if (FlightTime > Properties.CruiseTime + Properties.MaxEngineBurnTime) { currentStage = IRTG_Stage.Terminal; guidanceSystem.Activate(); diff --git a/ThreatSource/src/MIssile/MillimeterWaveTerminalGuidedMissile.cs b/ThreatSource/src/MIssile/MillimeterWaveTerminalGuidedMissile.cs index daff4f4..a1177b4 100644 --- a/ThreatSource/src/MIssile/MillimeterWaveTerminalGuidedMissile.cs +++ b/ThreatSource/src/MIssile/MillimeterWaveTerminalGuidedMissile.cs @@ -161,7 +161,7 @@ namespace ThreatSource.Missile /// private void UpdateCruiseStage(double deltaTime) { - if (FlightTime > Properties.CruiseTime) + if (FlightTime > Properties.CruiseTime + Properties.MaxEngineBurnTime) { currentStage = MWTG_Stage.Terminal; guidanceSystem.Activate(); // 激活制导系统