备份

MultiWheelC/StateEstimation/DetourVehicleStateProvider.cs

@@ -80,25 +80,25 @@ namespace MultiWheelC.StateEstimation
                 throw new ArgumentNullException(
                     nameof(velocityEstimator));
 
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 maximumLinearSpeedMetersPerSecond,
                 nameof(maximumLinearSpeedMetersPerSecond));
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 maximumAngularSpeedRadiansPerSecond,
                 nameof(maximumAngularSpeedRadiansPerSecond));
-            EnsureFiniteNonNegative(
+            NumericGuard.EnsureFiniteNonNegative(
                 positionJumpMarginMeters,
                 nameof(positionJumpMarginMeters));
-            EnsureFiniteNonNegative(
+            NumericGuard.EnsureFiniteNonNegative(
                 headingJumpMarginRadians,
                 nameof(headingJumpMarginRadians));
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 velocityPositionResidualMeters,
                 nameof(velocityPositionResidualMeters));
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 velocityHeadingResidualRadians,
                 nameof(velocityHeadingResidualRadians));
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 stationaryConfirmationSeconds,
                 nameof(stationaryConfirmationSeconds));
 
@@ -238,9 +238,9 @@ namespace MultiWheelC.StateEstimation
             var location =
                 DetourInterface.getCartLocation();
 
-            EnsureFinite(location.x, "DetourX");
-            EnsureFinite(location.y, "DetourY");
-            EnsureFinite(location.th, "DetourTheta");
+            NumericGuard.EnsureFinite(location.x, "DetourX");
+            NumericGuard.EnsureFinite(location.y, "DetourY");
+            NumericGuard.EnsureFinite(location.th, "DetourTheta");
 
             return new Pose2D(
                 location.x / MillimetersPerMeter,
@@ -268,25 +268,6 @@ namespace MultiWheelC.StateEstimation
             return _latestState;
         }
 
-        /// <summary>
-        /// 接受跳变后的新位姿基准，但不让该位移进入速度差分和低通滤波器。
-        /// </summary>
-        private VehicleState AcceptPoseAfterVelocityRebase(
-            Pose2D poseInWorld,
-            double timestampSeconds)
-        {
-            _latestState =
-                _velocityEstimator
-                    .RebasePreservingVelocity(
-                        poseInWorld,
-                        timestampSeconds);
-            _acceptedPoseInWorld = poseInWorld;
-            _acceptedTimestampSeconds =
-                timestampSeconds;
-            _stationaryHoldActive = false;
-            return _latestState;
-        }
-
         /// <summary>
         /// 接受首帧或静止后的首个新位姿并重新建立零速差分基准。
         /// </summary>
@@ -340,7 +321,7 @@ namespace MultiWheelC.StateEstimation
             Pose2D endPoseInWorld,
             double deltaTimeSeconds)
         {
-            if (!IsFinite(deltaTimeSeconds) ||
+            if (!NumericGuard.IsFinite(deltaTimeSeconds) ||
                 deltaTimeSeconds <= 0.0)
             {
                 return false;
@@ -448,62 +429,5 @@ namespace MultiWheelC.StateEstimation
                    HeadingEqualityToleranceRadians;
         }
 
-        /// <summary>
-        /// 检查数值是否为正有限值。
-        /// </summary>
-        private static void EnsureFinitePositive(
-            double value,
-            string parameterName)
-        {
-            EnsureFinite(value, parameterName);
-
-            if (value <= 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "状态源参数必须是正有限值。");
-            }
-        }
-
-        /// <summary>
-        /// 检查数值是否为非负有限值。
-        /// </summary>
-        private static void EnsureFiniteNonNegative(
-            double value,
-            string parameterName)
-        {
-            EnsureFinite(value, parameterName);
-
-            if (value < 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "状态源参数必须是非负有限值。");
-            }
-        }
-
-        /// <summary>
-        /// 检查数值是否为有限值。
-        /// </summary>
-        private static void EnsureFinite(
-            double value,
-            string parameterName)
-        {
-            if (!IsFinite(value))
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "状态源参数和Detour位姿必须是有限值。");
-            }
-        }
-
-        /// <summary>
-        /// 判断数值是否可用于状态估计。
-        /// </summary>
-        private static bool IsFinite(double value)
-        {
-            return !double.IsNaN(value) &&
-                   !double.IsInfinity(value);
-        }
     }
 }

