备份

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");
+            NumericGuard.EnsureFinite(location.x, "DetourX");
-            EnsureFinite(location.y, "DetourY");
+            NumericGuard.EnsureFinite(location.y, "DetourY");
-            EnsureFinite(location.th, "DetourTheta");
+            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,
-                    "车辆状态输入必须是有限值。");
-            }
-        }
     }
 }

Shared/Chassis/MultiWheelChassisAdapter.cs

@@ -11,7 +11,6 @@ namespace MyParking.Shared
     public sealed class MultiWheelChassisAdapter
     {
         #region 辅助内容
-        private const double RadiansToDegrees = 180.0 / Math.PI;
         private const float BiasTolerance = 0.001f;
         private readonly MultiWheelChassis _chassis;
         /// <summary>
@@ -44,15 +43,12 @@ namespace MyParking.Shared
             get => _chassis.SteeringAlignmentSigmaDegrees;
             set
             {
-                if (double.IsNaN(value) ||
+                NumericGuard.EnsureFinitePositive(
-                    double.IsInfinity(value) ||
+                    value,
-                    value <= 0.0 ||
+                    nameof(value));
-                    value > float.MaxValue)
+                EnsureRepresentableAsSingle(
-                {
+                    value,
-                    throw new ArgumentOutOfRangeException(
+                    nameof(value));
-                        nameof(value),
-                        "Steering alignment sigma must be a positive finite value.");
-                }
 
                 _chassis.SteeringAlignmentSigmaDegrees =
                     (float)value;
@@ -74,18 +70,18 @@ namespace MyParking.Shared
         private void EnsureMotionFrameIsActive(
             double motionDirectionRadians)
         {
-            ValidateFinite(
+            NumericGuard.EnsureFinite(
                 motionDirectionRadians,
                 nameof(motionDirectionRadians));
 
             var expectedBiasDegrees =
-                (float)(
+                ConvertRadiansToSingleDegrees(
-                    -FrameTransform2D.NormalizeAngle(
+                    -AngleMath.NormalizeRadians(
-                        motionDirectionRadians) *
+                        motionDirectionRadians),
-                    RadiansToDegrees);
+                    nameof(motionDirectionRadians));
             var bias = _chassis.GetOriginBias();
             var angleErrorDegrees =
-                NormalizeDegrees(
+                AngleMath.NormalizeDegrees(
                     bias.Z - expectedBiasDegrees);
 
             if (Math.Abs(bias.X) <= BiasTolerance &&
@@ -102,46 +98,32 @@ namespace MyParking.Shared
                 $"期望Th={expectedBiasDegrees}°。");
         }
 
-        /// <summary>
-        /// 将角度归一化到[-180°,180°]附近。
-        /// </summary>
-        private static float NormalizeDegrees(float degrees)
-        {
-            return (float)(
-                degrees -
-                Math.Round(degrees / 360.0) * 360.0);
-        }
         /// <summary>
         /// 检查底盘命令是否包含无效数值。
         /// </summary>
         private static void ValidateTwist(Twist2D twist)
         {
-            ValidateFinite(
+            EnsureRepresentableAsSingle(
                 twist.VxMetersPerSecond,
                 nameof(twist.VxMetersPerSecond));
 
-            ValidateFinite(
+            EnsureRepresentableAsSingle(
                 twist.VyMetersPerSecond,
                 nameof(twist.VyMetersPerSecond));
 
-            ValidateFinite(
+            EnsureRepresentableAsSingle(
-                twist.OmegaRadiansPerSecond,
+                AngleMath.RadiansToDegrees(
+                    twist.OmegaRadiansPerSecond),
                 nameof(twist.OmegaRadiansPerSecond));
         }
         /// <summary>
-        /// 检查数值是否为有限值。
+        /// 检查数值是否为有限值且可安全转换为float。
         /// </summary>
-        private static void ValidateFinite(
+        private static void EnsureRepresentableAsSingle(
             double value,
             string parameterName)
         {
-            if (double.IsNaN(value) ||
+            NumericGuard.EnsureFinite(value, parameterName);
-                double.IsInfinity(value))
-            {
-                throw new ArgumentOutOfRangeException(
-                    parameterName,
-                    "底盘速度命令不能是NaN或无穷大。");
-            }
 
             if (value > float.MaxValue ||
                 value < -float.MaxValue)
@@ -152,6 +134,21 @@ namespace MyParking.Shared
             }
         }
 
+        /// <summary>
+        /// 将有限弧度值转换为float可表示的角度值。
+        /// </summary>
+        private static float ConvertRadiansToSingleDegrees(
+            double angleRadians,
+            string parameterName)
+        {
+            var angleDegrees =
+                AngleMath.RadiansToDegrees(angleRadians);
+            EnsureRepresentableAsSingle(
+                angleDegrees,
+                parameterName);
+            return (float)angleDegrees;
+        }
+
         /// <summary>
         /// 获取最近一次底盘运动分解失败原因。
         /// </summary>
@@ -175,15 +172,15 @@ namespace MyParking.Shared
         public void ActivateMotionFrame(
             double motionDirectionRadians)
         {
-            ValidateFinite(
+            NumericGuard.EnsureFinite(
                 motionDirectionRadians,
                 nameof(motionDirectionRadians));
 
             var biasDegrees =
-                (float)(
+                ConvertRadiansToSingleDegrees(
-                    -FrameTransform2D.NormalizeAngle(
+                    -AngleMath.NormalizeRadians(
-                        motionDirectionRadians) *
+                        motionDirectionRadians),
-                    RadiansToDegrees);
+                    nameof(motionDirectionRadians));
             var currentBias =
                 _chassis.GetOriginBias();
 
@@ -192,7 +189,7 @@ namespace MyParking.Shared
                 Math.Abs(currentBias.Y) <=
                     BiasTolerance &&
                 Math.Abs(
-                    NormalizeDegrees(
+                    AngleMath.NormalizeDegrees(
                         currentBias.Z -
                         biasDegrees)) <=
                     BiasTolerance)
@@ -284,9 +281,9 @@ namespace MyParking.Shared
             var vyMetersPerSecond =
                 (float)command.BodyTwist.VyMetersPerSecond;
             var omegaDegreesPerSecond =
-                (float)(
+                ConvertRadiansToSingleDegrees(
-                    command.BodyTwist.OmegaRadiansPerSecond *
+                    command.BodyTwist.OmegaRadiansPerSecond,
-                    RadiansToDegrees);
+                    nameof(command.BodyTwist.OmegaRadiansPerSecond));
             var success = _chassis.SendXYThSpeed(
                                 vxMetersPerSecond,
                                 vyMetersPerSecond,
@@ -312,13 +309,13 @@ namespace MyParking.Shared
             double steeringRadians,
             TimeSpan? interval = null)
         {
-            ValidateFinite(
+            NumericGuard.EnsureFinite(
                 motionDirectionRadians,
                 nameof(motionDirectionRadians));
-            ValidateFinite(
+            EnsureRepresentableAsSingle(
                 speedMetersPerSecond,
                 nameof(speedMetersPerSecond));
-            ValidateFinite(
+            NumericGuard.EnsureFinite(
                 steeringRadians,
                 nameof(steeringRadians));
             EnsureMotionFrameIsActive(
@@ -333,9 +330,9 @@ namespace MyParking.Shared
             }
 
             var steeringDegrees =
-                (float)(
+                ConvertRadiansToSingleDegrees(
-                    steeringRadians *
+                    steeringRadians,
-                    RadiansToDegrees);
+                    nameof(steeringRadians));
             var success =
                 _chassis.SendMotion(
                     (float)speedMetersPerSecond,
@@ -360,13 +357,13 @@ namespace MyParking.Shared
             double rearAngleRadians,
             TimeSpan? interval = null)
         {
-            ValidateFinite(
+            EnsureRepresentableAsSingle(
                 speedMetersPerSecond,
                 nameof(speedMetersPerSecond));
-            ValidateFinite(
+            NumericGuard.EnsureFinite(
                 frontAngleRadians,
                 nameof(frontAngleRadians));
-            ValidateFinite(
+            NumericGuard.EnsureFinite(
                 rearAngleRadians,
                 nameof(rearAngleRadians));
             EnsureBodyFrameIsActive();
@@ -383,10 +380,12 @@ namespace MyParking.Shared
 
             var success = _chassis.SendMotion(
                 (float)speedMetersPerSecond,
-                (float)(frontAngleRadians *
+                ConvertRadiansToSingleDegrees(
-                        RadiansToDegrees),
+                    frontAngleRadians,
-                (float)(rearAngleRadians *
+                    nameof(frontAngleRadians)),
-                        RadiansToDegrees),
+                ConvertRadiansToSingleDegrees(
+                    rearAngleRadians,
+                    nameof(rearAngleRadians)),
                 interval);
 
             if (!success)
@@ -422,8 +421,11 @@ namespace MyParking.Shared
             double directionRadians)
         {
             EnsureBodyFrameIsActive();
-            var targetDegrees = (float)(FrameTransform2D.NormalizeAngle(directionRadians) *
+            var targetDegrees =
-                                    RadiansToDegrees);
+                ConvertRadiansToSingleDegrees(
+                    AngleMath.NormalizeRadians(
+                        directionRadians),
+                    nameof(directionRadians));
 
 #pragma warning disable CS0612, CS0618
             var wheels = _chassis.GetSteerWheels();
@@ -463,23 +465,21 @@ namespace MyParking.Shared
             double directionRadians,
             double toleranceRadians)
         {
-            if (double.IsNaN(toleranceRadians) ||
+            NumericGuard.EnsureFiniteNonNegative(
-        double.IsInfinity(toleranceRadians) ||
+                toleranceRadians,
-        toleranceRadians < 0.0)
+                nameof(toleranceRadians));
-            {
-                throw new ArgumentOutOfRangeException(
-                    nameof(toleranceRadians),
-                    "舵轮到位容差必须是非负有限值。");
-            }
 
             EnsureBodyFrameIsActive();
-            var targetDegrees = (float)(
+            var targetDegrees =
-                FrameTransform2D.NormalizeAngle(directionRadians) *
+                ConvertRadiansToSingleDegrees(
-                180.0 / Math.PI);
+                    AngleMath.NormalizeRadians(
+                        directionRadians),
+                    nameof(directionRadians));
 
-            var toleranceDegrees = (float)(
+            var toleranceDegrees =
-                Math.Abs(toleranceRadians) *
+                ConvertRadiansToSingleDegrees(
-                180.0 / Math.PI);
+                    toleranceRadians,
+                    nameof(toleranceRadians));
 
 #pragma warning disable CS0612, CS0618
             var wheels = _chassis.GetSteerWheels();
@@ -507,16 +507,12 @@ namespace MyParking.Shared
             TimeSpan? interval = null,
             double alignmentToleranceDegrees = 2.0)
         {
-            ValidateFinite(
+            NumericGuard.EnsureFiniteNonNegative(
+                alignmentToleranceDegrees,
+                nameof(alignmentToleranceDegrees));
+            EnsureRepresentableAsSingle(
                 alignmentToleranceDegrees,
                 nameof(alignmentToleranceDegrees));
-
-            if (alignmentToleranceDegrees < 0.0)
-            {
-                throw new ArgumentOutOfRangeException(
-                    nameof(alignmentToleranceDegrees),
-                    "自转舵轮到位容差必须是非负有限值。");
-            }
 
             EnsureBodyFrameIsActive();
 
@@ -540,23 +536,18 @@ namespace MyParking.Shared
             double toleranceRadians =
                 2.0 * Math.PI / 180.0)
         {
-            if (double.IsNaN(toleranceRadians) ||
+            NumericGuard.EnsureFiniteNonNegative(
-                double.IsInfinity(toleranceRadians) ||
+                toleranceRadians,
-                toleranceRadians < 0.0)
+                nameof(toleranceRadians));
-            {
-                throw new ArgumentOutOfRangeException(
-                    nameof(toleranceRadians),
-                    "自转状态交接容差必须是非负有限值。");
-            }
 
             EnsureBodyFrameIsActive();
 
             var success =
                 _chassis
                     .AdoptPreparedRotateWheelsForXYTh(
-                        (float)(
+                        ConvertRadiansToSingleDegrees(
-                            toleranceRadians *
+                            toleranceRadians,
-                            RadiansToDegrees));
+                            nameof(toleranceRadians)));
 
             if (!success)
             {

Shared/Mathematics/FrameTransform2D.cs

@@ -9,27 +9,6 @@ namespace MyParking.Shared
     /// </summary>
     public static class FrameTransform2D
     {
-        /// <summary>
-        /// 将角度归一化到[-π, π)范围。
-        /// </summary>
-        public static double NormalizeAngle(double angleRadians)
-        {
-            return AngleMath.NormalizeRadians(angleRadians);
-        }
-
-        /// <summary>
-        /// 计算从current到target的最短角度差。
-        /// 返回正值表示逆时针旋转。
-        /// </summary>
-        public static double ShortestAngleDifference(
-            double targetRadians,
-            double currentRadians)
-        {
-            return AngleMath.ShortestDifferenceRadians(
-                targetRadians,
-                currentRadians);
-        }
-
         /// <summary>
         /// 将源坐标系中的点变换到目标坐标系。
         /// sourcePoseInTarget表示源坐标系在目标坐标系中的位姿。
@@ -106,7 +85,7 @@ namespace MyParking.Shared
             return new Pose2D(
                 childPositionInParent.XMeters,
                 childPositionInParent.YMeters,
-                NormalizeAngle(
+                AngleMath.NormalizeRadians(
                     parentFromMiddle.YawRadians +
                     middleFromChild.YawRadians));
         }
@@ -127,7 +106,7 @@ namespace MyParking.Shared
                 sin * childPoseInParent.XMeters -
                 cos * childPoseInParent.YMeters,
 
-                NormalizeAngle(
+                AngleMath.NormalizeRadians(
                     -childPoseInParent.YawRadians));
         }
 

Shared/Models/ChassisCommand.cs

@@ -81,36 +81,6 @@ namespace MyParking.Shared
             new Twist2D(0.0, 0.0, 0.0);
     }
 
-    /// <summary>
-    /// 发送给单辆车的车体坐标系速度命令。
-    /// </summary>
-    public readonly struct ChassisCommand
-    {
-        public ChassisCommand(
-            int vehicleId,
-            Twist2D bodyTwist)
-        {
-            VehicleId = vehicleId;
-            BodyTwist = bodyTwist;
-        }
-
-        public int VehicleId { get; }
-
-        /// <summary>
-        /// 单车车体坐标系速度：X向前、Y向左、逆时针旋转为正。
-        /// </summary>
-        public Twist2D BodyTwist { get; }
-
-        /// <summary>
-        /// 创建指定车辆的停止命令。
-        /// </summary>
-        public static ChassisCommand Stop(int vehicleId)
-        {
-            return new ChassisCommand(
-                vehicleId,
-                Twist2D.Zero);
-        }
-    }
 
     /// <summary>
     /// 单辆车的车体坐标系在车队坐标系中的位姿。

Shared/Validation/NumericGuard.cs

@@ -7,6 +7,15 @@ namespace MyParking.Shared
     /// </summary>
     public static class NumericGuard
     {
+        /// <summary>
+        /// 判断指定浮点数是否既不是NaN也不是无穷大。
+        /// </summary>
+        public static bool IsFinite(double value)
+        {
+            return !double.IsNaN(value) &&
+                   !double.IsInfinity(value);
+        }
+
         /// <summary>
         /// 确保指定浮点数不是NaN或无穷大。
         /// </summary>
@@ -14,8 +23,7 @@ namespace MyParking.Shared
             double value,
             string parameterName)
         {
-            if (double.IsNaN(value) ||
+            if (!IsFinite(value))
-                double.IsInfinity(value))
             {
                 throw new ArgumentOutOfRangeException(
                     parameterName,
@@ -64,17 +72,31 @@ namespace MyParking.Shared
             Pose2D pose,
             string parameterName)
         {
-            if (double.IsNaN(pose.XMeters) ||
+            if (!IsFinite(pose.XMeters) ||
-                double.IsInfinity(pose.XMeters) ||
+                !IsFinite(pose.YMeters) ||
-                double.IsNaN(pose.YMeters) ||
+                !IsFinite(pose.YawRadians))
-                double.IsInfinity(pose.YMeters) ||
-                double.IsNaN(pose.YawRadians) ||
-                double.IsInfinity(pose.YawRadians))
             {
                 throw new ArgumentOutOfRangeException(
                     parameterName,
                     "二维位姿必须由有限值组成。");
             }
         }
+
+        /// <summary>
+        /// 确保二维刚体速度的两个线速度分量和角速度均为有限值。
+        /// </summary>
+        public static void EnsureFinite(
+            Twist2D twist,
+            string parameterName)
+        {
+            if (!IsFinite(twist.VxMetersPerSecond) ||
+                !IsFinite(twist.VyMetersPerSecond) ||
+                !IsFinite(twist.OmegaRadiansPerSecond))
+            {
+                throw new ArgumentOutOfRangeException(
+                    parameterName,
+                    "二维刚体速度必须由有限值组成。");
+            }
+        }
     }
 }

docs/数据融合.txt

@@ -1,94 +0,0 @@
-是的，强烈建议做系统辨识，尤其是你这种要把电机反馈和 SLAM 融合的场景。
-为什么需要系统辨识？
-卡尔曼滤波（或 EKF）的效果很大程度上取决于过程模型有多准。模型不准的话，会出现：
-
-预测步持续往错误方向跑
-滤波器过度依赖测量（SLAM），或者反过来过度信任错误的模型
-速度估计系统性偏大/偏小
-原地自转时航向纠正效果变差
-
-你现在已经知道电机反馈“偏大”，这本身就是典型的模型参数问题（可能是轮胎半径、减速比、编码器标定、打滑补偿等）。
-建议辨识的主要参数
-针对四轮差速车，优先考虑这些：
-
-运动学参数
-有效轮半径（或左右轮半径）
-轮距（左右轮中心距离）
-可能的轴距相关项（如果不是纯差速）
-
-速度相关
-电机速度到实际车速的缩放系数（你提到的“偏大”问题）
-左右轮速度的不对称性（如果存在）
-低速段的非线性（静摩擦、死区）
-
-可选但有用的
-速度偏置 / 零点漂移
-简单的打滑系数（如果经常在特定地面跑）
-
-
-怎么做比较实际？
-不需要一开始就上很复杂的在线辨识，可以分两步：
-离线标定（推荐先做）
-
-让车在平坦地面以不同速度直线行驶、原地旋转，记录：
-四个电机反馈速度
-SLAM 给出的位置和航向变化
-
-用最小二乘或简单优化，拟合出轮半径、轮距、速度缩放系数等
-原地自转特别适合标定轮距和左右轮差异
-
-在线微调（可选）
-
-把关键参数（比如速度缩放系数、轮半径）也放进状态向量里一起估计（Augmented EKF）
-或者用简单的自适应方法，定期用 SLAM 和电机速度的一致性去微调缩放因子
-
-实际建议顺序
-
-先建立基本运动学模型 + EKF 框架
-做一轮离线系统辨识，把明显偏差（尤其是速度偏大）修正掉
-再跑融合，看残差（innovation）是否变小、速度估计是否更合理
-如果还有残余误差，再考虑把个别参数做成在线估计
-
-总结：建完模型后做系统辨识，是让卡尔曼滤波真正好用的关键一步。不做的话，即使融合了电机和 SLAM，效果也容易打折扣。需要的话，我可以给你一个针对差速车的简单离线标定思路（需要记录哪些数据、怎么拟合）。
-
-有没有必要做卡尔曼/传感器融合？
-分两层看：
-
-如果只是为了速度闭环，且当前绿色线已经能让车跑得比较稳
-→ 性价比一般。
-你现在的处理已经解决了最严重的问题，继续花时间做完整的 EKF + 电机融合，短期内收益可能没有那么大。可以先把绿色线再优化一下（比如调整低通截止频率、加一点自适应滤波，或者用更温和的滑动平均），看能不能把残余抖动再压低一点。
-如果你对速度精度、平滑度要求比较高，或者后面还要做更精细的控制（轨迹跟踪、力控、高精度定位等）
-→ 值得做，性价比会变高。
-原因是：
-绿色线本质还是“从位置差分出来的速度”，天然会有延迟和残余噪声。
-电机反馈是更直接的速度源，频率高、相位更好。
-把电机速度和经过处理的 SLAM 差分速度做融合（哪怕是简单的互补滤波或一阶卡尔曼），通常能同时得到：
-更低的噪声
-更小的滞后
-对电机“偏大”问题的一定校正能力
-
-
-
-实际建议（按性价比排序）
-
-先低成本优化现有绿色线（最快）
-检查低通滤波的截止频率是否合适（太低会滞后，太高会留噪声）。
-可以尝试一阶低通 + 中值滤波组合，或者简单的互补滤波（位置差分 + 电机速度）。
-看看闭环时车的实际表现，如果抖动已经可接受，就先用着。
-
-中等成本：做简单融合（推荐优先考虑）
-不用完整 EKF，先做一个互补滤波或一维卡尔曼只融速度：
-高频信任电机反馈速度
-低频用处理后的 SLAM 差分速度去校正偏差
-
-实现简单，效果通常比单纯低通好一截，滞后也更小。
-
-完整 EKF + 系统辨识
-只有在你需要同时提升位置、航向、速度的整体一致性，或者后面要上更复杂的控制时，再上这个。
-工作量明显更大，但长期收益最高。
-
-
-总结
-从你这张图来看，目前绿色线已经能用，不是“必须立刻上卡尔曼”的紧急情况。
-但如果你觉得绿色线的残余抖动还是影响闭环效果，或者想要更干净、滞后更小的速度，那么把四个电机反馈融进去（哪怕先做简单融合）是性价比不错的下一步，比继续死磕低通滤波更有前途。
-你现在速度闭环时，车实际表现如何？是觉得绿色线的抖动已经导致控制不稳，还是只是看着不舒服？这能帮助判断要不要马上上融合。