采用滑动窗口和预热帧机制完善了毫米波导引系统
This commit is contained in:
parent
a780126d32
commit
5f76e5b067
@ -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 # 干扰抗性阈值 (瓦特)
|
||||
@ -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
|
||||
Wavelength = 3.0 # 波长 (微米)
|
||||
@ -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 # 干扰抗性阈值 (瓦特)
|
||||
@ -95,17 +95,17 @@ namespace ThreatSource.Guidance
|
||||
/// <summary>
|
||||
/// 当前正在跟踪或已锁定的目标ID
|
||||
/// </summary>
|
||||
private string? _currentlyTrackedTargetId;
|
||||
private string? currentlyTrackedTargetId;
|
||||
|
||||
/// <summary>
|
||||
/// Search模式下当前关注的候选目标ID
|
||||
/// </summary>
|
||||
private string? _searchModeFocusCandidateId;
|
||||
private string? searchModeFocusCandidateId;
|
||||
|
||||
/// <summary>
|
||||
/// 当前活动模式 (搜索焦点或跟踪稳定) 的识别历史队列
|
||||
/// </summary>
|
||||
private Queue<bool> _activeRecognitionHistory = new();
|
||||
private readonly Queue<bool> activeRecognitionHistory = new();
|
||||
|
||||
/// <summary>
|
||||
/// 识别历史窗口大小 (搜索和跟踪阶段共用)
|
||||
@ -120,7 +120,7 @@ namespace ThreatSource.Guidance
|
||||
/// <summary>
|
||||
/// 锁定模式下目标丢失计时器
|
||||
/// </summary>
|
||||
private double _lockModeTargetLostTimer = 0;
|
||||
private double lockModeTargetLostTimer = 0;
|
||||
|
||||
/// <summary>
|
||||
/// 初始化红外成像制导系统的新实例
|
||||
@ -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
|
||||
/// </remarks>
|
||||
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模式计时器）处理
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@ -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<SimulationElement>())
|
||||
{
|
||||
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)
|
||||
|
||||
@ -66,15 +66,6 @@ namespace ThreatSource.Guidance
|
||||
/// </remarks>
|
||||
private Vector3D? LastTargetVelocity { get; set; }
|
||||
|
||||
/// <summary>
|
||||
/// 目标丢失计时器
|
||||
/// </summary>
|
||||
/// <remarks>
|
||||
/// 记录目标丢失的时间
|
||||
/// 用于切换到搜索模式
|
||||
/// </remarks>
|
||||
private double targetLostTimer = 0;
|
||||
|
||||
/// <summary>
|
||||
/// 锁定确认计时器
|
||||
/// </summary>
|
||||
@ -94,6 +85,26 @@ namespace ThreatSource.Guidance
|
||||
/// </summary>
|
||||
private WorkMode currentMode = WorkMode.Search;
|
||||
|
||||
/// <summary>
|
||||
/// 当前正在跟踪或已锁定的目标ID (由Search模式切换时设置)
|
||||
/// </summary>
|
||||
private string? currentlyTrackedTargetId;
|
||||
|
||||
/// <summary>
|
||||
/// 当前活动模式 (Search焦点、Track、Lock) 的探测历史队列
|
||||
/// </summary>
|
||||
private readonly Queue<bool> activeDetectionHistory = new();
|
||||
|
||||
/// <summary>
|
||||
/// Search到Track模式切换的滑动窗口大小
|
||||
/// </summary>
|
||||
private const int COMMON_DETECTION_WINDOW_SIZE = 10;
|
||||
|
||||
/// <summary>
|
||||
/// Search到Track模式切换的成功率阈值
|
||||
/// </summary>
|
||||
private const double COMMON_DETECTION_SUCCESS_RATE_THRESHOLD = 0.8;
|
||||
|
||||
/// <summary>
|
||||
/// 是否有目标
|
||||
/// </summary>
|
||||
@ -129,6 +140,11 @@ namespace ThreatSource.Guidance
|
||||
/// </summary>
|
||||
private readonly SwerlingRcsModel swerlingRcsModel;
|
||||
|
||||
private int _framesInTrackMode = 0;
|
||||
private const int MIN_FRAMES_FOR_TRACK_STABILITY_CHECK = 5;
|
||||
|
||||
private double lastDetailSnrForDecision; // 新增类成员存储SNR供决策
|
||||
|
||||
/// <summary>
|
||||
/// 初始化毫米波制导系统的新实例
|
||||
/// </summary>
|
||||
@ -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<JammingEvent>(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
|
||||
/// <returns>如果应该处理则返回 true，否则返回 false</returns>
|
||||
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
|
||||
/// </remarks>
|
||||
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;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
@ -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<SimulationElement>())
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 更新系统状态
|
||||
/// </summary>
|
||||
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
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
@ -431,7 +431,7 @@ namespace ThreatSource.Missile
|
||||
/// </summary>
|
||||
protected virtual void UpdateCruiseStage(double deltaTime)
|
||||
{
|
||||
if (FlightTime >= Properties.CruiseTime)
|
||||
if (FlightTime >= Properties.CruiseTime + Properties.MaxEngineBurnTime)
|
||||
{
|
||||
currentFlightStage = CompositeFlightStage.Terminal;
|
||||
}
|
||||
|
||||
@ -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();
|
||||
|
||||
@ -161,7 +161,7 @@ namespace ThreatSource.Missile
|
||||
/// </remarks>
|
||||
private void UpdateCruiseStage(double deltaTime)
|
||||
{
|
||||
if (FlightTime > Properties.CruiseTime)
|
||||
if (FlightTime > Properties.CruiseTime + Properties.MaxEngineBurnTime)
|
||||
{
|
||||
currentStage = MWTG_Stage.Terminal;
|
||||
guidanceSystem.Activate(); // 激活制导系统
|
||||
|
||||
Loading…
Reference in New Issue
Block a user