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 VehicleState Update(
            Pose2D poseInWorld,
            double sampleTimestampSeconds)
        {
            NumericGuard.EnsureFinite(
                poseInWorld,
                nameof(poseInWorld));
            NumericGuard.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 Reset(
            Pose2D poseInWorld,
            double sampleTimestampSeconds)
        {
            NumericGuard.EnsureFinite(
                poseInWorld,
                nameof(poseInWorld));
            NumericGuard.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();
        }

    }
}