Initial commit

srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Services/MetricsCalculator.cs

@@ -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;
+}

srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Services/ParameterManager.cs

@@ -0,0 +1,198 @@
+using Microsoft.EntityFrameworkCore;
+using RobotNet10.NavigationTune.Data;
+using RobotNet10.NavigationTune.Shared.Interfaces;
+using RobotNet10.NavigationTune.Shared.Models;
+
+namespace RobotNet10.NavigationTune.Services;
+
+/// <summary>
+/// Parameter manager implementation
+/// </summary>
+public class ParameterManager(TuningDbContext context) : IParameterManager
+{
+    private readonly TuningDbContext _context = context;
+
+    public async Task<NavigationParameterSet?> GetByNameAsync(string name)
+    {
+        return await _context.ParameterSets
+            .FirstOrDefaultAsync(p => p.Name == name);
+    }
+
+    public async Task<NavigationParameterSet?> GetByIdAsync(Guid id)
+    {
+        return await _context.ParameterSets.FindAsync(id);
+    }
+
+    public async Task<List<NavigationParameterSet>> GetAllAsync()
+    {
+        return await _context.ParameterSets
+            .OrderByDescending(p => p.CreatedAt)
+            .ToListAsync();
+    }
+
+    public async Task<Guid> SaveAsync(NavigationParameterSet parameterSet)
+    {
+        if (parameterSet.Id == Guid.Empty)
+            parameterSet.Id = Guid.NewGuid();
+
+        parameterSet.CreatedAt = DateTime.UtcNow;
+        _context.ParameterSets.Add(parameterSet);
+        await _context.SaveChangesAsync();
+        return parameterSet.Id;
+    }
+
+    public async Task UpdateAsync(NavigationParameterSet parameterSet)
+    {
+        parameterSet.UpdatedAt = DateTime.UtcNow;
+        _context.ParameterSets.Update(parameterSet);
+        await _context.SaveChangesAsync();
+    }
+
+    public async Task DeleteAsync(Guid id)
+    {
+        var parameterSet = await GetByIdAsync(id);
+        if (parameterSet != null)
+        {
+            _context.ParameterSets.Remove(parameterSet);
+            await _context.SaveChangesAsync();
+        }
+    }
+
+    public ValidationResult Validate(NavigationParameterSet parameterSet)
+    {
+        var result = new ValidationResult { IsValid = true };
+
+        // Validate PID bounds
+        if (parameterSet.MovePidConfig.Kp < 0.1 || parameterSet.MovePidConfig.Kp > 5.0)
+            result.AddError($"Move PID Kp must be between 0.1 and 5.0, got {parameterSet.MovePidConfig.Kp}");
+
+        if (parameterSet.MovePidConfig.Ki < 0 || parameterSet.MovePidConfig.Ki > 2.0)
+            result.AddError($"Move PID Ki must be between 0 and 2.0, got {parameterSet.MovePidConfig.Ki}");
+
+        if (parameterSet.MovePidConfig.Kd < 0 || parameterSet.MovePidConfig.Kd > 1.0)
+            result.AddError($"Move PID Kd must be between 0 and 1.0, got {parameterSet.MovePidConfig.Kd}");
+
+        // Validate Pure Pursuit
+        if (parameterSet.PurePursuitConfig.LookaheadMax <= parameterSet.PurePursuitConfig.LookaheadMin)
+            result.AddError("LookaheadMax must be greater than LookaheadMin");
+
+        if (parameterSet.PurePursuitConfig.Kdd < 0.3 || parameterSet.PurePursuitConfig.Kdd > 2.0)
+            result.AddError($"Kdd must be between 0.3 and 2.0, got {parameterSet.PurePursuitConfig.Kdd}");
+
+        // Validate velocity limits
+        if (parameterSet.NavigationConfig.MaxLinearVelocity > 2.0)
+            result.AddWarning("MaxLinearVelocity > 2.0 m/s may be unsafe");
+
+        // Validate blend ratios
+        if (parameterSet.EstimatorConfig.GoodTrackingBlend < parameterSet.EstimatorConfig.PoorTrackingBlend)
+            result.AddError("GoodTrackingBlend must be greater than PoorTrackingBlend (trust encoder more when tracking is good)");
+
+        return result;
+    }
+
+    public NavigationParameterSet GetDefaultPreset()
+    {
+        return new NavigationParameterSet
+        {
+            Name = "Default",
+            Description = "Default parameter set",
+            IsDefault = true,
+            ControllerType = PathFollowingController.PurePursuit, // Default to Pure Pursuit
+            MovePidConfig = new PIDConfig { Kp = 1.0, Ki = 0.0001, Kd = 0.6 },
+            RotatePidConfig = new PIDConfig { Kp = 10.0, Ki = 0.01, Kd = 0.1 },
+            PurePursuitConfig = new PurePursuitConfig
+            {
+                LookaheadMin = 0.3,
+                Kdd = 1.0,
+                LookaheadMax = 2.0,
+                MaxAngularVelocity = 1.5,
+                ResolutionSplit = 0.05f,
+                FinalApproachThreshold = 0.2,
+                HeadingTolerance = 3.0,
+                GoalRegionDistance = 1.5,
+                KCurvature = 2.0,
+                MinLookaheadTimeRatio = 0.3,
+                MaxLookaheadTimeRatio = 2.0
+            },
+            StanleyConfig = new StanleyConfig
+            {
+                K = 2.5,
+                Ks = 0.1,
+                WheelBase = 0.5,
+                MaxSteeringAngle = 0.5,
+                EnableCurvatureFeedforward = true,
+                KCurvatureFF = 1.0,
+                GoalTolerance = 0.05,
+                HeadingTolerance = 5.0,
+                ResolutionSplit = 0.05,
+                GoalApproachDistance = 1.0,
+                GoalGainMultiplier = 2.0,
+                LowSpeedThreshold = 0.3,
+                LowSpeedAngularGain = 1.5
+            },
+            EstimatorConfig = new VelocityEstimatorConfig(),
+            SignalConfig = new VelocitySignalProcessingConfig(),
+            MotorDynamicsConfig = new MotorDynamicsConfig { Tau = 0.3, Delta = 0.05f },
+            NavigationConfig = new NavigationConfig
+            {
+                MaxLinearVelocity = 1.5,
+                MaxAngularVelocity = 6.0,
+                MinLinearVelocity = 0.1,
+                RotateAngularVelocity = 1.0,
+                ReachedRadius = 0.015,
+                InitialRotationThreshold = 5.0,
+                Acceleration = 0.5,
+                Deceleration = 0.5
+            }
+        };
+    }
+
+    public NavigationParameterSet GetAggressivePreset()
+    {
+        var preset = GetDefaultPreset();
+        preset.Name = "Aggressive";
+        preset.Description = "Aggressive tuning for fast response";
+        preset.MovePidConfig.Kp = 1.5;
+        preset.MovePidConfig.Ki = 0.2;
+        preset.MovePidConfig.Kd = 0.02;
+        preset.PurePursuitConfig.Kdd = 0.8f;
+        return preset;
+    }
+
+    public NavigationParameterSet GetSmoothPreset()
+    {
+        var preset = GetDefaultPreset();
+        preset.Name = "Smooth";
+        preset.Description = "Smooth tuning for gentle motion";
+        preset.MovePidConfig.Kp = 0.6;
+        preset.MovePidConfig.Ki = 0.05;
+        preset.MovePidConfig.Kd = 0.3;
+        preset.PurePursuitConfig.Kdd = 1.5;
+        preset.SignalConfig.AlphaFilter = 0.2;
+        return preset;
+    }
+
+    public NavigationParameterSet GetStanleyPreset()
+    {
+        var preset = GetDefaultPreset();
+        preset.Name = "Stanley";
+        preset.Description = "Stanley controller for high-speed path tracking";
+        preset.ControllerType = PathFollowingController.Stanley;
+
+        // Stanley-specific tuning
+        preset.StanleyConfig.K = 2.5;
+        preset.StanleyConfig.Ks = 0.1;
+        preset.StanleyConfig.WheelBase = 0.6;
+        preset.StanleyConfig.MaxSteeringAngle = 0.5;
+        preset.StanleyConfig.EnableCurvatureFeedforward = true;
+        preset.StanleyConfig.KCurvatureFF = 1.0;
+        preset.StanleyConfig.GoalTolerance = 0.05;
+        preset.StanleyConfig.HeadingTolerance = 5.0;
+        preset.StanleyConfig.GoalApproachDistance = 1.0;
+        preset.StanleyConfig.GoalGainMultiplier = 2.0;
+        preset.StanleyConfig.LowSpeedThreshold = 0.3;
+        preset.StanleyConfig.LowSpeedAngularGain = 1.5;
+
+        return preset;
+    }
+}

srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Services/RunningTestCancellationRegistry.cs

@@ -0,0 +1,39 @@
+using System.Collections.Concurrent;
+using RobotNet10.NavigationTune.Interfaces;
+
+namespace RobotNet10.NavigationTune.Services;
+
+/// <summary>
+/// Singleton registry mapping testRunId to CancellationTokenSource so Stop/EMC Stop can cancel the running test.
+/// </summary>
+public class RunningTestCancellationRegistry : IRunningTestCancellationRegistry
+{
+    private readonly ConcurrentDictionary<Guid, CancellationTokenSource> _map = new();
+
+    public void Register(Guid testRunId, CancellationTokenSource cts)
+    {
+        _map[testRunId] = cts;
+    }
+
+    public bool TryCancel(Guid testRunId)
+    {
+        if (_map.TryRemove(testRunId, out var cts))
+        {
+            try
+            {
+                cts.Cancel();
+                return true;
+            }
+            catch (ObjectDisposedException) { return false; }
+        }
+        return false;
+    }
+
+    public void Unregister(Guid testRunId)
+    {
+        if (_map.TryRemove(testRunId, out var cts))
+        {
+            try { cts.Dispose(); } catch (ObjectDisposedException) { }
+        }
+    }
+}

srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Services/SafetyMonitor.cs

