Initial commit

srcs/RobotNet10/Tests/RobotNet10.NavigationTune.Test/Navigation/Core/MotorDynamicsModelTests.cs

@@ -0,0 +1,135 @@
+using FluentAssertions;
+using Xunit;
+using RobotNet10.NavigationTune.Navigation.Core;
+using RobotNet10.NavigationTune.Shared.Models;
+
+namespace RobotNet10.NavigationTune.Test.Navigation.Core;
+
+public class MotorDynamicsModelTests
+{
+    [Fact]
+    public void Constructor_WithDefaultValues_ShouldInitialize()
+    {
+        // Act
+        var model = new MotorDynamicsModel();
+
+        // Assert
+        model.Tau.Should().Be(0.3);
+        model.Delta.Should().Be(0.05f);
+    }
+
+    [Fact]
+    public void Constructor_WithConfig_ShouldUseConfigValues()
+    {
+        // Arrange
+        var config = new MotorDynamicsConfig { Tau = 0.5, Delta = 0.1 };
+
+        // Act
+        var model = new MotorDynamicsModel(config);
+
+        // Assert
+        model.Tau.Should().Be(0.5);
+        model.Delta.Should().Be(0.1);
+    }
+
+    [Fact]
+    public void PredictVelocity_WithTimeBeforeDelay_ShouldReturnCurrentVelocity()
+    {
+        // Arrange
+        var model = new MotorDynamicsModel
+        {
+            Tau = 0.3,
+            Delta = 0.1
+        };
+        const double vCmd = 2.0;
+        const double vActual = 1.0;
+        const double timeAhead = 0.05f; // Less than Delta
+
+        // Act
+        var result = model.PredictVelocity(vCmd, vActual, timeAhead);
+
+        // Assert
+        result.Should().BeApproximately(vActual, 0.001f);
+    }
+
+    [Fact]
+    public void PredictVelocity_WithTimeAfterDelay_ShouldPredictFutureVelocity()
+    {
+        // Arrange
+        var model = new MotorDynamicsModel
+        {
+            Tau = 0.3,
+            Delta = 0.05f
+        };
+        const double vCmd = 2.0;
+        const double vActual = 0.0;
+        const double timeAhead = 0.2; // After delay
+
+        // Act
+        var result = model.PredictVelocity(vCmd, vActual, timeAhead);
+
+        // Assert
+        result.Should().BeGreaterThan(vActual);
+        result.Should().BeLessThan(vCmd);
+    }
+
+    [Fact]
+    public void PredictVelocity_WithLongTime_ShouldApproachCommandVelocity()
+    {
+        // Arrange
+        var model = new MotorDynamicsModel
+        {
+            Tau = 0.3,
+            Delta = 0.05f
+        };
+        const double vCmd = 2.0;
+        const double vActual = 0.0;
+        const double timeAhead = 2.0; // Long time
+
+        // Act
+        var result = model.PredictVelocity(vCmd, vActual, timeAhead);
+
+        // Assert
+        result.Should().BeApproximately(vCmd, 0.1);
+    }
+
+    [Fact]
+    public void PredictVelocity_WithDecreasingCommand_ShouldPredictDecrease()
+    {
+        // Arrange
+        var model = new MotorDynamicsModel
+        {
+            Tau = 0.3,
+            Delta = 0.05f
+        };
+        const double vCmd = 0.0;
+        const double vActual = 2.0;
+        const double timeAhead = 0.2;
+
+        // Act
+        var result = model.PredictVelocity(vCmd, vActual, timeAhead);
+
+        // Assert
+        result.Should().BeLessThan(vActual);
+        result.Should().BeGreaterThan(vCmd);
+    }
+
+    [Fact]
+    public void ToString_ShouldReturnFormattedString()
+    {
+        // Arrange
+        var model = new MotorDynamicsModel
+        {
+            Tau = 0.3,
+            Delta = 0.05f
+        };
+
+        // Act
+        var result = model.ToString();
+
+        // Assert
+        result.Should().Contain("MotorModel");
+        result.Should().Contain("τ=");
+        result.Should().Contain("δ=");
+    }
+}

srcs/RobotNet10/Tests/RobotNet10.NavigationTune.Test/Navigation/Core/PIDTests.cs

