Initial commit
This commit is contained in:
@@ -0,0 +1,571 @@
|
||||
using Microsoft.Extensions.Configuration;
|
||||
using Microsoft.Extensions.Hosting;
|
||||
using Microsoft.Extensions.Logging;
|
||||
using RobotNet10.RobotApp.Devices;
|
||||
using RobotNet10.Shared;
|
||||
using RobotNet10.Shared.Geometry;
|
||||
using RobotNet10.Shared.Sensor;
|
||||
using System;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace RobotNet10.RobotApp.Motion;
|
||||
|
||||
/// <summary>
|
||||
/// EKFService - Extended Kalman Filter service để fusion odometry và IMU data
|
||||
/// Cải thiện độ chính xác ước lượng trạng thái robot
|
||||
/// </summary>
|
||||
public class EKFService : IHostedService, IDisposable
|
||||
{
|
||||
private readonly EKFConfiguration _config;
|
||||
private readonly IDeviceProvider _deviceProvider;
|
||||
private readonly OdometryService _odometryService;
|
||||
private readonly ILogger<EKFService> _logger;
|
||||
private readonly object _lock = new();
|
||||
|
||||
// IMU device
|
||||
private IInertialMeasurementUnit? _imu;
|
||||
|
||||
// Extended Kalman Filter
|
||||
private ExtendedKalmanFilter? _ekf;
|
||||
|
||||
// Filtered odometry output
|
||||
private Odometry _filteredOdometry = new();
|
||||
private DateTime _lastUpdateTime = DateTime.UtcNow;
|
||||
private bool _isFirstUpdate = true;
|
||||
|
||||
// Yaw offset calibration (applied at EKF level, not IMU level)
|
||||
private double _yawOffset = 0.0;
|
||||
private bool _yawOffsetCalibrated = false;
|
||||
|
||||
// Sequence number for header
|
||||
private uint _sequenceNumber = 0;
|
||||
|
||||
// EKF's own orientation (integrated from angular velocity)
|
||||
private Quaternion _ekfOrientation = new Quaternion(0, 0, 0, 1); // Identity quaternion
|
||||
private DateTime _lastOrientationUpdate = DateTime.UtcNow;
|
||||
|
||||
// Dead zone for gyroscope to prevent drift when stationary
|
||||
private const double GYRO_DEADZONE_THRESHOLD = 0.001; // rad/s (~0.057 deg/s)
|
||||
|
||||
// Timer for periodic updates
|
||||
private Timer? _updateTimer;
|
||||
private bool _disposed = false;
|
||||
|
||||
/// <summary>
|
||||
/// Gets the filtered odometry data
|
||||
/// </summary>
|
||||
public Odometry FilteredOdometry
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
return _filteredOdometry;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public EKFService(
|
||||
IConfiguration configuration,
|
||||
IDeviceProvider deviceProvider,
|
||||
OdometryService odometryService,
|
||||
ILogger<EKFService> logger)
|
||||
{
|
||||
_deviceProvider = deviceProvider ?? throw new ArgumentNullException(nameof(deviceProvider));
|
||||
_odometryService = odometryService ?? throw new ArgumentNullException(nameof(odometryService));
|
||||
_logger = logger ?? throw new ArgumentNullException(nameof(logger));
|
||||
|
||||
// Load configuration
|
||||
var configSection = configuration.GetSection("Motion:EKF");
|
||||
if (!configSection.Exists())
|
||||
{
|
||||
throw new InvalidOperationException("Configuration section 'Motion:EKF' not found in appsettings.json");
|
||||
}
|
||||
|
||||
_config = new EKFConfiguration();
|
||||
configSection.Bind(_config);
|
||||
|
||||
// Validate configuration
|
||||
ValidateConfiguration();
|
||||
}
|
||||
|
||||
private void ValidateConfiguration()
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(_config.ImuDeviceId))
|
||||
throw new InvalidOperationException("ImuDeviceId is required");
|
||||
|
||||
if (_config.UpdateRateHz <= 0)
|
||||
throw new InvalidOperationException("UpdateRateHz must be greater than 0");
|
||||
|
||||
if (string.IsNullOrWhiteSpace(_config.FrameId))
|
||||
_config.FrameId = "odom";
|
||||
|
||||
if (string.IsNullOrWhiteSpace(_config.ChildFrameId))
|
||||
_config.ChildFrameId = "base_link";
|
||||
}
|
||||
|
||||
public async Task StartAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
_logger.LogInformation("Starting EKFService...");
|
||||
|
||||
try
|
||||
{
|
||||
// Wait for devices to be connected
|
||||
_logger.LogInformation("Waiting for all devices to be connected...");
|
||||
var connected = await _deviceProvider.WaitForDevicesConnectedAsync(TimeSpan.FromMinutes(5), cancellationToken);
|
||||
|
||||
if (!connected)
|
||||
{
|
||||
_logger.LogWarning("Timeout waiting for devices to connect. EKFService will not be initialized.");
|
||||
return;
|
||||
}
|
||||
|
||||
// Get IMU device
|
||||
var imuDevice = _deviceProvider.GetDevice(_config.ImuDeviceId);
|
||||
if (imuDevice == null)
|
||||
{
|
||||
_logger.LogError("IMU device '{DeviceId}' not found", _config.ImuDeviceId);
|
||||
return;
|
||||
}
|
||||
|
||||
if (imuDevice is not IInertialMeasurementUnit imu)
|
||||
{
|
||||
_logger.LogError("Device '{DeviceId}' is not an IInertialMeasurementUnit", _config.ImuDeviceId);
|
||||
return;
|
||||
}
|
||||
|
||||
if (!imuDevice.IsConnected)
|
||||
{
|
||||
_logger.LogWarning("IMU device '{DeviceId}' is not connected. Status: {Status}. EKFService will wait for connection.",
|
||||
_config.ImuDeviceId, imuDevice.Status);
|
||||
// Start retry task
|
||||
_ = Task.Run(async () => await RetryInitializationWhenDevicesReadyAsync(cancellationToken));
|
||||
return;
|
||||
}
|
||||
|
||||
_imu = imu;
|
||||
|
||||
// Initialize EKF
|
||||
InitializeEKF();
|
||||
|
||||
// Start periodic update timer
|
||||
var updateInterval = TimeSpan.FromMilliseconds(1000.0 / _config.UpdateRateHz);
|
||||
var LastTime = DateTime.UtcNow;
|
||||
_updateTimer = new Timer(OnTimerCallback, null, updateInterval, updateInterval);
|
||||
|
||||
_logger.LogInformation("EKFService started successfully with update rate {UpdateRate} Hz", _config.UpdateRateHz);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error starting EKFService");
|
||||
}
|
||||
|
||||
await Task.CompletedTask;
|
||||
}
|
||||
|
||||
private async Task RetryInitializationWhenDevicesReadyAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
const int maxRetries = 60; // 5 minutes with 5 second intervals
|
||||
int retryCount = 0;
|
||||
|
||||
while (retryCount < maxRetries && !cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
await Task.Delay(TimeSpan.FromSeconds(5), cancellationToken);
|
||||
|
||||
var imuDevice = _deviceProvider.GetDevice(_config.ImuDeviceId);
|
||||
|
||||
if (imuDevice != null && imuDevice.IsConnected && imuDevice is IInertialMeasurementUnit imu)
|
||||
{
|
||||
_logger.LogInformation("IMU device is now connected. Initializing EKFService...");
|
||||
|
||||
lock (_lock)
|
||||
{
|
||||
_imu = imu;
|
||||
}
|
||||
|
||||
InitializeEKF();
|
||||
|
||||
var updateInterval = TimeSpan.FromMilliseconds(1000.0 / _config.UpdateRateHz);
|
||||
_updateTimer = new Timer(OnTimerCallback, null, updateInterval, updateInterval);
|
||||
|
||||
_logger.LogInformation("EKFService initialized successfully after device connection");
|
||||
return;
|
||||
}
|
||||
|
||||
retryCount++;
|
||||
if (retryCount % 12 == 0) // Log every minute
|
||||
{
|
||||
_logger.LogInformation("Still waiting for IMU device to connect... (attempt {Attempt}/{MaxAttempts})",
|
||||
retryCount, maxRetries);
|
||||
}
|
||||
}
|
||||
|
||||
if (retryCount >= maxRetries)
|
||||
{
|
||||
_logger.LogWarning("Timeout waiting for IMU device to connect. EKFService will not be initialized.");
|
||||
}
|
||||
}
|
||||
|
||||
public async Task StopAsync(CancellationToken cancellationToken)
|
||||
{
|
||||
_logger.LogInformation("Stopping EKFService...");
|
||||
|
||||
try
|
||||
{
|
||||
_updateTimer?.Dispose();
|
||||
_updateTimer = null;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error stopping EKFService");
|
||||
}
|
||||
|
||||
await Task.CompletedTask;
|
||||
}
|
||||
|
||||
private void OnTimerCallback(object? state)
|
||||
{
|
||||
try
|
||||
{
|
||||
UpdateEKF();
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error updating EKF");
|
||||
}
|
||||
}
|
||||
|
||||
private void InitializeEKF()
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
// Get initial state from odometry
|
||||
var odom = _odometryService.CurrentOdometry;
|
||||
var initialX = odom.Pose.Pose.Position.X;
|
||||
var initialY = odom.Pose.Pose.Position.Y;
|
||||
var initialTheta = QuaternionToYaw(odom.Pose.Pose.Orientation);
|
||||
|
||||
// Initial state: [x, y, theta, vx, vy, omega]
|
||||
double[] initialState = new double[6]
|
||||
{
|
||||
initialX,
|
||||
initialY,
|
||||
initialTheta,
|
||||
0.0, // vx
|
||||
0.0, // vy
|
||||
0.0 // omega
|
||||
};
|
||||
|
||||
// Initial covariance (diagonal)
|
||||
double[] initialCovDiag = new double[6]
|
||||
{
|
||||
_config.InitialPositionUncertainty, // x
|
||||
_config.InitialPositionUncertainty, // y
|
||||
_config.InitialOrientationUncertainty, // theta
|
||||
_config.InitialVelocityUncertainty, // vx
|
||||
_config.InitialVelocityUncertainty, // vy
|
||||
_config.InitialVelocityUncertainty // omega
|
||||
};
|
||||
var initialCovariance = MatrixHelper.CreateDiagonal(initialCovDiag);
|
||||
|
||||
// Process noise covariance (diagonal)
|
||||
double[] processNoiseDiag = new double[6]
|
||||
{
|
||||
_config.ProcessNoisePosition, // x
|
||||
_config.ProcessNoisePosition, // y
|
||||
_config.ProcessNoiseOrientation, // theta
|
||||
_config.ProcessNoiseVelocity, // vx
|
||||
_config.ProcessNoiseVelocity, // vy
|
||||
_config.ProcessNoiseVelocity // omega
|
||||
};
|
||||
var processNoise = MatrixHelper.CreateDiagonal(processNoiseDiag);
|
||||
|
||||
// Measurement noise covariance (diagonal) - [x, y, theta]
|
||||
double[] measurementNoiseDiag = new double[3]
|
||||
{
|
||||
_config.MeasurementNoisePosition, // x
|
||||
_config.MeasurementNoisePosition, // y
|
||||
_config.MeasurementNoiseOrientation // theta
|
||||
};
|
||||
var measurementNoise = MatrixHelper.CreateDiagonal(measurementNoiseDiag);
|
||||
|
||||
// Create EKF
|
||||
_ekf = new ExtendedKalmanFilter(
|
||||
initialState,
|
||||
initialCovariance,
|
||||
processNoise,
|
||||
measurementNoise);
|
||||
|
||||
_lastUpdateTime = DateTime.UtcNow;
|
||||
_isFirstUpdate = true;
|
||||
|
||||
// Reset yaw offset - will be calibrated on first update
|
||||
_yawOffset = 0.0;
|
||||
_yawOffsetCalibrated = false;
|
||||
|
||||
// Initialize EKF orientation (start from identity quaternion)
|
||||
_ekfOrientation = new Quaternion(0, 0, 0, 1);
|
||||
_lastOrientationUpdate = DateTime.UtcNow;
|
||||
|
||||
_logger.LogInformation("EKF initialized with state: X={X:F3}, Y={Y:F3}, Theta={Theta:F3}",
|
||||
initialX, initialY, initialTheta);
|
||||
}
|
||||
}
|
||||
|
||||
private void UpdateEKF()
|
||||
{
|
||||
if (_ekf == null || _imu == null)
|
||||
return;
|
||||
|
||||
// Check if IMU is still connected
|
||||
var imuDevice = _deviceProvider.GetDevice(_config.ImuDeviceId);
|
||||
if (imuDevice == null || !imuDevice.IsConnected)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
lock (_lock)
|
||||
{
|
||||
var currentTime = DateTime.UtcNow;
|
||||
var dt = (currentTime - _lastUpdateTime).TotalSeconds;
|
||||
|
||||
if (dt <= 0 || _isFirstUpdate)
|
||||
{
|
||||
_lastUpdateTime = currentTime;
|
||||
_isFirstUpdate = false;
|
||||
return;
|
||||
}
|
||||
|
||||
// Get odometry data (for velocities)
|
||||
var odom = _odometryService.CurrentOdometry;
|
||||
var vx = odom.Twist.Twist.Linear.X;
|
||||
var vy = odom.Twist.Twist.Linear.Y;
|
||||
var omega = odom.Twist.Twist.Angular.Z;
|
||||
|
||||
// PREDICTION STEP: Propagate state using odometry velocities
|
||||
_ekf.Predict(vx, vy, omega, dt);
|
||||
|
||||
// Get IMU orientation (raw from sensor)
|
||||
// double rawYaw = 0.0;
|
||||
// var quaternion = _imu.CachedQuaternion;
|
||||
// if (quaternion.HasValue)
|
||||
// {
|
||||
// rawYaw = QuaternionToYaw(quaternion.Value.Quaternion);
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// // Fallback to Euler angles
|
||||
// var orientation = _imu.CachedOrientation;
|
||||
// rawYaw = orientation.Vector.Z; // Yaw is Z component
|
||||
// }
|
||||
|
||||
// Calibrate yaw offset on first reading (set current yaw as zero reference)
|
||||
// if (!_yawOffsetCalibrated)
|
||||
// {
|
||||
// _yawOffset = rawYaw;
|
||||
// _yawOffsetCalibrated = true;
|
||||
// _logger.LogInformation("IMU Yaw offset calibrated at EKF level: {Offset:F3} rad ({OffsetDeg:F1}°)",
|
||||
// _yawOffset, _yawOffset * 180.0 / Math.PI);
|
||||
// }
|
||||
|
||||
// ===== EKF TÍCH PHÂN ANGULAR VELOCITY ĐỂ TÍNH ORIENTATION =====
|
||||
// Get angular velocity from IMU (raw data, không dùng CachedOrientation)
|
||||
var angularVelocity = _imu.CachedAngularVelocity.Vector;
|
||||
double wx = angularVelocity.X;
|
||||
double wy = angularVelocity.Y;
|
||||
double wz = angularVelocity.Z;
|
||||
|
||||
// Transform from IMU frame to Odom frame (180° rotation around X-axis)
|
||||
// Rotation matrix: [1, 0, 0; 0, -1, 0; 0, 0, -1]
|
||||
// This accounts for IMU mounting orientation (upside down relative to base_link)
|
||||
wx = wx; // X unchanged
|
||||
wy = -wy; // Y negated
|
||||
wz = -wz; // Z negated
|
||||
|
||||
// Apply dead zone to filter gyro noise/drift when stationary
|
||||
// if (Math.Abs(wx) < GYRO_DEADZONE_THRESHOLD) wx = 0.0;
|
||||
// if (Math.Abs(wy) < GYRO_DEADZONE_THRESHOLD) wy = 0.0;
|
||||
// if (Math.Abs(wz) < GYRO_DEADZONE_THRESHOLD) wz = 0.0;
|
||||
|
||||
// Integrate quaternion using angular velocity
|
||||
// Formula: q_new = q_old + 0.5 * dt * [0, wx, wy, wz] * q_old
|
||||
var q = _ekfOrientation;
|
||||
|
||||
// Half of angular velocity components
|
||||
double halfWx = 0.5 * wx;
|
||||
double halfWy = 0.5 * wy;
|
||||
double halfWz = 0.5 * wz;
|
||||
|
||||
// Quaternion derivative: dq/dt = 0.5 * [0, wx, wy, wz] * q
|
||||
double dqx = halfWx * q.W + halfWy * q.Z - halfWz * q.Y;
|
||||
double dqy = halfWy * q.W - halfWx * q.Z + halfWz * q.X;
|
||||
double dqz = halfWz * q.W + halfWx * q.Y - halfWy * q.X;
|
||||
double dqw = -halfWx * q.X - halfWy * q.Y - halfWz * q.Z;
|
||||
|
||||
// Integrate: q_new = q_old + dq * dt
|
||||
double newQx = q.X + dqx * dt;
|
||||
double newQy = q.Y + dqy * dt;
|
||||
double newQz = q.Z + dqz * dt;
|
||||
double newQw = q.W + dqw * dt;
|
||||
|
||||
// Normalize quaternion to maintain unit length
|
||||
double norm = Math.Sqrt(newQx * newQx + newQy * newQy + newQz * newQz + newQw * newQw);
|
||||
if (norm > 1e-6) // Avoid division by zero
|
||||
{
|
||||
newQx /= norm;
|
||||
newQy /= norm;
|
||||
newQz /= norm;
|
||||
newQw /= norm;
|
||||
}
|
||||
|
||||
// Update EKF orientation
|
||||
_ekfOrientation = new Quaternion(newQx, newQy, newQz, newQw);
|
||||
_lastOrientationUpdate = currentTime;
|
||||
|
||||
// Convert quaternion to yaw for EKF update
|
||||
double measuredTheta = QuaternionToYaw(_ekfOrientation);
|
||||
measuredTheta = NormalizeAngle(measuredTheta);
|
||||
|
||||
// UPDATE STEP: Correct state using integrated orientation from angular velocity
|
||||
_ekf.UpdateOrientation(measuredTheta);
|
||||
// Console.WriteLine($"EKF Update: X={_ekf.X}, Y={_ekf.Y}, Theta={_ekf.Theta}, LinearVel={_ekf.Vx}, AngularVel={_ekf.Omega}");
|
||||
|
||||
// Build filtered odometry message
|
||||
var filteredPose = new Pose
|
||||
{
|
||||
Position = new Point
|
||||
{
|
||||
X = _ekf.X,
|
||||
Y = _ekf.Y,
|
||||
Z = 0.0
|
||||
},
|
||||
Orientation = new Quaternion
|
||||
{
|
||||
X = 0.0,
|
||||
Y = 0.0,
|
||||
Z = Math.Sin(_ekf.Theta / 2.0),
|
||||
W = Math.Cos(_ekf.Theta / 2.0)
|
||||
}
|
||||
};
|
||||
|
||||
var filteredTwist = new Twist
|
||||
{
|
||||
Linear = new Vector3
|
||||
{
|
||||
X = _ekf.Vx,
|
||||
Y = _ekf.Vy,
|
||||
Z = 0.0
|
||||
},
|
||||
Angular = new Vector3
|
||||
{
|
||||
X = 0.0,
|
||||
Y = 0.0,
|
||||
Z = _ekf.Omega
|
||||
}
|
||||
};
|
||||
|
||||
// Extract covariance from EKF state covariance
|
||||
var stateCov = _ekf.Covariance;
|
||||
var poseCovariance = new double[36]; // 6x6
|
||||
var twistCovariance = new double[36]; // 6x6
|
||||
|
||||
// Map state covariance to pose covariance
|
||||
// Pose: [x, y, z, roll, pitch, yaw]
|
||||
|
||||
poseCovariance[0] = stateCov[0, 0]; // x
|
||||
poseCovariance[7] = stateCov[1, 1]; // y
|
||||
poseCovariance[14] = 0.0; // z (not estimated)
|
||||
poseCovariance[21] = 0.0; // roll (not estimated)
|
||||
poseCovariance[28] = 0.0; // pitch (not estimated)
|
||||
poseCovariance[35] = stateCov[2, 2]; // yaw (theta)
|
||||
|
||||
// Map state covariance to twist covariance
|
||||
// Twist: [vx, vy, vz, wx, wy, ywz]
|
||||
|
||||
twistCovariance[0] = stateCov[3, 3]; // vx
|
||||
twistCovariance[7] = stateCov[4, 4]; // vy
|
||||
twistCovariance[14] = 0.0; // vz (not estimated)
|
||||
twistCovariance[21] = 0.0; // wx (not estimated)
|
||||
twistCovariance[28] = 0.0; // wy (not estimated)
|
||||
twistCovariance[35] = stateCov[5, 5]; // wz (omega)
|
||||
|
||||
_filteredOdometry = new Odometry
|
||||
{
|
||||
Header = new Header
|
||||
{
|
||||
Seq = _sequenceNumber++,
|
||||
Stamp = currentTime,
|
||||
FrameId = _config.FrameId
|
||||
},
|
||||
ChildFrameId = _config.ChildFrameId,
|
||||
Pose = new PoseWithCovariance
|
||||
{
|
||||
Pose = filteredPose,
|
||||
Covariance = poseCovariance
|
||||
},
|
||||
Twist = new TwistWithCovariance
|
||||
{
|
||||
Twist = filteredTwist,
|
||||
Covariance = twistCovariance
|
||||
}
|
||||
};
|
||||
|
||||
_lastUpdateTime = currentTime;
|
||||
|
||||
// Log periodically (every 2 seconds)
|
||||
if (_sequenceNumber % 40 == 0) // At 20Hz, 40 updates = 2 seconds
|
||||
{
|
||||
_logger.LogDebug("EKF State: X={X:F3}, Y={Y:F3}, Theta={Theta:F3}, Vx={Vx:F3}, Omega={Omega:F3}",
|
||||
_ekf.X, _ekf.Y, _ekf.Theta, _ekf.Vx, _ekf.Omega);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static double QuaternionToYaw(Quaternion q)
|
||||
{
|
||||
// Yaw = atan2(2*(w*z + x*y), 1 - 2*(y^2 + z^2))
|
||||
var sinYaw = 2.0 * (q.W * q.Z + q.X * q.Y);
|
||||
var cosYaw = 1.0 - 2.0 * (q.Y * q.Y + q.Z * q.Z);
|
||||
return Math.Atan2(sinYaw, cosYaw);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Normalize angle to [-π, π] range
|
||||
/// </summary>
|
||||
private static double NormalizeAngle(double angle)
|
||||
{
|
||||
while (angle > Math.PI) angle -= 2.0 * Math.PI;
|
||||
while (angle < -Math.PI) angle += 2.0 * Math.PI;
|
||||
return angle;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
// /// Manually recalibrate yaw offset - reset current yaw as new zero reference
|
||||
// /// </summary>
|
||||
// public void RecalibrateYawOffset()
|
||||
// {
|
||||
// lock (_lock)
|
||||
// {
|
||||
// _yawOffsetCalibrated = false;
|
||||
// _logger.LogInformation("Yaw offset recalibration requested. Will recalibrate on next update.");
|
||||
// }
|
||||
// }
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
if (_disposed)
|
||||
return;
|
||||
|
||||
_disposed = true;
|
||||
|
||||
try
|
||||
{
|
||||
_updateTimer?.Dispose();
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
_logger.LogError(ex, "Error disposing EKFService");
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user