@@ -0,0 +1,116 @@
+using RobotNet10.NavigationTune.Shared.Models;
+
+namespace RobotNet10.NavigationTune.Services;
+
+/// <summary>
+/// Safety monitor for test execution
+/// </summary>
+public class SafetyMonitor(SafetyConfig config)
+{
+    private readonly List<SafetyViolation> _violations = new();
+    private DateTime? _trackingErrorStart;
+
+    /// <summary>
+    /// Check safety conditions
+    /// </summary>
+    public bool CheckSafety(TelemetryData telemetry, ReferencePath referencePath)
+    {
+        bool isSafe = true;
+
+        // 1. Check cross-track error
+        if (telemetry.CrossTrackError > config.MaxCrossTrackError)
+        {
+            LogViolation(new SafetyViolation
+            {
+                Type = ViolationType.CrossTrackError,
+                Severity = ViolationSeverity.Critical,
+                Value = telemetry.CrossTrackError,
+                Threshold = config.MaxCrossTrackError,
+                Message = $"CTE {telemetry.CrossTrackError:F3}m exceeds limit {config.MaxCrossTrackError:F3}m",
+                Timestamp = DateTime.UtcNow
+            });
+            isSafe = false;
+        }
+
+        // 2. Check heading error
+        if (Math.Abs(telemetry.HeadingError) > config.MaxHeadingError)
+        {
+            LogViolation(new SafetyViolation
+            {
+                Type = ViolationType.HeadingError,
+                Severity = ViolationSeverity.Critical,
+                Value = Math.Abs(telemetry.HeadingError),
+                Threshold = config.MaxHeadingError,
+                Message = $"Heading error {telemetry.HeadingError * 180 / Math.PI:F1}° exceeds limit",
+                Timestamp = DateTime.UtcNow
+            });
+            isSafe = false;
+        }
+
+        // 3. Check velocity limits
+        if (Math.Abs(telemetry.RobotTwist.Linear) > config.MaxLinearVelocity * 1.1)
+        {
+            LogViolation(new SafetyViolation
+            {
+                Type = ViolationType.VelocityLimit,
+                Severity = ViolationSeverity.Warning,
+                Value = Math.Abs(telemetry.RobotTwist.Linear),
+                Threshold = config.MaxLinearVelocity,
+                Message = $"Linear velocity {telemetry.RobotTwist.Linear:F2} m/s exceeds limit",
+                Timestamp = DateTime.UtcNow
+            });
+        }
+
+        // 4. Check sustained tracking error
+        if (telemetry.CrossTrackError > config.MaxCrossTrackError * 0.5)
+        {
+            _trackingErrorStart ??= DateTime.UtcNow;
+
+            var duration = (DateTime.UtcNow - _trackingErrorStart.Value).TotalMilliseconds;
+            if (duration > config.MaxTrackingErrorDuration)
+            {
+                LogViolation(new SafetyViolation
+                {
+                    Type = ViolationType.SustainedTrackingError,
+                    Severity = ViolationSeverity.Critical,
+                    Value = duration,
+                    Threshold = config.MaxTrackingErrorDuration,
+                    Message = $"Tracking error sustained for {duration:F0}ms",
+                    Timestamp = DateTime.UtcNow
+                });
+                isSafe = false;
+            }
+        }
+        else
+        {
+            _trackingErrorStart = null;
+        }
+
+        return isSafe;
+    }
+
+    public List<SafetyViolation> GetViolations() => _violations;
+
+    public void Reset()
+    {
+        _violations.Clear();
+        _trackingErrorStart = null;
+    }
+
+    private void LogViolation(SafetyViolation violation)
+    {
+        _violations.Add(violation);
+    }
+}
+
+/// <summary>
+/// Safety configuration
+/// </summary>
+public class SafetyConfig
+{
+    public double MaxCrossTrackError { get; set; } = 0.5; // meters
+    public double MaxHeadingError { get; set; } = 45f * Math.PI / 180f; // radians (45 degrees)
+    public double MaxLinearVelocity { get; set; } = 1.5; // m/s
+    public double MaxAngularVelocity { get; set; } = 6.0; // rad/s
+    public int MaxTrackingErrorDuration { get; set; } = 3000; // milliseconds
+}

srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Services/TuningOrchestrator.cs

@@ -0,0 +1,51 @@
+using RobotNet10.NavigationTune.Shared.Interfaces;
+using RobotNet10.NavigationTune.Shared.Models;
+using BatchTestResult = RobotNet10.NavigationTune.Shared.Models.BatchTestResult;
+using ComparisonResult = RobotNet10.NavigationTune.Shared.Models.ComparisonResult;
+
+namespace RobotNet10.NavigationTune.Services;
+
+/// <summary>
+/// Main orchestrator for tuning operations
+/// </summary>
+public interface ITuningOrchestrator
+{
+    /// <summary>
+    /// Start a test and return immediately with testRunId and status Running.
+    /// Test runs in background; completion is notified via SignalR (ReceiveTestResult).
+    /// Use this for UI single-test execution so Stop/Pause buttons become active right away.
+    /// </summary>
+    Task<TestExecutionResult> StartTestAsync(
+        TestScenario scenario,
+        NavigationParameterSet parameters,
+        string? connectionId = null,
+        Guid? testRunId = null,
+        CancellationToken cancellationToken = default
+    );
+
+    Task<TestExecutionResult> RunSingleTestAsync(
+        TestScenario scenario,
+        NavigationParameterSet parameters,
+        string? connectionId = null,
+        Guid? testRunId = null,
+        CancellationToken cancellationToken = default
+    );
+
+    Task<BatchTestResult> RunBatchTestsAsync(
+        List<TestScenario> scenarios,
+        NavigationParameterSet parameters,
+        CancellationToken cancellationToken = default
+    );
+
+    Task<ComparisonResult> CompareConfigurationsAsync(
+        List<NavigationParameterSet> parameterSets,
+        TestScenario scenario,
+        CancellationToken cancellationToken = default
+    );
+
+    void PauseTest(string testRunId);
+    void ResumeTest(string testRunId);
+    void StopTest(string testRunId);
+    void EmergencyStop(string testRunId);
+}
+

srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Services/TuningOrchestratorImplementation.cs