@@ -0,0 +1,179 @@
+using FluentAssertions;
+using Xunit;
+using RobotNet10.NavigationTune.Navigation.Core;
+using RobotNet10.NavigationTune.Shared.Models;
+using RobotNet10.NavigationTune.Test.Helpers;
+
+namespace RobotNet10.NavigationTune.Test.Navigation.Core;
+
+public class PIDTests
+{
+    [Fact]
+    public void Constructor_WithConfig_ShouldInitializeCorrectly()
+    {
+        // Arrange
+        var config = new PIDConfig { Kp = 1.5, Ki = 0.01, Kd = 0.5 };
+
+        // Act
+        var pid = new PID(config);
+
+        // Assert
+        pid.Should().NotBeNull();
+    }
+
+    [Fact]
+    public void PID_step_WithZeroError_ShouldReturnZero()
+    {
+        // Arrange
+        var config = new PIDConfig { Kp = 1.0, Ki = 0.0, Kd = 0.0 };
+        var pid = new PID(config);
+        const double dt = 0.02; // 50Hz
+
+        // Act
+        var result = pid.PID_step(0.0, 10.0, -10.0, dt);
+
+        // Assert
+        result.Should().BeApproximately(0.0, 0.001);
+    }
+
+    [Fact]
+    public void PID_step_WithProportionalOnly_ShouldReturnProportionalOutput()
+    {
+        // Arrange
+        var config = new PIDConfig { Kp = 2.0, Ki = 0.0, Kd = 0.0 };
+        var pid = new PID(config);
+        const double error = 5.0;
+        const double dt = 0.02;
+
+        // Act
+        var result = pid.PID_step(error, 10.0, -10.0, dt);
+
+        // Assert
+        // First call: P_part = Kp * (error - Pre_Error) = 2.0 * (5.0 - 0) = 10.0
+        // But since it's incremental: Out = Pre_Out + P_part = 0 + 10.0 = 10.0
+        result.Should().BeApproximately(10.0, 0.1);
+    }
+
+    [Fact]
+    public void PID_step_WithIntegralTerm_ShouldAccumulateError()
+    {
+        // Arrange
+        var config = new PIDConfig { Kp = 1.0, Ki = 0.1, Kd = 0.0 };
+        var pid = new PID(config);
+        const double error = 1.0;
+        const double dt = 0.02;
+
+        // Act
+        var result1 = pid.PID_step(error, 10.0, -10.0, dt);
+        var result2 = pid.PID_step(error, 10.0, -10.0, dt);
+
+        // Assert
+        // Second call should have accumulated integral term
+        result2.Should().BeGreaterThan(result1);
+    }
+
+    [Fact]
+    public void PID_step_WithDerivativeTerm_ShouldReactToErrorChange()
+    {
+        // Arrange
+        var config = new PIDConfig { Kp = 1.0, Ki = 0.0, Kd = 1.0 };
+        var pid = new PID(config);
+        const double dt = 0.02;
+
+        // Act
+        var result1 = pid.PID_step(5.0, 10.0, -10.0, dt);
+        var result2 = pid.PID_step(3.0, 10.0, -10.0, dt); // Decreasing error
+
+        // Assert
+        // With decreasing error, derivative term should reduce output
+        result2.Should().BeLessThan(result1);
+    }
+
+    [Fact]
+    public void PID_step_ShouldClampToMaxMin()
+    {
+        // Arrange
+        var config = new PIDConfig { Kp = 100.0, Ki = 0.0, Kd = 0.0 };
+        var pid = new PID(config);
+        const double error = 10.0;
+        const double max = 5.0;
+        const double min = -5.0;
+        const double dt = 0.02;
+
+        // Act
+        var result = pid.PID_step(error, max, min, dt);
+
+        // Assert
+        result.Should().BeLessOrEqualTo(max);
+        result.Should().BeGreaterOrEqualTo(min);
+    }
+
+    [Fact]
+    public void Reset_ShouldClearInternalState()
+    {
+        // Arrange
+        var config = new PIDConfig { Kp = 1.0, Ki = 0.1, Kd = 0.0 };
+        var pid = new PID(config);
+        const double dt = 0.02;
+
+        // Act
+        pid.PID_step(5.0, 10.0, -10.0, dt);
+        pid.Reset();
+        var result = pid.PID_step(5.0, 10.0, -10.0, dt);
+
+        // Assert
+        // After reset, should behave like first call
+        result.Should().BeApproximately(5.0, 0.1);
+    }
+
+    [Fact]
+    public void WithKp_ShouldUpdateKpValue()
+    {
+        // Arrange
+        var config = new PIDConfig { Kp = 1.0, Ki = 0.0, Kd = 0.0 };
+        var pid = new PID(config);
+
+        // Act
+        var updatedPid = pid.WithKp(2.0);
+        const double dt = 0.02;
+        var result = updatedPid.PID_step(5.0, 10.0, -10.0, dt);
+
+        // Assert
+        result.Should().BeApproximately(10.0, 0.1);
+    }
+
+    [Fact]
+    public void WithKi_ShouldUpdateKiValue()
+    {
+        // Arrange
+        var config = new PIDConfig { Kp = 1.0, Ki = 0.0, Kd = 0.0 };
+        var pid = new PID(config);
+
+        // Act
+        var updatedPid = pid.WithKi(0.1);
+        const double dt = 0.02;
+        var result1 = updatedPid.PID_step(1.0, 10.0, -10.0, dt);
+        var result2 = updatedPid.PID_step(1.0, 10.0, -10.0, dt);
+
+        // Assert
+        result2.Should().BeGreaterThan(result1);
+    }
+
+    [Fact]
+    public void WithKd_ShouldUpdateKdValue()
+    {
+        // Arrange
+        var config = new PIDConfig { Kp = 1.0, Ki = 0.0, Kd = 0.0 };
+        var pid = new PID(config);
+
+        // Act
+        var updatedPid = pid.WithKd(1.0);
+        const double dt = 0.02;
+        var result1 = updatedPid.PID_step(5.0, 10.0, -10.0, dt);
+        var result2 = updatedPid.PID_step(3.0, 10.0, -10.0, dt);
+
+        // Assert
+        // With derivative, decreasing error should reduce output
+        result2.Should().BeLessThan(result1);
+    }
+}

