Initial commit
This commit is contained in:
@@ -0,0 +1,509 @@
|
||||
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;
|
||||
}
|
||||
Reference in New Issue
Block a user