129 lines
3.9 KiB
C#
129 lines
3.9 KiB
C#
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;
|
|
}
|