srcs/RobotNet10/Tests/RobotNet10.NavigationTune.Test/Navigation/Core/PurePursuitSimplifiedTests.cs

@@ -0,0 +1,163 @@
+using FluentAssertions;
+using Xunit;
+using RobotNet10.NavigationTune.Shared.Models;
+using RobotNet10.NavigationTune.Navigation.Core;
+using RobotNet10.NavigationTune.Test.Helpers;
+
+namespace RobotNet10.NavigationTune.Test.Navigation.Core;
+
+public class PurePursuitSimplifiedTests
+{
+    [Fact]
+    public void Constructor_WithConfig_ShouldInitialize()
+    {
+        // Arrange
+        var config = new PurePursuitConfig();
+
+        // Act
+        var purePursuit = new PurePursuitSimplified(config, new StanleyConfig());
+
+        // Assert
+        purePursuit.Should().NotBeNull();
+    }
+
+    [Fact]
+    public void SetPath_WithValidPath_ShouldSetPath()
+    {
+        // Arrange
+        var config = new PurePursuitConfig();
+        var purePursuit = new PurePursuitSimplified(config, new StanleyConfig());
+        var path = TestHelpers.CreateSimplePath(10, 10.0);
+
+        // Act
+        purePursuit.SetPath(path);
+
+        // Assert
+        // Path should be set (no exception thrown)
+        purePursuit.Should().NotBeNull();
+    }
+
+    [Fact]
+    public void CalculateAngularVelocity_WithStraightPath_ShouldReturnZero()
+    {
+        // Arrange
+        var config = new PurePursuitConfig
+        {
+            LookaheadMin = 0.5,
+            LookaheadMax = 2.0
+        };
+        var purePursuit = new PurePursuitSimplified(config, new StanleyConfig());
+        var path = TestHelpers.CreateSimplePath(10, 10.0);
+        purePursuit.SetPath(path);
+
+        // Act
+        var (linear, angular) = purePursuit.CalculateAngularVelocity(0.0, 0.0, 0.0, 1.0, 1.0);
+
+        // Assert
+        // On straight path, angular velocity should be near zero
+        Math.Abs(angular).Should().BeLessThan(0.1);
+    }
+
+    [Fact]
+    public void CalculateAngularVelocity_WithCurvedPath_ShouldReturnNonZero()
+    {
+        // Arrange
+        var config = new PurePursuitConfig
+        {
+            LookaheadMin = 0.5,
+            LookaheadMax = 2.0
+        };
+        var purePursuit = new PurePursuitSimplified(config, new StanleyConfig());
+        
+        // Create curved path (circle)
+        var path = new List<PathPoint>();
+        for (int i = 0; i < 20; i++)
+        {
+            double angle = i * Math.PI / 10;
+            path.Add(new PathPoint
+            {
+                X = Math.Cos(angle) * 2.0,
+                Y = Math.Sin(angle) * 2.0,
+                DistanceFromStart = angle * 2.0
+            });
+        }
+        purePursuit.SetPath(path);
+
+        // Act
+        var (linear, angular) = purePursuit.CalculateAngularVelocity(2.0, 0.0, 0.0, 1.0, 1.0);
+
+        // Assert
+        // On curved path, should have non-zero angular velocity
+        Math.Abs(angular).Should().BeGreaterThan(0.01);
+    }
+
+    [Fact]
+    public void GetCurrentLookahead_WithLowVelocity_ShouldReturnMinLookahead()
+    {
+        // Arrange
+        var config = new PurePursuitConfig
+        {
+            LookaheadMin = 0.5,
+            LookaheadMax = 2.0
+        };
+        var purePursuit = new PurePursuitSimplified(config, new StanleyConfig());
+
+        // Act
+        var lookahead = purePursuit.GetCurrentLookahead(0.1, 1.0);
+
+        // Assert
+        lookahead.Should().BeApproximately(config.LookaheadMin, 0.1);
+    }
+
+    [Fact]
+    public void GetCurrentLookahead_WithHighVelocity_ShouldReturnMaxLookahead()
+    {
+        // Arrange
+        var config = new PurePursuitConfig
+        {
+            LookaheadMin = 0.5,
+            LookaheadMax = 2.0
+        };
+        var purePursuit = new PurePursuitSimplified(config, new StanleyConfig());
+
+        // Act
+        var lookahead = purePursuit.GetCurrentLookahead(5.0, 1.0);
+
+        // Assert
+        lookahead.Should().BeApproximately(config.LookaheadMax, 0.1);
+    }
+
+    [Fact]
+    public void CalculateAngularVelocity_WithLowConfidence_ShouldAdjustLookahead()
+    {
+        // Arrange
+        var config = new PurePursuitConfig
+        {
+            LookaheadMin = 0.5,
+            LookaheadMax = 2.0
+        };
+        var purePursuit = new PurePursuitSimplified(config, new StanleyConfig());
+        
+        // Create curved path
+        var path = new List<PathPoint>();
+        for (int i = 0; i < 10; i++)
+        {
+            double angle = i * Math.PI / 5;
+            path.Add(new PathPoint
+            {
+                X = Math.Cos(angle) * 2.0,
+                Y = Math.Sin(angle) * 2.0,
+                DistanceFromStart = angle * 2.0
+            });
+        }
+        purePursuit.SetPath(path);
+
+        // Act
+        var lookaheadHighConf = purePursuit.GetCurrentLookahead(1.0, 1.0);
+        var lookaheadLowConf = purePursuit.GetCurrentLookahead(1.0, 0.3);
+
+        // Assert
+        // Low confidence should reduce lookahead distance
+        lookaheadLowConf.Should().BeLessThanOrEqualTo(lookaheadHighConf);
+    }
+}