@@ -0,0 +1,369 @@
+using Microsoft.AspNetCore.SignalR;
+using Microsoft.Extensions.DependencyInjection;
+using Microsoft.Extensions.Logging;
+using RobotNet10.NavigationTune.Hubs;
+using RobotNet10.NavigationTune.Interfaces;
+using RobotNet10.NavigationTune.Shared.Hubs;
+using RobotNet10.NavigationTune.Shared.Interfaces;
+using RobotNet10.NavigationTune.Shared.Models;
+
+namespace RobotNet10.NavigationTune.Services;
+
+/// <summary>
+/// Tuning orchestrator implementation
+/// </summary>
+public class TuningOrchestrator(
+    ITuningNavigation tuningNavigation,
+    IMetricsCalculator metricsCalculator,
+    ITestRepository testRepository,
+    IParameterManager parameterManager,
+    IServiceScopeFactory scopeFactory,
+    IRunningTestCancellationRegistry cancellationRegistry,
+    IHubContext<TuningHub>? hubContext = null,
+    ILogger<TuningOrchestrator>? logger = null) : ITuningOrchestrator
+{
+    private readonly ITuningNavigation _tuningNavigation = tuningNavigation;
+    private readonly IMetricsCalculator _metricsCalculator = metricsCalculator;
+    private readonly ITestRepository _testRepository = testRepository;
+    private readonly IParameterManager _parameterManager = parameterManager;
+    private readonly IServiceScopeFactory _scopeFactory = scopeFactory;
+    private readonly IRunningTestCancellationRegistry _cancellationRegistry = cancellationRegistry;
+    private readonly IHubContext<TuningHub>? _hubContext = hubContext;
+    private readonly ILogger<TuningOrchestrator>? _logger = logger;
+    private volatile bool _isTestRunning;
+
+    /// <summary>
+    /// Start test and return immediately with testRunId and status Running.
+    /// Control loop runs on WatchThread; completion is sent via SignalR (ReceiveTestResult).
+    /// </summary>
+    public async Task<TestExecutionResult> StartTestAsync(
+        TestScenario scenario,
+        NavigationParameterSet parameters,
+        string? connectionId = null,
+        Guid? testRunId = null,
+        CancellationToken cancellationToken = default)
+    {
+        if (_isTestRunning)
+            throw new InvalidOperationException("A test is already running. Stop it before starting another.");
+
+        var validation = _parameterManager.Validate(parameters);
+        if (!validation.IsValid)
+            throw new InvalidOperationException($"Invalid parameters: {string.Join(", ", validation.Errors)}");
+
+        var testRun = new TestRun
+        {
+            Id = testRunId ?? Guid.NewGuid(),
+            ScenarioId = scenario.Id,
+            ParameterSetId = parameters.Id,
+            StartTime = DateTime.UtcNow,
+            Status = TestStatus.Preparing
+        };
+
+        if (connectionId != null && _hubContext != null)
+            await _hubContext.Groups.AddToGroupAsync(connectionId, $"test_{testRun.Id}", cancellationToken);
+
+        await _testRepository.SaveAsync(testRun);
+
+        // Notify UI immediately so Stop/Pause buttons become active
+        if (_hubContext != null)
+        {
+            await _hubContext.Clients.Group($"test_{testRun.Id}")
+                .SendAsync("ReceiveTestStatus", new TestStatusUpdateDto
+                {
+                    TestRunId = testRun.Id,
+                    Status = TestStatus.Running,
+                    ProgressPercent = 0,
+                    Message = "Running"
+                }, cancellationToken: cancellationToken);
+        }
+
+        _isTestRunning = true;
+
+        // Register CTS so Stop/EMC Stop (different HTTP request) can cancel this test.
+        // Do NOT use "using var cts" - the CTS must stay alive until the test completes (onComplete calls Unregister which disposes it).
+        var cts = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken);
+        _cancellationRegistry.Register(testRun.Id, cts);
+
+        // Execute on WatchThread; returns immediately; onComplete runs when test finishes
+        var runningResult = await _tuningNavigation.ExecuteTestAsync(
+            scenario,
+            parameters,
+            testRun.Id,
+            cts.Token,
+            onComplete: result => _ = SaveResultAndNotifyAsync(testRun.Id, testRun.StartTime, result, parameters));
+
+        return runningResult;
+    }
+
+    private async Task SaveResultAndNotifyAsync(Guid testRunId, DateTime startTime, TestExecutionResult result, NavigationParameterSet? parameters = null)
+    {
+        // Use new scope: completion runs on WatchThread after HTTP request may have ended
+        using var scope = _scopeFactory.CreateScope();
+        var testRepository = scope.ServiceProvider.GetRequiredService<ITestRepository>();
+        try
+        {
+            await testRepository.UpdateFromResultAsync(
+                testRunId,
+                result.Status,
+                result.EndTime,
+                result.Duration,
+                result.ErrorMessage,
+                result.Metrics,
+                result.SafetyViolations);
+
+            // Generate tuning suggestions if test completed with enough telemetry
+            if (result.Status == TestStatus.Completed &&
+                result.TelemetryData?.Count > 10 &&
+                result.Metrics != null &&
+                parameters != null)
+            {
+                try
+                {
+                    var tuningAdvisor = scope.ServiceProvider.GetRequiredService<ITuningAdvisor>();
+                    result.TuningReport = tuningAdvisor.Analyze(
+                        result.TelemetryData,
+                        result.Metrics,
+                        parameters);
+                    result.TuningReport.TestRunId = testRunId;
+                }
+                catch (Exception ex)
+                {
+                    _logger?.LogWarning(ex, "Tuning advisor analysis failed for test {TestRunId}", testRunId);
+                }
+            }
+
+            if (_hubContext != null)
+            {
+                await _hubContext.Clients.Group($"test_{testRunId}")
+                    .SendAsync("ReceiveTestResult", result);
+            }
+        }
+        catch (Exception ex)
+        {
+            _logger?.LogError(ex, "Error saving test result");
+            if (_hubContext != null)
+            {
+                var errorResult = new TestExecutionResult
+                {
+                    TestRunId = testRunId,
+                    Status = TestStatus.Error,
+                    ErrorMessage = ex.Message,
+                    StartTime = startTime,
+                    EndTime = DateTime.UtcNow
+                };
+                await _hubContext.Clients.Group($"test_{testRunId}")
+                    .SendAsync("ReceiveTestResult", errorResult);
+            }
+        }
+        finally
+        {
+            _cancellationRegistry.Unregister(testRunId);
+            _isTestRunning = false;
+        }
+    }
+
+    public async Task<TestExecutionResult> RunSingleTestAsync(
+        TestScenario scenario,
+        NavigationParameterSet parameters,
+        string? connectionId = null,
+        Guid? testRunId = null,
+        CancellationToken cancellationToken = default)
+    {
+        // Validate parameters
+        var validation = _parameterManager.Validate(parameters);
+        if (!validation.IsValid)
+        {
+            throw new InvalidOperationException($"Invalid parameters: {string.Join(", ", validation.Errors)}");
+        }
+
+        // Create test run record (use provided testRunId so client can join group before execute and receive real-time telemetry)
+        var testRun = new TestRun
+        {
+            Id = testRunId ?? Guid.NewGuid(),
+            ScenarioId = scenario.Id,
+            ParameterSetId = parameters.Id,
+            StartTime = DateTime.UtcNow,
+            Status = TestStatus.Preparing
+        };
+
+        // Join SignalR group if connectionId provided
+        if (connectionId != null && _hubContext != null)
+        {
+            await _hubContext.Groups.AddToGroupAsync(connectionId, $"test_{testRun.Id}", cancellationToken);
+        }
+
+        try
+        {
+            // Execute test (pass testRun.Id so real-time telemetry is sent to group test_{testRun.Id})
+            var result = await _tuningNavigation.ExecuteTestAsync(scenario, parameters, testRun.Id, cancellationToken);
+
+            // Update test run
+            testRun.Status = result.Status;
+            testRun.EndTime = result.EndTime;
+            testRun.Duration = result.Duration;
+            testRun.ErrorMessage = result.ErrorMessage;
+            testRun.SafetyViolations = result.SafetyViolations;
+            testRun.Metrics = result.Metrics;
+
+            // Save to database
+            await _testRepository.SaveAsync(testRun);
+
+            // Generate tuning suggestions
+            if (result.Status == TestStatus.Completed &&
+                result.TelemetryData?.Count > 10 &&
+                result.Metrics != null)
+            {
+                try
+                {
+                    using var scope = _scopeFactory.CreateScope();
+                    var tuningAdvisor = scope.ServiceProvider.GetRequiredService<ITuningAdvisor>();
+                    result.TuningReport = tuningAdvisor.Analyze(
+                        result.TelemetryData,
+                        result.Metrics,
+                        parameters);
+                    result.TuningReport.TestRunId = testRun.Id;
+                }
+                catch (Exception ex)
+                {
+                    _logger?.LogWarning(ex, "Tuning advisor analysis failed for test {TestRunId}", testRun.Id);
+                }
+            }
+
+            // Publish completion
+            if (_hubContext != null)
+            {
+                await _hubContext.Clients.Group($"test_{testRun.Id}")
+                    .SendAsync("ReceiveTestResult", result, cancellationToken: cancellationToken);
+            }
+
+            return result;
+        }
+        catch (Exception ex)
+        {
+            _logger?.LogError(ex, "Error executing test");
+            testRun.Status = TestStatus.Error;
+            testRun.ErrorMessage = ex.Message;
+            testRun.EndTime = DateTime.UtcNow;
+            await _testRepository.SaveAsync(testRun);
+            throw;
+        }
+    }
+
+    public async Task<BatchTestResult> RunBatchTestsAsync(
+        List<TestScenario> scenarios,
+        NavigationParameterSet parameters,
+        CancellationToken cancellationToken = default)
+    {
+        var results = new List<TestExecutionResult>();
+        var batchId = Guid.NewGuid();
+
+        _logger?.LogInformation("Starting batch test with {Count} scenarios", scenarios.Count);
+
+        for (int i = 0; i < scenarios.Count; i++)
+        {
+            if (cancellationToken.IsCancellationRequested)
+            {
+                _logger?.LogWarning("Batch test cancelled at scenario {Index}", i);
+                break;
+            }
+
+            var scenario = scenarios[i];
+
+            try
+            {
+                var result = await RunSingleTestAsync(scenario, parameters, cancellationToken: cancellationToken);
+                results.Add(result);
+
+                _logger?.LogInformation(
+                    "Completed scenario {Index}/{Total}: {Name}",
+                    i + 1,
+                    scenarios.Count,
+                    scenario.Name
+                );
+            }
+            catch (Exception ex)
+            {
+                _logger?.LogError(
+                    ex,
+                    "Failed scenario {Index}/{Total}: {Name}",
+                    i + 1,
+                    scenarios.Count,
+                    scenario.Name
+                );
+
+                // Continue with remaining scenarios
+            }
+        }
+
+        var batchResult = new BatchTestResult
+        {
+            BatchId = batchId,
+            Parameters = parameters,
+            Results = results,
+            SuccessCount = results.Count(r => r.Status == TestStatus.Completed),
+            FailureCount = results.Count(r => r.Status != TestStatus.Completed),
+            AverageScore = results.Where(r => r.Metrics != null).Average(r => r.Metrics!.OverallScore)
+        };
+
+        return batchResult;
+    }
+
+    public async Task<ComparisonResult> CompareConfigurationsAsync(
+        List<NavigationParameterSet> parameterSets,
+        TestScenario scenario,
+        CancellationToken cancellationToken = default)
+    {
+        var results = new Dictionary<string, TestExecutionResult>();
+
+        foreach (var parameters in parameterSets)
+        {
+            try
+            {
+                var result = await RunSingleTestAsync(scenario, parameters, cancellationToken: cancellationToken);
+                results[parameters.Name] = result;
+            }
+            catch (Exception ex)
+            {
+                _logger?.LogError(ex, "Failed to test configuration {Name}", parameters.Name);
+            }
+        }
+
+        var comparison = new ComparisonResult
+        {
+            Scenario = scenario,
+            Configurations = parameterSets,
+            Results = results,
+            BestConfiguration = results
+                .Where(r => r.Value.Metrics != null)
+                .OrderByDescending(r => r.Value.Metrics!.OverallScore)
+                .FirstOrDefault()
+                .Key ?? string.Empty
+        };
+
+        return comparison;
+    }
+
+    public void PauseTest(string testRunId)
+    {
+        _tuningNavigation.Pause();
+    }
+
+    public void ResumeTest(string testRunId)
+    {
+        _tuningNavigation.Resume();
+    }
+
+    public void StopTest(string testRunId)
+    {
+        if (Guid.TryParse(testRunId, out var id) && _cancellationRegistry.TryCancel(id))
+            return;
+        _tuningNavigation.Stop();
+    }
+
+    public void EmergencyStop(string testRunId)
+    {
+        if (Guid.TryParse(testRunId, out var id) && _cancellationRegistry.TryCancel(id))
+            return;
+        _tuningNavigation.EmergencyStop();
+    }
+}