1601 lines
67 KiB
C#
1601 lines
67 KiB
C#
using System.Reflection;
|
|
using System.Text.Json;
|
|
using RobotNet10.NavigationTune.Shared.Interfaces;
|
|
using RobotNet10.NavigationTune.Shared.Models;
|
|
|
|
namespace RobotNet10.NavigationTune.Services;
|
|
|
|
/// <summary>
|
|
/// Rule-based tuning advisor that analyzes telemetry and metrics
|
|
/// to produce parameter adjustment suggestions after each test run.
|
|
/// Phase-aware: analyzes each navigation phase independently with phase-specific thresholds.
|
|
/// </summary>
|
|
public class TuningAdvisor : ITuningAdvisor
|
|
{
|
|
private const double Deg2Rad = Math.PI / 180.0;
|
|
|
|
#region Phase Thresholds
|
|
|
|
/// <summary>
|
|
/// Per-phase thresholds for pattern detection.
|
|
/// null means the detector is skipped for that phase.
|
|
/// </summary>
|
|
private static class PhaseThresholds
|
|
{
|
|
// CTE RMS threshold (meters) — triggers LargeCTE detector
|
|
public static double? GetCteRmsThreshold(TelemetryPhase phase) => phase switch
|
|
{
|
|
TelemetryPhase.PathFollowing => 0.10,
|
|
TelemetryPhase.FinalApproach => 0.02,
|
|
_ => null // InitialRotation, FinalRotation: CTE not relevant
|
|
};
|
|
|
|
// CTE Peak threshold (meters)
|
|
public static double? GetCtePeakThreshold(TelemetryPhase phase) => phase switch
|
|
{
|
|
TelemetryPhase.PathFollowing => 0.20,
|
|
TelemetryPhase.FinalApproach => 0.05,
|
|
_ => null
|
|
};
|
|
|
|
// Heading error RMS threshold (degrees)
|
|
public static double? GetHeadingRmsThresholdDeg(TelemetryPhase phase) => phase switch
|
|
{
|
|
TelemetryPhase.InitialRotation => 10.0,
|
|
TelemetryPhase.PathFollowing => 8.0,
|
|
TelemetryPhase.FinalApproach => 2.0,
|
|
TelemetryPhase.FinalRotation => 3.0,
|
|
_ => null
|
|
};
|
|
|
|
// Angular velocity StdDev threshold (rad/s) — triggers Oscillation detector
|
|
public static double? GetAngVelStdDevThreshold(TelemetryPhase phase) => phase switch
|
|
{
|
|
TelemetryPhase.InitialRotation => 1.5,
|
|
TelemetryPhase.PathFollowing => 0.3,
|
|
TelemetryPhase.FinalApproach => 0.2,
|
|
TelemetryPhase.FinalRotation => 1.0,
|
|
_ => null
|
|
};
|
|
|
|
// Zero-crossing rate threshold (crossings/sec) — triggers Oscillation detector
|
|
public static double GetZeroCrossingThreshold(TelemetryPhase phase) => phase switch
|
|
{
|
|
TelemetryPhase.PathFollowing => 3.0,
|
|
TelemetryPhase.FinalApproach => 2.5,
|
|
_ => 3.0
|
|
};
|
|
|
|
// Acceleration StdDev threshold (m/s²) — triggers JerkyMotion detector
|
|
public static double? GetAccelStdDevThreshold(TelemetryPhase phase) => phase switch
|
|
{
|
|
TelemetryPhase.PathFollowing => 0.5,
|
|
TelemetryPhase.FinalApproach => 0.3,
|
|
_ => null
|
|
};
|
|
|
|
// Which detectors to run per phase
|
|
public static bool ShouldRunOscillation(TelemetryPhase phase) =>
|
|
phase is TelemetryPhase.PathFollowing or TelemetryPhase.FinalApproach;
|
|
|
|
public static bool ShouldRunLargeCTE(TelemetryPhase phase) =>
|
|
phase is TelemetryPhase.PathFollowing or TelemetryPhase.FinalApproach;
|
|
|
|
public static bool ShouldRunLargeHeadingError(TelemetryPhase phase) =>
|
|
phase is TelemetryPhase.PathFollowing or TelemetryPhase.FinalApproach
|
|
or TelemetryPhase.FinalRotation;
|
|
|
|
public static bool ShouldRunCornerCutting(TelemetryPhase phase) =>
|
|
phase is TelemetryPhase.PathFollowing;
|
|
|
|
public static bool ShouldRunGoalOvershoot(TelemetryPhase phase) =>
|
|
phase is TelemetryPhase.FinalApproach;
|
|
|
|
public static bool ShouldRunSluggishResponse(TelemetryPhase phase) =>
|
|
phase is TelemetryPhase.PathFollowing or TelemetryPhase.FinalApproach;
|
|
|
|
public static bool ShouldRunJerkyMotion(TelemetryPhase phase) =>
|
|
phase is TelemetryPhase.PathFollowing or TelemetryPhase.FinalApproach;
|
|
|
|
public static bool ShouldRunVelocityEstimation(TelemetryPhase phase) =>
|
|
phase is TelemetryPhase.PathFollowing;
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Public API
|
|
|
|
public TuningReport Analyze(
|
|
List<TelemetryData> telemetryData,
|
|
TestMetrics metrics,
|
|
NavigationParameterSet currentParameters,
|
|
ReferencePath? referencePath = null)
|
|
{
|
|
var report = new TuningReport
|
|
{
|
|
ControllerType = currentParameters.ControllerType,
|
|
TelemetrySamplesAnalyzed = telemetryData.Count
|
|
};
|
|
|
|
// Step 1: Segment telemetry by phase
|
|
var phaseSegments = SegmentByPhase(telemetryData);
|
|
|
|
// Step 2: Compute per-phase metrics
|
|
var phaseMetricsMap = new Dictionary<TelemetryPhase, PhaseMetrics>();
|
|
foreach (var (phase, data) in phaseSegments)
|
|
{
|
|
phaseMetricsMap[phase] = ComputePhaseMetrics(phase, data);
|
|
}
|
|
|
|
// Step 3: Populate report phase info
|
|
report.PhaseSampleCounts = phaseSegments.ToDictionary(kv => kv.Key, kv => kv.Value.Count);
|
|
report.PhaseMetricsMap = phaseMetricsMap;
|
|
|
|
// Step 4: Detect patterns per phase with phase-specific thresholds
|
|
var patterns = DetectPatternsPhaseAware(phaseSegments, phaseMetricsMap, metrics);
|
|
report.DetectedPatterns = patterns;
|
|
|
|
// Step 5: Generate suggestions from detected patterns (phase-aware)
|
|
var suggestions = GenerateSuggestions(patterns, currentParameters, metrics);
|
|
|
|
// Step 6: Resolve conflicts (group by ParameterPath + TargetPhase)
|
|
suggestions = ResolveConflicts(suggestions);
|
|
|
|
// Step 7: Sort by priority then confidence
|
|
suggestions = suggestions
|
|
.OrderByDescending(s => s.Priority)
|
|
.ThenByDescending(s => s.Confidence)
|
|
.ToList();
|
|
|
|
report.Suggestions = suggestions;
|
|
|
|
// Step 8: Generate phase-aware overall assessment
|
|
report.OverallAssessment = GenerateOverallAssessment(metrics, patterns, suggestions, phaseMetricsMap);
|
|
|
|
return report;
|
|
}
|
|
|
|
public NavigationParameterSet ApplySuggestion(
|
|
NavigationParameterSet parameters,
|
|
TuningSuggestion suggestion)
|
|
{
|
|
var clone = DeepClone(parameters);
|
|
SetParameterValue(clone, suggestion.ParameterPath, suggestion.SuggestedValue);
|
|
clone.UpdatedAt = DateTime.UtcNow;
|
|
return clone;
|
|
}
|
|
|
|
public NavigationParameterSet ApplyAllSuggestions(
|
|
NavigationParameterSet parameters,
|
|
List<TuningSuggestion> suggestions)
|
|
{
|
|
var clone = DeepClone(parameters);
|
|
foreach (var suggestion in suggestions)
|
|
{
|
|
SetParameterValue(clone, suggestion.ParameterPath, suggestion.SuggestedValue);
|
|
}
|
|
clone.UpdatedAt = DateTime.UtcNow;
|
|
return clone;
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Pattern Detection (Phase-Aware)
|
|
|
|
/// <summary>
|
|
/// Detect patterns across all phases using phase-specific thresholds.
|
|
/// </summary>
|
|
private List<DiagnosticPattern> DetectPatternsPhaseAware(
|
|
Dictionary<TelemetryPhase, List<TelemetryData>> phaseSegments,
|
|
Dictionary<TelemetryPhase, PhaseMetrics> phaseMetricsMap,
|
|
TestMetrics globalMetrics)
|
|
{
|
|
var patterns = new List<DiagnosticPattern>();
|
|
|
|
foreach (var (phase, data) in phaseSegments)
|
|
{
|
|
if (phase == TelemetryPhase.Completed || data.Count < 5)
|
|
continue;
|
|
|
|
if (!phaseMetricsMap.TryGetValue(phase, out var pm))
|
|
continue;
|
|
|
|
// Oscillation
|
|
if (PhaseThresholds.ShouldRunOscillation(phase))
|
|
{
|
|
var p = DetectOscillation(data, pm,
|
|
PhaseThresholds.GetZeroCrossingThreshold(phase),
|
|
PhaseThresholds.GetAngVelStdDevThreshold(phase) ?? 0.3,
|
|
phase);
|
|
if (p != null) patterns.Add(p);
|
|
}
|
|
|
|
// Large CTE
|
|
if (PhaseThresholds.ShouldRunLargeCTE(phase))
|
|
{
|
|
var threshold = PhaseThresholds.GetCteRmsThreshold(phase);
|
|
if (threshold.HasValue)
|
|
{
|
|
var p = DetectLargeCTE(data, pm, threshold.Value, phase);
|
|
if (p != null) patterns.Add(p);
|
|
}
|
|
}
|
|
|
|
// Large Heading Error (phase-specific)
|
|
if (PhaseThresholds.ShouldRunLargeHeadingError(phase))
|
|
{
|
|
var thresholdDeg = PhaseThresholds.GetHeadingRmsThresholdDeg(phase);
|
|
if (thresholdDeg.HasValue)
|
|
{
|
|
var p = DetectLargeHeadingError(pm, thresholdDeg.Value, phase);
|
|
if (p != null) patterns.Add(p);
|
|
}
|
|
}
|
|
|
|
// Corner Cutting (PathFollowing only)
|
|
if (PhaseThresholds.ShouldRunCornerCutting(phase))
|
|
{
|
|
var p = DetectCornerCutting(data, pm, phase);
|
|
if (p != null) patterns.Add(p);
|
|
}
|
|
|
|
// Goal Overshoot (FinalApproach only — uses actual phase data instead of 80% approximation)
|
|
if (PhaseThresholds.ShouldRunGoalOvershoot(phase))
|
|
{
|
|
var p = DetectGoalOvershoot(data, pm, phase);
|
|
if (p != null) patterns.Add(p);
|
|
}
|
|
|
|
// Sluggish Response
|
|
if (PhaseThresholds.ShouldRunSluggishResponse(phase))
|
|
{
|
|
var spikeThreshold = phase == TelemetryPhase.FinalApproach ? 0.03 : 0.08;
|
|
var recoveryThreshold = phase == TelemetryPhase.FinalApproach ? 0.02 : 0.05;
|
|
var p = DetectSluggishResponse(data, spikeThreshold, recoveryThreshold, phase);
|
|
if (p != null) patterns.Add(p);
|
|
}
|
|
|
|
// Jerky Motion
|
|
if (PhaseThresholds.ShouldRunJerkyMotion(phase))
|
|
{
|
|
var accelThreshold = PhaseThresholds.GetAccelStdDevThreshold(phase) ?? 0.5;
|
|
var p = DetectJerkyMotion(data, pm, accelThreshold, phase);
|
|
if (p != null) patterns.Add(p);
|
|
}
|
|
|
|
// Velocity Estimation (PathFollowing only)
|
|
if (PhaseThresholds.ShouldRunVelocityEstimation(phase))
|
|
{
|
|
var p = DetectVelocityEstimationIssues(data, phase);
|
|
if (p != null) patterns.Add(p);
|
|
}
|
|
}
|
|
|
|
// Global detectors (use TestMetrics, not phase-specific)
|
|
var goalHeading = DetectGoalHeadingError(globalMetrics);
|
|
if (goalHeading != null)
|
|
{
|
|
goalHeading.DetectedInPhase = TelemetryPhase.FinalRotation;
|
|
patterns.Add(goalHeading);
|
|
}
|
|
|
|
var pathEfficiency = DetectPathEfficiencyIssues(globalMetrics);
|
|
if (pathEfficiency != null)
|
|
{
|
|
pathEfficiency.DetectedInPhase = TelemetryPhase.PathFollowing;
|
|
patterns.Add(pathEfficiency);
|
|
}
|
|
|
|
return patterns;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Detect angular velocity oscillation via zero-crossing rate (phase-aware).
|
|
/// </summary>
|
|
private DiagnosticPattern? DetectOscillation(
|
|
List<TelemetryData> data,
|
|
PhaseMetrics pm,
|
|
double zeroCrossingThreshold,
|
|
double angVelStdDevThreshold,
|
|
TelemetryPhase phase)
|
|
{
|
|
if (data.Count < 10) return null;
|
|
|
|
var angularVelocities = data.Select(d => d.RobotTwist.Angular).ToList();
|
|
|
|
int crossings = 0;
|
|
for (int i = 1; i < angularVelocities.Count; i++)
|
|
{
|
|
if (angularVelocities[i - 1] * angularVelocities[i] < 0)
|
|
crossings++;
|
|
}
|
|
|
|
double durationS = pm.DurationMs / 1000.0;
|
|
if (durationS <= 0) return null;
|
|
|
|
double crossingRate = crossings / durationS;
|
|
|
|
if (crossingRate < zeroCrossingThreshold || pm.AngularVelocityStdDev < angVelStdDevThreshold)
|
|
return null;
|
|
|
|
double severity = Math.Clamp((crossingRate - zeroCrossingThreshold) / 5.0, 0.3, 1.0);
|
|
|
|
return new DiagnosticPattern
|
|
{
|
|
Category = DiagnosticCategory.Oscillation,
|
|
Description = $"[{phase}] Angular velocity oscillation: {crossingRate:F1} zero-crossings/sec, " +
|
|
$"stddev = {pm.AngularVelocityStdDev:F3} rad/s",
|
|
Severity = severity,
|
|
DetectedInPhase = phase,
|
|
Evidence = new()
|
|
{
|
|
["ZeroCrossingRate"] = crossingRate,
|
|
["AngularVelocityStdDev"] = pm.AngularVelocityStdDev
|
|
}
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// Detect consistently large cross-track error (phase-aware with configurable threshold).
|
|
/// </summary>
|
|
private DiagnosticPattern? DetectLargeCTE(
|
|
List<TelemetryData> data,
|
|
PhaseMetrics pm,
|
|
double threshold,
|
|
TelemetryPhase phase)
|
|
{
|
|
if (pm.CrossTrackErrorRMS < threshold * 0.7) return null;
|
|
|
|
int exceedCount = data.Count(d => d.CrossTrackError > threshold);
|
|
double exceedFraction = data.Count > 0 ? (double)exceedCount / data.Count : 0;
|
|
|
|
if (pm.CrossTrackErrorRMS < threshold && exceedFraction < 0.30) return null;
|
|
|
|
double severity = Math.Clamp(pm.CrossTrackErrorRMS / threshold - 0.5, 0.3, 1.0);
|
|
|
|
return new DiagnosticPattern
|
|
{
|
|
Category = DiagnosticCategory.LargeCTE,
|
|
Description = $"[{phase}] Large cross-track error: RMS={pm.CrossTrackErrorRMS:F4}m, " +
|
|
$"Peak={pm.CrossTrackErrorPeak:F4}m, {exceedFraction:P0} exceed {threshold}m (target: {threshold}m)",
|
|
Severity = severity,
|
|
DetectedInPhase = phase,
|
|
Evidence = new()
|
|
{
|
|
["CTE_RMS"] = pm.CrossTrackErrorRMS,
|
|
["CTE_Peak"] = pm.CrossTrackErrorPeak,
|
|
["CTE_Mean"] = pm.CrossTrackErrorMean,
|
|
["ExceedFraction"] = exceedFraction,
|
|
["Threshold"] = threshold
|
|
}
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// Detect large heading error for a specific phase.
|
|
/// </summary>
|
|
private DiagnosticPattern? DetectLargeHeadingError(
|
|
PhaseMetrics pm,
|
|
double thresholdDeg,
|
|
TelemetryPhase phase)
|
|
{
|
|
double headingRmsDeg = pm.HeadingErrorRMS / Deg2Rad;
|
|
if (headingRmsDeg < thresholdDeg * 0.7) return null;
|
|
|
|
double severity = Math.Clamp((headingRmsDeg - thresholdDeg * 0.7) / (thresholdDeg * 0.6), 0.3, 1.0);
|
|
|
|
return new DiagnosticPattern
|
|
{
|
|
Category = DiagnosticCategory.GoalHeadingError,
|
|
Description = $"[{phase}] Large heading error: RMS={headingRmsDeg:F1}°, " +
|
|
$"Peak={pm.HeadingErrorPeak / Deg2Rad:F1}° (target: {thresholdDeg:F0}°)",
|
|
Severity = severity,
|
|
DetectedInPhase = phase,
|
|
Evidence = new()
|
|
{
|
|
["HeadingErrorRMSDeg"] = headingRmsDeg,
|
|
["HeadingErrorPeakDeg"] = pm.HeadingErrorPeak / Deg2Rad,
|
|
["ThresholdDeg"] = thresholdDeg
|
|
}
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// Detect corner cutting: CTE spikes at high-curvature path segments (phase-aware).
|
|
/// </summary>
|
|
private DiagnosticPattern? DetectCornerCutting(
|
|
List<TelemetryData> data,
|
|
PhaseMetrics pm,
|
|
TelemetryPhase phase)
|
|
{
|
|
if (data.Count < 20) return null;
|
|
|
|
var curvatures = new List<(int Index, double Curvature)>();
|
|
for (int i = 1; i < data.Count; i++)
|
|
{
|
|
double dTheta = Math.Abs(NormalizeAngle(
|
|
data[i].ReferencePose.Theta - data[i - 1].ReferencePose.Theta));
|
|
double dx = data[i].ReferencePose.X - data[i - 1].ReferencePose.X;
|
|
double dy = data[i].ReferencePose.Y - data[i - 1].ReferencePose.Y;
|
|
double ds = Math.Sqrt(dx * dx + dy * dy);
|
|
if (ds > 0.001)
|
|
curvatures.Add((i, dTheta / ds));
|
|
}
|
|
|
|
if (curvatures.Count == 0) return null;
|
|
|
|
double medianCurvature = GetMedian(curvatures.Select(c => c.Curvature).ToList());
|
|
double curvatureThreshold = Math.Max(1.0, medianCurvature * 3.0);
|
|
|
|
var highCurvatureIndices = curvatures
|
|
.Where(c => c.Curvature > curvatureThreshold)
|
|
.Select(c => c.Index)
|
|
.ToHashSet();
|
|
|
|
if (highCurvatureIndices.Count < 3) return null;
|
|
|
|
if (pm.CrossTrackErrorMean < 0.001) return null;
|
|
|
|
double cteMeanAtCurves = highCurvatureIndices
|
|
.Select(i => data[i].CrossTrackError)
|
|
.Average();
|
|
|
|
double ratio = cteMeanAtCurves / pm.CrossTrackErrorMean;
|
|
|
|
if (ratio < 1.5 || cteMeanAtCurves < 0.08) return null;
|
|
|
|
double severity = Math.Clamp((ratio - 1.5) / 2.0, 0.3, 1.0);
|
|
|
|
return new DiagnosticPattern
|
|
{
|
|
Category = DiagnosticCategory.CornerCutting,
|
|
Description = $"[{phase}] Corner cutting: CTE at curves = {cteMeanAtCurves:F4}m " +
|
|
$"({ratio:F1}x mean {pm.CrossTrackErrorMean:F4}m)",
|
|
Severity = severity,
|
|
DetectedInPhase = phase,
|
|
Evidence = new()
|
|
{
|
|
["CTEAtCurves"] = cteMeanAtCurves,
|
|
["CTEMeanOverall"] = pm.CrossTrackErrorMean,
|
|
["CurveToOverallRatio"] = ratio,
|
|
["HighCurvaturePoints"] = highCurvatureIndices.Count
|
|
}
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// Detect goal overshoot using actual FinalApproach phase data (phase-aware).
|
|
/// </summary>
|
|
private DiagnosticPattern? DetectGoalOvershoot(
|
|
List<TelemetryData> data,
|
|
PhaseMetrics pm,
|
|
TelemetryPhase phase)
|
|
{
|
|
if (data.Count < 5) return null;
|
|
|
|
double minDist = double.MaxValue;
|
|
int minIdx = 0;
|
|
for (int i = 0; i < data.Count; i++)
|
|
{
|
|
if (data[i].DistanceToGoal < minDist)
|
|
{
|
|
minDist = data[i].DistanceToGoal;
|
|
minIdx = i;
|
|
}
|
|
}
|
|
|
|
double maxAfterMin = 0;
|
|
for (int i = minIdx + 1; i < data.Count; i++)
|
|
{
|
|
maxAfterMin = Math.Max(maxAfterMin, data[i].DistanceToGoal);
|
|
}
|
|
|
|
double overshootMagnitude = maxAfterMin - minDist;
|
|
if (overshootMagnitude < 0.03) return null;
|
|
|
|
double severity = Math.Clamp(overshootMagnitude / 0.10, 0.3, 1.0);
|
|
|
|
return new DiagnosticPattern
|
|
{
|
|
Category = DiagnosticCategory.GoalOvershoot,
|
|
Description = $"[{phase}] Goal overshoot: overshot by {overshootMagnitude:F4}m " +
|
|
$"(min {minDist:F4}m → increased to {maxAfterMin:F4}m)",
|
|
Severity = severity,
|
|
DetectedInPhase = phase,
|
|
Evidence = new()
|
|
{
|
|
["OvershootMagnitude"] = overshootMagnitude,
|
|
["MinDistanceToGoal"] = minDist,
|
|
["MaxDistanceAfterMin"] = maxAfterMin,
|
|
["GoalPositionError"] = pm.GoalPositionError
|
|
}
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// Detect sluggish error correction with configurable thresholds (phase-aware).
|
|
/// </summary>
|
|
private DiagnosticPattern? DetectSluggishResponse(
|
|
List<TelemetryData> data,
|
|
double spikeThreshold,
|
|
double recoveryThreshold,
|
|
TelemetryPhase phase)
|
|
{
|
|
if (data.Count < 20) return null;
|
|
|
|
var correctionTimes = new List<double>();
|
|
bool inSpike = false;
|
|
long spikeStartMs = 0;
|
|
|
|
for (int i = 0; i < data.Count; i++)
|
|
{
|
|
if (!inSpike && data[i].CrossTrackError > spikeThreshold)
|
|
{
|
|
inSpike = true;
|
|
spikeStartMs = data[i].TimestampMs;
|
|
}
|
|
else if (inSpike && data[i].CrossTrackError < recoveryThreshold)
|
|
{
|
|
double correctionTimeS = (data[i].TimestampMs - spikeStartMs) / 1000.0;
|
|
if (correctionTimeS > 0)
|
|
correctionTimes.Add(correctionTimeS);
|
|
inSpike = false;
|
|
}
|
|
}
|
|
|
|
if (correctionTimes.Count < 2) return null;
|
|
|
|
double avgCorrectionTime = correctionTimes.Average();
|
|
double correctionTimeThreshold = phase == TelemetryPhase.FinalApproach ? 1.0 : 2.0;
|
|
if (avgCorrectionTime < correctionTimeThreshold) return null;
|
|
|
|
double severity = Math.Clamp((avgCorrectionTime - correctionTimeThreshold) / 3.0, 0.3, 1.0);
|
|
|
|
return new DiagnosticPattern
|
|
{
|
|
Category = DiagnosticCategory.SluggishResponse,
|
|
Description = $"[{phase}] Sluggish error correction: avg {avgCorrectionTime:F1}s to correct CTE " +
|
|
$"({correctionTimes.Count} events)",
|
|
Severity = severity,
|
|
DetectedInPhase = phase,
|
|
Evidence = new()
|
|
{
|
|
["AvgCorrectionTimeS"] = avgCorrectionTime,
|
|
["CorrectionEventCount"] = correctionTimes.Count,
|
|
["MaxCorrectionTimeS"] = correctionTimes.Max()
|
|
}
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// Detect jerky motion from high acceleration variance (phase-aware).
|
|
/// </summary>
|
|
private DiagnosticPattern? DetectJerkyMotion(
|
|
List<TelemetryData> data,
|
|
PhaseMetrics pm,
|
|
double accelThreshold,
|
|
TelemetryPhase phase)
|
|
{
|
|
if (pm.AccelerationStdDev < accelThreshold) return null;
|
|
|
|
var velocities = data.Select(d => d.RobotTwist.Linear).ToList();
|
|
int jerkEvents = 0;
|
|
for (int i = 2; i < velocities.Count; i++)
|
|
{
|
|
double accel1 = velocities[i - 1] - velocities[i - 2];
|
|
double accel2 = velocities[i] - velocities[i - 1];
|
|
if (accel1 * accel2 < 0)
|
|
jerkEvents++;
|
|
}
|
|
|
|
double durationS = pm.DurationMs / 1000.0;
|
|
double jerkRate = durationS > 0 ? jerkEvents / durationS : 0;
|
|
|
|
double severity = Math.Clamp((pm.AccelerationStdDev - accelThreshold) / 1.0, 0.3, 1.0);
|
|
|
|
return new DiagnosticPattern
|
|
{
|
|
Category = DiagnosticCategory.JerkyMotion,
|
|
Description = $"[{phase}] Jerky motion: accel stddev = {pm.AccelerationStdDev:F3} m/s², " +
|
|
$"vel stddev = {pm.VelocityStdDev:F3} m/s, jerk rate = {jerkRate:F1}/s",
|
|
Severity = severity,
|
|
DetectedInPhase = phase,
|
|
Evidence = new()
|
|
{
|
|
["AccelerationStdDev"] = pm.AccelerationStdDev,
|
|
["VelocityStdDev"] = pm.VelocityStdDev,
|
|
["JerkRate"] = jerkRate
|
|
}
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// Detect velocity estimation issues (phase-aware).
|
|
/// </summary>
|
|
private DiagnosticPattern? DetectVelocityEstimationIssues(
|
|
List<TelemetryData> data,
|
|
TelemetryPhase phase)
|
|
{
|
|
if (data.Count < 10) return null;
|
|
|
|
double sumSquaredGap = 0;
|
|
int lowConfidenceCount = 0;
|
|
|
|
foreach (var d in data)
|
|
{
|
|
double gap = d.CommandTwist.Linear - d.RobotTwist.Linear;
|
|
sumSquaredGap += gap * gap;
|
|
if (d.ModelConfidence < 0.5)
|
|
lowConfidenceCount++;
|
|
}
|
|
|
|
double rmsGap = Math.Sqrt(sumSquaredGap / data.Count);
|
|
double avgCommand = data.Where(d => Math.Abs(d.CommandTwist.Linear) > 0.01)
|
|
.Select(d => Math.Abs(d.CommandTwist.Linear))
|
|
.DefaultIfEmpty(1.0)
|
|
.Average();
|
|
double gapPercent = avgCommand > 0 ? rmsGap / avgCommand : 0;
|
|
double lowConfidenceFraction = (double)lowConfidenceCount / data.Count;
|
|
|
|
if (gapPercent < 0.20 && lowConfidenceFraction < 0.40) return null;
|
|
|
|
double severity = Math.Clamp(Math.Max(gapPercent - 0.15, lowConfidenceFraction - 0.3), 0.3, 1.0);
|
|
|
|
return new DiagnosticPattern
|
|
{
|
|
Category = DiagnosticCategory.VelocityEstimation,
|
|
Description = $"[{phase}] Velocity estimation issues: RMS gap = {rmsGap:F3} m/s ({gapPercent:P0}), " +
|
|
$"low confidence in {lowConfidenceFraction:P0} of samples",
|
|
Severity = severity,
|
|
DetectedInPhase = phase,
|
|
Evidence = new()
|
|
{
|
|
["RMSGap"] = rmsGap,
|
|
["GapPercent"] = gapPercent,
|
|
["LowConfidenceFraction"] = lowConfidenceFraction,
|
|
["AvgConfidence"] = data.Average(d => d.ModelConfidence)
|
|
}
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// Detect large heading error at goal (global — uses TestMetrics).
|
|
/// </summary>
|
|
private DiagnosticPattern? DetectGoalHeadingError(TestMetrics metrics)
|
|
{
|
|
double thresholdRad = 5.0 * Deg2Rad;
|
|
|
|
if (metrics.GoalHeadingError < thresholdRad) return null;
|
|
|
|
double severity = Math.Clamp((metrics.GoalHeadingError - thresholdRad) / (10.0 * Deg2Rad), 0.3, 1.0);
|
|
|
|
return new DiagnosticPattern
|
|
{
|
|
Category = DiagnosticCategory.GoalHeadingError,
|
|
Description = $"[FinalRotation] Large heading error at goal: {metrics.GoalHeadingError / Deg2Rad:F1}° " +
|
|
$"(threshold: {thresholdRad / Deg2Rad:F0}°)",
|
|
Severity = severity,
|
|
Evidence = new()
|
|
{
|
|
["GoalHeadingErrorDeg"] = metrics.GoalHeadingError / Deg2Rad,
|
|
["GoalHeadingErrorRad"] = metrics.GoalHeadingError
|
|
}
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// Detect path efficiency issues (global — uses TestMetrics).
|
|
/// </summary>
|
|
private DiagnosticPattern? DetectPathEfficiencyIssues(TestMetrics metrics)
|
|
{
|
|
if (metrics.PathLengthRatio < 1.15) return null;
|
|
|
|
double severity = Math.Clamp((metrics.PathLengthRatio - 1.15) / 0.30, 0.3, 1.0);
|
|
|
|
return new DiagnosticPattern
|
|
{
|
|
Category = DiagnosticCategory.PathEfficiency,
|
|
Description = $"[PathFollowing] Path efficiency issue: actual path is {(metrics.PathLengthRatio - 1.0) * 100:F1}% " +
|
|
$"longer than reference (ratio = {metrics.PathLengthRatio:F3})",
|
|
Severity = severity,
|
|
Evidence = new()
|
|
{
|
|
["PathLengthRatio"] = metrics.PathLengthRatio,
|
|
["CompletionTime"] = metrics.CompletionTime,
|
|
["AverageSpeed"] = metrics.AverageSpeed
|
|
}
|
|
};
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Suggestion Generation (Phase-Aware)
|
|
|
|
private List<TuningSuggestion> GenerateSuggestions(
|
|
List<DiagnosticPattern> patterns,
|
|
NavigationParameterSet parameters,
|
|
TestMetrics metrics)
|
|
{
|
|
var suggestions = new List<TuningSuggestion>();
|
|
double dataQuality = Math.Min(1.0, metrics.CompletionTime > 0 ? 1.0 : 0.5);
|
|
|
|
bool isPurePursuit = parameters.ControllerType == PathFollowingController.PurePursuit;
|
|
|
|
// Group patterns by phase for targeted suggestions
|
|
// DetectedInPhase is always set by DetectPatternsPhaseAware, but model allows null for backward compat
|
|
foreach (var group in patterns.GroupBy(p => p.DetectedInPhase))
|
|
{
|
|
var phasePatterns = group.ToDictionary(p => p.Category, p => p);
|
|
var phase = group.Key;
|
|
|
|
if (isPurePursuit)
|
|
suggestions.AddRange(SuggestForPurePursuit(phasePatterns, parameters, dataQuality, phase));
|
|
else
|
|
suggestions.AddRange(SuggestForStanley(phasePatterns, parameters, dataQuality, phase));
|
|
|
|
suggestions.AddRange(SuggestForCommon(phasePatterns, parameters, dataQuality, metrics, phase));
|
|
}
|
|
|
|
return suggestions;
|
|
}
|
|
|
|
private List<TuningSuggestion> SuggestForPurePursuit(
|
|
Dictionary<DiagnosticCategory, DiagnosticPattern> patterns,
|
|
NavigationParameterSet p,
|
|
double dataQuality,
|
|
TelemetryPhase? phase)
|
|
{
|
|
var suggestions = new List<TuningSuggestion>();
|
|
var pp = p.PurePursuitConfig;
|
|
|
|
// --- Oscillation (PathFollowing) ---
|
|
if (patterns.TryGetValue(DiagnosticCategory.Oscillation, out var oscillation))
|
|
{
|
|
// PathFollowing oscillation → PP lookahead/angular velocity
|
|
if (phase is null or TelemetryPhase.PathFollowing)
|
|
{
|
|
suggestions.Add(CreateSuggestion(
|
|
"PurePursuitConfig.LookaheadMin", "PP Lookahead Min",
|
|
pp.LookaheadMin,
|
|
Math.Min(pp.LookaheadMin * 1.4, pp.LookaheadMax * 0.5),
|
|
$"[{phase}] Increase minimum lookahead to reduce oscillation",
|
|
SuggestionPriority.High, oscillation.Severity * 0.9 * dataQuality,
|
|
DiagnosticCategory.Oscillation,
|
|
"Smoother angular velocity, less jitter",
|
|
phase));
|
|
|
|
suggestions.Add(CreateSuggestion(
|
|
"PurePursuitConfig.Kdd", "PP Velocity Lookahead Gain",
|
|
pp.Kdd,
|
|
pp.Kdd * 1.2,
|
|
$"[{phase}] Increase velocity-based lookahead gain for smoother tracking at speed",
|
|
SuggestionPriority.Medium, oscillation.Severity * 0.7 * dataQuality,
|
|
DiagnosticCategory.Oscillation,
|
|
"More predictive at higher speeds",
|
|
phase));
|
|
|
|
suggestions.Add(CreateSuggestion(
|
|
"PurePursuitConfig.MaxAngularVelocity", "PP Max Angular Velocity",
|
|
pp.MaxAngularVelocity,
|
|
Math.Max(pp.MaxAngularVelocity * 0.85, 0.8),
|
|
$"[{phase}] Reduce max angular velocity to dampen oscillation",
|
|
SuggestionPriority.Medium, oscillation.Severity * 0.8 * dataQuality,
|
|
DiagnosticCategory.Oscillation,
|
|
"Less aggressive turning, reduced overshoot",
|
|
phase));
|
|
}
|
|
|
|
// FinalApproach oscillation → FinalApproach params
|
|
if (phase == TelemetryPhase.FinalApproach)
|
|
{
|
|
suggestions.Add(CreateSuggestion(
|
|
"PurePursuitConfig.FinalApproachThreshold", "PP Final Approach Threshold",
|
|
pp.FinalApproachThreshold,
|
|
Math.Min(pp.FinalApproachThreshold * 1.3, 0.5),
|
|
"[FinalApproach] Switch to precision mode earlier to reduce approach oscillation",
|
|
SuggestionPriority.High, oscillation.Severity * 0.85 * dataQuality,
|
|
DiagnosticCategory.Oscillation,
|
|
"Earlier precision mode, less oscillation near goal",
|
|
TelemetryPhase.FinalApproach));
|
|
}
|
|
}
|
|
|
|
// --- Large CTE ---
|
|
if (patterns.TryGetValue(DiagnosticCategory.LargeCTE, out var largeCTE))
|
|
{
|
|
if (phase is null or TelemetryPhase.PathFollowing)
|
|
{
|
|
suggestions.Add(CreateSuggestion(
|
|
"PurePursuitConfig.LookaheadMin", "PP Lookahead Min",
|
|
pp.LookaheadMin,
|
|
Math.Max(pp.LookaheadMin * 0.8, 0.15),
|
|
$"[{phase}] Decrease minimum lookahead for tighter path following",
|
|
SuggestionPriority.High, largeCTE.Severity * 0.9 * dataQuality,
|
|
DiagnosticCategory.LargeCTE,
|
|
"Reduced cross-track error, tighter path following",
|
|
phase));
|
|
|
|
suggestions.Add(CreateSuggestion(
|
|
"PurePursuitConfig.KCurvature", "PP Curvature Sensitivity",
|
|
pp.KCurvature,
|
|
Math.Min(pp.KCurvature * 1.3, 5.0),
|
|
$"[{phase}] Increase curvature sensitivity to reduce lookahead on curves",
|
|
SuggestionPriority.Medium, largeCTE.Severity * 0.7 * dataQuality,
|
|
DiagnosticCategory.LargeCTE,
|
|
"Better tracking on curved sections",
|
|
phase));
|
|
}
|
|
|
|
// FinalApproach Large CTE → precision parameters
|
|
if (phase == TelemetryPhase.FinalApproach)
|
|
{
|
|
suggestions.Add(CreateSuggestion(
|
|
"PurePursuitConfig.FinalApproachThreshold", "PP Final Approach Threshold",
|
|
pp.FinalApproachThreshold,
|
|
Math.Min(pp.FinalApproachThreshold * 1.5, 0.5),
|
|
"[FinalApproach] Switch to precision mode earlier — CTE exceeds 0.02m target",
|
|
SuggestionPriority.High, largeCTE.Severity * 0.95 * dataQuality,
|
|
DiagnosticCategory.LargeCTE,
|
|
"Earlier precision mode for tighter final approach tracking",
|
|
TelemetryPhase.FinalApproach));
|
|
|
|
suggestions.Add(CreateSuggestion(
|
|
"PurePursuitConfig.GoalRegionDistance", "PP Goal Region Distance",
|
|
pp.GoalRegionDistance,
|
|
Math.Min(pp.GoalRegionDistance * 1.3, 3.0),
|
|
"[FinalApproach] Increase goal region to start reducing lookahead earlier for better precision",
|
|
SuggestionPriority.High, largeCTE.Severity * 0.85 * dataQuality,
|
|
DiagnosticCategory.LargeCTE,
|
|
"Smoother transition to precision tracking near goal",
|
|
TelemetryPhase.FinalApproach));
|
|
|
|
suggestions.Add(CreateSuggestion(
|
|
"MovePidConfig.Kp", "Move PID Kp",
|
|
p.MovePidConfig.Kp,
|
|
Math.Min(p.MovePidConfig.Kp * 1.2, 3.0),
|
|
"[FinalApproach] Increase deceleration PID gain for more responsive speed control near goal",
|
|
SuggestionPriority.Medium, largeCTE.Severity * 0.7 * dataQuality,
|
|
DiagnosticCategory.LargeCTE,
|
|
"More responsive deceleration for precision approach",
|
|
TelemetryPhase.FinalApproach));
|
|
}
|
|
}
|
|
|
|
// --- Corner Cutting (PathFollowing only) ---
|
|
if (patterns.TryGetValue(DiagnosticCategory.CornerCutting, out var cornerCutting)
|
|
&& phase is null or TelemetryPhase.PathFollowing)
|
|
{
|
|
suggestions.Add(CreateSuggestion(
|
|
"PurePursuitConfig.LookaheadMax", "PP Lookahead Max",
|
|
pp.LookaheadMax,
|
|
Math.Max(pp.LookaheadMax * 0.8, pp.LookaheadMin * 1.5),
|
|
$"[{phase}] Reduce max lookahead to prevent cutting corners",
|
|
SuggestionPriority.High, cornerCutting.Severity * 0.9 * dataQuality,
|
|
DiagnosticCategory.CornerCutting,
|
|
"Less corner cutting, tighter curve following",
|
|
phase));
|
|
|
|
suggestions.Add(CreateSuggestion(
|
|
"PurePursuitConfig.KCurvature", "PP Curvature Sensitivity",
|
|
pp.KCurvature,
|
|
Math.Min(pp.KCurvature * 1.4, 6.0),
|
|
$"[{phase}] Increase curvature sensitivity to shorten lookahead on curves",
|
|
SuggestionPriority.High, cornerCutting.Severity * 0.85 * dataQuality,
|
|
DiagnosticCategory.CornerCutting,
|
|
"Significantly tighter tracking through curves",
|
|
phase));
|
|
|
|
suggestions.Add(CreateSuggestion(
|
|
"PurePursuitConfig.Kdd", "PP Velocity Lookahead Gain",
|
|
pp.Kdd,
|
|
Math.Max(pp.Kdd * 0.8, 0.5),
|
|
$"[{phase}] Decrease velocity-based lookahead to reduce lookahead at speed on curves",
|
|
SuggestionPriority.Medium, cornerCutting.Severity * 0.6 * dataQuality,
|
|
DiagnosticCategory.CornerCutting,
|
|
"Less speed-dependent lookahead stretch",
|
|
phase));
|
|
}
|
|
|
|
// --- Goal Overshoot (FinalApproach) ---
|
|
if (patterns.TryGetValue(DiagnosticCategory.GoalOvershoot, out var goalOvershoot))
|
|
{
|
|
suggestions.Add(CreateSuggestion(
|
|
"PurePursuitConfig.FinalApproachThreshold", "PP Final Approach Threshold",
|
|
pp.FinalApproachThreshold,
|
|
Math.Min(pp.FinalApproachThreshold * 1.5, 0.5),
|
|
"[FinalApproach] Increase final approach distance to switch to precision mode earlier",
|
|
SuggestionPriority.High, goalOvershoot.Severity * 0.9 * dataQuality,
|
|
DiagnosticCategory.GoalOvershoot,
|
|
"Earlier deceleration near goal, less overshoot",
|
|
TelemetryPhase.FinalApproach));
|
|
|
|
suggestions.Add(CreateSuggestion(
|
|
"PurePursuitConfig.GoalRegionDistance", "PP Goal Region Distance",
|
|
pp.GoalRegionDistance,
|
|
Math.Min(pp.GoalRegionDistance * 1.3, 3.0),
|
|
"[FinalApproach] Increase goal region to start reducing lookahead earlier",
|
|
SuggestionPriority.Medium, goalOvershoot.Severity * 0.7 * dataQuality,
|
|
DiagnosticCategory.GoalOvershoot,
|
|
"Smoother deceleration profile near goal",
|
|
TelemetryPhase.FinalApproach));
|
|
}
|
|
|
|
// --- Sluggish Response ---
|
|
if (patterns.TryGetValue(DiagnosticCategory.SluggishResponse, out var sluggish))
|
|
{
|
|
if (phase is null or TelemetryPhase.PathFollowing)
|
|
{
|
|
suggestions.Add(CreateSuggestion(
|
|
"PurePursuitConfig.LookaheadMin", "PP Lookahead Min",
|
|
pp.LookaheadMin,
|
|
Math.Max(pp.LookaheadMin * 0.85, 0.15),
|
|
$"[{phase}] Decrease minimum lookahead for more reactive error correction",
|
|
SuggestionPriority.Medium, sluggish.Severity * 0.7 * dataQuality,
|
|
DiagnosticCategory.SluggishResponse,
|
|
"Faster error correction, more responsive tracking",
|
|
phase));
|
|
|
|
suggestions.Add(CreateSuggestion(
|
|
"PurePursuitConfig.MaxAngularVelocity", "PP Max Angular Velocity",
|
|
pp.MaxAngularVelocity,
|
|
Math.Min(pp.MaxAngularVelocity * 1.2, 3.0),
|
|
$"[{phase}] Increase max angular velocity for faster corrections",
|
|
SuggestionPriority.Medium, sluggish.Severity * 0.8 * dataQuality,
|
|
DiagnosticCategory.SluggishResponse,
|
|
"Faster angular corrections during tracking",
|
|
phase));
|
|
}
|
|
}
|
|
|
|
// --- FinalApproach Heading Error ---
|
|
if (patterns.TryGetValue(DiagnosticCategory.GoalHeadingError, out var headingErr)
|
|
&& phase == TelemetryPhase.FinalApproach)
|
|
{
|
|
suggestions.Add(CreateSuggestion(
|
|
"PurePursuitConfig.HeadingTolerance", "PP Heading Tolerance",
|
|
pp.HeadingTolerance,
|
|
Math.Max(pp.HeadingTolerance * 0.7, 1.0),
|
|
"[FinalApproach] Tighten heading tolerance for more precise final alignment",
|
|
SuggestionPriority.High, headingErr.Severity * 0.85 * dataQuality,
|
|
DiagnosticCategory.GoalHeadingError,
|
|
"More precise heading alignment near goal",
|
|
TelemetryPhase.FinalApproach));
|
|
}
|
|
|
|
return suggestions;
|
|
}
|
|
|
|
private List<TuningSuggestion> SuggestForStanley(
|
|
Dictionary<DiagnosticCategory, DiagnosticPattern> patterns,
|
|
NavigationParameterSet p,
|
|
double dataQuality,
|
|
TelemetryPhase? phase)
|
|
{
|
|
var suggestions = new List<TuningSuggestion>();
|
|
var sc = p.StanleyConfig;
|
|
|
|
// --- Oscillation ---
|
|
if (patterns.TryGetValue(DiagnosticCategory.Oscillation, out var oscillation))
|
|
{
|
|
if (phase is null or TelemetryPhase.PathFollowing)
|
|
{
|
|
suggestions.Add(CreateSuggestion(
|
|
"StanleyConfig.K", "Stanley CTE Gain (K)",
|
|
sc.K,
|
|
Math.Max(sc.K * 0.8, 1.0),
|
|
$"[{phase}] Decrease CTE gain to reduce oscillation",
|
|
SuggestionPriority.High, oscillation.Severity * 0.9 * dataQuality,
|
|
DiagnosticCategory.Oscillation,
|
|
"Less aggressive CTE correction, reduced oscillation",
|
|
phase));
|
|
|
|
suggestions.Add(CreateSuggestion(
|
|
"StanleyConfig.Ks", "Stanley Softening (Ks)",
|
|
sc.Ks,
|
|
Math.Min(sc.Ks * 1.3, 0.3),
|
|
$"[{phase}] Increase softening constant to reduce aggressiveness at low speed",
|
|
SuggestionPriority.Medium, oscillation.Severity * 0.7 * dataQuality,
|
|
DiagnosticCategory.Oscillation,
|
|
"Gentler correction especially at low speeds",
|
|
phase));
|
|
}
|
|
|
|
if (phase == TelemetryPhase.FinalApproach)
|
|
{
|
|
suggestions.Add(CreateSuggestion(
|
|
"StanleyConfig.LowSpeedAngularGain", "Stanley Low Speed Angular Gain",
|
|
sc.LowSpeedAngularGain,
|
|
Math.Max(sc.LowSpeedAngularGain * 0.8, 0.5),
|
|
"[FinalApproach] Reduce low-speed angular gain to prevent oscillation near goal",
|
|
SuggestionPriority.High, oscillation.Severity * 0.85 * dataQuality,
|
|
DiagnosticCategory.Oscillation,
|
|
"Less aggressive rotation at low speed near goal",
|
|
TelemetryPhase.FinalApproach));
|
|
|
|
suggestions.Add(CreateSuggestion(
|
|
"StanleyConfig.GoalGainMultiplier", "Stanley Goal Gain Multiplier",
|
|
sc.GoalGainMultiplier,
|
|
Math.Max(sc.GoalGainMultiplier * 0.85, 1.2),
|
|
"[FinalApproach] Decrease goal gain multiplier to reduce oscillation near goal",
|
|
SuggestionPriority.Medium, oscillation.Severity * 0.7 * dataQuality,
|
|
DiagnosticCategory.Oscillation,
|
|
"Less aggressive K increase near goal",
|
|
TelemetryPhase.FinalApproach));
|
|
}
|
|
}
|
|
|
|
// --- Large CTE ---
|
|
if (patterns.TryGetValue(DiagnosticCategory.LargeCTE, out var largeCTE))
|
|
{
|
|
if (phase is null or TelemetryPhase.PathFollowing)
|
|
{
|
|
suggestions.Add(CreateSuggestion(
|
|
"StanleyConfig.K", "Stanley CTE Gain (K)",
|
|
sc.K,
|
|
Math.Min(sc.K * 1.25, 6.0),
|
|
$"[{phase}] Increase CTE gain for tighter path following",
|
|
SuggestionPriority.High, largeCTE.Severity * 0.9 * dataQuality,
|
|
DiagnosticCategory.LargeCTE,
|
|
"Faster CTE correction, tighter tracking",
|
|
phase));
|
|
}
|
|
|
|
// FinalApproach Large CTE → precision parameters
|
|
if (phase == TelemetryPhase.FinalApproach)
|
|
{
|
|
suggestions.Add(CreateSuggestion(
|
|
"StanleyConfig.GoalGainMultiplier", "Stanley Goal Gain Multiplier",
|
|
sc.GoalGainMultiplier,
|
|
Math.Min(sc.GoalGainMultiplier * 1.3, 3.5),
|
|
"[FinalApproach] Increase goal gain multiplier — CTE exceeds 0.02m target",
|
|
SuggestionPriority.High, largeCTE.Severity * 0.95 * dataQuality,
|
|
DiagnosticCategory.LargeCTE,
|
|
"Tighter K near goal for precision tracking",
|
|
TelemetryPhase.FinalApproach));
|
|
|
|
suggestions.Add(CreateSuggestion(
|
|
"StanleyConfig.GoalApproachDistance", "Stanley Goal Approach Distance",
|
|
sc.GoalApproachDistance,
|
|
Math.Min(sc.GoalApproachDistance * 1.4, 2.5),
|
|
"[FinalApproach] Increase goal approach distance to start tightening K earlier",
|
|
SuggestionPriority.High, largeCTE.Severity * 0.9 * dataQuality,
|
|
DiagnosticCategory.LargeCTE,
|
|
"Earlier gain increase for better final approach precision",
|
|
TelemetryPhase.FinalApproach));
|
|
|
|
suggestions.Add(CreateSuggestion(
|
|
"StanleyConfig.LowSpeedAngularGain", "Stanley Low Speed Angular Gain",
|
|
sc.LowSpeedAngularGain,
|
|
Math.Min(sc.LowSpeedAngularGain * 1.25, 3.0),
|
|
"[FinalApproach] Increase low-speed angular gain for better correction at low speed near goal",
|
|
SuggestionPriority.Medium, largeCTE.Severity * 0.8 * dataQuality,
|
|
DiagnosticCategory.LargeCTE,
|
|
"Better low-speed correction for precision approach",
|
|
TelemetryPhase.FinalApproach));
|
|
}
|
|
}
|
|
|
|
// --- Corner Cutting (PathFollowing only) ---
|
|
if (patterns.TryGetValue(DiagnosticCategory.CornerCutting, out var cornerCutting)
|
|
&& phase is null or TelemetryPhase.PathFollowing)
|
|
{
|
|
suggestions.Add(CreateSuggestion(
|
|
"StanleyConfig.K", "Stanley CTE Gain (K)",
|
|
sc.K,
|
|
Math.Min(sc.K * 1.2, 5.0),
|
|
$"[{phase}] Increase K gain to correct CTE faster on curves",
|
|
SuggestionPriority.High, cornerCutting.Severity * 0.8 * dataQuality,
|
|
DiagnosticCategory.CornerCutting,
|
|
"Tighter tracking through curves",
|
|
phase));
|
|
|
|
suggestions.Add(CreateSuggestion(
|
|
"StanleyConfig.KCurvatureFF", "Stanley Curvature Feedforward",
|
|
sc.KCurvatureFF,
|
|
Math.Min(sc.KCurvatureFF * 1.25, 2.0),
|
|
$"[{phase}] Increase curvature feedforward for better curve anticipation",
|
|
SuggestionPriority.High, cornerCutting.Severity * 0.85 * dataQuality,
|
|
DiagnosticCategory.CornerCutting,
|
|
"Better curve anticipation, less lag through turns",
|
|
phase));
|
|
}
|
|
|
|
// --- Goal Overshoot (FinalApproach) ---
|
|
if (patterns.TryGetValue(DiagnosticCategory.GoalOvershoot, out var goalOvershoot))
|
|
{
|
|
suggestions.Add(CreateSuggestion(
|
|
"StanleyConfig.GoalApproachDistance", "Stanley Goal Approach Distance",
|
|
sc.GoalApproachDistance,
|
|
Math.Min(sc.GoalApproachDistance * 1.4, 2.5),
|
|
"[FinalApproach] Increase goal approach distance to start tightening K earlier",
|
|
SuggestionPriority.High, goalOvershoot.Severity * 0.9 * dataQuality,
|
|
DiagnosticCategory.GoalOvershoot,
|
|
"Earlier gain increase near goal, better precision",
|
|
TelemetryPhase.FinalApproach));
|
|
|
|
suggestions.Add(CreateSuggestion(
|
|
"StanleyConfig.GoalGainMultiplier", "Stanley Goal Gain Multiplier",
|
|
sc.GoalGainMultiplier,
|
|
Math.Max(sc.GoalGainMultiplier * 0.8, 1.2),
|
|
"[FinalApproach] Decrease goal gain multiplier to reduce overshoot (less aggressive)",
|
|
SuggestionPriority.Medium, goalOvershoot.Severity * 0.7 * dataQuality,
|
|
DiagnosticCategory.GoalOvershoot,
|
|
"Less aggressive correction near goal, reduced oscillation",
|
|
TelemetryPhase.FinalApproach));
|
|
}
|
|
|
|
// --- Sluggish Response ---
|
|
if (patterns.TryGetValue(DiagnosticCategory.SluggishResponse, out var sluggish))
|
|
{
|
|
if (phase is null or TelemetryPhase.PathFollowing)
|
|
{
|
|
suggestions.Add(CreateSuggestion(
|
|
"StanleyConfig.K", "Stanley CTE Gain (K)",
|
|
sc.K,
|
|
Math.Min(sc.K * 1.2, 5.0),
|
|
$"[{phase}] Increase K gain for faster error correction",
|
|
SuggestionPriority.Medium, sluggish.Severity * 0.8 * dataQuality,
|
|
DiagnosticCategory.SluggishResponse,
|
|
"Faster CTE correction",
|
|
phase));
|
|
}
|
|
|
|
if (phase == TelemetryPhase.FinalApproach)
|
|
{
|
|
suggestions.Add(CreateSuggestion(
|
|
"StanleyConfig.LowSpeedAngularGain", "Stanley Low Speed Angular Gain",
|
|
sc.LowSpeedAngularGain,
|
|
Math.Min(sc.LowSpeedAngularGain * 1.25, 3.0),
|
|
"[FinalApproach] Increase low-speed angular gain for faster correction near goal",
|
|
SuggestionPriority.Medium, sluggish.Severity * 0.8 * dataQuality,
|
|
DiagnosticCategory.SluggishResponse,
|
|
"Better low-speed correction ability near goal",
|
|
TelemetryPhase.FinalApproach));
|
|
}
|
|
}
|
|
|
|
// --- Jerky Motion ---
|
|
if (patterns.TryGetValue(DiagnosticCategory.JerkyMotion, out var jerky))
|
|
{
|
|
suggestions.Add(CreateSuggestion(
|
|
"StanleyConfig.MaxSteeringAngle", "Stanley Max Steering Angle",
|
|
sc.MaxSteeringAngle,
|
|
Math.Max(sc.MaxSteeringAngle * 0.85, 0.3),
|
|
$"[{phase}] Reduce max steering angle to limit abrupt steering changes",
|
|
SuggestionPriority.Medium, jerky.Severity * 0.7 * dataQuality,
|
|
DiagnosticCategory.JerkyMotion,
|
|
"Smoother steering transitions",
|
|
phase));
|
|
}
|
|
|
|
// --- FinalApproach Heading Error ---
|
|
if (patterns.TryGetValue(DiagnosticCategory.GoalHeadingError, out var headingErr)
|
|
&& phase == TelemetryPhase.FinalApproach)
|
|
{
|
|
suggestions.Add(CreateSuggestion(
|
|
"StanleyConfig.GoalGainMultiplier", "Stanley Goal Gain Multiplier",
|
|
sc.GoalGainMultiplier,
|
|
Math.Min(sc.GoalGainMultiplier * 1.2, 3.5),
|
|
"[FinalApproach] Increase goal gain for tighter heading correction — heading exceeds 2° target",
|
|
SuggestionPriority.High, headingErr.Severity * 0.85 * dataQuality,
|
|
DiagnosticCategory.GoalHeadingError,
|
|
"Tighter heading correction near goal",
|
|
TelemetryPhase.FinalApproach));
|
|
|
|
suggestions.Add(CreateSuggestion(
|
|
"StanleyConfig.LowSpeedAngularGain", "Stanley Low Speed Angular Gain",
|
|
sc.LowSpeedAngularGain,
|
|
Math.Min(sc.LowSpeedAngularGain * 1.3, 3.0),
|
|
"[FinalApproach] Increase low-speed angular gain for better heading correction near goal",
|
|
SuggestionPriority.High, headingErr.Severity * 0.8 * dataQuality,
|
|
DiagnosticCategory.GoalHeadingError,
|
|
"Better heading correction at low speed",
|
|
TelemetryPhase.FinalApproach));
|
|
}
|
|
|
|
return suggestions;
|
|
}
|
|
|
|
private List<TuningSuggestion> SuggestForCommon(
|
|
Dictionary<DiagnosticCategory, DiagnosticPattern> patterns,
|
|
NavigationParameterSet p,
|
|
double dataQuality,
|
|
TestMetrics metrics,
|
|
TelemetryPhase? phase)
|
|
{
|
|
var suggestions = new List<TuningSuggestion>();
|
|
|
|
// --- Jerky Motion (common) ---
|
|
if (patterns.TryGetValue(DiagnosticCategory.JerkyMotion, out var jerky))
|
|
{
|
|
var targetPhase = phase ?? TelemetryPhase.PathFollowing;
|
|
|
|
suggestions.Add(CreateSuggestion(
|
|
"NavigationConfig.Acceleration", "Max Acceleration",
|
|
p.NavigationConfig.Acceleration,
|
|
Math.Max(p.NavigationConfig.Acceleration * 0.8, 0.2),
|
|
$"[{phase}] Reduce acceleration limit for smoother speed changes",
|
|
SuggestionPriority.Medium, jerky.Severity * 0.8 * dataQuality,
|
|
DiagnosticCategory.JerkyMotion,
|
|
"Smoother acceleration profile",
|
|
targetPhase));
|
|
|
|
suggestions.Add(CreateSuggestion(
|
|
"NavigationConfig.Deceleration", "Max Deceleration",
|
|
p.NavigationConfig.Deceleration,
|
|
Math.Max(p.NavigationConfig.Deceleration * 0.8, 0.2),
|
|
$"[{phase}] Reduce deceleration limit for smoother braking",
|
|
SuggestionPriority.Medium, jerky.Severity * 0.7 * dataQuality,
|
|
DiagnosticCategory.JerkyMotion,
|
|
"Smoother deceleration profile",
|
|
targetPhase));
|
|
|
|
suggestions.Add(CreateSuggestion(
|
|
"SignalConfig.AlphaFilter", "EMA Filter Alpha",
|
|
p.SignalConfig.AlphaFilter,
|
|
Math.Max(p.SignalConfig.AlphaFilter * 0.75, 0.1),
|
|
$"[{phase}] Decrease EMA filter alpha for more aggressive noise filtering",
|
|
SuggestionPriority.Low, jerky.Severity * 0.5 * dataQuality,
|
|
DiagnosticCategory.JerkyMotion,
|
|
"Smoother velocity signal, less noise propagation",
|
|
targetPhase));
|
|
}
|
|
|
|
// --- Goal Heading Error (FinalRotation) ---
|
|
if (patterns.TryGetValue(DiagnosticCategory.GoalHeadingError, out var headingErr)
|
|
&& phase is null or TelemetryPhase.FinalRotation)
|
|
{
|
|
suggestions.Add(CreateSuggestion(
|
|
"RotatePidConfig.Kp", "Rotate PID Kp",
|
|
p.RotatePidConfig.Kp,
|
|
Math.Min(p.RotatePidConfig.Kp * 1.2, 15.0),
|
|
"[FinalRotation] Increase rotation PID proportional gain for faster heading correction",
|
|
SuggestionPriority.High, headingErr.Severity * 0.85 * dataQuality,
|
|
DiagnosticCategory.GoalHeadingError,
|
|
"Faster and more precise heading alignment at goal",
|
|
TelemetryPhase.FinalRotation));
|
|
}
|
|
|
|
// --- Goal Overshoot (FinalApproach - PID) ---
|
|
if (patterns.TryGetValue(DiagnosticCategory.GoalOvershoot, out var goalOvershoot))
|
|
{
|
|
suggestions.Add(CreateSuggestion(
|
|
"MovePidConfig.Kd", "Move PID Kd",
|
|
p.MovePidConfig.Kd,
|
|
Math.Min(p.MovePidConfig.Kd * 1.25, 2.0),
|
|
"[FinalApproach] Increase derivative gain for better deceleration damping near goal",
|
|
SuggestionPriority.Medium, goalOvershoot.Severity * 0.7 * dataQuality,
|
|
DiagnosticCategory.GoalOvershoot,
|
|
"Better damping, less velocity overshoot near goal",
|
|
TelemetryPhase.FinalApproach));
|
|
}
|
|
|
|
// --- Velocity Estimation (PathFollowing) ---
|
|
if (patterns.TryGetValue(DiagnosticCategory.VelocityEstimation, out var velEst))
|
|
{
|
|
suggestions.Add(CreateSuggestion(
|
|
"EstimatorConfig.ConfidenceDecayRate", "Velocity Estimator Confidence Decay",
|
|
p.EstimatorConfig.ConfidenceDecayRate,
|
|
Math.Max(p.EstimatorConfig.ConfidenceDecayRate * 0.97, 0.88),
|
|
"[PathFollowing] Decrease confidence decay rate for faster adaptation",
|
|
SuggestionPriority.Low, velEst.Severity * 0.6 * dataQuality,
|
|
DiagnosticCategory.VelocityEstimation,
|
|
"Faster adaptation of blend ratio",
|
|
TelemetryPhase.PathFollowing));
|
|
}
|
|
|
|
// --- Path Efficiency + Large CTE = robot moving too fast ---
|
|
if (patterns.ContainsKey(DiagnosticCategory.PathEfficiency) &&
|
|
patterns.ContainsKey(DiagnosticCategory.LargeCTE))
|
|
{
|
|
var effPattern = patterns[DiagnosticCategory.PathEfficiency];
|
|
suggestions.Add(CreateSuggestion(
|
|
"NavigationConfig.MaxLinearVelocity", "Max Linear Velocity",
|
|
p.NavigationConfig.MaxLinearVelocity,
|
|
Math.Max(p.NavigationConfig.MaxLinearVelocity * 0.85, 0.5),
|
|
"[PathFollowing] Reduce max velocity — robot may be too fast for accurate tracking",
|
|
SuggestionPriority.Medium, effPattern.Severity * 0.7 * dataQuality,
|
|
DiagnosticCategory.PathEfficiency,
|
|
"Better tracking at lower speed, reduced path length ratio",
|
|
TelemetryPhase.PathFollowing));
|
|
}
|
|
|
|
return suggestions;
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Conflict Resolution (Phase-Aware)
|
|
|
|
/// <summary>
|
|
/// Resolve conflicts grouping by (ParameterPath, TargetPhase).
|
|
/// Different phases can have different suggestions for the same parameter.
|
|
/// </summary>
|
|
private List<TuningSuggestion> ResolveConflicts(List<TuningSuggestion> suggestions)
|
|
{
|
|
// Group by (ParameterPath, TargetPhase) — same param can have different suggestions per phase
|
|
var grouped = suggestions.GroupBy(s => (s.ParameterPath, s.TargetPhase));
|
|
var resolved = new List<TuningSuggestion>();
|
|
|
|
foreach (var group in grouped)
|
|
{
|
|
var items = group.ToList();
|
|
if (items.Count == 1)
|
|
{
|
|
resolved.Add(items[0]);
|
|
continue;
|
|
}
|
|
|
|
// Check for conflicting directions within the same (param, phase)
|
|
bool hasIncrease = items.Any(s => s.SuggestedValue > s.CurrentValue);
|
|
bool hasDecrease = items.Any(s => s.SuggestedValue < s.CurrentValue);
|
|
|
|
if (hasIncrease && hasDecrease)
|
|
{
|
|
var bestItem = items.OrderByDescending(s => s.Confidence).First();
|
|
|
|
var secondBest = items.OrderByDescending(s => s.Confidence).Skip(1).First();
|
|
if (Math.Abs(bestItem.Confidence - secondBest.Confidence) < 0.15)
|
|
{
|
|
double compromiseValue = (bestItem.SuggestedValue + bestItem.CurrentValue) / 2.0;
|
|
bestItem.SuggestedValue = Math.Round(compromiseValue, 4);
|
|
bestItem.Reason += " [Compromise: conflicting suggestions detected - change reduced]";
|
|
bestItem.Confidence *= 0.7;
|
|
}
|
|
else
|
|
{
|
|
bestItem.Reason += $" [Note: conflicting suggestion from {secondBest.RelatedPatterns.FirstOrDefault()} was overridden]";
|
|
}
|
|
|
|
resolved.Add(bestItem);
|
|
}
|
|
else
|
|
{
|
|
// Same direction: pick the most aggressive
|
|
var bestItem = items.OrderByDescending(s => Math.Abs(s.SuggestedValue - s.CurrentValue)).First();
|
|
bestItem.RelatedPatterns = items.SelectMany(s => s.RelatedPatterns).Distinct().ToList();
|
|
resolved.Add(bestItem);
|
|
}
|
|
}
|
|
|
|
return resolved;
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Assessment Generation (Phase-Aware)
|
|
|
|
private string GenerateOverallAssessment(
|
|
TestMetrics metrics,
|
|
List<DiagnosticPattern> patterns,
|
|
List<TuningSuggestion> suggestions,
|
|
Dictionary<TelemetryPhase, PhaseMetrics> phaseMetricsMap)
|
|
{
|
|
if (patterns.Count == 0)
|
|
return $"Excellent performance! Overall score: {metrics.OverallScore:F0}/100. " +
|
|
"No significant issues detected. Parameters are well-tuned for this scenario.";
|
|
|
|
int criticalCount = suggestions.Count(s => s.Priority == SuggestionPriority.Critical);
|
|
int highCount = suggestions.Count(s => s.Priority == SuggestionPriority.High);
|
|
|
|
var parts = new List<string>();
|
|
parts.Add($"Overall score: {metrics.OverallScore:F0}/100.");
|
|
|
|
if (criticalCount > 0)
|
|
parts.Add($"{criticalCount} critical issue(s) require immediate attention.");
|
|
if (highCount > 0)
|
|
parts.Add($"{highCount} high-priority suggestion(s) for improvement.");
|
|
|
|
// Per-phase assessment
|
|
foreach (var (phase, pm) in phaseMetricsMap.OrderBy(kv => kv.Key))
|
|
{
|
|
if (phase == TelemetryPhase.Completed || pm.SampleCount < 5)
|
|
continue;
|
|
|
|
var phaseStatus = new List<string>();
|
|
|
|
var cteThreshold = PhaseThresholds.GetCteRmsThreshold(phase);
|
|
if (cteThreshold.HasValue)
|
|
{
|
|
string cteStatus = pm.CrossTrackErrorRMS <= cteThreshold.Value ? "Good" : "Needs improvement";
|
|
phaseStatus.Add($"CTE RMS={pm.CrossTrackErrorRMS:F4}m ({cteStatus}, target: {cteThreshold.Value}m)");
|
|
}
|
|
|
|
var headingThreshold = PhaseThresholds.GetHeadingRmsThresholdDeg(phase);
|
|
if (headingThreshold.HasValue)
|
|
{
|
|
double headingDeg = pm.HeadingErrorRMS / Deg2Rad;
|
|
string headingStatus = headingDeg <= headingThreshold.Value ? "Good" : "Needs improvement";
|
|
phaseStatus.Add($"Heading RMS={headingDeg:F1}° ({headingStatus}, target: {headingThreshold.Value}°)");
|
|
}
|
|
|
|
if (phaseStatus.Count > 0)
|
|
parts.Add($"{phase}: {string.Join(", ", phaseStatus)}.");
|
|
}
|
|
|
|
// Summarize main issues by phase
|
|
var patternsByPhase = patterns.GroupBy(p => p.DetectedInPhase);
|
|
foreach (var group in patternsByPhase.OrderBy(g => g.Key))
|
|
{
|
|
int phaseHighCount = suggestions.Count(s => s.TargetPhase == group.Key && s.Priority >= SuggestionPriority.High);
|
|
if (phaseHighCount > 0)
|
|
parts.Add($"{phaseHighCount} high-priority suggestion(s) for {group.Key}.");
|
|
}
|
|
|
|
parts.Add($"{suggestions.Count} total parameter adjustment(s) suggested.");
|
|
|
|
if (metrics.PassedCriteria)
|
|
parts.Add("Acceptance criteria: PASSED.");
|
|
else
|
|
parts.Add("Acceptance criteria: NOT PASSED.");
|
|
|
|
return string.Join(" ", parts);
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Phase Segmentation Helpers
|
|
|
|
private static Dictionary<TelemetryPhase, List<TelemetryData>> SegmentByPhase(List<TelemetryData> data)
|
|
{
|
|
var segments = new Dictionary<TelemetryPhase, List<TelemetryData>>();
|
|
foreach (var d in data)
|
|
{
|
|
// Legacy data (Phase == null) → PathFollowing
|
|
var phase = d.Phase ?? TelemetryPhase.PathFollowing;
|
|
if (!segments.TryGetValue(phase, out var list))
|
|
{
|
|
list = new List<TelemetryData>();
|
|
segments[phase] = list;
|
|
}
|
|
list.Add(d);
|
|
}
|
|
return segments;
|
|
}
|
|
|
|
private static PhaseMetrics ComputePhaseMetrics(TelemetryPhase phase, List<TelemetryData> data)
|
|
{
|
|
var pm = new PhaseMetrics
|
|
{
|
|
Phase = phase,
|
|
SampleCount = data.Count
|
|
};
|
|
|
|
if (data.Count == 0) return pm;
|
|
|
|
pm.DurationMs = data.Count > 1 ? data[^1].TimestampMs - data[0].TimestampMs : 0;
|
|
|
|
// CTE
|
|
var ctes = data.Select(d => d.CrossTrackError).ToList();
|
|
pm.CrossTrackErrorMean = ctes.Average();
|
|
pm.CrossTrackErrorPeak = ctes.Max();
|
|
pm.CrossTrackErrorRMS = Math.Sqrt(ctes.Average(c => c * c));
|
|
|
|
// Heading error
|
|
var headings = data.Select(d => Math.Abs(d.HeadingError)).ToList();
|
|
pm.HeadingErrorPeak = headings.Max();
|
|
pm.HeadingErrorRMS = Math.Sqrt(headings.Average(h => h * h));
|
|
|
|
// Angular velocity StdDev
|
|
var angVels = data.Select(d => d.RobotTwist.Angular).ToList();
|
|
pm.AngularVelocityStdDev = CalculateStdDev(angVels);
|
|
|
|
// Linear velocity StdDev
|
|
var linVels = data.Select(d => d.RobotTwist.Linear).ToList();
|
|
pm.VelocityStdDev = CalculateStdDev(linVels);
|
|
|
|
// Acceleration StdDev (approximate from velocity differences)
|
|
if (data.Count >= 3)
|
|
{
|
|
var accels = new List<double>();
|
|
for (int i = 1; i < data.Count; i++)
|
|
{
|
|
double dt = (data[i].TimestampMs - data[i - 1].TimestampMs) / 1000.0;
|
|
if (dt > 0)
|
|
accels.Add((data[i].RobotTwist.Linear - data[i - 1].RobotTwist.Linear) / dt);
|
|
}
|
|
pm.AccelerationStdDev = accels.Count > 0 ? CalculateStdDev(accels) : 0;
|
|
}
|
|
|
|
// Goal metrics (last sample)
|
|
var last = data[^1];
|
|
pm.GoalPositionError = last.DistanceToGoal;
|
|
pm.GoalHeadingErrorDeg = Math.Abs(last.HeadingError) / Deg2Rad;
|
|
|
|
return pm;
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Helpers
|
|
|
|
private static TuningSuggestion CreateSuggestion(
|
|
string paramPath,
|
|
string displayName,
|
|
double currentValue,
|
|
double suggestedValue,
|
|
string reason,
|
|
SuggestionPriority priority,
|
|
double confidence,
|
|
DiagnosticCategory relatedPattern,
|
|
string expectedImpact,
|
|
TelemetryPhase? targetPhase = null)
|
|
{
|
|
// Round suggested value to reasonable precision
|
|
suggestedValue = Math.Round(suggestedValue, 4);
|
|
|
|
// Don't suggest if the change is negligible (< 1%)
|
|
if (currentValue != 0 && Math.Abs(suggestedValue - currentValue) / Math.Abs(currentValue) < 0.01)
|
|
suggestedValue = currentValue; // Will be filtered out
|
|
|
|
return new TuningSuggestion
|
|
{
|
|
ParameterPath = paramPath,
|
|
ParameterDisplayName = displayName,
|
|
CurrentValue = currentValue,
|
|
SuggestedValue = suggestedValue,
|
|
Reason = reason,
|
|
Priority = priority,
|
|
Confidence = Math.Clamp(confidence, 0, 1),
|
|
RelatedPatterns = [relatedPattern],
|
|
ExpectedImpact = expectedImpact,
|
|
TargetPhase = targetPhase
|
|
};
|
|
}
|
|
|
|
private static double CalculateStdDev(List<double> values)
|
|
{
|
|
if (values.Count == 0) return 0;
|
|
double mean = values.Average();
|
|
double variance = values.Average(v => (v - mean) * (v - mean));
|
|
return Math.Sqrt(variance);
|
|
}
|
|
|
|
private static double GetMedian(List<double> values)
|
|
{
|
|
if (values.Count == 0) return 0;
|
|
var sorted = values.OrderBy(v => v).ToList();
|
|
int mid = sorted.Count / 2;
|
|
return sorted.Count % 2 == 0
|
|
? (sorted[mid - 1] + sorted[mid]) / 2.0
|
|
: sorted[mid];
|
|
}
|
|
|
|
private static double NormalizeAngle(double angle)
|
|
{
|
|
while (angle > Math.PI) angle -= 2 * Math.PI;
|
|
while (angle < -Math.PI) angle += 2 * Math.PI;
|
|
return angle;
|
|
}
|
|
|
|
private static NavigationParameterSet DeepClone(NavigationParameterSet source)
|
|
{
|
|
var json = JsonSerializer.Serialize(source);
|
|
return JsonSerializer.Deserialize<NavigationParameterSet>(json)!;
|
|
}
|
|
|
|
private static void SetParameterValue(NavigationParameterSet target, string dotPath, double value)
|
|
{
|
|
var parts = dotPath.Split('.');
|
|
object current = target;
|
|
|
|
for (int i = 0; i < parts.Length - 1; i++)
|
|
{
|
|
var prop = current.GetType().GetProperty(parts[i],
|
|
BindingFlags.Public | BindingFlags.Instance);
|
|
if (prop == null)
|
|
throw new ArgumentException($"Property '{parts[i]}' not found on {current.GetType().Name}");
|
|
current = prop.GetValue(current)!;
|
|
}
|
|
|
|
var finalProp = current.GetType().GetProperty(parts[^1],
|
|
BindingFlags.Public | BindingFlags.Instance);
|
|
if (finalProp == null)
|
|
throw new ArgumentException($"Property '{parts[^1]}' not found on {current.GetType().Name}");
|
|
|
|
finalProp.SetValue(current, value);
|
|
}
|
|
|
|
#endregion
|
|
}
|