Initial commit

srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Navigation/Core/CircularBuffer.cs

@@ -0,0 +1,58 @@
+using RobotNet10.Shared.Numbers;
+using SysNum = System.Numerics;
+
+namespace RobotNet10.NavigationTune.Navigation.Core;
+
+/// <summary>
+/// Circular buffer để lưu lịch sử (fixed size)
+/// </summary>
+public class CircularBuffer<T>(int capacity) where T : SysNum.INumber<T>
+{
+    private readonly T[] _buffer = new T[capacity];
+    private int _head = 0;
+    private int _count = 0;
+    private readonly int _capacity = capacity;
+
+    public int Count => _count;
+    public int Capacity => _capacity;
+
+    public void Add(T item)
+    {
+        _buffer[_head] = item;
+        _head = (_head + 1) % _capacity;
+
+        if (_count < _capacity)
+            _count++;
+    }
+
+    public void Clear()
+    {
+        _head = 0;
+        _count = 0;
+        Array.Clear(_buffer, 0, _capacity);
+    }
+
+    public T[] ToArray()
+    {
+        T[] result = new T[_count];
+        for (int i = 0; i < _count; i++)
+        {
+            int index = (_head - _count + i + _capacity) % _capacity;
+            result[i] = _buffer[index];
+        }
+        return result;
+    }
+
+    public double Average()
+    {
+        if (_count == 0) return 0;
+
+        T sum = T.Zero;
+        for (int i = 0; i < _count; i++)
+        {
+            int index = (_head - _count + i + _capacity) % _capacity;
+            sum += _buffer[index];
+        }
+        return double.CreateChecked(sum) / _count;
+    }
+}

srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Navigation/Core/MotorDynamicsModel.cs

@@ -0,0 +1,63 @@
+using RobotNet10.NavigationTune.Shared.Models;
+
+namespace RobotNet10.NavigationTune.Navigation.Core;
+
+/// <summary>
+/// Mô hình động học của motor driver
+/// First-order system: v(t) = v_cmd × (1 - e^(-(t-δ)/τ))
+/// </summary>
+public class MotorDynamicsModel
+{
+    public double Tau { get; set; }
+    public double Delta { get; set; }
+
+    /// <summary>
+    /// Constructor với giá trị mặc định
+    /// </summary>
+    public MotorDynamicsModel()
+    {
+        Tau = 0.3;      // 300ms time constant
+        Delta = 0.05f;   // 50ms delay
+    }
+
+    public MotorDynamicsModel(MotorDynamicsConfig config)
+    {
+        Tau = config.Tau;
+        Delta = config.Delta;
+    }
+
+    /// <summary>
+    /// Predict vận tốc tại thời điểm tương lai
+    /// </summary>
+    /// <param name="vCmd">Velocity command đã gửi</param>
+    /// <param name="vActual">Velocity thực tế hiện tại</param>
+    /// <param name="timeAhead">Thời gian dự đoán về tương lai (s)</param>
+    /// <returns>Vận tốc dự đoán</returns>
+    public double PredictVelocity(double vCmd, double vActual, double timeAhead)
+    {
+        // Nếu thời gian dự đoán < delay
+        // → Motor chưa bắt đầu phản ứng
+        if (timeAhead < Delta)
+        {
+            return vActual;
+        }
+
+        // Thời gian hiệu dụng (sau khi trừ delay)
+        double effectiveTime = timeAhead - Delta;
+
+        // First-order system response
+        // response = 1 - e^(-t/τ)
+        double response = 1.0 - Math.Exp(-effectiveTime / Tau);
+
+        // Velocity prediction
+        // v_future = v_actual + (v_cmd - v_actual) × response
+        double vPredicted = vActual + (vCmd - vActual) * response;
+
+        return vPredicted;
+    }
+
+    public override string ToString()
+    {
+        return $"MotorModel(τ={Tau:F3}s, δ={Delta:F3}s)";
+    }
+}

srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Navigation/Core/NavigationMath.cs

@@ -0,0 +1,27 @@
+namespace RobotNet10.NavigationTune.Navigation.Core;
+
+/// <summary>
+/// Shared math utilities for navigation algorithms
+/// </summary>
+public static class NavigationMath
+{
+    /// <summary>
+    /// Normalize angle to [-π, π]
+    /// </summary>
+    public static double NormalizeAngle(double angle)
+    {
+        while (angle > Math.PI) angle -= 2 * Math.PI;
+        while (angle < -Math.PI) angle += 2 * Math.PI;
+        return angle;
+    }
+
+    /// <summary>
+    /// Calculate Euclidean distance between two points
+    /// </summary>
+    public static double CalculateDistance(double x1, double y1, double x2, double y2)
+    {
+        double dx = x2 - x1;
+        double dy = y2 - y1;
+        return Math.Sqrt(dx * dx + dy * dy);
+    }
+}

srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Navigation/Core/PID.cs

@@ -0,0 +1,69 @@
+using RobotNet10.NavigationTune.Shared.Models;
+
+namespace RobotNet10.NavigationTune.Navigation.Core;
+
+public class PID(PIDConfig config)
+{
+    private double Kp = config.Kp;
+    private double Ki = config.Ki;
+    private double Kd = config.Kd;
+    private double IntegralZone = config.IntegralZone;
+
+    private double _prevError;
+    private double _integral;
+
+    public PID WithKp(double kp)
+    {
+        Kp = kp;
+        return this;
+    }
+
+    public PID WithKi(double ki)
+    {
+        Ki = ki;
+        return this;
+    }
+
+    public PID WithKd(double kd)
+    {
+        Kd = kd;
+        return this;
+    }
+
+    public PID WithIntegralZone(double integralZone)
+    {
+        IntegralZone = integralZone;
+        return this;
+    }
+
+    public double PID_step(double error, double max, double min, double timeSample)
+    {
+        double integralStep = 0.5 * (error + _prevError) * timeSample;
+
+        bool inIntegralZone = IntegralZone <= 0 || Math.Abs(error) <= IntegralZone;
+        if (inIntegralZone)
+            _integral += integralStep;
+        else
+            _integral = 0;
+
+        double derivative = (error - _prevError) / timeSample;
+        _prevError = error;
+
+        double Out = Kp * error
+                   + Ki * _integral
+                   + Kd * derivative;
+
+        // Anti-windup
+        double clamped = Math.Clamp(Out, min, max);
+        if (clamped != Out && inIntegralZone)
+            _integral -= integralStep;
+
+        return clamped;
+    }
+
+    public void Reset()
+    {
+        _prevError = 0;
+        _integral = 0;
+    }
+}

srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Navigation/Core/PurePursuitSimplified.cs

@@ -0,0 +1,477 @@
+using RobotNet10.NavigationTune.Shared.Models;
+
+namespace RobotNet10.NavigationTune.Navigation.Core;
+
+/// <summary>
+/// Simplified Pure Pursuit controller for tuning system
+/// Works with simple PathPoint list instead of OrderNode/Edge
+/// </summary>
+public class PurePursuitSimplified(PurePursuitConfig config, StanleyConfig stanleyConfig)
+{
+    private List<PathPoint> _waypoints = new();
+    private int _currentWaypointIndex = 0;
+    private int _currentWaypointAheadIndex = 0;
+    private PathPoint? _goalPoint;
+    public bool IsInFinalApproach { get; private set; }
+
+    /// <summary>
+    /// Set path from PathPoint list
+    /// </summary>
+    public void SetPath(List<PathPoint> waypoints)
+    {
+        if (waypoints.Count < 2)
+            throw new ArgumentException("Path must have at least 2 waypoints", nameof(waypoints));
+
+        _waypoints = waypoints;
+        _currentWaypointIndex = 0;
+        _currentWaypointAheadIndex = 0;
+        _goalPoint = waypoints[^1];
+    }
+
+    /// <summary>
+    /// Update goal point
+    /// </summary>
+    public void UpdateGoal(PathPoint goal)
+    {
+        _goalPoint = goal;
+    }
+
+    /// <summary>
+    /// Get closest waypoint to current position
+    /// </summary>
+    public (PathPoint point, int index) GetClosestWaypoint(double x, double y)
+    {
+        if (_waypoints.Count == 0)
+            throw new InvalidOperationException("Path not set");
+
+        double minDistance = double.MaxValue;
+        int closestIndex = 0;
+
+        // Start search from current index for efficiency
+        for (int i = _currentWaypointIndex; i < _waypoints.Count; i++)
+        {
+            double dx = x - _waypoints[i].X;
+            double dy = y - _waypoints[i].Y;
+            double distance = Math.Sqrt(dx * dx + dy * dy);
+
+            if (distance < minDistance)
+            {
+                minDistance = distance;
+                closestIndex = i;
+            }
+        }
+
+        // Also check previous waypoints in case robot moved backwards
+        for (int i = 0; i < _currentWaypointIndex; i++)
+        {
+            double dx = x - _waypoints[i].X;
+            double dy = y - _waypoints[i].Y;
+            double distance = Math.Sqrt(dx * dx + dy * dy);
+
+            if (distance < minDistance)
+            {
+                minDistance = distance;
+                closestIndex = i;
+            }
+        }
+
+        _currentWaypointIndex = closestIndex;
+        return (_waypoints[closestIndex], closestIndex);
+    }
+
+    /// <summary>
+    /// Calculate adaptive lookahead distance based on velocity, confidence, distance to goal, and path curvature
+    /// Lookahead adapts to:
+    /// 1. Velocity (faster = look further ahead)
+    /// 2. Distance to goal (near goal = shorter lookahead for precision)
+    /// 3. Path curvature (sharp curves = shorter lookahead for tighter tracking)
+    /// 4. Confidence (low confidence = shorter lookahead for safety)
+    /// </summary>
+    private double GetLookaheadDistance(double vHybrid, double robotX, double robotY, int closestIndex)
+    {
+        // 1. Base lookahead from velocity
+        double baseLookahead = config.LookaheadMin + config.Kdd * Math.Abs(vHybrid);
+
+        // 2. Distance-to-goal adaptation
+        double distanceToGoal = CalculateDistanceToGoal(robotX, robotY);
+        double goalFactor = 1.0;
+        if (distanceToGoal < config.GoalRegionDistance)
+        {
+            // Gradually reduce lookahead as we approach goal
+            // At goal: factor = 0.5, At GoalRegionDistance: factor = 1.0
+            goalFactor = 0.5 + 0.5 * (distanceToGoal / config.GoalRegionDistance);
+        }
+
+        // 3. Curvature adaptation
+        double curvature = CalculateCurvature(closestIndex);
+        // curvatureFactor ranges from 1.0 (straight) to ~0.33 (very sharp curve with KCurvature=2.0)
+        double curvatureFactor = 1.0 / (1.0 + config.KCurvature * curvature);
+
+        // 5. Combine all factors
+        double adaptiveLookahead = baseLookahead * goalFactor * curvatureFactor;
+        double minLookahead = config.LookaheadMin;
+        double maxLookahead = config.LookaheadMax;
+        if(distanceToGoal < config.GoalRegionDistance && Math.Abs(vHybrid) > 0.0)
+        {
+            // 6. Apply velocity-based dynamic limits
+            // Minimum: look at least 0.3 seconds ahead or 0.5m (whichever is larger)
+            minLookahead = Math.Max(config.LookaheadMin, Math.Abs(vHybrid) * config.MinLookaheadTimeRatio);
+
+            // Maximum: look at most 2 seconds ahead or LookaheadMax (whichever is smaller)
+            maxLookahead = Math.Min(config.LookaheadMax, Math.Abs(vHybrid) * config.MaxLookaheadTimeRatio);
+
+            // Ensure min < max
+            if (minLookahead > maxLookahead)
+                minLookahead = maxLookahead;
+
+        }
+
+        adaptiveLookahead = Math.Clamp(adaptiveLookahead, minLookahead, maxLookahead);
+        if(adaptiveLookahead is double.NaN || adaptiveLookahead <= 0)
+        {
+            adaptiveLookahead = config.LookaheadMin;
+        }
+        //Console.WriteLine($"Lookahead Calc: Base={baseLookahead:F3}, GoalF={goalFactor:F3}, CurvF={curvatureFactor:F3}, Curvature={curvature:F3}, Lookahead={adaptiveLookahead:F3}, Min={minLookahead:F3}, Max={maxLookahead:F3}, DTG={distanceToGoal:F3}, vHybrid={vHybrid:F3}");
+        return adaptiveLookahead;
+    }
+
+    /// <summary>
+    /// Calculate angular velocity using Pure Pursuit algorithm
+    /// </summary>
+    public (double linear, double angular) CalculateAngularVelocity(
+        double robotX,
+        double robotY,
+        double robotTheta,
+        double actualLinearVelocity,
+        double maxLinearVelocity)
+    {
+        if (_waypoints.Count < 2 || _goalPoint == null)
+            throw new InvalidOperationException("Path not properly initialized");
+
+        // Get closest waypoint
+        (PathPoint closesPoint, int closestIndex) = GetClosestWaypoint(robotX, robotY);
+
+        // Calculate adaptive lookahead distance
+        double lookaheadDistance = GetLookaheadDistance(actualLinearVelocity, robotX, robotY, closestIndex);
+
+        // Find target point at lookahead distance
+        PathPoint? targetPoint = FindTargetPoint(closestIndex, lookaheadDistance);
+
+        targetPoint ??= _goalPoint;
+
+        // Normalize theta
+        // Backward adjustment for Pure Pursuit
+        if (targetPoint.Direction == RobotDirection.BACKWARD) robotTheta += Math.PI;
+        robotTheta = NavigationMath.NormalizeAngle(robotTheta);
+
+        // Check for final approach
+        if (targetPoint.X == _goalPoint.X && targetPoint.Y == _goalPoint.Y)
+        {
+            double distanceToGoal = CalculateDistanceToGoal(robotX, robotY);
+            if (distanceToGoal <= config.FinalApproachThreshold)
+            {
+                IsInFinalApproach = true;
+            }
+        }
+
+        // When approaching goal, switch to Stanley controller for precise CTE-based tracking
+        // Reuse closesPoint (already computed above) and normalized robotTheta
+        if (IsInFinalApproach) return FinalApproachController(robotX, robotY, robotTheta, _goalPoint, actualLinearVelocity, maxLinearVelocity, targetPoint.Direction == RobotDirection.BACKWARD);
+
+        // Calculate angle to target
+        double dx = targetPoint.X - robotX;
+        double dy = targetPoint.Y - robotY;
+        double alpha = Math.Atan2(dy, dx) - robotTheta;
+
+        // Normalize angle to [-π, π]
+        while (alpha > Math.PI) alpha -= 2 * Math.PI;
+        while (alpha < -Math.PI) alpha += 2 * Math.PI;
+
+        // Pure Pursuit formula: ω = 2 * v * sin(α) / L
+        double angularVelocity = 2.0 * actualLinearVelocity * Math.Sin(alpha) / lookaheadDistance;
+
+        // Clamp to max angular velocity
+        if (Math.Abs(angularVelocity) > config.MaxAngularVelocity)
+        {
+            angularVelocity = Math.Sign(angularVelocity) * config.MaxAngularVelocity;
+        }
+
+        if (targetPoint.Direction == RobotDirection.BACKWARD)
+        {
+            maxLinearVelocity = -maxLinearVelocity;
+            angularVelocity = -angularVelocity;
+        }
+
+        Console.WriteLine($"PP: CurP=({robotX:F3},{robotY:F3}, {robotTheta:F3}), Alpha={alpha * 180 / Math.PI:F2} deg, AVel={actualLinearVelocity:F5}, LVel={maxLinearVelocity:F2}, AnVel={angularVelocity:F5}");
+        return (maxLinearVelocity, angularVelocity);
+    }
+
+    /// <summary>
+    /// Find target point at lookahead distance from current position
+    /// </summary>
+    private PathPoint? FindTargetPoint(int startIndex, double lookaheadDistance)
+    {
+        if (startIndex >= _waypoints.Count - 1)
+            return _goalPoint;
+
+        double accumulatedDistance = 0;
+
+        for (int i = startIndex; i < _waypoints.Count - 1; i++)
+        {
+            double dx = _waypoints[i + 1].X - _waypoints[i].X;
+            double dy = _waypoints[i + 1].Y - _waypoints[i].Y;
+            double segmentLength = Math.Sqrt(dx * dx + dy * dy);
+
+            if (accumulatedDistance + segmentLength >= lookaheadDistance)
+            {
+                // Interpolate within this segment
+                double t = (lookaheadDistance - accumulatedDistance) / segmentLength;
+                return new PathPoint
+                {
+                    X = _waypoints[i].X + t * (_waypoints[i + 1].X - _waypoints[i].X),
+                    Y = _waypoints[i].Y + t * (_waypoints[i + 1].Y - _waypoints[i].Y),
+                    Direction = _waypoints[i].Direction,
+                    DistanceFromStart = _waypoints[i].DistanceFromStart + (lookaheadDistance - accumulatedDistance)
+                };
+            }
+
+            accumulatedDistance += segmentLength;
+        }
+
+        return _goalPoint;
+    }
+
+    /// <summary>
+    /// Get current lookahead distance (for debugging/testing)
+    /// Note: This simplified version doesn't account for curvature/distance adaptations
+    /// For actual adaptive lookahead, use the version called within CalculateAngularVelocity
+    /// </summary>
+    public double GetCurrentLookahead(double linearVelocity, double confidence)
+    {
+        // Simplified version for backward compatibility
+        // Uses center of path as reference point
+        double lookahead = config.LookaheadMin + config.Kdd * Math.Abs(linearVelocity);
+        lookahead = Math.Clamp(lookahead, config.LookaheadMin, config.LookaheadMax);
+        return lookahead;
+    }
+
+    /// <summary>
+    /// Calculate distance from robot to goal point
+    /// </summary>
+    private double CalculateDistanceToGoal(double robotX, double robotY)
+    {
+        if (_goalPoint == null)
+            return double.MaxValue;
+
+        double dx = _goalPoint.X - robotX;
+        double dy = _goalPoint.Y - robotY;
+        return Math.Sqrt(dx * dx + dy * dy);
+    }
+
+    /// <summary>
+    /// Calculate path curvature at given waypoint index using 3-point circle fitting (Menger curvature)
+    /// Returns curvature in 1/meters (larger value = sharper curve)
+    /// </summary>
+    private double CalculateCurvature(int index)
+    {
+        // Need at least 3 points for curvature calculation
+        if (_waypoints.Count < 3 || index <= 0 || index >= _waypoints.Count - 1)
+            return 0.0;
+
+        var p1 = _waypoints[index - 1];
+        var p2 = _waypoints[index];
+        var p3 = _waypoints[index + 1];
+
+        // Calculate vectors
+        double dx1 = p2.X - p1.X;
+        double dy1 = p2.Y - p1.Y;
+        double dx2 = p3.X - p2.X;
+        double dy2 = p3.Y - p2.Y;
+
+        // Cross product magnitude (2 * triangle area)
+        double cross = Math.Abs(dx1 * dy2 - dy1 * dx2);
+
+        // Side lengths of triangle
+        double a = Math.Sqrt(dx1 * dx1 + dy1 * dy1);
+        double b = Math.Sqrt(dx2 * dx2 + dy2 * dy2);
+        double c = Math.Sqrt((p3.X - p1.X) * (p3.X - p1.X) + (p3.Y - p1.Y) * (p3.Y - p1.Y));
+
+        // Menger curvature formula: k = 4 * Area / (a * b * c)
+        // Area of triangle = cross / 2, so k = 2 * cross / (a * b * c)
+        double curvature = 2.0 * cross / (a * b * c + 1e-9);  // Add small epsilon to avoid division by zero
+
+        return curvature;
+    }
+
+    /// <summary>
+    /// Stanley-based final approach controller
+    /// When robot enters goal region (IsInFinalApproach), uses Stanley algorithm for precise CTE-based tracking
+    /// Reuses closestWaypoint and normalized robotTheta from CalculateAngularVelocity
+    /// </summary>
+    private (double linear, double angular) FinalApproachController(
+        double robotX,
+        double robotY,
+        double robotTheta,
+        PathPoint goal,
+        double actualLinearVelocity,
+        double maxLinearVelocity,
+        bool isBackward)
+    {
+        // Calculate front axle position
+        double frontX = robotX + stanleyConfig.WheelBase * Math.Cos(robotTheta);
+        double frontY = robotY + stanleyConfig.WheelBase * Math.Sin(robotTheta);
+
+        // Find closest point on path to front axle
+        (PathPoint closestPoint, int closestIndex) = GetClosestAheadWaypoint(frontX, frontY);
+
+        // Calculate heading at closest point (path direction)
+        double pathHeading = CalculateStanleyPathHeading(closestIndex);
+
+        // Calculate cross-track error (signed distance from front axle to path)
+        double crossTrackError = CalculateStanleyCrossTrackError(frontX, frontY, closestPoint, pathHeading);
+        if (isBackward) crossTrackError = -crossTrackError;
+
+        // Calculate heading error (path heading - robot heading)
+        double headingError = NavigationMath.NormalizeAngle(pathHeading - robotTheta);
+
+        // Calculate curvature at closest point (for feedforward)
+        double curvature = 0;
+        if (stanleyConfig.EnableCurvatureFeedforward)
+        {
+            curvature = CalculateCurvature(closestIndex);
+        }
+
+        // Adaptive K gain: increase when close to goal for tighter tracking
+        double distanceToGoal = NavigationMath.CalculateDistance(robotX, robotY, goal.X, goal.Y);
+        double adaptiveK = stanleyConfig.K;
+        if (distanceToGoal < stanleyConfig.GoalApproachDistance)
+        {
+            // Linearly increase K from K to K*GoalGainMultiplier as distance decreases
+            double approachRatio = 1.0 - (distanceToGoal / stanleyConfig.GoalApproachDistance);
+            adaptiveK = stanleyConfig.K * (1.0 + approachRatio * (stanleyConfig.GoalGainMultiplier - 1.0));
+        }
+
+        // Stanley formula: δ = ψ + arctan(K × e / (v + Ks))
+        double crossTrackTerm = Math.Atan2(adaptiveK * crossTrackError, Math.Abs(actualLinearVelocity) + stanleyConfig.Ks);
+
+        // Add curvature feedforward if enabled
+        double curvatureTerm = 0;
+        if (stanleyConfig.EnableCurvatureFeedforward && curvature != 0)
+        {
+            curvatureTerm = stanleyConfig.KCurvatureFF * Math.Atan(curvature * stanleyConfig.WheelBase);
+        }
+
+        // Total steering angle
+        double steeringAngle = headingError + crossTrackTerm + curvatureTerm;
+
+        // Clamp to maximum steering angle
+        steeringAngle = Math.Clamp(steeringAngle, -stanleyConfig.MaxSteeringAngle, stanleyConfig.MaxSteeringAngle);
+
+        // Convert steering angle to angular velocity using bicycle model
+        double angularVelocity = (actualLinearVelocity * Math.Tan(steeringAngle)) / stanleyConfig.WheelBase;
+
+        // Apply direction
+        if (isBackward)
+        {
+            maxLinearVelocity = -maxLinearVelocity;
+        }
+
+        // Debug output
+        Console.WriteLine($"FA-Stanley: Pose=({robotX:F3},{robotY:F3},{robotTheta * 180 / Math.PI:F1}°), " +
+                         $"CTE={crossTrackError:F3}m, HeadErr={headingError * 180 / Math.PI:F1}°, " +
+                         $"Curv={curvature:F3}, SteerAng={steeringAngle * 180 / Math.PI:F1}°, " +
+                         $"K={adaptiveK:F2}, DTG={distanceToGoal:F3}m, " +
+                         $"LVel={maxLinearVelocity:F2}, AVel={angularVelocity:F3}");
+
+        return (maxLinearVelocity, angularVelocity);
+    }
+
+    #region Stanley Helper Methods (for FinalApproachController)
+
+    /// <summary>
+    /// Get closest waypoint ahead to given position (for Stanley front axle tracking)
+    /// </summary>
+    private (PathPoint point, int index) GetClosestAheadWaypoint(double x, double y)
+    {
+        if (_waypoints.Count == 0)
+            throw new InvalidOperationException("Path not set");
+
+        double minDistance = double.MaxValue;
+        int closestIndex = 0;
+
+        // Start search from current index for efficiency
+        for (int i = _currentWaypointAheadIndex; i < _waypoints.Count; i++)
+        {
+            double distance = NavigationMath.CalculateDistance(x, y, _waypoints[i].X, _waypoints[i].Y);
+
+            if (distance < minDistance)
+            {
+                minDistance = distance;
+                closestIndex = i;
+            }
+        }
+
+        // Also check previous waypoints in case robot moved backwards
+        for (int i = 0; i < _currentWaypointAheadIndex; i++)
+        {
+            double distance = NavigationMath.CalculateDistance(x, y, _waypoints[i].X, _waypoints[i].Y);
+
+            if (distance < minDistance)
+            {
+                minDistance = distance;
+                closestIndex = i;
+            }
+        }
+
+        _currentWaypointAheadIndex = closestIndex;
+        return (_waypoints[closestIndex], closestIndex);
+    }
+
+    /// <summary>
+    /// Calculate path heading at given waypoint index (for Stanley)
+    /// Uses current point and next point to determine direction
+    /// </summary>
+    private double CalculateStanleyPathHeading(int index)
+    {
+        if (index >= _waypoints.Count - 1)
+        {
+            // Last point - use previous segment direction
+            if (index > 0)
+            {
+                double dx = _waypoints[index].X - _waypoints[index - 1].X;
+                double dy = _waypoints[index].Y - _waypoints[index - 1].Y;
+                return Math.Atan2(dy, dx);
+            }
+            return 0;
+        }
+
+        // Use current to next point
+        double dxNext = _waypoints[index + 1].X - _waypoints[index].X;
+        double dyNext = _waypoints[index + 1].Y - _waypoints[index].Y;
+        return Math.Atan2(dyNext, dxNext);
+    }
+
+    /// <summary>
+    /// Calculate signed cross-track error (for Stanley)
+    /// Positive: front axle is to the left of path
+    /// Negative: front axle is to the right of path
+    /// </summary>
+    private static double CalculateStanleyCrossTrackError(double frontX, double frontY, PathPoint closestPoint, double pathHeading)
+    {
+        // Vector from closest point to front axle
+        double dx = frontX - closestPoint.X;
+        double dy = frontY - closestPoint.Y;
+
+        // Path direction vector
+        double pathDx = Math.Cos(pathHeading);
+        double pathDy = Math.Sin(pathHeading);
+
+        // Cross product to get signed perpendicular distance
+        // positive = left, negative = right
+        double crossTrackError = dx * pathDy - dy * pathDx;
+
+        return crossTrackError;
+    }
+
+    #endregion
+}

srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Navigation/Core/StanleySimplified.cs

@@ -0,0 +1,311 @@
+using RobotNet10.NavigationTune.Shared.Models;
+
+namespace RobotNet10.NavigationTune.Navigation.Core;
+
+/// <summary>
+/// Simplified Stanley controller for path tracking
+/// Uses cross-track error + heading error for steering control
+/// References:
+/// - Stanford's DARPA Grand Challenge winner
+/// - Better than Pure Pursuit at high speeds and curved paths
+/// </summary>
+public class StanleySimplified(StanleyConfig config)
+{
+    private List<PathPoint> _waypoints = new();
+    private int _currentWaypointAheadIndex = 0;
+    private int _currentWaypointIndex = 0;
+    private PathPoint? _goalPoint;
+
+    /// <summary>
+    /// Set path from PathPoint list
+    /// </summary>
+    public void SetPath(List<PathPoint> waypoints)
+    {
+        if (waypoints.Count < 2)
+            throw new ArgumentException("Path must have at least 2 waypoints", nameof(waypoints));
+
+        _waypoints = waypoints;
+        _currentWaypointAheadIndex = 0;
+        _currentWaypointIndex = 0;
+        _goalPoint = waypoints[^1];
+    }
+
+    /// <summary>
+    /// Update goal point
+    /// </summary>
+    public void UpdateGoal(PathPoint goal)
+    {
+        _goalPoint = goal;
+    }
+
+    /// <summary>
+    /// Calculate velocities using Stanley controller
+    /// Returns (linear, angular) velocity commands
+    /// </summary>
+    public (double linear, double angular) CalculateVelocity(
+        double robotX,
+        double robotY,
+        double robotTheta,
+        double actualLinearVelocity,
+        double maxLinearVelocity)
+    {
+        if (_waypoints.Count < 2 || _goalPoint == null)
+            throw new InvalidOperationException("Path not properly initialized");
+
+        // Get closest waypoint
+        (PathPoint closestCurrentPoint, _) = GetClosestWaypoint(robotX, robotY);
+
+        // Handle backward motion
+        bool isBackward = closestCurrentPoint.Direction == RobotDirection.BACKWARD;
+        if (isBackward)
+        {
+            // Adjust for backward motion
+            robotTheta += Math.PI;
+            robotTheta = NavigationMath.NormalizeAngle(robotTheta);
+        }
+
+        // Calculate front axle position
+        double frontX = robotX + config.WheelBase * Math.Cos(robotTheta);
+        double frontY = robotY + config.WheelBase * Math.Sin(robotTheta);
+
+        // Find closest point on path to front axle
+        (PathPoint closestPoint, int closestIndex) = GetClosestAheadWaypoint(frontX, frontY);
+
+        // Calculate heading at closest point (path direction)
+        double pathHeading = CalculatePathHeading(closestIndex);
+
+        // Calculate cross-track error (signed distance from front axle to path)
+        double crossTrackError = CalculateCrossTrackError(frontX, frontY, closestPoint, pathHeading);
+        if(isBackward) crossTrackError = -crossTrackError;
+
+        // Calculate heading error (path heading - robot heading)
+        double headingError = NavigationMath.NormalizeAngle(pathHeading - robotTheta);
+
+        // Calculate curvature at closest point (for feedforward)
+        double curvature = 0;
+        if (config.EnableCurvatureFeedforward)
+        {
+            curvature = CalculateCurvature(closestIndex);
+        }
+
+        // Adaptive K gain: increase when close to goal for tighter tracking
+        double distanceToGoal = NavigationMath.CalculateDistance(robotX, robotY, _goalPoint.X, _goalPoint.Y);
+        double adaptiveK = config.K;
+        if (distanceToGoal < config.GoalApproachDistance)
+        {
+            // Linearly increase K from K to K*GoalGainMultiplier as distance decreases
+            double approachRatio = 1.0 - (distanceToGoal / config.GoalApproachDistance);
+            adaptiveK = config.K * (1.0 + approachRatio * (config.GoalGainMultiplier - 1.0));
+        }
+
+        // Stanley formula: δ = ψ + arctan(K × e / (v + Ks))
+        double crossTrackTerm = Math.Atan2(adaptiveK * crossTrackError, Math.Abs(actualLinearVelocity) + config.Ks);
+
+        // Add curvature feedforward if enabled
+        double curvatureTerm = 0;
+        if (config.EnableCurvatureFeedforward && curvature != 0)
+        {
+            curvatureTerm = config.KCurvatureFF * Math.Atan(curvature * config.WheelBase);
+        }
+
+        // Total steering angle
+        double steeringAngle = headingError + crossTrackTerm + curvatureTerm;
+
+        // Clamp to maximum steering angle
+        steeringAngle = Math.Clamp(steeringAngle, -config.MaxSteeringAngle, config.MaxSteeringAngle);
+
+        // Convert steering angle to angular velocity
+        // At low speeds, bicycle model (ω = v*tan(δ)/L) produces near-zero angular velocity
+        // even when large steering correction is needed.
+        // Solution: blend bicycle model with direct proportional control based on speed.
+        double angularVelocity;
+        angularVelocity = (actualLinearVelocity * Math.Tan(steeringAngle)) / config.WheelBase;
+
+        // Apply direction
+        if (isBackward)
+        {
+            maxLinearVelocity = -maxLinearVelocity;
+        }
+
+        // Debug output
+        Console.WriteLine($"Stanley: Front=({frontX:F3},{frontY:F3}), Closest=({closestPoint.X:F3},{closestPoint.Y:F3}), " +
+                         $"Pose=({robotX:F3},{robotY:F3},{robotTheta * 180 / Math.PI:F1}°), " +
+                         $"CTE={crossTrackError:F3}m, HeadErr={headingError * 180 / Math.PI:F1}°, " +
+                         $"Curv={curvature:F3}, SteerAng={steeringAngle * 180 / Math.PI:F1}°, " +
+                         $"K={adaptiveK:F2}, DTG={distanceToGoal:F3}m, " +
+                         $"LVel={maxLinearVelocity:F2}, AVel={angularVelocity:F3}");
+
+        return (maxLinearVelocity, angularVelocity);
+    }
+
+    /// <summary>
+    /// Get closest waypoint to given position
+    /// </summary>
+    private (PathPoint point, int index) GetClosestWaypoint(double x, double y)
+    {
+        if (_waypoints.Count == 0)
+            throw new InvalidOperationException("Path not set");
+
+        double minDistance = double.MaxValue;
+        int closestIndex = 0;
+
+        // Start search from current index for efficiency
+        for (int i = _currentWaypointIndex; i < _waypoints.Count; i++)
+        {
+            double distance = NavigationMath.CalculateDistance(x, y, _waypoints[i].X, _waypoints[i].Y);
+
+            if (distance < minDistance)
+            {
+                minDistance = distance;
+                closestIndex = i;
+            }
+        }
+
+        // Also check previous waypoints in case robot moved backwards
+        for (int i = 0; i < _currentWaypointIndex; i++)
+        {
+            double distance = NavigationMath.CalculateDistance(x, y, _waypoints[i].X, _waypoints[i].Y);
+
+            if (distance < minDistance)
+            {
+                minDistance = distance;
+                closestIndex = i;
+            }
+        }
+
+        _currentWaypointIndex = closestIndex;
+        return (_waypoints[closestIndex], closestIndex);
+    }
+
+    /// <summary>
+    /// Get closest waypoint to given position
+    /// </summary>
+    private (PathPoint point, int index) GetClosestAheadWaypoint(double x, double y)
+    {
+        if (_waypoints.Count == 0)
+            throw new InvalidOperationException("Path not set");
+
+        double minDistance = double.MaxValue;
+        int closestIndex = 0;
+
+        // Start search from current index for efficiency
+        for (int i = _currentWaypointAheadIndex; i < _waypoints.Count; i++)
+        {
+            double distance = NavigationMath.CalculateDistance(x, y, _waypoints[i].X, _waypoints[i].Y);
+
+            if (distance < minDistance)
+            {
+                minDistance = distance;
+                closestIndex = i;
+            }
+        }
+
+        // Also check previous waypoints in case robot moved backwards
+        for (int i = 0; i < _currentWaypointAheadIndex; i++)
+        {
+            double distance = NavigationMath.CalculateDistance(x, y, _waypoints[i].X, _waypoints[i].Y);
+
+            if (distance < minDistance)
+            {
+                minDistance = distance;
+                closestIndex = i;
+            }
+        }
+
+        _currentWaypointAheadIndex = closestIndex;
+        return (_waypoints[closestIndex], closestIndex);
+    }
+
+    /// <summary>
+    /// Calculate path heading at given waypoint index
+    /// Uses current point and next point to determine direction
+    /// </summary>
+    private double CalculatePathHeading(int index)
+    {
+        if (index >= _waypoints.Count - 1)
+        {
+            // Last point - use previous segment direction
+            if (index > 0)
+            {
+                double dx = _waypoints[index].X - _waypoints[index - 1].X;
+                double dy = _waypoints[index].Y - _waypoints[index - 1].Y;
+                return Math.Atan2(dy, dx);
+            }
+            return 0;
+        }
+
+        // Use current to next point
+        double dxNext = _waypoints[index + 1].X - _waypoints[index].X;
+        double dyNext = _waypoints[index + 1].Y - _waypoints[index].Y;
+        return Math.Atan2(dyNext, dxNext);
+    }
+
+    /// <summary>
+    /// Calculate signed cross-track error
+    /// Positive: front axle is to the left of path
+    /// Negative: front axle is to the right of path
+    /// </summary>
+    private double CalculateCrossTrackError(double frontX, double frontY, PathPoint closestPoint, double pathHeading)
+    {
+        // Vector from closest point to front axle
+        double dx = frontX - closestPoint.X;
+        double dy = frontY - closestPoint.Y;
+
+        // Path direction vector
+        double pathDx = Math.Cos(pathHeading);
+        double pathDy = Math.Sin(pathHeading);
+
+        // Cross product to get signed perpendicular distance
+        // positive = left, negative = right
+        double crossTrackError = dx * pathDy - dy * pathDx;
+
+        return crossTrackError;
+    }
+
+    /// <summary>
+    /// Calculate path curvature at given waypoint index
+    /// Uses Menger curvature (3-point circle fitting)
+    /// Returns curvature in 1/meters (larger value = sharper curve)
+    /// </summary>
+    private double CalculateCurvature(int index)
+    {
+        // Need at least 3 points for curvature calculation
+        if (_waypoints.Count < 3 || index <= 0 || index >= _waypoints.Count - 1)
+            return 0.0;
+
+        var p1 = _waypoints[index - 1];
+        var p2 = _waypoints[index];
+        var p3 = _waypoints[index + 1];
+
+        // Calculate vectors
+        double dx1 = p2.X - p1.X;
+        double dy1 = p2.Y - p1.Y;
+        double dx2 = p3.X - p2.X;
+        double dy2 = p3.Y - p2.Y;
+
+        // Cross product magnitude (2 * triangle area)
+        double cross = Math.Abs(dx1 * dy2 - dy1 * dx2);
+
+        // Side lengths of triangle
+        double a = Math.Sqrt(dx1 * dx1 + dy1 * dy1);
+        double b = Math.Sqrt(dx2 * dx2 + dy2 * dy2);
+        double c = Math.Sqrt((p3.X - p1.X) * (p3.X - p1.X) + (p3.Y - p1.Y) * (p3.Y - p1.Y));
+
+        // Menger curvature formula: k = 4 * Area / (a * b * c)
+        // Area of triangle = cross / 2, so k = 2 * cross / (a * b * c)
+        double curvature = 2.0 * cross / (a * b * c + 1e-9);  // Add small epsilon to avoid division by zero
+
+        return curvature;
+    }
+
+
+    /// <summary>
+    /// Get current waypoint index (for debugging)
+    /// </summary>
+    public int GetCurrentWaypointIndex() => _currentWaypointIndex;
+
+    /// <summary>
+    /// Get goal point
+    /// </summary>
+    public PathPoint? GetGoalPoint() => _goalPoint;
+}

srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Navigation/Core/VelocityEstimatorSimplified.cs

@@ -0,0 +1,172 @@
+using RobotNet10.NavigationTune.Shared.Models;
+
+namespace RobotNet10.NavigationTune.Navigation.Core;
+
+/// <summary>
+/// Simplified Velocity Estimator for tuning system
+/// Combines encoder measurements with motor dynamics model
+/// </summary>
+public class VelocityEstimatorSimplified
+{
+    private readonly VelocityEstimatorConfig _estimatorConfig;
+    private readonly VelocitySignalProcessingConfig _signalConfig;
+    private readonly MotorDynamicsModel _motorModel;
+    private readonly CircularBuffer<double> _predictionErrors;
+
+    private double _filteredEncoderVel = 0;
+    private double _currentConfidence = 1.0;
+    private double _blendRatio;
+
+    public VelocityEstimatorSimplified(
+        VelocityEstimatorConfig estimatorConfig,
+        VelocitySignalProcessingConfig signalConfig,
+        MotorDynamicsConfig motorConfig)
+    {
+        _estimatorConfig = estimatorConfig;
+        _signalConfig = signalConfig;
+        _motorModel = new MotorDynamicsModel(motorConfig);
+        _predictionErrors = new CircularBuffer<double>(20);
+        _blendRatio = estimatorConfig.DefaultBlendRatio;
+    }
+
+    /// <summary>
+    /// Estimate velocity using hybrid approach
+    /// </summary>
+    public double EstimateVelocity(
+        double vCmd,
+        double vActual,
+        double dt)
+    {
+        // 1. Filter encoder velocity (exponential moving average)
+        _filteredEncoderVel = LowPassFilter(vActual, _filteredEncoderVel, _signalConfig.AlphaFilter);
+
+        // 2. Predict velocity using model
+        double predictionHorizon = CalculatePredictionHorizon(_filteredEncoderVel);
+        double vModel = _motorModel.PredictVelocity(vCmd, _filteredEncoderVel, predictionHorizon);
+
+        // 3. Calculate tracking error
+        double trackingError = CalculateTrackingError(vModel, _filteredEncoderVel);
+
+        // 4. Adaptive blending based on tracking quality
+        _blendRatio = CalculateAdaptiveBlendRatio(trackingError);
+
+        // 5. Update confidence
+        UpdateModelConfidence(vModel, _filteredEncoderVel);
+
+        // 6. Hybrid estimation
+        double vHybrid = _blendRatio * vModel + (1.0 - _blendRatio) * _filteredEncoderVel;
+
+        return vHybrid;
+    }
+
+    /// <summary>
+    /// Get current model confidence
+    /// </summary>
+    public double GetConfidence()
+    {
+        return _currentConfidence;
+    }
+
+    /// <summary>
+    /// Reset estimator state
+    /// </summary>
+    public void Reset()
+    {
+        _filteredEncoderVel = 0;
+        _currentConfidence = 1.0;
+        _blendRatio = _estimatorConfig.DefaultBlendRatio;
+        _predictionErrors.Clear();
+    }
+
+    /// <summary>
+    /// Low-pass filter for encoder velocity
+    /// </summary>
+    private static double LowPassFilter(double newValue, double oldValue, double alpha)
+    {
+        if (alpha < 0 || alpha > 1)
+            throw new ArgumentException("Alpha must be between 0 and 1", nameof(alpha));
+
+        return alpha * newValue + (1.0 - alpha) * oldValue;
+    }
+
+    /// <summary>
+    /// Calculate prediction horizon based on lookahead
+    /// </summary>
+    private double CalculatePredictionHorizon(double vActual)
+    {
+        // Simple estimation: use a fixed time horizon
+        // In real implementation, this would be based on Pure Pursuit lookahead
+        double predictionTime = 0.5; // 500ms default
+
+        if (Math.Abs(vActual) > 0.1)
+        {
+            // Adjust based on velocity
+            predictionTime = Math.Clamp(0.3 / Math.Abs(vActual), 0.1, 2.0);
+        }
+
+        return predictionTime;
+    }
+
+    /// <summary>
+    /// Calculate tracking error (normalized)
+    /// </summary>
+    private static double CalculateTrackingError(double vModel, double vActual)
+    {
+        double error = Math.Abs(vModel - vActual);
+        double normalizedError = error / Math.Max(Math.Abs(vActual), 0.1);
+        return normalizedError;
+    }
+
+    /// <summary>
+    /// Calculate adaptive blend ratio based on tracking quality
+    /// </summary>
+    private double CalculateAdaptiveBlendRatio(double trackingError)
+    {
+        double blendRatio;
+
+        if (trackingError < _estimatorConfig.GoodTrackingThreshold)
+        {
+            // Good tracking → trust model more
+            blendRatio = _estimatorConfig.GoodTrackingBlend;
+        }
+        else if (trackingError < _estimatorConfig.ModerateTrackingThreshold)
+        {
+            // Moderate tracking → balanced
+            blendRatio = _estimatorConfig.ModerateTrackingBlend;
+        }
+        else
+        {
+            // Poor tracking → trust encoder more
+            blendRatio = _estimatorConfig.PoorTrackingBlend;
+        }
+
+        // Adjust based on confidence
+        blendRatio *= _currentConfidence;
+
+        // Clamp to valid range
+        blendRatio = Math.Clamp(blendRatio, _estimatorConfig.MinBlendRatio, _estimatorConfig.MaxBlendRatio);
+
+        return blendRatio;
+    }
+
+    /// <summary>
+    /// Update model confidence based on prediction accuracy
+    /// </summary>
+    private void UpdateModelConfidence(double vPredicted, double vActual)
+    {
+        double predError = Math.Abs(vPredicted - vActual) / Math.Max(Math.Abs(vActual), 0.1);
+        _predictionErrors.Add(predError);
+
+        if (_predictionErrors.Count > 0)
+        {
+            double avgError = _predictionErrors.Average();
+            double newConfidence = Math.Clamp(1.0 - avgError, 0.0, 1.0);
+
+            // Smooth update with decay
+            _currentConfidence = _estimatorConfig.ConfidenceDecayRate * _currentConfidence +
+                                (1.0 - _estimatorConfig.ConfidenceDecayRate) * newConfidence;
+
+            _currentConfidence = Math.Max(_currentConfidence, _estimatorConfig.MinConfidence);
+        }
+    }
+}