备份

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);
-        }
     }
 }