ParkingRobot/MultiWheelC/StateEstimation/VelocityEstimator2D.cs

using System;
using MyParking.Shared;

namespace MultiWheelC.StateEstimation
{
    /// <summary>
    /// 根据连续有效的Detour世界位姿和真实时间差估算车辆二维速度。
    /// </summary>
    public sealed class VelocityEstimator2D
    {
        public const double DefaultLinearFilterTimeConstantSeconds =
            0.15;
        public const double DefaultAngularFilterTimeConstantSeconds =
            0.20;

        private readonly FirstOrderLowPassFilter
            _worldVelocityXFilter;
        private readonly FirstOrderLowPassFilter
            _worldVelocityYFilter;
        private readonly FirstOrderLowPassFilter
            _angularVelocityFilter;

        private bool _hasPreviousSample;
        private Pose2D _previousPoseInWorld;
        private double _previousTimestampSeconds;

        /// <summary>
        /// 创建使用默认0.15s线速度和0.20s角速度时间常数的估计器。
        /// </summary>
        public VelocityEstimator2D()
            : this(
                DefaultLinearFilterTimeConstantSeconds,
                DefaultAngularFilterTimeConstantSeconds)
        {
        }

        /// <summary>
        /// 创建使用指定线速度和角速度滤波时间常数的估计器。
        /// </summary>
        public VelocityEstimator2D(
            double linearFilterTimeConstantSeconds,
            double angularFilterTimeConstantSeconds)
        {
            _worldVelocityXFilter =
                new FirstOrderLowPassFilter(
                    linearFilterTimeConstantSeconds);
            _worldVelocityYFilter =
                new FirstOrderLowPassFilter(
                    linearFilterTimeConstantSeconds);
            _angularVelocityFilter =
                new FirstOrderLowPassFilter(
                    angularFilterTimeConstantSeconds);
        }

        /// <summary>
        /// 获取是否已经保存了可用于下一次差分的位姿基准。
        /// </summary>
        public bool HasPreviousSample =>
            _hasPreviousSample;

        /// <summary>
        /// 使用一个新的有效定位样本更新并返回车辆状态。
        /// </summary>
        public VehicleState Update(
            Pose2D poseInWorld,
            double sampleTimestampSeconds)
        {
            EnsureFinitePose(
                poseInWorld,
                nameof(poseInWorld));
            EnsureFiniteNonNegative(
                sampleTimestampSeconds,
                nameof(sampleTimestampSeconds));

            var normalizedPoseInWorld =
                new Pose2D(
                    poseInWorld.XMeters,
                    poseInWorld.YMeters,
                    AngleMath.NormalizeRadians(
                        poseInWorld.YawRadians));

            if (!_hasPreviousSample)
            {
                return Reset(
                    normalizedPoseInWorld,
                    sampleTimestampSeconds);
            }

            var deltaTimeSeconds =
                sampleTimestampSeconds -
                _previousTimestampSeconds;

            if (deltaTimeSeconds <= 0.0)
            {
                throw new ArgumentOutOfRangeException(
                    nameof(sampleTimestampSeconds),
                    "新定位样本的单调时间戳必须严格大于上一帧。");
            }

            var rawVelocityXInWorld =
                (normalizedPoseInWorld.XMeters -
                 _previousPoseInWorld.XMeters) /
                deltaTimeSeconds;
            var rawVelocityYInWorld =
                (normalizedPoseInWorld.YMeters -
                 _previousPoseInWorld.YMeters) /
                deltaTimeSeconds;
            var rawAngularVelocity =
                AngleMath.ShortestDifferenceRadians(
                    normalizedPoseInWorld.YawRadians,
                    _previousPoseInWorld.YawRadians) /
                deltaTimeSeconds;

            var filteredTwistInWorld =
                new Twist2D(
                    _worldVelocityXFilter.Update(
                        rawVelocityXInWorld,
                        deltaTimeSeconds),
                    _worldVelocityYFilter.Update(
                        rawVelocityYInWorld,
                        deltaTimeSeconds),
                    _angularVelocityFilter.Update(
                        rawAngularVelocity,
                        deltaTimeSeconds));

            _previousPoseInWorld =
                normalizedPoseInWorld;
            _previousTimestampSeconds =
                sampleTimestampSeconds;

            return new VehicleState(
                sampleTimestampSeconds,
                normalizedPoseInWorld,
                filteredTwistInWorld,
                true);
        }

