510 lines
19 KiB
C#
510 lines
19 KiB
C#
using RobotNet10.NavigationTune.Shared.Interfaces;
|
|
using RobotNet10.NavigationTune.Shared.Models;
|
|
|
|
namespace RobotNet10.NavigationTune.Services;
|
|
|
|
/// <summary>
|
|
/// Metrics calculator implementation
|
|
/// </summary>
|
|
public class MetricsCalculator : IMetricsCalculator
|
|
{
|
|
public TestMetrics CalculateMetrics(
|
|
List<TelemetryData> telemetryData,
|
|
ReferencePath referencePath)
|
|
{
|
|
if (telemetryData.Count == 0)
|
|
throw new ArgumentException("Telemetry data cannot be empty", nameof(telemetryData));
|
|
|
|
var tracking = CalculateTrackingAccuracy(telemetryData, referencePath);
|
|
var smoothness = CalculateSmoothness(telemetryData);
|
|
var efficiency = CalculateEfficiency(telemetryData, referencePath);
|
|
|
|
var metrics = new TestMetrics
|
|
{
|
|
// Tracking Accuracy
|
|
CrossTrackErrorRMS = tracking.CrossTrackErrorRMS,
|
|
CrossTrackErrorPeak = tracking.CrossTrackErrorPeak,
|
|
CrossTrackErrorMean = tracking.CrossTrackErrorMean,
|
|
CrossTrackErrorStdDev = tracking.CrossTrackErrorStdDev,
|
|
HeadingErrorRMS = tracking.HeadingErrorRMS,
|
|
HeadingErrorPeak = tracking.HeadingErrorPeak,
|
|
GoalPositionError = tracking.GoalPositionError,
|
|
GoalHeadingError = tracking.GoalHeadingError,
|
|
|
|
// Smoothness
|
|
VelocityStdDev = smoothness.VelocityStdDev,
|
|
AccelerationStdDev = smoothness.AccelerationStdDev,
|
|
|
|
// Efficiency
|
|
PathLengthRatio = efficiency.PathLengthRatio,
|
|
CompletionTime = efficiency.CompletionTime,
|
|
AverageSpeed = efficiency.AverageSpeed,
|
|
MaxSpeed = efficiency.MaxSpeed
|
|
};
|
|
|
|
// Calculate scores
|
|
var weights = new ScoringWeights();
|
|
metrics.TrackingScore = CalculateTrackingScore(tracking);
|
|
metrics.SmoothnessScore = CalculateSmoothnessScore(smoothness);
|
|
metrics.EfficiencyScore = CalculateEfficiencyScore(efficiency);
|
|
metrics.OverallScore = CalculateOverallScore(metrics, weights);
|
|
|
|
// Check if passed criteria
|
|
metrics.PassedCriteria = CheckAcceptanceCriteria(metrics);
|
|
|
|
// Ensure no NaN/Infinity so SignalR and DB serialization do not fail
|
|
SanitizeMetrics(metrics);
|
|
return metrics;
|
|
}
|
|
|
|
private static double ToFinite(double value, double fallback = 0)
|
|
{
|
|
return double.IsFinite(value) ? value : fallback;
|
|
}
|
|
|
|
private static void SanitizeMetrics(TestMetrics m)
|
|
{
|
|
m.CrossTrackErrorRMS = ToFinite(m.CrossTrackErrorRMS);
|
|
m.CrossTrackErrorPeak = ToFinite(m.CrossTrackErrorPeak);
|
|
m.CrossTrackErrorMean = ToFinite(m.CrossTrackErrorMean);
|
|
m.CrossTrackErrorStdDev = ToFinite(m.CrossTrackErrorStdDev);
|
|
m.HeadingErrorRMS = ToFinite(m.HeadingErrorRMS);
|
|
m.HeadingErrorPeak = ToFinite(m.HeadingErrorPeak);
|
|
m.GoalPositionError = ToFinite(m.GoalPositionError);
|
|
m.GoalHeadingError = ToFinite(m.GoalHeadingError);
|
|
m.VelocityStdDev = ToFinite(m.VelocityStdDev);
|
|
m.AccelerationStdDev = ToFinite(m.AccelerationStdDev);
|
|
m.PathLengthRatio = ToFinite(m.PathLengthRatio, 1);
|
|
m.CompletionTime = ToFinite(m.CompletionTime);
|
|
m.AverageSpeed = ToFinite(m.AverageSpeed);
|
|
m.MaxSpeed = ToFinite(m.MaxSpeed);
|
|
m.OverallScore = ToFinite(m.OverallScore);
|
|
m.TrackingScore = ToFinite(m.TrackingScore);
|
|
m.SmoothnessScore = ToFinite(m.SmoothnessScore);
|
|
m.EfficiencyScore = ToFinite(m.EfficiencyScore);
|
|
}
|
|
|
|
public TrackingAccuracyMetrics CalculateTrackingAccuracy(
|
|
List<TelemetryData> telemetryData,
|
|
ReferencePath referencePath)
|
|
{
|
|
var cteValues = new List<double>();
|
|
var headingErrors = new List<double>();
|
|
|
|
foreach (var data in telemetryData)
|
|
{
|
|
cteValues.Add(data.CrossTrackError);
|
|
headingErrors.Add(Math.Abs(data.HeadingError));
|
|
}
|
|
|
|
// Calculate RMS
|
|
double cteRMS = CalculateRMS(cteValues);
|
|
double headingRMS = CalculateRMS(headingErrors);
|
|
|
|
// Goal accuracy (last 10% of data)
|
|
int goalSampleCount = Math.Max(1, telemetryData.Count / 10);
|
|
var finalData = telemetryData.TakeLast(goalSampleCount).ToList();
|
|
double goalPositionError = finalData.Average(d => d.DistanceToGoal);
|
|
double goalHeadingError = finalData.Average(d => Math.Abs(d.HeadingError));
|
|
|
|
return new TrackingAccuracyMetrics
|
|
{
|
|
CrossTrackErrorRMS = cteRMS,
|
|
CrossTrackErrorPeak = cteValues.Max(),
|
|
CrossTrackErrorMean = cteValues.Average(),
|
|
CrossTrackErrorStdDev = CalculateStdDev(cteValues),
|
|
HeadingErrorRMS = headingRMS,
|
|
HeadingErrorPeak = headingErrors.Max(),
|
|
GoalPositionError = goalPositionError,
|
|
GoalHeadingError = goalHeadingError
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// Nominal control loop period (50Hz) in seconds.
|
|
/// </summary>
|
|
private const double NominalDtSeconds = 1.0 / 50.0;
|
|
|
|
/// <summary>
|
|
/// Max dt (s) for smoothness calculation.
|
|
/// </summary>
|
|
private const double MaxDtSeconds = 0.5;
|
|
|
|
/// <summary>
|
|
/// Percentage of samples to trim from start/end to remove transient periods (startup/shutdown).
|
|
/// 5% means skip first 5% and last 5% of trajectory.
|
|
/// </summary>
|
|
private const double TransientTrimPercent = 0.05f;
|
|
|
|
/// <summary>
|
|
/// Maximum physically plausible acceleration for the robot (m/s²).
|
|
/// Velocity changes exceeding this per sample are considered outliers.
|
|
/// </summary>
|
|
private const double MaxPlausibleAcceleration = 3.0;
|
|
|
|
/// <summary>
|
|
/// Threshold multiplier for spike detection.
|
|
/// A point is considered a spike if it deviates from neighbors by more than
|
|
/// SpikeThresholdMultiplier * median_change_of_neighbors.
|
|
/// </summary>
|
|
private const double SpikeThresholdMultiplier = 3.0;
|
|
|
|
/// <summary>
|
|
/// Minimum absolute deviation (m/s) to consider as potential spike.
|
|
/// Prevents small natural variations from being filtered.
|
|
/// </summary>
|
|
private const double MinSpikeDeviation = 0.02f;
|
|
|
|
public SmoothnessMetrics CalculateSmoothness(List<TelemetryData> telemetryData)
|
|
{
|
|
if (telemetryData.Count < 3)
|
|
return new SmoothnessMetrics();
|
|
|
|
// Step 1: Trim transient periods (startup/shutdown)
|
|
var stableData = TrimTransientPeriod(telemetryData, TransientTrimPercent);
|
|
if (stableData.Count < 3)
|
|
return new SmoothnessMetrics();
|
|
|
|
// Step 2: Calculate dt from stable data
|
|
long totalSpanMs = stableData[^1].TimestampMs - stableData[0].TimestampMs;
|
|
int intervalCount = stableData.Count - 1;
|
|
double avgDtSeconds = intervalCount > 0 && totalSpanMs > 0
|
|
? (totalSpanMs / 1000.0) / intervalCount
|
|
: NominalDtSeconds;
|
|
double dt = Math.Clamp(avgDtSeconds, NominalDtSeconds, MaxDtSeconds);
|
|
|
|
// Step 3: Extract and clean velocity data (multi-stage filtering)
|
|
var rawVelocities = stableData.Select(d => d.RobotTwist.Linear).ToList();
|
|
|
|
// Stage 3a: Remove single-cycle spikes first (noise from sensor glitches)
|
|
var despikedVelocities = RemoveSingleCycleSpikes(rawVelocities);
|
|
|
|
// Stage 3b: Remove remaining outliers using acceleration-based detection
|
|
var velocities = RemoveVelocityOutliers(despikedVelocities, dt, MaxPlausibleAcceleration);
|
|
|
|
// Step 4: Calculate accelerations (linear: m/s²)
|
|
var accelerations = new List<double>();
|
|
for (int i = 1; i < velocities.Count; i++)
|
|
{
|
|
double accel = (velocities[i] - velocities[i - 1]) / dt;
|
|
accelerations.Add(accel);
|
|
}
|
|
|
|
// Step 5: Compute standard deviations
|
|
return new SmoothnessMetrics
|
|
{
|
|
VelocityStdDev = CalculateStdDev(velocities),
|
|
AccelerationStdDev = accelerations.Count > 0 ? CalculateStdDev(accelerations) : 0
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// Trim transient periods from start and end of trajectory.
|
|
/// Transient periods (startup/shutdown) naturally have high velocity/acceleration variance.
|
|
/// </summary>
|
|
private static List<TelemetryData> TrimTransientPeriod(List<TelemetryData> data, double trimPercent)
|
|
{
|
|
if (data.Count < 10) return data; // Too short to trim
|
|
|
|
int trimCount = Math.Max(1, (int)(data.Count * trimPercent));
|
|
int startIndex = trimCount;
|
|
int endIndex = data.Count - trimCount;
|
|
|
|
if (endIndex <= startIndex) return data; // Would result in empty list
|
|
|
|
return data.Skip(startIndex).Take(endIndex - startIndex).ToList();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Remove single-cycle spikes from velocity data using multi-pass filtering.
|
|
/// A spike is detected when a single point deviates significantly from both neighbors,
|
|
/// while the neighbors themselves are consistent with each other.
|
|
///
|
|
/// Detection criteria for point i:
|
|
/// 1. |v[i] - v[i-1]| > threshold (large jump from previous)
|
|
/// 2. |v[i] - v[i+1]| > threshold (large jump to next)
|
|
/// 3. |v[i+1] - v[i-1]| <= threshold (neighbors are consistent)
|
|
///
|
|
/// When spike is detected, replace with average of neighbors.
|
|
/// Multi-pass ensures consecutive spikes are also handled.
|
|
/// </summary>
|
|
private static List<double> RemoveSingleCycleSpikes(List<double> velocities)
|
|
{
|
|
if (velocities.Count < 3)
|
|
return [.. velocities];
|
|
|
|
var current = velocities;
|
|
const int maxPasses = 3; // Multiple passes for consecutive spikes
|
|
|
|
for (int pass = 0; pass < maxPasses; pass++)
|
|
{
|
|
var cleaned = RemoveSingleCycleSpikesOnePass(current);
|
|
|
|
// Check if any changes were made
|
|
bool changed = false;
|
|
for (int i = 0; i < current.Count && !changed; i++)
|
|
{
|
|
if (Math.Abs(current[i] - cleaned[i]) > 1e-9)
|
|
changed = true;
|
|
}
|
|
|
|
current = cleaned;
|
|
if (!changed) break; // No more spikes found
|
|
}
|
|
|
|
return current;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Single pass of spike removal.
|
|
/// </summary>
|
|
private static List<double> RemoveSingleCycleSpikesOnePass(List<double> velocities)
|
|
{
|
|
var cleaned = new List<double>(velocities.Count) { velocities[0] };
|
|
|
|
// Calculate median absolute change for adaptive threshold
|
|
var changes = new List<double>();
|
|
for (int i = 1; i < velocities.Count; i++)
|
|
{
|
|
double change = Math.Abs(velocities[i] - velocities[i - 1]);
|
|
if (change > 1e-9) // Ignore zero changes
|
|
changes.Add(change);
|
|
}
|
|
|
|
double medianChange = changes.Count > 0 ? GetMedian(changes) : 0.01f;
|
|
double spikeThreshold = Math.Max(SpikeThresholdMultiplier * medianChange, MinSpikeDeviation);
|
|
|
|
// Process middle points using 3-point window
|
|
for (int i = 1; i < velocities.Count - 1; i++)
|
|
{
|
|
double prev = cleaned[^1]; // Use already-cleaned previous value
|
|
double curr = velocities[i];
|
|
double next = velocities[i + 1];
|
|
|
|
double changeToPrev = Math.Abs(curr - prev);
|
|
double changeToNext = Math.Abs(curr - next);
|
|
double neighborConsistency = Math.Abs(next - prev);
|
|
|
|
// Spike detection: current deviates from both neighbors, but neighbors are consistent
|
|
bool isSpike = changeToPrev > spikeThreshold &&
|
|
changeToNext > spikeThreshold &&
|
|
neighborConsistency <= spikeThreshold;
|
|
|
|
if (isSpike)
|
|
{
|
|
// Replace spike with average of neighbors
|
|
cleaned.Add((prev + next) / 2.0);
|
|
}
|
|
else
|
|
{
|
|
cleaned.Add(curr);
|
|
}
|
|
}
|
|
|
|
cleaned.Add(velocities[^1]); // Keep last point
|
|
|
|
return cleaned;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Calculate median of a list.
|
|
/// </summary>
|
|
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;
|
|
|
|
if (sorted.Count % 2 == 0)
|
|
return (sorted[mid - 1] + sorted[mid]) / 2.0;
|
|
else
|
|
return sorted[mid];
|
|
}
|
|
|
|
/// <summary>
|
|
/// Remove velocity outliers using acceleration-based detection.
|
|
/// If velocity change between consecutive samples exceeds physically plausible acceleration,
|
|
/// the point is considered an outlier and interpolated.
|
|
/// </summary>
|
|
private static List<double> RemoveVelocityOutliers(List<double> velocities, double dt, double maxAcceleration)
|
|
{
|
|
if (velocities.Count < 2) return velocities;
|
|
|
|
var cleaned = new List<double>(velocities.Count) { velocities[0] };
|
|
double maxVelocityChange = maxAcceleration * dt;
|
|
|
|
for (int i = 1; i < velocities.Count; i++)
|
|
{
|
|
double change = Math.Abs(velocities[i] - cleaned[^1]);
|
|
|
|
if (change <= maxVelocityChange)
|
|
{
|
|
// Normal change, keep the value
|
|
cleaned.Add(velocities[i]);
|
|
}
|
|
else
|
|
{
|
|
// Outlier detected - use linear interpolation
|
|
// Look ahead to find next valid point
|
|
double interpolatedValue = InterpolateOutlier(velocities, cleaned, i, maxVelocityChange);
|
|
cleaned.Add(interpolatedValue);
|
|
}
|
|
}
|
|
|
|
return cleaned;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Interpolate an outlier value by looking at surrounding valid points.
|
|
/// </summary>
|
|
private static double InterpolateOutlier(List<double> original, List<double> cleaned, int outlierIndex, double maxChange)
|
|
{
|
|
double lastValid = cleaned[^1];
|
|
|
|
// Look ahead to find next valid point (within 5 samples)
|
|
for (int lookAhead = 1; lookAhead <= Math.Min(5, original.Count - outlierIndex - 1); lookAhead++)
|
|
{
|
|
int nextIndex = outlierIndex + lookAhead;
|
|
double nextValue = original[nextIndex];
|
|
double totalChange = Math.Abs(nextValue - lastValid);
|
|
double allowedChange = maxChange * (lookAhead + 1);
|
|
|
|
if (totalChange <= allowedChange)
|
|
{
|
|
// Found a valid point - interpolate linearly
|
|
double step = (nextValue - lastValid) / (lookAhead + 1);
|
|
return lastValid + step;
|
|
}
|
|
}
|
|
|
|
// No valid point found - use last valid value (hold)
|
|
return lastValid;
|
|
}
|
|
|
|
public EfficiencyMetrics CalculateEfficiency(
|
|
List<TelemetryData> telemetryData,
|
|
ReferencePath referencePath)
|
|
{
|
|
if (telemetryData.Count < 2)
|
|
return new EfficiencyMetrics();
|
|
|
|
// Calculate actual path length
|
|
double actualPathLength = 0;
|
|
for (int i = 1; i < telemetryData.Count; i++)
|
|
{
|
|
double dx = telemetryData[i].RobotPose.X - telemetryData[i - 1].RobotPose.X;
|
|
double dy = telemetryData[i].RobotPose.Y - telemetryData[i - 1].RobotPose.Y;
|
|
actualPathLength += Math.Sqrt(dx * dx + dy * dy);
|
|
}
|
|
|
|
// Reference path length
|
|
double referencePathLength = referencePath.TotalLength;
|
|
|
|
// Completion time
|
|
long duration = telemetryData[^1].TimestampMs - telemetryData[0].TimestampMs;
|
|
double completionTime = duration / 1000.0;
|
|
|
|
// Speeds
|
|
var speeds = telemetryData.Select(d => Math.Abs(d.RobotTwist.Linear)).ToList();
|
|
|
|
return new EfficiencyMetrics
|
|
{
|
|
PathLengthRatio = referencePathLength > 0 ? actualPathLength / referencePathLength : 1.0,
|
|
CompletionTime = completionTime,
|
|
AverageSpeed = speeds.Average(),
|
|
MaxSpeed = speeds.Max()
|
|
};
|
|
}
|
|
|
|
public double CalculateOverallScore(TestMetrics metrics, ScoringWeights weights)
|
|
{
|
|
double score = 100.0;
|
|
|
|
// Tracking accuracy penalties (50% weight)
|
|
score -= weights.TrackingAccuracy * (
|
|
NormalizePenalty(metrics.CrossTrackErrorRMS, 0.10f, 20f) +
|
|
NormalizePenalty(metrics.HeadingErrorRMS, 10f * Deg2Rad, 20f) +
|
|
NormalizePenalty(metrics.GoalPositionError, 0.05f, 10f)
|
|
);
|
|
|
|
// Smoothness penalties (30% weight)
|
|
score -= weights.Smoothness * (
|
|
NormalizePenalty(metrics.VelocityStdDev, 0.1, 15f) +
|
|
NormalizePenalty(metrics.AccelerationStdDev, 0.5, 15f)
|
|
);
|
|
|
|
// Efficiency penalties (20% weight)
|
|
score -= weights.Efficiency * (
|
|
NormalizePenalty(metrics.PathLengthRatio - 1.0, 0.15f, 20f)
|
|
);
|
|
|
|
return Math.Max(0, score);
|
|
}
|
|
|
|
private double CalculateTrackingScore(TrackingAccuracyMetrics tracking)
|
|
{
|
|
double score = 100.0;
|
|
score -= NormalizePenalty(tracking.CrossTrackErrorRMS, 0.10f, 40f);
|
|
score -= NormalizePenalty(tracking.HeadingErrorRMS, 10f * Deg2Rad, 40f);
|
|
score -= NormalizePenalty(tracking.GoalPositionError, 0.05f, 20f);
|
|
return Math.Max(0, score);
|
|
}
|
|
|
|
private double CalculateSmoothnessScore(SmoothnessMetrics smoothness)
|
|
{
|
|
double score = 100.0;
|
|
score -= NormalizePenalty(smoothness.VelocityStdDev, 0.1, 50f);
|
|
score -= NormalizePenalty(smoothness.AccelerationStdDev, 0.5, 50f);
|
|
return Math.Max(0, score);
|
|
}
|
|
|
|
private double CalculateEfficiencyScore(EfficiencyMetrics efficiency)
|
|
{
|
|
double score = 100.0;
|
|
score -= NormalizePenalty(efficiency.PathLengthRatio - 1.0, 0.15f, 100f);
|
|
return Math.Max(0, score);
|
|
}
|
|
|
|
private bool CheckAcceptanceCriteria(TestMetrics metrics)
|
|
{
|
|
// Primary criteria (tracking)
|
|
if (metrics.CrossTrackErrorRMS > 0.10f) return false;
|
|
if (metrics.CrossTrackErrorPeak > 0.20f) return false;
|
|
if (metrics.HeadingErrorRMS > 10f * Deg2Rad) return false;
|
|
if (metrics.GoalPositionError > 0.05f) return false;
|
|
|
|
// Secondary criteria (efficiency)
|
|
if (metrics.PathLengthRatio > 1.15f) return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
private double NormalizePenalty(double actual, double threshold, double maxPenalty)
|
|
{
|
|
if (!double.IsFinite(actual)) return maxPenalty;
|
|
if (actual <= threshold) return 0;
|
|
|
|
double excess = actual - threshold;
|
|
double penalty = (excess / threshold) * maxPenalty;
|
|
return Math.Min(penalty, maxPenalty);
|
|
}
|
|
|
|
private double CalculateRMS(List<double> values)
|
|
{
|
|
if (values.Count == 0) return 0;
|
|
double sumSquares = values.Sum(v => v * v);
|
|
return Math.Sqrt(sumSquares / values.Count);
|
|
}
|
|
|
|
private 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 const double Deg2Rad = Math.PI / 180.0;
|
|
}
|