MultiWheelC/StateEstimation/FirstOrderLowPassFilter.cs

@@ -1,4 +1,4 @@
-using System;
+using MyParking.Shared;
 
 namespace MultiWheelC.StateEstimation
 {
@@ -17,7 +17,7 @@ namespace MultiWheelC.StateEstimation
         public FirstOrderLowPassFilter(
             double timeConstantSeconds)
         {
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 timeConstantSeconds,
                 nameof(timeConstantSeconds));
 
@@ -25,35 +25,6 @@ namespace MultiWheelC.StateEstimation
                 timeConstantSeconds;
         }
 
-        /// <summary>
-        /// 获取滤波时间常数，单位为s；数值越大，滤波越强但响应越慢。
-        /// </summary>
-        public double TimeConstantSeconds =>
-            _timeConstantSeconds;
-
-        /// <summary>
-        /// 获取滤波器是否已经接收过有效初值。
-        /// </summary>
-        public bool IsInitialized =>
-            _isInitialized;
-
-        /// <summary>
-        /// 获取当前滤波输出；尚未初始化时读取会抛出异常。
-        /// </summary>
-        public double Value
-        {
-            get
-            {
-                if (!_isInitialized)
-                {
-                    throw new InvalidOperationException(
-                        "一阶低通滤波器尚未初始化。");
-                }
-
-                return _value;
-            }
-        }
-
         /// <summary>
         /// 使用当前输入和真实采样间隔更新滤波结果。
         /// </summary>
@@ -61,10 +32,10 @@ namespace MultiWheelC.StateEstimation
             double input,
             double deltaTimeSeconds)
         {
-            EnsureFinite(
+            NumericGuard.EnsureFinite(
                 input,
                 nameof(input));
-            EnsureFinitePositive(
+            NumericGuard.EnsureFinitePositive(
                 deltaTimeSeconds,
                 nameof(deltaTimeSeconds));
 
@@ -98,7 +69,7 @@ namespace MultiWheelC.StateEstimation
         /// </summary>
         public void Reset(double initialValue)
         {
-            EnsureFinite(
+            NumericGuard.EnsureFinite(
                 initialValue,
                 nameof(initialValue));
 
@@ -106,37 +77,5 @@ namespace MultiWheelC.StateEstimation
             _isInitialized = true;
         }
 
-        /// <summary>
-        /// 检查数值是否为正有限值。
-        /// </summary>
-        private static void EnsureFinitePositive(
-            double value,
-            string parameterName)
-        {
-            EnsureFinite(value, parameterName);
-
-            if (value <= 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "滤波时间常数和采样间隔必须是正有限值。");
-            }
-        }
-
-        /// <summary>
-        /// 检查数值是否为有限值。
-        /// </summary>
-        private static void EnsureFinite(
-            double value,
-            string parameterName)
-        {
-            if (double.IsNaN(value) ||
-                double.IsInfinity(value))
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "滤波输入必须是有限值。");
-            }
-        }
     }
 }

MultiWheelC/StateEstimation/VehicleState.cs

@@ -1,4 +1,3 @@
-using System;
 using MyParking.Shared;
 
 namespace MultiWheelC.StateEstimation