        /// <summary>
        /// 更新位姿差分基准但保留当前滤波速度，避免定位跳变形成虚假速度尖峰。
        /// </summary>
        public VehicleState RebasePreservingVelocity(
            Pose2D poseInWorld,
            double sampleTimestampSeconds)
        {
            EnsureFinitePose(
                poseInWorld,
                nameof(poseInWorld));
            EnsureFiniteNonNegative(
                sampleTimestampSeconds,
                nameof(sampleTimestampSeconds));

            var normalizedPoseInWorld =
                new Pose2D(
                    poseInWorld.XMeters,
                    poseInWorld.YMeters,
                    AngleMath.NormalizeRadians(
                        poseInWorld.YawRadians));

            _previousPoseInWorld =
                normalizedPoseInWorld;
            _previousTimestampSeconds =
                sampleTimestampSeconds;
            _hasPreviousSample = true;

            var hasValidVelocityEstimate =
                _worldVelocityXFilter.IsInitialized &&
                _worldVelocityYFilter.IsInitialized &&
                _angularVelocityFilter.IsInitialized;

            var retainedTwistInWorld =
                hasValidVelocityEstimate
                    ? new Twist2D(
                        _worldVelocityXFilter.Value,
                        _worldVelocityYFilter.Value,
                        _angularVelocityFilter.Value)
                    : Twist2D.Zero;

            return new VehicleState(
                sampleTimestampSeconds,
                normalizedPoseInWorld,
                retainedTwistInWorld,
                hasValidVelocityEstimate);
        }

        /// <summary>
        /// 使用当前定位重新建立差分基准，并返回速度无效的零速状态。
        /// </summary>
        public VehicleState Reset(
            Pose2D poseInWorld,
            double sampleTimestampSeconds)
        {
            EnsureFinitePose(
                poseInWorld,
                nameof(poseInWorld));
            EnsureFiniteNonNegative(
                sampleTimestampSeconds,
                nameof(sampleTimestampSeconds));

            _previousPoseInWorld =
                new Pose2D(
                    poseInWorld.XMeters,
                    poseInWorld.YMeters,
                    AngleMath.NormalizeRadians(
                        poseInWorld.YawRadians));
            _previousTimestampSeconds =
                sampleTimestampSeconds;
            _hasPreviousSample = true;

            _worldVelocityXFilter.Reset();
            _worldVelocityYFilter.Reset();
            _angularVelocityFilter.Reset();

            return new VehicleState(
                sampleTimestampSeconds,
                _previousPoseInWorld,
                Twist2D.Zero,
                false);
        }

        /// <summary>
        /// 清除差分基准和全部滤波历史，使下一帧重新初始化估计器。
        /// </summary>
        public void Reset()
        {
            _hasPreviousSample = false;
            _previousPoseInWorld = Pose2D.Identity;
            _previousTimestampSeconds = 0.0;

            _worldVelocityXFilter.Reset();
            _worldVelocityYFilter.Reset();
            _angularVelocityFilter.Reset();
        }

        /// <summary>
        /// 检查位姿是否由有限数值组成。
        /// </summary>
        private static void EnsureFinitePose(
            Pose2D pose,
            string parameterName)
        {
            if (!IsFinite(pose.XMeters) ||
                !IsFinite(pose.YMeters) ||
                !IsFinite(pose.YawRadians))
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "速度估计使用的车辆位姿必须由有限数值组成。");
            }
        }

        /// <summary>
        /// 检查数值是否为非负有限值。
        /// </summary>
        private static void EnsureFiniteNonNegative(
            double value,
            string parameterName)
        {
            if (!IsFinite(value) || value < 0.0)
            {
                throw new ArgumentOutOfRangeException(
                    parameterName,
                    "速度估计使用的采样时刻必须是非负有限值。");
            }
        }

        /// <summary>
        /// 判断数值是否可用于速度估计。
        /// </summary>
        private static bool IsFinite(double value)
        {
            return !double.IsNaN(value) &&
                   !double.IsInfinity(value);
        }
    }
}