Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Motion/EKFConfiguration.cs

@@ -0,0 +1,128 @@
+namespace RobotNet10.RobotApp.Motion;
+
+/// <summary>
+/// Configuration for Extended Kalman Filter
+/// </summary>
+public class EKFConfiguration
+{
+    /// <summary>
+    /// Device ID của IMU sensor
+    /// </summary>
+    public string ImuDeviceId { get; set; } = string.Empty;
+
+    /// <summary>
+    /// Frame ID của odometry frame (thường là "odom")
+    /// </summary>
+    public string FrameId { get; set; } = "odom";
+
+    /// <summary>
+    /// Child frame ID (thường là "base_link")
+    /// </summary>
+    public string ChildFrameId { get; set; } = "base_link";
+
+    /// <summary>
+    /// Tần số cập nhật EKF (Hz)
+    /// </summary>
+    public double UpdateRateHz { get; set; } = 20.0;
+
+    // Process noise covariance (diagonal values)
+
+    /// <summary>
+    /// Process noise cho position (m^2)
+    /// </summary>
+    public double ProcessNoisePosition { get; set; } = 0.01;
+
+    /// <summary>
+    /// Process noise cho orientation (rad^2)
+    /// </summary>
+    public double ProcessNoiseOrientation { get; set; } = 0.01;
+
+    /// <summary>
+    /// Process noise cho velocity (m^2/s^2 hoặc rad^2/s^2)
+    /// </summary>
+    public double ProcessNoiseVelocity { get; set; } = 0.1;
+
+    // Measurement noise covariance
+
+    /// <summary>
+    /// Measurement noise cho position từ odometry (m^2)
+    /// </summary>
+    public double MeasurementNoisePosition { get; set; } = 0.05;
+
+    /// <summary>
+    /// Measurement noise cho orientation từ IMU (rad^2)
+    /// </summary>
+    public double MeasurementNoiseOrientation { get; set; } = 0.01;
+
+    // Initial state uncertainty
+
+    /// <summary>
+    /// Initial uncertainty cho position (m^2)
+    /// </summary>
+    public double InitialPositionUncertainty { get; set; } = 0.1;
+
+    /// <summary>
+    /// Initial uncertainty cho orientation (rad^2)
+    /// </summary>
+    public double InitialOrientationUncertainty { get; set; } = 0.1;
+
+    /// <summary>
+    /// Initial uncertainty cho velocity (m^2/s^2 hoặc rad^2/s^2)
+    /// </summary>
+    public double InitialVelocityUncertainty { get; set; } = 0.5;
+
+    // Slip Detection Configuration
+
+    /// <summary>
+    /// Enable wheel slip detection
+    /// </summary>
+    public bool EnableSlipDetection { get; set; } = true;
+
+    /// <summary>
+    /// Enable wheels-in-air detection (uses Z-axis IMU)
+    /// </summary>
+    public bool EnableWheelsInAirDetection { get; set; } = true;
+
+    /// <summary>
+    /// Enable adaptive noise scaling when slip detected
+    /// If false, noise scale always stays at 1.0
+    /// </summary>
+    public bool EnableAdaptiveNoise { get; set; } = true;
+
+    /// <summary>
+    /// Enable slip event logging
+    /// </summary>
+    public bool EnableSlipLogging { get; set; } = true;
+
+    /// <summary>
+    /// Acceleration mismatch threshold for slip detection (m/s²)
+    /// </summary>
+    public double AccelerationMismatchThreshold { get; set; } = 0.5;
+
+    /// <summary>
+    /// Innovation threshold for slip detection (sigma/standard deviations)
+    /// </summary>
+    public double InnovationThreshold { get; set; } = 3.0;
+
+    /// <summary>
+    /// Maximum process noise scale factor when slip detected
+    /// </summary>
+    public double MaxNoiseScale { get; set; } = 10.0;
+
+    /// <summary>
+    /// Z-axis acceleration threshold for freefall detection (m/s²)
+    /// Value close to 9.81 indicates wheels in air
+    /// </summary>
+    public double ZAxisFreefallThreshold { get; set; } = 0.15;
+
+    /// <summary>
+    /// Horizontal motion threshold for slip classification (m/s²)
+    /// </summary>
+    public double HorizontalMotionThreshold { get; set; } = 0.1;
+
+    /// <summary>
+    /// Process noise scale factor for wheels in air scenario
+    /// Very high value effectively freezes position estimation
+    /// </summary>
+    public double WheelsInAirNoiseScale { get; set; } = 100.0;
+}