@@ -17,13 +16,13 @@ namespace MultiWheelC.StateEstimation
             Twist2D twistInWorld,
             bool hasValidVelocityEstimate)
         {
-            EnsureFiniteNonNegative(
+            NumericGuard.EnsureFiniteNonNegative(
                 sampleTimestampSeconds,
                 nameof(sampleTimestampSeconds));
-            EnsureFinitePose(
+            NumericGuard.EnsureFinite(
                 poseInWorld,
                 nameof(poseInWorld));
-            EnsureFiniteTwist(
+            NumericGuard.EnsureFinite(
                 twistInWorld,
                 nameof(twistInWorld));
 
@@ -73,66 +72,9 @@ namespace MultiWheelC.StateEstimation
         public Twist2D TwistInBody { get; }
 
         /// <summary>
-        /// 获取当前速度是否已由至少两个连续有效定位样本估算得到。
+        /// 获取当前速度估计是否已经初始化并可用于闭环控制。
         /// </summary>
         public bool HasValidVelocityEstimate { get; }
 
-        /// <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 EnsureFiniteTwist(
-            Twist2D twist,
-            string parameterName)
-        {
-            if (!IsFinite(twist.VxMetersPerSecond) ||
-                !IsFinite(twist.VyMetersPerSecond) ||
-                !IsFinite(twist.OmegaRadiansPerSecond))
-            {
-                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);
-        }
     }
 }

MultiWheelC/StateEstimation/VelocityEstimator2D.cs

@@ -52,12 +52,6 @@ namespace MultiWheelC.StateEstimation
                     angularFilterTimeConstantSeconds);
         }
 
-        /// <summary>
-        /// 获取是否已经保存了可用于下一次差分的位姿基准。
-        /// </summary>
-        public bool HasPreviousSample =>
-            _hasPreviousSample;
-
         /// <summary>
         /// 使用一个新的有效定位样本更新并返回车辆状态。
         /// </summary>
@@ -65,10 +59,10 @@ namespace MultiWheelC.StateEstimation
             Pose2D poseInWorld,
             double sampleTimestampSeconds)
         {
-            EnsureFinitePose(
+            NumericGuard.EnsureFinite(
                 poseInWorld,
                 nameof(poseInWorld));
-            EnsureFiniteNonNegative(
+            NumericGuard.EnsureFiniteNonNegative(
                 sampleTimestampSeconds,
                 nameof(sampleTimestampSeconds));
 
@@ -135,53 +129,6 @@ namespace MultiWheelC.StateEstimation
                 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>
@@ -189,10 +136,10 @@ namespace MultiWheelC.StateEstimation
             Pose2D poseInWorld,
             double sampleTimestampSeconds)
         {
-            EnsureFinitePose(
+            NumericGuard.EnsureFinite(
                 poseInWorld,
                 nameof(poseInWorld));
-            EnsureFiniteNonNegative(
+            NumericGuard.EnsureFiniteNonNegative(
                 sampleTimestampSeconds,
                 nameof(sampleTimestampSeconds));
 
@@ -231,45 +178,5 @@ namespace MultiWheelC.StateEstimation
             _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);
-        }
     }
 }

MultiWheelC/StateEstimation/WheelFeedbackVehicleStateProvider.cs

@@ -90,7 +90,7 @@ namespace MultiWheelC.StateEstimation
                     var rawLongitudinalSpeedMetersPerSecond =
                         (double)actualCarSpeed.Vx;
 
-                    EnsureFinite(
+                    NumericGuard.EnsureFinite(
                         rawLongitudinalSpeedMetersPerSecond,
                         "电机反馈车体纵向速度");
 
@@ -205,7 +205,7 @@ namespace MultiWheelC.StateEstimation
             double timestampSeconds,
             out bool hasValidWheelSpeedEstimate)
         {
-            EnsureFiniteNonNegative(
+            NumericGuard.EnsureFiniteNonNegative(
                 timestampSeconds,
                 nameof(timestampSeconds));
 
@@ -238,37 +238,5 @@ namespace MultiWheelC.StateEstimation
                 deltaTimeSeconds);
         }
 
-        /// <summary>
-        /// 检查采样时刻是否为非负有限值。
-        /// </summary>
-        private static void EnsureFiniteNonNegative(
-            double value,
-            string parameterName)
-        {
-            EnsureFinite(value, parameterName);
-
-            if (value < 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "采样时刻必须是非负有限值。");
-            }
-        }
-
-        /// <summary>
-        /// 检查状态输入是否为有限值。
-        /// </summary>
-        private static void EnsureFinite(
-            double value,
-            string parameterName)
-        {
-            if (double.IsNaN(value) ||
-                double.IsInfinity(value))
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "车辆状态输入必须是有限值。");
-            }
-        }
     }
 }