srcs/RobotNet10/Tests/RobotNet10.NavigationTune.Test/Navigation/Core/VelocityEstimatorSimplifiedTests.cs

@@ -0,0 +1,266 @@
+using FluentAssertions;
+using Xunit;
+using RobotNet10.NavigationTune.Navigation.Core;
+using RobotNet10.NavigationTune.Shared.Models;
+
+namespace RobotNet10.NavigationTune.Test.Navigation.Core;
+
+public class VelocityEstimatorSimplifiedTests
+{
+    private VelocityEstimatorConfig CreateDefaultEstimatorConfig()
+    {
+        return new VelocityEstimatorConfig
+        {
+            MinBlendRatio = 0.15f,
+            MaxBlendRatio = 0.8f,
+            DefaultBlendRatio = 0.6,
+            GoodTrackingThreshold = 0.12f,
+            ModerateTrackingThreshold = 0.3,
+            GoodTrackingBlend = 0.7,
+            ModerateTrackingBlend = 0.5,
+            PoorTrackingBlend = 0.25f,
+            ConfidenceDecayRate = 0.95f,
+            MinConfidence = 0.3
+        };
+    }
+
+    private VelocitySignalProcessingConfig CreateDefaultSignalConfig()
+    {
+        return new VelocitySignalProcessingConfig
+        {
+            AlphaFilter = 0.3,
+            NoiseThreshold = 0.5
+        };
+    }
+
+    private MotorDynamicsConfig CreateDefaultMotorConfig()
+    {
+        return new MotorDynamicsConfig
+        {
+            Tau = 0.3,
+            Delta = 0.05f
+        };
+    }
+
+    [Fact]
+    public void Constructor_WithConfigs_ShouldInitializeCorrectly()
+    {
+        // Arrange
+        var estimatorConfig = CreateDefaultEstimatorConfig();
+        var signalConfig = CreateDefaultSignalConfig();
+        var motorConfig = CreateDefaultMotorConfig();
+
+        // Act
+        var estimator = new VelocityEstimatorSimplified(estimatorConfig, signalConfig, motorConfig);
+
+        // Assert
+        estimator.Should().NotBeNull();
+        estimator.GetConfidence().Should().BeApproximately(1.0, 0.01f);
+    }
+
+    [Fact]
+    public void EstimateVelocity_WithPerfectTracking_ShouldUseHighModelBlend()
+    {
+        // Arrange
+        var estimator = new VelocityEstimatorSimplified(
+            CreateDefaultEstimatorConfig(),
+            CreateDefaultSignalConfig(),
+            CreateDefaultMotorConfig());
+
+        const double vCmd = 1.0;
+        const double vActual = 1.0;
+        const double dt = 0.02f;
+
+        // Act
+        var result = estimator.EstimateVelocity(vCmd, vActual, dt);
+
+        // Assert
+        result.Should().BeGreaterThan(0);
+        estimator.GetConfidence().Should().BeGreaterThan(0.5);
+    }
+
+    [Fact]
+    public void EstimateVelocity_WithLargeError_ShouldUseLowModelBlend()
+    {
+        // Arrange
+        var estimator = new VelocityEstimatorSimplified(
+            CreateDefaultEstimatorConfig(),
+            CreateDefaultSignalConfig(),
+            CreateDefaultMotorConfig());
+
+        const double vCmd = 2.0;
+        const double vActual = 0.5; // Large error
+        const double dt = 0.02f;
+
+        // Act
+        var result = estimator.EstimateVelocity(vCmd, vActual, dt);
+
+        // Assert
+        result.Should().BeGreaterThan(0);
+        // With large error, should rely more on encoder (lower blend ratio)
+    }
+
+    [Fact]
+    public void EstimateVelocity_WithZeroCommand_ShouldReturnLowVelocity()
+    {
+        // Arrange
+        var estimator = new VelocityEstimatorSimplified(
+            CreateDefaultEstimatorConfig(),
+            CreateDefaultSignalConfig(),
+            CreateDefaultMotorConfig());
+
+        const double vCmd = 0.0;
+        const double vActual = 0.1;
+        const double dt = 0.02f;
+
+        // Act
+        var result = estimator.EstimateVelocity(vCmd, vActual, dt);
+
+        // Assert
+        result.Should().BeLessThan(0.5);
+    }
+
+    [Fact]
+    public void EstimateVelocity_WithFiltering_ShouldSmoothNoise()
+    {
+        // Arrange
+        var signalConfig = CreateDefaultSignalConfig();
+        signalConfig.AlphaFilter = 0.1; // Strong filtering
+        var estimator = new VelocityEstimatorSimplified(
+            CreateDefaultEstimatorConfig(),
+            signalConfig,
+            CreateDefaultMotorConfig());
+
+        const double vCmd = 1.0;
+        const double dt = 0.02f;
+
+        // Act - Simulate noisy encoder readings
+        var results = new List<double>();
+        for (int i = 0; i < 10; i++)
+        {
+            double noisyReading = 1.0 + ((i % 2) * 0.2 - 0.1); // Alternating noise
+            var result = estimator.EstimateVelocity(vCmd, noisyReading, dt);
+            results.Add(result);
+        }
+
+        // Assert - Results should be smoother than input
+        results.Should().NotBeEmpty();
+        // Variance should be less than input variance
+    }
+
+    [Fact]
+    public void GetConfidence_Initially_ShouldReturnOne()
+    {
+        // Arrange
+        var estimator = new VelocityEstimatorSimplified(
+            CreateDefaultEstimatorConfig(),
+            CreateDefaultSignalConfig(),
+            CreateDefaultMotorConfig());
+
+        // Act
+        var confidence = estimator.GetConfidence();
+
+        // Assert
+        confidence.Should().BeApproximately(1.0, 0.01f);
+    }
+
+    [Fact]
+    public void GetConfidence_AfterPoorTracking_ShouldDecrease()
+    {
+        // Arrange
+        var estimator = new VelocityEstimatorSimplified(
+            CreateDefaultEstimatorConfig(),
+            CreateDefaultSignalConfig(),
+            CreateDefaultMotorConfig());
+
+        const double vCmd = 2.0;
+        const double vActual = 0.1; // Poor tracking
+        const double dt = 0.02f;
+
+        // Act - Run multiple iterations with poor tracking
+        for (int i = 0; i < 20; i++)
+        {
+            estimator.EstimateVelocity(vCmd, vActual, dt);
+        }
+
+        var confidence = estimator.GetConfidence();
+
+        // Assert
+        confidence.Should().BeLessThan(1.0);
+        confidence.Should().BeGreaterOrEqualTo(0.3); // Min confidence
+    }
+
+    [Fact]
+    public void Reset_ShouldRestoreInitialState()
+    {
+        // Arrange
+        var estimator = new VelocityEstimatorSimplified(
+            CreateDefaultEstimatorConfig(),
+            CreateDefaultSignalConfig(),
+            CreateDefaultMotorConfig());
+
+        // Run some estimations
+        for (int i = 0; i < 10; i++)
+        {
+            estimator.EstimateVelocity(1.0, 0.5, 0.02f);
+        }
+
+        // Act
+        estimator.Reset();
+
+        // Assert
+        estimator.GetConfidence().Should().BeApproximately(1.0, 0.01f);
+    }
+
+    [Fact]
+    public void EstimateVelocity_WithGoodTracking_ShouldMaintainHighConfidence()
+    {
+        // Arrange
+        var estimator = new VelocityEstimatorSimplified(
+            CreateDefaultEstimatorConfig(),
+            CreateDefaultSignalConfig(),
+            CreateDefaultMotorConfig());
+
+        const double vCmd = 1.0;
+        const double vActual = 1.0;
+        const double dt = 0.02f;
+
+        // Act - Run multiple iterations with good tracking
+        for (int i = 0; i < 20; i++)
+        {
+            estimator.EstimateVelocity(vCmd, vActual, dt);
+        }
+
+        var confidence = estimator.GetConfidence();
+
+        // Assert
+        confidence.Should().BeGreaterThan(0.7);
+    }
+
+    [Fact]
+    public void EstimateVelocity_WithSteadyState_ShouldConverge()
+    {
+        // Arrange
+        var estimator = new VelocityEstimatorSimplified(
+            CreateDefaultEstimatorConfig(),
+            CreateDefaultSignalConfig(),
+            CreateDefaultMotorConfig());
+
+        const double vCmd = 1.5;
+        const double vActual = 1.5;
+        const double dt = 0.02f;
+
+        // Act - Run many iterations
+        var results = new List<double>();
+        for (int i = 0; i < 50; i++)
+        {
+            var result = estimator.EstimateVelocity(vCmd, vActual, dt);
+            results.Add(result);
+        }
+
+        // Assert - Should converge to steady state
+        var lastFew = results.TakeLast(10).ToList();
+        var variance = lastFew.Select(v => Math.Abs(v - lastFew.Average())).Average();
+        variance.Should().BeLessThan(0.1);
+    }
+}