完善Detour状态估计与轨迹跟踪验证

MultiWheelC/Configuration/PilotConfig.ParkingControl.cs

@@ -27,9 +27,28 @@ public partial class PilotConfig
     [FieldMember(desc = "停车控制：Detour速度预测航向残差(deg)")]
     public float ParkingDetourVelocityHeadingResidualDegrees = 5f;
 
+    [FieldMember(desc = "停车控制：Detour航向异常确认新帧数")]
+    public int ParkingDetourHeadingOutlierConfirmationFrames = 3;
+
+    [FieldMember(desc = "停车控制：Detour航向异常短时预测超时(s)")]
+    public float ParkingDetourHeadingOutlierPredictionTimeoutSeconds =
+        0.30f;
+
     [FieldMember(desc = "停车控制：Detour静止确认时间(s)")]
     public float ParkingDetourStationaryConfirmationSeconds = 0.35f;
 
+    [FieldMember(desc = "停车控制：Detour跳变确认新帧数")]
+    public int ParkingDetourJumpConfirmationFrames = 3;
+
+    [FieldMember(desc = "停车控制：Detour跳变确认超时(s)")]
+    public float ParkingDetourJumpConfirmationTimeoutSeconds = 0.60f;
+
+    [FieldMember(desc = "停车控制：Detour自动坐标连续化最大平移(m)")]
+    public float ParkingDetourMaximumAutomaticFrameShift = 0.15f;
+
+    [FieldMember(desc = "停车控制：Detour自动坐标连续化最大航向变化(deg)")]
+    public float ParkingDetourMaximumAutomaticHeadingShiftDegrees = 5f;
+
     [FieldMember(desc = "停车控制：Detour线速度滤波时间常数(s)")]
     public float ParkingDetourLinearVelocityFilterSeconds = 0.15f;
 
@@ -115,10 +134,12 @@ public partial class PilotConfig
     public float InPlaceRotateMinimumSpeed = 1f;
 
     [FieldMember(desc = "停车控制：原地自转最大角速度(deg/s)")]
-    public float InPlaceRotateMaxSpeed = 47.5f;
+    // public float InPlaceRotateMaxSpeed = 47.5f;
+    public float InPlaceRotateMaxSpeed = 30f;
 
     [FieldMember(desc = "停车控制：原地自转角加速度(deg/s²)")]
-    public float InPlaceRotateAcc = 60f;
+    // public float InPlaceRotateAcc = 60f;
+    public float InPlaceRotateAcc = 40f;
 
     [FieldMember(desc = "停车控制：原地自转超时(s)")]
     public float InPlaceRotateTimeoutSec = 15f;

MultiWheelC/Control/Abstractions/PathTrackingContext.cs

@@ -93,7 +93,7 @@ namespace MultiWheelC.Control.Abstractions
         public double FeedforwardCurvaturePerMeter { get; }
 
         /// <summary>
-        /// 获取参考轨迹相对车辆的有符号横向误差，单位为m，轨迹在车辆左侧时为正。
+        /// 获取相对轨迹执行点序的有符号横向误差，单位为m，参考轨迹位于执行方向左侧时为正。
         /// </summary>
         public double LateralErrorMeters =>
             Projection.LateralErrorMeters;

MultiWheelC/Control/Lateral/StanleyLateralController.cs

@@ -123,8 +123,9 @@ namespace MultiWheelC.Control.Lateral
                     context.HeadingErrorRadians,
                     MaximumHeadingCorrectionRadians);
 
+            // 横向误差已经按轨迹执行点序定义；倒车轨迹的点序会自然
+            // 翻转横向轴，因此共同转角不能再按行驶方向重复反号。
             var commonAngleRadians =
-                travelDirection *
                 crossTrackCorrectionRadians;
             var differentialAngleRadians =
                 feedforwardAngleRadians +

MultiWheelC/Experiments/CompositeMotionPlanTests.cs

@@ -157,7 +157,8 @@ namespace MultiWheelC
                     (float)AccelerationMetersPerSecondSquared,
                 referenceDecelerationMetersPerSecondSquared:
                     (float)DecelerationMetersPerSecondSquared,
-                diagnosticChassis: chassis);
+                diagnosticChassis: chassis,
+                diagnosticStateProvider: stateProvider);
             _recorder.Start();
 
             var controlPointRadiusMeters =

MultiWheelC/Experiments/DetourStaticDiagnosticTest.cs

@@ -0,0 +1,696 @@
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Globalization;
+using System.IO;
+using System.Text;
+using System.Threading;
+using ClumsyCore;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using CommonUsage.Chassis;
+using FundamentalLib;
+using MyParking.Shared;
+
+namespace MultiWheelC
+{
+    /// <summary>
+    /// 从C层测试界面启动定时的Detour静态定位与轮组反馈诊断记录。
+    /// </summary>
+    [MovementTest(name = "诊断：Detour静态定位记录")]
+    public sealed class DetourStaticDiagnosticTest : MovementTest
+    {
+        private sealed class DiagnosticSample
+        {
+            public double ElapsedSeconds;
+            public string LocalTimestamp;
+            public bool DetourReadSucceeded;
+            public double DetourCallDurationMilliseconds;
+            public long? DetourTickRaw;
+            public string DetourTimestamp;
+            public bool DetourTimestampValid;
+            public double? DetourDataAgeMilliseconds;
+            public double? DetourLStep;
+            public double? DetourXMillimeters;
+            public double? DetourYMillimeters;
+            public double? DetourYawDegrees;
+            public double? LocalDeltaMilliseconds;
+            public double? DetourTickDeltaMilliseconds;
+            public double? DeltaXMillimeters;
+            public double? DeltaYMillimeters;
+            public double? DeltaYawDegrees;
+            public double? DeltaPositionMillimeters;
+            public bool IsRepeatedTick;
+            public bool IsRepeatedPose;
+            public bool IsOutOfOrderTick;
+            public bool WheelReadSucceeded;
+            public double? WheelBodyVxMetersPerSecond;
+            public double? WheelBodyVyMetersPerSecond;
+            public double? WheelBodyOmegaDegreesPerSecond;
+            public double? ActualSteerLeftFrontDegrees;
+            public double? ActualSteerLeftRearDegrees;
+            public double? ActualSteerRightFrontDegrees;
+            public double? ActualSteerRightRearDegrees;
+            public double? ActualSpeedLeftFrontMetersPerSecond;
+            public double? ActualSpeedLeftRearMetersPerSecond;
+            public double? ActualSpeedRightFrontMetersPerSecond;
+            public double? ActualSpeedRightRearMetersPerSecond;
+            public string FailureReason;
+        }
+
+        private readonly object _sampleSyncRoot = new object();
+        private readonly List<DiagnosticSample> _samples =
+            new List<DiagnosticSample>();
+        private readonly Stopwatch _clock = new Stopwatch();
+
+        private MultiWheelChassis _chassis;
+        private Thread _samplingThread;
+        private volatile bool _sampling;
+        private int _testRunning;
+        private int _stopRequested;
+        private int _sessionId;
+        private bool _hasPreviousDetourSample;
+        private double _previousElapsedSeconds;
+        private long _previousDetourTick;
+        private double _previousDetourXMillimeters;
+        private double _previousDetourYMillimeters;
+        private double _previousDetourYawDegrees;
+
+        /// <summary>
+        /// 获取或设置自动结束前的记录时长，单位为min。
+        /// </summary>
+        public double DurationMinutes = 20.0;
+
+        /// <summary>
+        /// 获取或设置本机主动读取Detour的周期，单位为ms。
+        /// </summary>
+        public int SampleIntervalMilliseconds = 50;
+
+        /// <summary>
+        /// 获取最近一次静态诊断CSV的完整路径。
+        /// </summary>
+        public string SavedFilePath { get; private set; } =
+            string.Empty;
+
+        /// <summary>
+        /// 停车后开始静态采样，并在到达设定时长时自动保存CSV。
+        /// </summary>
+        public override void Test()
+        {
+            if (Interlocked.CompareExchange(
+                    ref _testRunning,
+                    1,
+                    0) != 0)
+            {
+                Console.WriteLine("Detour静态诊断已经在运行。");
+                return;
+            }
+
+            var samplingStarted = false;
+            var completedAutomatically = false;
+            Exception testFailure = null;
+
+            try
+            {
+                ValidateSettings();
+                _chassis =
+                    PilotDefinition.Chassis as MultiWheelChassis;
+                if (_chassis == null)
+                {
+                    throw new InvalidOperationException(
+                        "当前底盘不是MultiWheelChassis，无法读取四轮反馈。");
+                }
+
+                var sessionId = ResetSession();
+                _chassis.PredefinedDriveStop();
+                _sampling = true;
+                _clock.Restart();
+                samplingStarted = true;
+
+                _samplingThread = new Thread(
+                    () => SamplingLoop(sessionId))
+                {
+                    IsBackground = true,
+                    Name = "DetourStaticDiagnostic"
+                };
+                _samplingThread.Start();
+
+                Console.WriteLine(
+                    $"Detour静态诊断开始：时长={DurationMinutes:F1}min，" +
+                    $"主动读取周期={SampleIntervalMilliseconds}ms，" +
+                    "车辆必须保持静止。");
+                Hedingben.ToastText(
+                    $"Detour静态诊断开始，预计{DurationMinutes:F1}分钟后自动结束。");
+
+                var durationSeconds = DurationMinutes * 60.0;
+                while (Volatile.Read(ref _stopRequested) == 0 &&
+                       _clock.Elapsed.TotalSeconds < durationSeconds)
+                {
+                    Thread.Sleep(100);
+                }
+
+                completedAutomatically =
+                    Volatile.Read(ref _stopRequested) == 0;
+            }
+            catch (Exception exception)
+            {
+                testFailure = exception;
+                Console.WriteLine(
+                    "Detour静态诊断失败：" +
+                    exception.Message);
+            }
+            finally
+            {
+                _sampling = false;
+                _chassis?.PredefinedDriveStop();
+
+                if (_samplingThread != null &&
+                    _samplingThread != Thread.CurrentThread)
+                {
+                    _samplingThread.Join(
+                        Math.Max(
+                            1000,
+                            SampleIntervalMilliseconds * 4));
+                }
+
+                _clock.Stop();
+
+                if (samplingStarted)
+                {
+                    try
+                    {
+                        SaveCsvAndReport(
+                            completedAutomatically,
+                            testFailure);
+                    }
+                    catch (Exception exception)
+                    {
+                        Console.WriteLine(
+                            "Detour静态诊断CSV保存失败：" +
+                            exception.Message);
+                        Hedingben.ToastText(
+                            "Detour静态诊断CSV保存失败：" +
+                            exception.Message);
+                    }
+                }
+
+                _samplingThread = null;
+                _chassis = null;
+                Interlocked.Exchange(ref _testRunning, 0);
+            }
+        }
+
+        /// <summary>
+        /// 请求提前停止采样；测试线程随后保存已经采集的数据。
+        /// </summary>
+        public override void TestStop()
+        {
+            Interlocked.Exchange(ref _stopRequested, 1);
+            _sampling = false;
+            _chassis?.PredefinedDriveStop();
+        }
+
+        /// <summary>
+        /// 清除上一次测试的样本、时间基准和输出路径。
+        /// </summary>
+        private int ResetSession()
+        {
+            lock (_sampleSyncRoot)
+            {
+                _samples.Clear();
+            }
+
+            Interlocked.Exchange(ref _stopRequested, 0);
+            _hasPreviousDetourSample = false;
+            _previousElapsedSeconds = 0.0;
+            _previousDetourTick = 0;
+            _previousDetourXMillimeters = 0.0;
+            _previousDetourYMillimeters = 0.0;
+            _previousDetourYawDegrees = 0.0;
+            SavedFilePath = string.Empty;
+            return Interlocked.Increment(ref _sessionId);
+        }
+
+        /// <summary>
+        /// 检查测试时长和主动采样周期是否适合执行。
+        /// </summary>
+        private void ValidateSettings()
+        {
+            NumericGuard.EnsureFinitePositive(
+                DurationMinutes,
+                nameof(DurationMinutes));
+
+            if (SampleIntervalMilliseconds < 20 ||
+                SampleIntervalMilliseconds > 5000)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(SampleIntervalMilliseconds),
+                    "Detour主动读取周期必须在20ms到5000ms之间。");
+            }
+        }
+
+        /// <summary>
+        /// 按设定周期持续采样，直至测试到时或收到停止请求。
+        /// </summary>
+        private void SamplingLoop(int sessionId)
+        {
+            while (_sampling &&
+                   sessionId == Volatile.Read(ref _sessionId))
+            {
+                CaptureSample(sessionId);
+                Thread.Sleep(SampleIntervalMilliseconds);
+            }
+        }
+
+        /// <summary>
+        /// 采集一帧原始Detour定位、接口耗时和四轮实际反馈。
+        /// </summary>
+        private void CaptureSample(int sessionId)
+        {
+            var sample = new DiagnosticSample
+            {
+                ElapsedSeconds = _clock.Elapsed.TotalSeconds,
+                LocalTimestamp =
+                    DateTimeOffset.Now.ToString(
+                        "O",
+                        CultureInfo.InvariantCulture),
+                FailureReason = string.Empty
+            };
+
+            CaptureDetour(sample, sessionId);
+
+            if (sessionId != Volatile.Read(ref _sessionId))
+            {
+                return;
+            }
+
+            CaptureWheelFeedback(sample);
+
+            if (!_sampling ||
+                sessionId != Volatile.Read(ref _sessionId))
+            {
+                return;
+            }
+
+            lock (_sampleSyncRoot)
+            {
+                _samples.Add(sample);
+            }
+        }
+
+        /// <summary>
+        /// 读取Detour原始字段并计算与上一成功读取之间的时间和位姿差。
+        /// </summary>
+        private void CaptureDetour(
+            DiagnosticSample sample,
+            int sessionId)
+        {
+            var callClock = Stopwatch.StartNew();
+
+            try
+            {
+                var location =
+                    DetourInterface.getCartLocation();
+                callClock.Stop();
+
+                sample.DetourCallDurationMilliseconds =
+                    callClock.Elapsed.TotalMilliseconds;
+                sample.DetourReadSucceeded = true;
+                sample.DetourTickRaw = Convert.ToInt64(
+                    location.tick,
+                    CultureInfo.InvariantCulture);
+                sample.DetourLStep = Convert.ToDouble(
+                    location.l_step,
+                    CultureInfo.InvariantCulture);
+                sample.DetourXMillimeters = Convert.ToDouble(
+                    location.x,
+                    CultureInfo.InvariantCulture);
+                sample.DetourYMillimeters = Convert.ToDouble(
+                    location.y,
+                    CultureInfo.InvariantCulture);
+                sample.DetourYawDegrees = Convert.ToDouble(
+                    location.th,
+                    CultureInfo.InvariantCulture);
+
+                if (sessionId != Volatile.Read(ref _sessionId))
+                {
+                    return;
+                }
+
+                CaptureDetourTimestamp(sample);
+                CaptureDetourDelta(sample);
+            }
+            catch (Exception exception)
+            {
+                callClock.Stop();
+                sample.DetourCallDurationMilliseconds =
+                    callClock.Elapsed.TotalMilliseconds;
+                AppendFailure(
+                    sample,
+                    "Detour读取失败：" +
+                    exception.Message);
+            }
+        }
+
+        /// <summary>
+        /// 将Detour原始tick按.NET DateTime ticks解释并记录数据年龄。
+        /// </summary>
+        private static void CaptureDetourTimestamp(
+            DiagnosticSample sample)
+        {
+            try
+            {
+                var detourTime = new DateTime(
+                    sample.DetourTickRaw.Value,
+                    DateTimeKind.Local);
+                sample.DetourTimestamp =
+                    detourTime.ToString(
+                        "O",
+                        CultureInfo.InvariantCulture);
+                sample.DetourDataAgeMilliseconds =
+                    (DateTime.Now - detourTime)
+                    .TotalMilliseconds;
+                sample.DetourTimestampValid = true;
+            }
+            catch (ArgumentOutOfRangeException)
+            {
+                sample.DetourTimestamp = string.Empty;
+            }
+        }
+
+        /// <summary>
+        /// 计算Detour帧间差并更新下一帧使用的原始基准。
+        /// </summary>
+        private void CaptureDetourDelta(
+            DiagnosticSample sample)
+        {
+            var tick = sample.DetourTickRaw.Value;
+            var xMillimeters = sample.DetourXMillimeters.Value;
+            var yMillimeters = sample.DetourYMillimeters.Value;
+            var yawDegrees = sample.DetourYawDegrees.Value;
+
+            if (_hasPreviousDetourSample)
+            {
+                sample.LocalDeltaMilliseconds =
+                    (sample.ElapsedSeconds -
+                     _previousElapsedSeconds) * 1000.0;
+                sample.DetourTickDeltaMilliseconds =
+                    (tick - _previousDetourTick) /
+                    (double)TimeSpan.TicksPerMillisecond;
+                sample.DeltaXMillimeters =
+                    xMillimeters - _previousDetourXMillimeters;
+                sample.DeltaYMillimeters =
+                    yMillimeters - _previousDetourYMillimeters;
+                sample.DeltaYawDegrees =
+                    AngleMath.ShortestDifferenceDegrees(
+                        yawDegrees,
+                        _previousDetourYawDegrees);
+                sample.DeltaPositionMillimeters = Math.Sqrt(
+                    sample.DeltaXMillimeters.Value *
+                    sample.DeltaXMillimeters.Value +
+                    sample.DeltaYMillimeters.Value *
+                    sample.DeltaYMillimeters.Value);
+                sample.IsRepeatedTick =
+                    tick == _previousDetourTick;
+                sample.IsOutOfOrderTick =
+                    tick < _previousDetourTick;
+                sample.IsRepeatedPose =
+                    sample.DeltaPositionMillimeters.Value <= 1e-6 &&
+                    Math.Abs(sample.DeltaYawDegrees.Value) <= 1e-9;
+            }
+
+            _hasPreviousDetourSample = true;
+            _previousElapsedSeconds = sample.ElapsedSeconds;
+            _previousDetourTick = tick;
+            _previousDetourXMillimeters = xMillimeters;
+            _previousDetourYMillimeters = yMillimeters;
+            _previousDetourYawDegrees = yawDegrees;
+        }
+
+        /// <summary>
+        /// 读取底盘反算速度与按物理安装位置识别的四轮实际反馈。
+        /// </summary>
+        private void CaptureWheelFeedback(
+            DiagnosticSample sample)
+        {
+            try
+            {
+                var carSpeed = _chassis.GetCarSpeed(true);
+                sample.WheelBodyVxMetersPerSecond = carSpeed.Vx;
+                sample.WheelBodyVyMetersPerSecond = carSpeed.Vy;
+                // CommonUsage的CarSpeed.Vw以deg/s表达。
+                sample.WheelBodyOmegaDegreesPerSecond = carSpeed.Vw;
+
+#pragma warning disable CS0612, CS0618
+                var wheels = _chassis.GetSteerWheels();
+#pragma warning restore CS0612, CS0618
+
+                var leftFront = FindWheel(wheels, true, true);
+                var leftRear = FindWheel(wheels, false, true);
+                var rightFront = FindWheel(wheels, true, false);
+                var rightRear = FindWheel(wheels, false, false);
+
+                if (leftFront == null || leftRear == null ||
+                    rightFront == null || rightRear == null)
+                {
+                    throw new InvalidOperationException(
+                        "未能按物理安装位置识别四个舵轮。");
+                }
+
+                sample.ActualSteerLeftFrontDegrees =
+                    leftFront.ReadAngle();
+                sample.ActualSteerLeftRearDegrees =
+                    leftRear.ReadAngle();
+                sample.ActualSteerRightFrontDegrees =
+                    rightFront.ReadAngle();
+                sample.ActualSteerRightRearDegrees =
+                    rightRear.ReadAngle();
+                sample.ActualSpeedLeftFrontMetersPerSecond =
+                    leftFront.ReadSpeed();
+                sample.ActualSpeedLeftRearMetersPerSecond =
+                    leftRear.ReadSpeed();
+                sample.ActualSpeedRightFrontMetersPerSecond =
+                    rightFront.ReadSpeed();
+                sample.ActualSpeedRightRearMetersPerSecond =
+                    rightRear.ReadSpeed();
+                sample.WheelReadSucceeded = true;
+            }
+            catch (Exception exception)
+            {
+                AppendFailure(
+                    sample,
+                    "四轮反馈读取失败：" +
+                    exception.Message);
+            }
+        }
+
+        /// <summary>
+        /// 根据真实车体X向前、Y向左的物理安装位置查找指定舵轮。
+        /// </summary>
+        private static SteerWheel FindWheel(
+            IReadOnlyList<SteerWheel> wheels,
+            bool requireFront,
+            bool requireLeft)
+        {
+            foreach (var wheel in wheels)
+            {
+                var isFront = wheel.PhysicalPosition.X >= 0f;
+                var isLeft = wheel.PhysicalPosition.Y >= 0f;
+
+                if (isFront == requireFront &&
+                    isLeft == requireLeft)
+                {
+                    return wheel;
+                }
+            }
+
+            return null;
+        }
+
+        /// <summary>
+        /// 追加本帧诊断失败原因且保留先前错误信息。
+        /// </summary>
+        private static void AppendFailure(
+            DiagnosticSample sample,
+            string reason)
+        {
+            sample.FailureReason =
+                string.IsNullOrWhiteSpace(sample.FailureReason)
+                    ? reason
+                    : sample.FailureReason + "；" + reason;
+        }
+
+        /// <summary>
+        /// 保存采样快照并输出自动结束或手动停止后的摘要提示。
+        /// </summary>
+        private void SaveCsvAndReport(
+            bool completedAutomatically,
+            Exception testFailure)
+        {
+            List<DiagnosticSample> snapshot;
+            lock (_sampleSyncRoot)
+            {
+                snapshot =
+                    new List<DiagnosticSample>(_samples);
+            }
+
+            var outputDirectory = Path.Combine(
+                AppContext.BaseDirectory,
+                "DetourStaticDiagnostics");
+            Directory.CreateDirectory(outputDirectory);
+            SavedFilePath = Path.Combine(
+                outputDirectory,
+                $"{DateTime.Now:yyyyMMdd_HHmmss_fff}_" +
+                "DetourStaticDiagnostic.csv");
+
+            using (var writer = new StreamWriter(
+                       SavedFilePath,
+                       false,
+                       new UTF8Encoding(true)))
+            {
+                WriteCsvRow(writer,
+                    "ElapsedSeconds", "LocalTimestamp",
+                    "DetourReadSucceeded", "DetourCallDurationMilliseconds",
+                    "DetourTickRaw", "DetourTimestamp",
+                    "DetourTimestampValid", "DetourDataAgeMilliseconds",
+                    "DetourLStep", "DetourXMillimeters",
+                    "DetourYMillimeters", "DetourYawDegrees",
+                    "LocalDeltaMilliseconds", "DetourTickDeltaMilliseconds",
+                    "DeltaXMillimeters", "DeltaYMillimeters",
+                    "DeltaYawDegrees", "DeltaPositionMillimeters",
+                    "IsRepeatedTick", "IsRepeatedPose", "IsOutOfOrderTick",
+                    "WheelReadSucceeded", "WheelBodyVxMetersPerSecond",
+                    "WheelBodyVyMetersPerSecond",
+                    "WheelBodyOmegaDegreesPerSecond",
+                    "ActualSteerLeftFrontDegrees",
+                    "ActualSteerLeftRearDegrees",
+                    "ActualSteerRightFrontDegrees",
+                    "ActualSteerRightRearDegrees",
+                    "ActualSpeedLeftFrontMetersPerSecond",
+                    "ActualSpeedLeftRearMetersPerSecond",
+                    "ActualSpeedRightFrontMetersPerSecond",
+                    "ActualSpeedRightRearMetersPerSecond",
+                    "FailureReason");
+
+                foreach (var sample in snapshot)
+                {
+                    WriteCsvRow(writer,
+                        sample.ElapsedSeconds, sample.LocalTimestamp,
+                        sample.DetourReadSucceeded,
+                        sample.DetourCallDurationMilliseconds,
+                        sample.DetourTickRaw, sample.DetourTimestamp,
+                        sample.DetourTimestampValid,
+                        sample.DetourDataAgeMilliseconds,
+                        sample.DetourLStep, sample.DetourXMillimeters,
+                        sample.DetourYMillimeters, sample.DetourYawDegrees,
+                        sample.LocalDeltaMilliseconds,
+                        sample.DetourTickDeltaMilliseconds,
+                        sample.DeltaXMillimeters, sample.DeltaYMillimeters,
+                        sample.DeltaYawDegrees,
+                        sample.DeltaPositionMillimeters,
+                        sample.IsRepeatedTick, sample.IsRepeatedPose,
+                        sample.IsOutOfOrderTick,
+                        sample.WheelReadSucceeded,
+                        sample.WheelBodyVxMetersPerSecond,
+                        sample.WheelBodyVyMetersPerSecond,
+                        sample.WheelBodyOmegaDegreesPerSecond,
+                        sample.ActualSteerLeftFrontDegrees,
+                        sample.ActualSteerLeftRearDegrees,
+                        sample.ActualSteerRightFrontDegrees,
+                        sample.ActualSteerRightRearDegrees,
+                        sample.ActualSpeedLeftFrontMetersPerSecond,
+                        sample.ActualSpeedLeftRearMetersPerSecond,
+                        sample.ActualSpeedRightFrontMetersPerSecond,
+                        sample.ActualSpeedRightRearMetersPerSecond,
+                        sample.FailureReason);
+                }
+            }
+
+            var successfulSamples = 0;
+            var maximumPositionStepMillimeters = 0.0;
+            var maximumAbsoluteYawStepDegrees = 0.0;
+            foreach (var sample in snapshot)
+            {
+                if (sample.DetourReadSucceeded)
+                {
+                    successfulSamples++;
+                }
+
+                maximumPositionStepMillimeters = Math.Max(
+                    maximumPositionStepMillimeters,
+                    sample.DeltaPositionMillimeters ?? 0.0);
+                maximumAbsoluteYawStepDegrees = Math.Max(
+                    maximumAbsoluteYawStepDegrees,
+                    Math.Abs(sample.DeltaYawDegrees ?? 0.0));
+            }
+
+            var completionReason = testFailure != null
+                ? "因异常提前结束"
+                : completedAutomatically
+                    ? "到达设定时长，已自动结束"
+                    : "收到手动停止请求";
+            var message =
+                $"Detour静态诊断{completionReason}；" +
+                $"样本={snapshot.Count}，有效Detour样本={successfulSamples}，" +
+                $"最大位置阶跃={maximumPositionStepMillimeters:F2}mm，" +
+                $"最大航向阶跃={maximumAbsoluteYawStepDegrees:F3}°；" +
+                $"CSV={SavedFilePath}";
+
+            Console.WriteLine(message);
+            Hedingben.ToastText(message);
+        }
+
+        /// <summary>
+        /// 使用InvariantCulture格式化并转义一行CSV字段。
+        /// </summary>
+        private static void WriteCsvRow(
+            TextWriter writer,
+            params object[] values)
+        {
+            var fields = new string[values.Length];
+            for (var index = 0; index < values.Length; index++)
+            {
+                fields[index] = FormatCsvValue(values[index]);
+            }
+
+            writer.WriteLine(string.Join(",", fields));
+        }
+
+        /// <summary>
+        /// 将单个值转换为区域无关且符合CSV转义规则的文本。
+        /// </summary>
+        private static string FormatCsvValue(object value)
+        {
+            if (value == null)
+            {
+                return string.Empty;
+            }
+
+            string text;
+            if (value is bool boolean)
+            {
+                text = boolean ? "1" : "0";
+            }
+            else if (value is IFormattable formattable)
+            {
+                text = formattable.ToString(
+                    null,
+                    CultureInfo.InvariantCulture);
+            }
+            else
+            {
+                text = value.ToString();
+            }
+
+            if (text.IndexOfAny(
+                    new[] { ',', '"', '\r', '\n' }) < 0)
+            {
+                return text;
+            }
+
+            return "\"" +
+                   text.Replace("\"", "\"\"") +
+                   "\"";
+        }
+    }
+}

MultiWheelC/Experiments/NewControllerTrackingTests.cs

@@ -231,7 +231,8 @@ namespace MultiWheelC
                         (float)AccelerationMetersPerSecondSquared,
                     referenceDecelerationMetersPerSecondSquared:
                         (float)DecelerationMetersPerSecondSquared,
-                    diagnosticChassis: chassis);
+                    diagnosticChassis: chassis,
+                    diagnosticStateProvider: stateProvider);
             _recorder = recorder;
 
             var controlPointRadiusMeters =
@@ -521,7 +522,7 @@ namespace MultiWheelC
     /// 从当前Detour位姿开始执行“3m直线—左半圆—3m直线”新版控制器跟踪实验。
     /// </summary>
     [MovementTest(name = "新版控制器：直线-左半圆-直线轨迹跟踪")]
-    public sealed class NewControllerStraightSemicircleStraightTest
+    public class NewControllerStraightSemicircleStraightTest
         : MovementTest
     {
         private const float MillimetersPerMeter = 1000f;
@@ -579,6 +580,24 @@ namespace MultiWheelC
         /// </summary>
         public double PointSpacingMeters = 0.02;
 
+        /// <summary>
+        /// 获取组合轨迹主运动方向相对车头的夹角，单位为rad。
+        /// </summary>
+        protected virtual double MotionDirectionInBodyRadians =>
+            0.0;
+
+        /// <summary>
+        /// 获取轨迹完成后是否需要将舵轮主动恢复到车头方向。
+        /// </summary>
+        protected virtual bool ReturnWheelsForwardAfterCompletion =>
+            false;
+
+        /// <summary>
+        /// 获取实验记录使用的轨迹基础名称。
+        /// </summary>
+        protected virtual string ExperimentTrajectoryBaseName =>
+            "ProfiledStraightSmoothLeftTurnStraight";
+
         /// <summary>
         /// 读取当前位姿、绘制组合轨迹并启动新版轨迹跟踪动作。
         /// </summary>
@@ -637,7 +656,8 @@ namespace MultiWheelC
                         SemicircleMaximumSpeedMetersPerSecond,
                         AccelerationMetersPerSecondSquared,
                         DecelerationMetersPerSecondSquared,
-                        PointSpacingMeters);
+                        PointSpacingMeters,
+                        MotionDirectionInBodyRadians);
 
             DrawTrajectory(trajectory);
 
@@ -650,7 +670,7 @@ namespace MultiWheelC
                     controllerName: "NewStanleyPid",
                     trajectoryName:
                         TrajectoryExperimentInput.BuildTrajectoryName(
-                            "ProfiledStraightSmoothLeftTurnStraight",
+                            ExperimentTrajectoryBaseName,
                             lateralOffsetMeters),
                     trialNumber: TrialNumber,
                     referenceStart: referenceStart,
@@ -658,11 +678,15 @@ namespace MultiWheelC
                     referenceSpeed:
                         (float)StraightMaximumSpeedMetersPerSecond,
                     sampleIntervalMs: 50,
+                    referenceMotionFrameYawDegrees:
+                        (float)AngleMath.RadiansToDegrees(
+                            MotionDirectionInBodyRadians),
                     referenceAccelerationMetersPerSecondSquared:
                         (float)AccelerationMetersPerSecondSquared,
                     referenceDecelerationMetersPerSecondSquared:
                         (float)DecelerationMetersPerSecondSquared,
-                    diagnosticChassis: chassis);
+                    diagnosticChassis: chassis,
+                    diagnosticStateProvider: stateProvider);
             _recorder = recorder;
 
             var controlPointRadiusMeters =
@@ -673,6 +697,10 @@ namespace MultiWheelC
                 {
                     Trajectory = trajectory,
                     StateProvider = _stateProvider,
+                    MotionDirectionInBodyRadians =
+                        MotionDirectionInBodyRadians,
+                    ReturnWheelsForwardAfterCompletion =
+                        ReturnWheelsForwardAfterCompletion,
                     CycleObserver = controller =>
                         RecordControlCycle(
                             recorder,
@@ -894,4 +922,30 @@ namespace MultiWheelC
                     MillimetersPerMeter));
         }
     }
+
+    /// <summary>
+    /// 将舵轮准备到车体左前45°，跟踪直线—左半圆—直线轨迹，并在停车后恢复车头方向。
+    /// </summary>
+    [MovementTest(name = "新版控制器：45°蟹行直线-左半圆-直线轨迹跟踪")]
+    public sealed class NewControllerCrab45StraightSemicircleStraightTest
+        : NewControllerStraightSemicircleStraightTest
+    {
+        /// <summary>
+        /// 使用车体左前45°作为组合轨迹的固定运动方向。
+        /// </summary>
+        protected override double MotionDirectionInBodyRadians =>
+            Math.PI / 4.0;
+
+        /// <summary>
+        /// 蟹行组合轨迹正常完成后主动将四个舵轮恢复到车头方向。
+        /// </summary>
+        protected override bool ReturnWheelsForwardAfterCompletion =>
+            true;
+
+        /// <summary>
+        /// 将45°蟹行组合实验与普通组合轨迹实验的CSV名称明确区分。
+        /// </summary>
+        protected override string ExperimentTrajectoryBaseName =>
+            "ProfiledCrab45StraightSmoothLeftTurnStraight";
+    }
 }

MultiWheelC/Experiments/RotationTests.cs

@@ -7,10 +7,12 @@ using System.Threading;
 using ClumsyCore;
 using ClumsyCore.Interfaces;
 using ClumsyCore.Pilot;
+using CommonUsage.Chassis;
 using FundamentalLib;
 using MDCSToolBox.Clumsy.Movements;
 using MDCSToolBox.Clumsy.Pilot;
 using MyParking.Shared;
+using MultiWheelC.StateEstimation;
 
 namespace MultiWheelC
 {
@@ -66,6 +68,26 @@ namespace MultiWheelC
                 return;
             }
 
+            var chassis =
+                PilotDefinition.Chassis as MultiWheelChassis;
+            if (chassis == null)
+            {
+                Console.WriteLine(
+                    "当前底盘不是MultiWheelChassis，无法执行原地旋转测试。");
+                return;
+            }
+
+            var stateProvider =
+                ParkingVehicleStateProviderFactory.Create(
+                    chassis);
+            if (!stateProvider.TryGetState(out _))
+            {
+                Console.WriteLine(
+                    "无法读取原地旋转起点状态：" +
+                    stateProvider.LastFailureReason);
+                return;
+            }
+
             var rotationCenter =
                 new Vector2((float)location.x, (float)location.y);
             var targetWorldAngle =
@@ -98,7 +120,9 @@ namespace MultiWheelC
                 referenceSpeed: 0f,
                 referenceAngularSpeed:
                     (float)AngleMath.DegreesToRadians(
-                        config.InPlaceRotateMaxSpeed));
+                        config.InPlaceRotateMaxSpeed),
+                diagnosticChassis: chassis,
+                diagnosticStateProvider: stateProvider);
             _recorder.Start();
 
             try
@@ -108,6 +132,8 @@ namespace MultiWheelC
                     {
                         // MultiWheelRotateInPlace接收世界坐标系绝对航向。
                         AngleTarget = targetWorldAngle,
+                        Chassis = chassis,
+                        StateProvider = stateProvider,
                         CommandAngularSpeedObserver =
                             commandAngularSpeed =>
                                 _recorder?.UpdateCommand(
@@ -175,6 +201,26 @@ namespace MultiWheelC
                 return;
             }
 
+            var chassis =
+                PilotDefinition.Chassis as MultiWheelChassis;
+            if (chassis == null)
+            {
+                Console.WriteLine(
+                    "当前底盘不是MultiWheelChassis，无法执行原地旋转测试。");
+                return;
+            }
+
+            var stateProvider =
+                ParkingVehicleStateProviderFactory.Create(
+                    chassis);
+            if (!stateProvider.TryGetState(out _))
+            {
+                Console.WriteLine(
+                    "无法读取原地旋转起点状态：" +
+                    stateProvider.LastFailureReason);
+                return;
+            }
+
             if (Math.Abs(relativeAngleDegrees) < 1e-3f)
             {
                 Console.WriteLine("旋转角度不能为0，测试已经取消。");

MultiWheelC/Experiments/TestTrajectoryFactory.cs

@@ -125,7 +125,7 @@ namespace MultiWheelC
         }
 
         /// <summary>
-        /// 从当前位姿按速度符号生成“3m直线、沿行进方向平滑左弯180°、3m直线”的轨迹。
+        /// 从当前位姿沿指定车体运动方向生成“3m直线、平滑左弯180°、3m直线”的轨迹。
         /// </summary>
         public static Trajectory2D CreateStraightLeftSemicircleStraight(
             Pose2D startPoseInWorld,
@@ -136,7 +136,8 @@ namespace MultiWheelC
             double semicircleMaximumSpeedMetersPerSecond = 0.25,
             double accelerationMetersPerSecondSquared = 0.20,
             double decelerationMetersPerSecondSquared = 0.12,
-            double pointSpacingMeters = 0.02)
+            double pointSpacingMeters = 0.02,
+            double motionDirectionInBodyRadians = 0.0)
         {
             return CreateStraightSmoothLeftTurnStraight(
                 startPoseInWorld,
@@ -148,11 +149,12 @@ namespace MultiWheelC
                 semicircleMaximumSpeedMetersPerSecond,
                 accelerationMetersPerSecondSquared,
                 decelerationMetersPerSecondSquared,
-                pointSpacingMeters);
+                pointSpacingMeters,
+                motionDirectionInBodyRadians);
         }
 
         /// <summary>
-        /// 按共同速度符号生成“直线、沿行进方向平滑左弯、直线”轨迹，并使总转角严格等于指定角度。
+        /// 沿指定车体运动方向生成“直线、平滑左弯、直线”轨迹，并使总转角严格等于指定角度。
         /// </summary>
         public static Trajectory2D CreateStraightSmoothLeftTurnStraight(
             Pose2D startPoseInWorld,
@@ -164,7 +166,8 @@ namespace MultiWheelC
             double turnMaximumSpeedMetersPerSecond,
             double accelerationMetersPerSecondSquared,
             double decelerationMetersPerSecondSquared,
-            double pointSpacingMeters)
+            double pointSpacingMeters,
+            double motionDirectionInBodyRadians = 0.0)
         {
             NumericGuard.EnsureFinite(
                 startPoseInWorld,
@@ -195,6 +198,9 @@ namespace MultiWheelC
             NumericGuard.EnsureFinitePositive(
                 pointSpacingMeters,
                 nameof(pointSpacingMeters));
+            NumericGuard.EnsureFinite(
+                motionDirectionInBodyRadians,
+                nameof(motionDirectionInBodyRadians));
 
             if (turnAngleRadians > 2.0 * Math.PI)
             {
@@ -276,10 +282,13 @@ namespace MultiWheelC
 
             var points = new List<TrajectoryPoint>(
                 sampleArcLengths.Count);
+            var worldMotionStartYawRadians =
+                startPoseInWorld.YawRadians +
+                motionDirectionInBodyRadians;
             var startCos = Math.Cos(
-                startPoseInWorld.YawRadians);
+                worldMotionStartYawRadians);
             var startSin = Math.Sin(
-                startPoseInWorld.YawRadians);
+                worldMotionStartYawRadians);
             var localX = 0.0;
             var localY = 0.0;
             var localYawRadians = 0.0;

MultiWheelC/Experiments/TrackingExperimentRecorder.cs

@@ -24,6 +24,27 @@ namespace MultiWheelC
         public double DetourX;
         public double DetourY;
         public double DetourTheta;
+        public long DetourTickRaw;
+        public double DetourLStep;
+
+        // Detour状态估计内部诊断；偏移量仅在跳变候选有效时有意义。
+        public bool HasDetourStateDiagnostics;
+        public bool DetourJumpCandidateActive;
+        public int DetourJumpCandidateConsistentFrameCount;
+        public double DetourEstimatedShiftDistanceMeters;
+        public double DetourEstimatedShiftHeadingRadians;
+        public int DetourAutomaticFrameShiftCount;
+        public string DetourStateStatusReason;
+        public double DetourDataAgeMilliseconds;
+        public double DetourSourceFrameIntervalMilliseconds;
+        public double DetourMotionPredictionTimestampSeconds;
+        public bool HasDetourInnovationDiagnostics;
+        public double DetourPositionInnovationMeters;
+        public double DetourAllowedPositionInnovationMeters;
+        public double DetourHeadingInnovationRadians;
+        public double DetourAllowedHeadingInnovationRadians;
+        public string DetourLastJumpTriggerReason;
+        public string DetourStateStatus;
 
         // 车体速度单位为m/s，角速度统一使用rad/s。
         public float CommandSpeed;
@@ -62,6 +83,10 @@ namespace MultiWheelC
         public double WheelFeedbackRawBodyVyMetersPerSecond;
         public double WheelFeedbackFilteredBodyVyMetersPerSecond;
         public bool WheelFeedbackVelocityEstimateValid;
+        public bool HasWheelFeedbackAngularVelocityDiagnostics;
+        public double WheelFeedbackRawBodyOmegaRadiansPerSecond;
+        public double WheelFeedbackFilteredBodyOmegaRadiansPerSecond;
+        public double WheelFeedbackSampleTimestampSeconds;
 
         // 四舵轮机械角使用deg，前后虚拟GCP命令角使用rad。
         public bool HasSteeringDiagnostics;
@@ -130,6 +155,8 @@ namespace MultiWheelC
         private readonly float _referenceDecelerationMetersPerSecondSquared;
         private readonly int _sampleIntervalMs;
         private readonly MultiWheelChassis _diagnosticChassis;
+        private readonly WheelFeedbackVehicleStateProvider
+            _diagnosticStateProvider;
 
         private readonly List<TrackingSample> _samples =
             new List<TrackingSample>();
@@ -203,7 +230,9 @@ namespace MultiWheelC
             float referenceMotionFrameYawDegrees = 0f,
             float referenceAccelerationMetersPerSecondSquared = 0f,
             float referenceDecelerationMetersPerSecondSquared = 0f,
-            MultiWheelChassis diagnosticChassis = null)
+            MultiWheelChassis diagnosticChassis = null,
+            WheelFeedbackVehicleStateProvider
+                diagnosticStateProvider = null)
         {
             if (string.IsNullOrWhiteSpace(controllerName))
                 throw new ArgumentException(
@@ -235,6 +264,7 @@ namespace MultiWheelC
                 referenceDecelerationMetersPerSecondSquared;
             _sampleIntervalMs = sampleIntervalMs;
             _diagnosticChassis = diagnosticChassis;
+            _diagnosticStateProvider = diagnosticStateProvider;
         }
 
         // 保存成功后的CSV绝对路径；尚未保存时为空。
@@ -534,6 +564,47 @@ namespace MultiWheelC
                 var location =
                     DetourInterface.getCartLocation();
 
+                var hasDetourStateDiagnostics = false;
+                var detourJumpCandidateActive = false;
+                var detourJumpCandidateConsistentFrameCount = 0;
+                var detourEstimatedShiftDistanceMeters = 0.0;
+                var detourEstimatedShiftHeadingRadians = 0.0;
+                var detourAutomaticFrameShiftCount = 0;
+                var detourStateStatusReason = string.Empty;
+                var detourDataAgeMilliseconds = 0.0;
+                var detourSourceFrameIntervalSeconds = 0.0;
+                var detourMotionPredictionTimestampSeconds = 0.0;
+                var hasDetourInnovationDiagnostics = false;
+                var detourPositionInnovationMeters = 0.0;
+                var detourAllowedPositionInnovationMeters = 0.0;
+                var detourHeadingInnovationRadians = 0.0;
+                var detourAllowedHeadingInnovationRadians = 0.0;
+                var detourLastJumpTriggerReason = string.Empty;
+                var detourStateStatus = string.Empty;
+
+                if (_diagnosticStateProvider != null)
+                {
+                    hasDetourStateDiagnostics =
+                        _diagnosticStateProvider
+                            .TryGetLatestDetourDiagnostics(
+                                out detourJumpCandidateActive,
+                                out detourJumpCandidateConsistentFrameCount,
+                                out detourEstimatedShiftDistanceMeters,
+                                out detourEstimatedShiftHeadingRadians,
+                                out detourAutomaticFrameShiftCount,
+                                out detourSourceFrameIntervalSeconds,
+                                out detourMotionPredictionTimestampSeconds,
+                                out hasDetourInnovationDiagnostics,
+                                out detourPositionInnovationMeters,
+                                out detourAllowedPositionInnovationMeters,
+                                out detourHeadingInnovationRadians,
+                                out detourAllowedHeadingInnovationRadians,
+                                out detourLastJumpTriggerReason,
+                                out detourStateStatus,
+                                out detourStateStatusReason,
+                                out detourDataAgeMilliseconds);
+                }
+
                 float commandSpeed;
                 float commandVx;
                 float commandVy;
@@ -556,6 +627,10 @@ namespace MultiWheelC
                 double wheelFeedbackRawBodyVyMetersPerSecond;
                 double wheelFeedbackFilteredBodyVyMetersPerSecond;
                 bool wheelFeedbackVelocityEstimateValid;
+                var hasWheelFeedbackAngularVelocityDiagnostics = false;
+                var wheelFeedbackRawBodyOmegaRadiansPerSecond = 0.0;
+                var wheelFeedbackFilteredBodyOmegaRadiansPerSecond = 0.0;
+                var wheelFeedbackSampleTimestampSeconds = 0.0;
                 bool hasGcpCommand;
                 double requestedFrontGcpAngleRadians;
                 double requestedRearGcpAngleRadians;
@@ -655,6 +730,46 @@ namespace MultiWheelC
                         _commandRearGcpAngleRadians;
                 }
 
+                // 直接从状态源读取最新完整轮组诊断，使原地自转等没有
+                // 控制周期回调的实验也能记录原始/滤波Vw及其采样时间。
+                if (_diagnosticStateProvider != null &&
+                    _diagnosticStateProvider
+                        .TryGetLatestVelocityDiagnostics(
+                            out var directDetourBodyVx,
+                            out var directDetourVelocityValid,
+                            out var directRawWheelBodyVx,
+                            out var directFilteredWheelBodyVx,
+                            out var directRawWheelBodyVy,
+                            out var directFilteredWheelBodyVy,
+                            out var directRawWheelBodyOmega,
+                            out var directFilteredWheelBodyOmega,
+                            out var directWheelSampleTimestampSeconds,
+                            out var directWheelVelocityValid))
+                {
+                    hasVelocityDiagnostics = true;
+                    detourEstimatedBodyVxMetersPerSecond =
+                        directDetourBodyVx;
+                    detourVelocityEstimateValid =
+                        directDetourVelocityValid;
+                    wheelFeedbackRawBodyVxMetersPerSecond =
+                        directRawWheelBodyVx;
+                    wheelFeedbackFilteredBodyVxMetersPerSecond =
+                        directFilteredWheelBodyVx;
+                    wheelFeedbackRawBodyVyMetersPerSecond =
+                        directRawWheelBodyVy;
+                    wheelFeedbackFilteredBodyVyMetersPerSecond =
+                        directFilteredWheelBodyVy;
+                    wheelFeedbackRawBodyOmegaRadiansPerSecond =
+                        directRawWheelBodyOmega;
+                    wheelFeedbackFilteredBodyOmegaRadiansPerSecond =
+                        directFilteredWheelBodyOmega;
+                    wheelFeedbackSampleTimestampSeconds =
+                        directWheelSampleTimestampSeconds;
+                    wheelFeedbackVelocityEstimateValid =
+                        directWheelVelocityValid;
+                    hasWheelFeedbackAngularVelocityDiagnostics = true;
+                }
+
                 var sample = new TrackingSample
                 {
                     ElapsedSeconds =
@@ -662,6 +777,45 @@ namespace MultiWheelC
                     DetourX = location.x,
                     DetourY = location.y,
                     DetourTheta = location.th,
+                    DetourTickRaw = Convert.ToInt64(
+                        location.tick,
+                        CultureInfo.InvariantCulture),
+                    DetourLStep = Convert.ToDouble(
+                        location.l_step,
+                        CultureInfo.InvariantCulture),
+                    HasDetourStateDiagnostics =
+                        hasDetourStateDiagnostics,
+                    DetourJumpCandidateActive =
+                        detourJumpCandidateActive,
+                    DetourJumpCandidateConsistentFrameCount =
+                        detourJumpCandidateConsistentFrameCount,
+                    DetourEstimatedShiftDistanceMeters =
+                        detourEstimatedShiftDistanceMeters,
+                    DetourEstimatedShiftHeadingRadians =
+                        detourEstimatedShiftHeadingRadians,
+                    DetourAutomaticFrameShiftCount =
+                        detourAutomaticFrameShiftCount,
+                    DetourStateStatusReason =
+                        detourStateStatusReason,
+                    DetourDataAgeMilliseconds =
+                        detourDataAgeMilliseconds,
+                    DetourSourceFrameIntervalMilliseconds =
+                        detourSourceFrameIntervalSeconds * 1000.0,
+                    DetourMotionPredictionTimestampSeconds =
+                        detourMotionPredictionTimestampSeconds,
+                    HasDetourInnovationDiagnostics =
+                        hasDetourInnovationDiagnostics,
+                    DetourPositionInnovationMeters =
+                        detourPositionInnovationMeters,
+                    DetourAllowedPositionInnovationMeters =
+                        detourAllowedPositionInnovationMeters,
+                    DetourHeadingInnovationRadians =
+                        detourHeadingInnovationRadians,
+                    DetourAllowedHeadingInnovationRadians =
+                        detourAllowedHeadingInnovationRadians,
+                    DetourLastJumpTriggerReason =
+                        detourLastJumpTriggerReason,
+                    DetourStateStatus = detourStateStatus,
                     CommandSpeed = commandSpeed,
                     CommandVx = commandVx,
                     CommandVy = commandVy,
@@ -703,6 +857,14 @@ namespace MultiWheelC
                         wheelFeedbackFilteredBodyVyMetersPerSecond,
                     WheelFeedbackVelocityEstimateValid =
                         wheelFeedbackVelocityEstimateValid,
+                    HasWheelFeedbackAngularVelocityDiagnostics =
+                        hasWheelFeedbackAngularVelocityDiagnostics,
+                    WheelFeedbackRawBodyOmegaRadiansPerSecond =
+                        wheelFeedbackRawBodyOmegaRadiansPerSecond,
+                    WheelFeedbackFilteredBodyOmegaRadiansPerSecond =
+                        wheelFeedbackFilteredBodyOmegaRadiansPerSecond,
+                    WheelFeedbackSampleTimestampSeconds =
+                        wheelFeedbackSampleTimestampSeconds,
                     HasGcpCommand = hasGcpCommand,
                     RequestedFrontGcpAngleRadians =
                         requestedFrontGcpAngleRadians,
@@ -885,6 +1047,25 @@ namespace MultiWheelC
                     "DetourX," +
                     "DetourY," +
                     "DetourTheta," +
+                    "DetourTickRaw," +
+                    "DetourLStep," +
+                    "HasDetourStateDiagnostics," +
+                    "DetourJumpCandidateActive," +
+                    "DetourJumpCandidateConsistentFrameCount," +
+                    "DetourEstimatedShiftDistanceMeters," +
+                    "DetourEstimatedShiftHeadingRadians," +
+                    "DetourAutomaticFrameShiftCount," +
+                    "DetourStateStatusReason," +
+                    "DetourDataAgeMilliseconds," +
+                    "DetourSourceFrameIntervalMilliseconds," +
+                    "DetourMotionPredictionTimestampSeconds," +
+                    "HasDetourInnovationDiagnostics," +
+                    "DetourPositionInnovationMeters," +
+                    "DetourAllowedPositionInnovationMeters," +
+                    "DetourHeadingInnovationRadians," +
+                    "DetourAllowedHeadingInnovationRadians," +
+                    "DetourLastJumpTriggerReason," +
+                    "DetourStateStatus," +
                     "CommandSpeed," +
                     // 保留旧列（deg/s）供历史Python脚本兼容。
                     "CommandAngularSpeed," +
@@ -928,6 +1109,10 @@ namespace MultiWheelC
                     "WheelFeedbackRawBodyVyMetersPerSecond," +
                     "WheelFeedbackFilteredBodyVyMetersPerSecond," +
                     "WheelFeedbackVelocityEstimateValid," +
+                    "HasWheelFeedbackAngularVelocityDiagnostics," +
+                    "WheelFeedbackRawBodyOmegaRadiansPerSecond," +
+                    "WheelFeedbackFilteredBodyOmegaRadiansPerSecond," +
+                    "WheelFeedbackSampleTimestampSeconds," +
                     "HasSteeringDiagnostics," +
                     "TargetSteerLeftFrontDegrees," +
                     "TargetSteerLeftRearDegrees," +
@@ -959,6 +1144,73 @@ namespace MultiWheelC
                         Format(sample.DetourX),
                         Format(sample.DetourY),
                         Format(sample.DetourTheta),
+                        sample.DetourTickRaw.ToString(
+                            CultureInfo.InvariantCulture),
+                        Format(sample.DetourLStep),
+                        sample.HasDetourStateDiagnostics
+                            ? "1"
+                            : "0",
+                        FormatOptionalBoolean(
+                            sample.HasDetourStateDiagnostics,
+                            sample.DetourJumpCandidateActive),
+                        sample.HasDetourStateDiagnostics
+                            ? sample
+                                .DetourJumpCandidateConsistentFrameCount
+                                .ToString(
+                                    CultureInfo.InvariantCulture)
+                            : string.Empty,
+                        FormatOptional(
+                            sample.HasDetourStateDiagnostics &&
+                            sample.DetourJumpCandidateActive,
+                            sample.DetourEstimatedShiftDistanceMeters),
+                        FormatOptional(
+                            sample.HasDetourStateDiagnostics &&
+                            sample.DetourJumpCandidateActive,
+                            sample.DetourEstimatedShiftHeadingRadians),
+                        sample.HasDetourStateDiagnostics
+                            ? sample.DetourAutomaticFrameShiftCount
+                                .ToString(
+                                    CultureInfo.InvariantCulture)
+                            : string.Empty,
+                        sample.HasDetourStateDiagnostics
+                            ? EscapeCsv(
+                                sample.DetourStateStatusReason)
+                            : string.Empty,
+                        FormatOptional(
+                            sample.HasDetourStateDiagnostics,
+                            sample.DetourDataAgeMilliseconds),
+                        FormatOptional(
+                            sample.HasDetourStateDiagnostics,
+                            sample.DetourSourceFrameIntervalMilliseconds),
+                        FormatOptional(
+                            sample.HasDetourStateDiagnostics,
+                            sample.DetourMotionPredictionTimestampSeconds),
+                        FormatOptionalBoolean(
+                            sample.HasDetourStateDiagnostics,
+                            sample.HasDetourInnovationDiagnostics),
+                        FormatOptional(
+                            sample.HasDetourStateDiagnostics &&
+                            sample.HasDetourInnovationDiagnostics,
+                            sample.DetourPositionInnovationMeters),
+                        FormatOptional(
+                            sample.HasDetourStateDiagnostics &&
+                            sample.HasDetourInnovationDiagnostics,
+                            sample.DetourAllowedPositionInnovationMeters),
+                        FormatOptional(
+                            sample.HasDetourStateDiagnostics &&
+                            sample.HasDetourInnovationDiagnostics,
+                            sample.DetourHeadingInnovationRadians),
+                        FormatOptional(
+                            sample.HasDetourStateDiagnostics &&
+                            sample.HasDetourInnovationDiagnostics,
+                            sample.DetourAllowedHeadingInnovationRadians),
+                        sample.HasDetourStateDiagnostics
+                            ? EscapeCsv(
+                                sample.DetourLastJumpTriggerReason)
+                            : string.Empty,
+                        sample.HasDetourStateDiagnostics
+                            ? EscapeCsv(sample.DetourStateStatus)
+                            : string.Empty,
                         Format(sample.CommandSpeed),
                         Format(
                             AngleMath.RadiansToDegrees(
@@ -1067,6 +1319,21 @@ namespace MultiWheelC
                                 ? "1"
                                 : "0"
                             : string.Empty,
+                        FormatOptionalBoolean(
+                            sample.HasVelocityDiagnostics,
+                            sample.HasWheelFeedbackAngularVelocityDiagnostics),
+                        FormatOptional(
+                            sample.HasVelocityDiagnostics &&
+                            sample.HasWheelFeedbackAngularVelocityDiagnostics,
+                            sample.WheelFeedbackRawBodyOmegaRadiansPerSecond),
+                        FormatOptional(
+                            sample.HasVelocityDiagnostics &&
+                            sample.HasWheelFeedbackAngularVelocityDiagnostics,
+                            sample.WheelFeedbackFilteredBodyOmegaRadiansPerSecond),
+                        FormatOptional(
+                            sample.HasVelocityDiagnostics &&
+                            sample.HasWheelFeedbackAngularVelocityDiagnostics,
+                            sample.WheelFeedbackSampleTimestampSeconds),
                         sample.HasSteeringDiagnostics
                             ? "1"
                             : "0",

MultiWheelC/Movements/RotateInPlace.cs

@@ -200,6 +200,8 @@ namespace MultiWheelC
                         adapter
                             .StopXYThDrivePreserveSteeringState();
 
+                        // 到位稳定只依赖已经单独校验的航向。
+                        // 位置候选留到停车后处理，避免位置抖动中断航向闭环。
                         if (thPid.IsArrived())
                             break;
 
@@ -253,6 +255,40 @@ namespace MultiWheelC
                 CommandAngularSpeedObserver?.Invoke(0f);
                 adapter.StopXYThDrivePreserveSteeringState();
 
+                if (IsLocalizationRecoveryPending(
+                        stateProvider))
+                {
+                    BeginPostRotationPositionRecovery(
+                        stateProvider);
+                    var recoveryStarted = DateTime.Now;
+
+                    while (true)
+                    {
+                        adapter
+                            .StopXYThDrivePreserveSteeringState();
+
+                        if (stateProvider.TryGetState(out _) &&
+                            !IsLocalizationRecoveryPending(
+                                stateProvider))
+                        {
+                            break;
+                        }
+
+                        if ((DateTime.Now - recoveryStarted)
+                                .TotalSeconds >
+                            config
+                                .ParkingDetourJumpConfirmationTimeoutSeconds)
+                        {
+                            throw new InvalidOperationException(
+                                "原地自转完成后Detour位置在限定时间内未恢复。" +
+                                GetStateProviderFailureReason(
+                                    stateProvider));
+                        }
+
+                        yield return true;
+                    }
+                }
+
                 // 航向正常到位后复用统一回正动作；异常或取消会直接进入finally停车。
                 var wheelPreparation =
                     new PrepareWheelsForward();
@@ -356,6 +392,36 @@ namespace MultiWheelC
                     angleDegrees);
             }
 
+            if (stateProvider is
+                WheelFeedbackVehicleStateProvider wheelProvider)
+            {
+                if (!wheelProvider.TryGetHeadingRadians(
+                        out var wheelHeadingRadians))
+                {
+                    throw new InvalidOperationException(
+                        "无法从Detour状态源读取有效车辆航向。" +
+                        wheelProvider.LastHeadingFailureReason);
+                }
+
+                return (float)AngleMath.RadiansToDegrees(
+                    wheelHeadingRadians);
+            }
+
+            if (stateProvider is
+                DetourVehicleStateProvider detourProvider)
+            {
+                if (!detourProvider.TryGetHeadingRadians(
+                        out var detourHeadingRadians))
+                {
+                    throw new InvalidOperationException(
+                        "无法从Detour状态源读取有效车辆航向。" +
+                        detourProvider.LastHeadingFailureReason);
+                }
+
+                return (float)AngleMath.RadiansToDegrees(
+                    detourHeadingRadians);
+            }
+
             if (stateProvider == null ||
                 !stateProvider.TryGetState(out var state))
             {
@@ -369,6 +435,26 @@ namespace MultiWheelC
                 state.PoseInWorld.YawRadians);
         }
 
+        /// <summary>
+        /// 通知配置化状态源：车辆已经停车，可以重新确认旋转期间的位置候选。
+        /// </summary>
+        private static void BeginPostRotationPositionRecovery(
+            IVehicleStateProvider stateProvider)
+        {
+            if (stateProvider is
+                WheelFeedbackVehicleStateProvider wheelProvider)
+            {
+                wheelProvider.BeginPostRotationPositionRecovery();
+                return;
+            }
+
+            if (stateProvider is
+                DetourVehicleStateProvider detourProvider)
+            {
+                detourProvider.BeginPostRotationPositionRecovery();
+            }
+        }
+
         /// <summary>
         /// 获取已知停车状态源最近一次失败原因，未知实现返回空字符串。
         /// </summary>
@@ -390,6 +476,31 @@ namespace MultiWheelC
             return string.Empty;
         }
 
+        /// <summary>
+        /// 判断Detour是否仍在使用轮组预测确认疑似位姿不连续。
+        /// </summary>
+        private static bool IsLocalizationRecoveryPending(
+            IVehicleStateProvider stateProvider)
+        {
+            if (stateProvider is
+                    WheelFeedbackVehicleStateProvider wheelProvider)
+            {
+                return wheelProvider.TryGetLatestDetourDiagnostics(
+                           out var jumpCandidateActive,
+                           out _,
+                           out _,
+                           out _,
+                           out _,
+                           out _,
+                           out _) &&
+                       jumpCandidateActive;
+            }
+
+            return stateProvider is
+                       DetourVehicleStateProvider detourProvider &&
+                   detourProvider.IsJumpCandidateActive;
+        }
+
         /// <summary>
         /// 检查原地自转参数是否为正有限值，部分时间和容差参数允许为零。
         /// </summary>

MultiWheelC/StateEstimation/ParkingVehicleStateProviderFactory.cs

@@ -55,7 +55,16 @@ namespace MultiWheelC.StateEstimation
                     AngleMath.DegreesToRadians(
                         config
                             .ParkingDetourVelocityHeadingResidualDegrees),
-                    config.ParkingDetourStationaryConfirmationSeconds);
+                    config.ParkingDetourStationaryConfirmationSeconds,
+                    config.ParkingDetourHeadingOutlierConfirmationFrames,
+                    config
+                        .ParkingDetourHeadingOutlierPredictionTimeoutSeconds,
+                    config.ParkingDetourJumpConfirmationFrames,
+                    config.ParkingDetourJumpConfirmationTimeoutSeconds,
+                    config.ParkingDetourMaximumAutomaticFrameShift,
+                    AngleMath.DegreesToRadians(
+                        config
+                            .ParkingDetourMaximumAutomaticHeadingShiftDegrees));
 
             return new WheelFeedbackVehicleStateProvider(
                 detourStateProvider,

MultiWheelC/StateEstimation/VehicleState.cs

@@ -57,7 +57,8 @@ namespace MultiWheelC.StateEstimation
         public double SampleTimestampSeconds { get; }
 
         /// <summary>
-        /// 获取车体中心在Detour世界坐标系中的位姿，单位为m和rad。
+        /// 获取车体中心在状态源输出世界坐标系中的位姿，单位为m和rad。
+        /// Detour发生经确认的小幅坐标跳变后，该坐标系会保持任务内连续。
         /// </summary>
         public Pose2D PoseInWorld { get; }
 

MultiWheelC/StateEstimation/WheelFeedbackVehicleStateProvider.cs

@@ -6,7 +6,7 @@ using System.Diagnostics;
 namespace MultiWheelC.StateEstimation
 {
     /// <summary>
-    /// 保留外部状态源的Detour位姿，并以舵轮电机反馈解算的车体平面速度替换Detour差分线速度。
+    /// 保留外部状态源的Detour位姿，以轮组反馈替换平面线速度，并向短时位姿预测提供角速度。
     /// </summary>
     public sealed class WheelFeedbackVehicleStateProvider
         : IVehicleStateProvider
@@ -20,6 +20,7 @@ namespace MultiWheelC.StateEstimation
         private readonly MultiWheelChassis _chassis;
         private readonly FirstOrderLowPassFilter _longitudinalSpeedFilter;
         private readonly FirstOrderLowPassFilter _lateralSpeedFilter;
+        private readonly FirstOrderLowPassFilter _angularSpeedFilter;
 
         private bool _hasPreviousTimestamp;
         private double _previousTimestampSeconds;
@@ -30,7 +31,11 @@ namespace MultiWheelC.StateEstimation
         private double _latestFilteredWheelBodyVxMetersPerSecond;
         private double _latestRawWheelBodyVyMetersPerSecond;
         private double _latestFilteredWheelBodyVyMetersPerSecond;
+        private double _latestRawWheelBodyOmegaRadiansPerSecond;
+        private double _latestFilteredWheelBodyOmegaRadiansPerSecond;
+        private double _latestWheelSampleTimestampSeconds;
         private bool _latestWheelVelocityValid;
+        private bool _latestWheelFeedbackReadSucceeded;
 
         /// <summary>
         /// 创建使用默认0.10s低通时间常数的电机反馈平面速度状态源。
@@ -65,6 +70,9 @@ namespace MultiWheelC.StateEstimation
             _lateralSpeedFilter =
                 new FirstOrderLowPassFilter(
                     velocityFilterTimeConstantSeconds);
+            _angularSpeedFilter =
+                new FirstOrderLowPassFilter(
+                    velocityFilterTimeConstantSeconds);
         }
 
         /// <summary>
@@ -73,6 +81,12 @@ namespace MultiWheelC.StateEstimation
         public string LastFailureReason { get; private set; } =
             string.Empty;
 
+        /// <summary>
+        /// 获取最近一次航向读取失败的原因；位置单独异常时保持为空。
+        /// </summary>
+        public string LastHeadingFailureReason { get; private set; } =
+            string.Empty;
+
         /// <summary>
         /// 读取Detour位姿和电机反馈速度，并组合成统一车辆状态。
         /// </summary>
@@ -80,15 +94,6 @@ namespace MultiWheelC.StateEstimation
         {
             lock (_syncRoot)
             {
-                if (!_poseProvider.TryGetState(
-                        out var poseState))
-                {
-                    state = default;
-                    LastFailureReason =
-                        "基础位姿状态源暂时不可用。";
-                    return false;
-                }
-
                 try
                 {
                     var actualCarSpeed =
@@ -97,6 +102,11 @@ namespace MultiWheelC.StateEstimation
                         (double)actualCarSpeed.Vx;
                     var rawBodyVyMetersPerSecond =
                         (double)actualCarSpeed.Vy;
+                    // CommonUsage.CarSpeed.Vw在旧底盘边界使用deg/s；
+                    // 状态估计内部统一转换为rad/s。
+                    var rawBodyOmegaRadiansPerSecond =
+                        AngleMath.DegreesToRadians(
+                            actualCarSpeed.Vw);
 
                     NumericGuard.EnsureFinite(
                         rawBodyVxMetersPerSecond,
@@ -104,6 +114,9 @@ namespace MultiWheelC.StateEstimation
                     NumericGuard.EnsureFinite(
                         rawBodyVyMetersPerSecond,
                         "电机反馈车体横向速度");
+                    NumericGuard.EnsureFinite(
+                        rawBodyOmegaRadiansPerSecond,
+                        "电机反馈车体角速度");
 
                     var wheelSpeedTimestampSeconds =
                         _wheelSpeedClock.Elapsed.TotalSeconds;
@@ -111,15 +124,13 @@ namespace MultiWheelC.StateEstimation
                     UpdateBodyVelocityFilters(
                         rawBodyVxMetersPerSecond,
                         rawBodyVyMetersPerSecond,
+                        rawBodyOmegaRadiansPerSecond,
                         wheelSpeedTimestampSeconds,
                         out var filteredBodyVxMetersPerSecond,
                         out var filteredBodyVyMetersPerSecond,
+                        out var filteredBodyOmegaRadiansPerSecond,
                         out var hasValidWheelSpeedEstimate);
 
-                    _latestDetourBodyVxMetersPerSecond =
-                        poseState.TwistInBody.VxMetersPerSecond;
-                    _latestDetourVelocityValid =
-                        poseState.HasValidVelocityEstimate;
                     _latestRawWheelBodyVxMetersPerSecond =
                         rawBodyVxMetersPerSecond;
                     _latestFilteredWheelBodyVxMetersPerSecond =
@@ -128,8 +139,40 @@ namespace MultiWheelC.StateEstimation
                         rawBodyVyMetersPerSecond;
                     _latestFilteredWheelBodyVyMetersPerSecond =
                         filteredBodyVyMetersPerSecond;
+                    _latestRawWheelBodyOmegaRadiansPerSecond =
+                        rawBodyOmegaRadiansPerSecond;
+                    _latestFilteredWheelBodyOmegaRadiansPerSecond =
+                        filteredBodyOmegaRadiansPerSecond;
+                    _latestWheelSampleTimestampSeconds =
+                        wheelSpeedTimestampSeconds;
                     _latestWheelVelocityValid =
                         hasValidWheelSpeedEstimate;
+                    _latestWheelFeedbackReadSucceeded = true;
+
+                    // Detour位姿跳变确认期间需要用轮速维持短时运动预测。
+                    if (_poseProvider is DetourVehicleStateProvider
+                            detourStateProvider)
+                    {
+                        detourStateProvider.UpdateWheelVelocityEstimate(
+                            filteredBodyVxMetersPerSecond,
+                            filteredBodyVyMetersPerSecond,
+                            filteredBodyOmegaRadiansPerSecond,
+                            hasValidWheelSpeedEstimate);
+                    }
+
+                    if (!_poseProvider.TryGetState(
+                            out var poseState))
+                    {
+                        state = default;
+                        LastFailureReason =
+                            GetPoseProviderFailureReason();
+                        return false;
+                    }
+
+                    _latestDetourBodyVxMetersPerSecond =
+                        poseState.TwistInBody.VxMetersPerSecond;
+                    _latestDetourVelocityValid =
+                        poseState.HasValidVelocityEstimate;
                     _hasVelocityDiagnostics = true;
 
                     // 车体平面线速度来自四轮电机和舵角反馈；角速度继续使用Detour，
@@ -157,6 +200,7 @@ namespace MultiWheelC.StateEstimation
                 }
                 catch (Exception exception)
                 {
+                    _latestWheelFeedbackReadSucceeded = false;
                     state = default;
                     LastFailureReason =
                         "舵轮电机反馈车体速度解算失败：" +
@@ -166,6 +210,85 @@ namespace MultiWheelC.StateEstimation
             }
         }
 
+        /// <summary>
+        /// 读取Detour独立校验后的航向，同时保持轮组速度预测输入更新。
+        /// </summary>
+        public bool TryGetHeadingRadians(
+            out double headingRadians)
+        {
+            lock (_syncRoot)
+            {
+                TryGetState(out _);
+
+                if (!_latestWheelFeedbackReadSucceeded)
+                {
+                    headingRadians = 0.0;
+                    LastHeadingFailureReason =
+                        string.IsNullOrWhiteSpace(
+                            LastFailureReason)
+                            ? "舵轮反馈当前不可用，无法校验航向。"
+                            : LastFailureReason;
+                    return false;
+                }
+
+                if (_poseProvider is DetourVehicleStateProvider
+                        detourStateProvider)
+                {
+                    var success = detourStateProvider
+                        .TryGetLatestReliableHeadingRadians(
+                            out headingRadians);
+                    LastHeadingFailureReason = success
+                        ? string.Empty
+                        : detourStateProvider
+                            .LastHeadingFailureReason;
+                    return success;
+                }
+
+                if (_poseProvider.TryGetState(
+                        out var poseState))
+                {
+                    headingRadians =
+                        poseState.PoseInWorld.YawRadians;
+                    LastHeadingFailureReason = string.Empty;
+                    return true;
+                }
+
+                headingRadians = 0.0;
+                LastHeadingFailureReason =
+                    GetPoseProviderFailureReason();
+                return false;
+            }
+        }
+
+        /// <summary>
+        /// 原地自转停车后，允许基础Detour状态源重新确认有限位置偏移。
+        /// </summary>
+        public void BeginPostRotationPositionRecovery()
+        {
+            lock (_syncRoot)
+            {
+                if (_poseProvider is DetourVehicleStateProvider
+                        detourStateProvider)
+                {
+                    detourStateProvider
+                        .BeginPostRotationPositionRecovery();
+                }
+            }
+        }
+
+        private string GetPoseProviderFailureReason()
+        {
+            if (_poseProvider is DetourVehicleStateProvider
+                    detourStateProvider &&
+                !string.IsNullOrWhiteSpace(
+                    detourStateProvider.LastFailureReason))
+            {
+                return detourStateProvider.LastFailureReason;
+            }
+
+            return "基础位姿状态源暂时不可用。";
+        }
+
         /// <summary>
         /// 读取最近一帧Detour纵向速度和轮速解算平面速度，供实验记录使用。
         /// </summary>
@@ -199,14 +322,188 @@ namespace MultiWheelC.StateEstimation
         }
 
         /// <summary>
-        /// 清除电机反馈速度的时间基准和低通滤波历史。
+        /// 读取最近一帧Detour纵向速度及轮组原始/滤波Vx、Vy、Vw和采样时间。
+        /// </summary>
+        public bool TryGetLatestVelocityDiagnostics(
+            out double detourBodyVxMetersPerSecond,
+            out bool detourVelocityValid,
+            out double rawWheelBodyVxMetersPerSecond,
+            out double filteredWheelBodyVxMetersPerSecond,
+            out double rawWheelBodyVyMetersPerSecond,
+            out double filteredWheelBodyVyMetersPerSecond,
+            out double rawWheelBodyOmegaRadiansPerSecond,
+            out double filteredWheelBodyOmegaRadiansPerSecond,
+            out double wheelSampleTimestampSeconds,
+            out bool wheelVelocityValid)
+        {
+            lock (_syncRoot)
+            {
+                detourBodyVxMetersPerSecond =
+                    _latestDetourBodyVxMetersPerSecond;
+                detourVelocityValid =
+                    _latestDetourVelocityValid;
+                rawWheelBodyVxMetersPerSecond =
+                    _latestRawWheelBodyVxMetersPerSecond;
+                filteredWheelBodyVxMetersPerSecond =
+                    _latestFilteredWheelBodyVxMetersPerSecond;
+                rawWheelBodyVyMetersPerSecond =
+                    _latestRawWheelBodyVyMetersPerSecond;
+                filteredWheelBodyVyMetersPerSecond =
+                    _latestFilteredWheelBodyVyMetersPerSecond;
+                rawWheelBodyOmegaRadiansPerSecond =
+                    _latestRawWheelBodyOmegaRadiansPerSecond;
+                filteredWheelBodyOmegaRadiansPerSecond =
+                    _latestFilteredWheelBodyOmegaRadiansPerSecond;
+                wheelSampleTimestampSeconds =
+                    _latestWheelSampleTimestampSeconds;
+                wheelVelocityValid =
+                    _latestWheelVelocityValid;
+                return _hasVelocityDiagnostics;
+            }
+        }
+
+        /// <summary>
+        /// 读取Detour跳变候选、自动坐标连续化和数据新鲜度诊断。
+        /// </summary>
+        public bool TryGetLatestDetourDiagnostics(
+            out bool jumpCandidateActive,
+            out int jumpCandidateConsistentFrameCount,
+            out double estimatedShiftDistanceMeters,
+            out double estimatedShiftHeadingRadians,
+            out int automaticFrameShiftCount,
+            out string stateStatusReason,
+            out double detourDataAgeMilliseconds)
+        {
+            lock (_syncRoot)
+            {
+                if (_poseProvider is DetourVehicleStateProvider
+                        detourStateProvider)
+                {
+                    var hasDiagnostics = detourStateProvider
+                        .TryGetLatestDiagnostics(
+                            out jumpCandidateActive,
+                            out jumpCandidateConsistentFrameCount,
+                            out estimatedShiftDistanceMeters,
+                            out estimatedShiftHeadingRadians,
+                            out automaticFrameShiftCount,
+                            out stateStatusReason,
+                            out detourDataAgeMilliseconds);
+
+                    if (string.IsNullOrWhiteSpace(
+                            stateStatusReason) &&
+                        !string.IsNullOrWhiteSpace(
+                            LastFailureReason))
+                    {
+                        stateStatusReason = LastFailureReason;
+                    }
+
+                    return hasDiagnostics;
+                }
+
+                jumpCandidateActive = false;
+                jumpCandidateConsistentFrameCount = 0;
+                estimatedShiftDistanceMeters = 0.0;
+                estimatedShiftHeadingRadians = 0.0;
+                automaticFrameShiftCount = 0;
+                stateStatusReason = LastFailureReason;
+                detourDataAgeMilliseconds = 0.0;
+                return false;
+            }
+        }
+
+        /// <summary>
+        /// 读取Detour源帧、轮速预测、创新门限、候选原因和状态诊断。
+        /// </summary>
+        public bool TryGetLatestDetourDiagnostics(
+            out bool jumpCandidateActive,
+            out int jumpCandidateConsistentFrameCount,
+            out double estimatedShiftDistanceMeters,
+            out double estimatedShiftHeadingRadians,
+            out int automaticFrameShiftCount,
+            out double sourceFrameIntervalSeconds,
+            out double motionPredictionTimestampSeconds,
+            out bool hasInnovationDiagnostics,
+            out double positionInnovationMeters,
+            out double allowedPositionInnovationMeters,
+            out double headingInnovationRadians,
+            out double allowedHeadingInnovationRadians,
+            out string jumpCandidateTriggerReason,
+            out string stateStatus,
+            out string stateStatusReason,
+            out double detourDataAgeMilliseconds)
+        {
+            lock (_syncRoot)
+            {
+                if (_poseProvider is DetourVehicleStateProvider
+                        detourStateProvider)
+                {
+                    var hasDiagnostics = detourStateProvider
+                        .TryGetLatestDiagnostics(
+                            out jumpCandidateActive,
+                            out jumpCandidateConsistentFrameCount,
+                            out estimatedShiftDistanceMeters,
+                            out estimatedShiftHeadingRadians,
+                            out automaticFrameShiftCount,
+                            out sourceFrameIntervalSeconds,
+                            out motionPredictionTimestampSeconds,
+                            out hasInnovationDiagnostics,
+                            out positionInnovationMeters,
+                            out allowedPositionInnovationMeters,
+                            out headingInnovationRadians,
+                            out allowedHeadingInnovationRadians,
+                            out jumpCandidateTriggerReason,
+                            out stateStatus,
+                            out stateStatusReason,
+                            out detourDataAgeMilliseconds);
+
+                    if (string.IsNullOrWhiteSpace(
+                            stateStatusReason) &&
+                        !string.IsNullOrWhiteSpace(
+                            LastFailureReason))
+                    {
+                        stateStatus = "Unavailable";
+                        stateStatusReason = LastFailureReason;
+                    }
+
+                    return hasDiagnostics;
+                }
+
+                jumpCandidateActive = false;
+                jumpCandidateConsistentFrameCount = 0;
+                estimatedShiftDistanceMeters = 0.0;
+                estimatedShiftHeadingRadians = 0.0;
+                automaticFrameShiftCount = 0;
+                sourceFrameIntervalSeconds = 0.0;
+                motionPredictionTimestampSeconds = 0.0;
+                hasInnovationDiagnostics = false;
+                positionInnovationMeters = 0.0;
+                allowedPositionInnovationMeters = 0.0;
+                headingInnovationRadians = 0.0;
+                allowedHeadingInnovationRadians = 0.0;
+                jumpCandidateTriggerReason = string.Empty;
+                stateStatus = "Unavailable";
+                stateStatusReason = LastFailureReason;
+                detourDataAgeMilliseconds = 0.0;
+                return false;
+            }
+        }
+
+        /// <summary>
+        /// 清除基础位姿状态、坐标连续化状态以及电机反馈速度滤波历史。
         /// </summary>
         public void Reset()
         {
             lock (_syncRoot)
             {
+                if (_poseProvider is DetourVehicleStateProvider
+                        detourStateProvider)
+                {
+                    detourStateProvider.Reset();
+                }
+
                 _longitudinalSpeedFilter.Reset();
                 _lateralSpeedFilter.Reset();
+                _angularSpeedFilter.Reset();
                 _wheelSpeedClock.Restart();
                 _hasPreviousTimestamp = false;
                 _previousTimestampSeconds = 0.0;
@@ -217,20 +514,27 @@ namespace MultiWheelC.StateEstimation
                 _latestFilteredWheelBodyVxMetersPerSecond = 0.0;
                 _latestRawWheelBodyVyMetersPerSecond = 0.0;
                 _latestFilteredWheelBodyVyMetersPerSecond = 0.0;
+                _latestRawWheelBodyOmegaRadiansPerSecond = 0.0;
+                _latestFilteredWheelBodyOmegaRadiansPerSecond = 0.0;
+                _latestWheelSampleTimestampSeconds = 0.0;
                 _latestWheelVelocityValid = false;
+                _latestWheelFeedbackReadSucceeded = false;
                 LastFailureReason = string.Empty;
+                LastHeadingFailureReason = string.Empty;
             }
         }
 
         /// <summary>
-        /// 使用同一个真实采样间隔更新车体Vx和Vy低通滤波，并在首帧建立共同时间基准。
+        /// 使用同一个真实采样间隔更新车体Vx、Vy和Omega低通滤波，并在首帧建立共同时间基准。
         /// </summary>
         private void UpdateBodyVelocityFilters(
             double rawBodyVxMetersPerSecond,
             double rawBodyVyMetersPerSecond,
+            double rawBodyOmegaRadiansPerSecond,
             double timestampSeconds,
             out double filteredBodyVxMetersPerSecond,
             out double filteredBodyVyMetersPerSecond,
+            out double filteredBodyOmegaRadiansPerSecond,
             out bool hasValidWheelSpeedEstimate)
         {
             NumericGuard.EnsureFiniteNonNegative(
@@ -243,6 +547,8 @@ namespace MultiWheelC.StateEstimation
                     rawBodyVxMetersPerSecond);
                 _lateralSpeedFilter.Reset(
                     rawBodyVyMetersPerSecond);
+                _angularSpeedFilter.Reset(
+                    rawBodyOmegaRadiansPerSecond);
                 _previousTimestampSeconds = timestampSeconds;
                 _hasPreviousTimestamp = true;
                 hasValidWheelSpeedEstimate = false;
@@ -250,6 +556,8 @@ namespace MultiWheelC.StateEstimation
                     rawBodyVxMetersPerSecond;
                 filteredBodyVyMetersPerSecond =
                     rawBodyVyMetersPerSecond;
+                filteredBodyOmegaRadiansPerSecond =
+                    rawBodyOmegaRadiansPerSecond;
                 return;
             }
 
@@ -264,11 +572,15 @@ namespace MultiWheelC.StateEstimation
                     rawBodyVxMetersPerSecond);
                 _lateralSpeedFilter.Reset(
                     rawBodyVyMetersPerSecond);
+                _angularSpeedFilter.Reset(
+                    rawBodyOmegaRadiansPerSecond);
                 hasValidWheelSpeedEstimate = false;
                 filteredBodyVxMetersPerSecond =
                     rawBodyVxMetersPerSecond;
                 filteredBodyVyMetersPerSecond =
                     rawBodyVyMetersPerSecond;
+                filteredBodyOmegaRadiansPerSecond =
+                    rawBodyOmegaRadiansPerSecond;
                 return;
             }
 
@@ -281,6 +593,10 @@ namespace MultiWheelC.StateEstimation
                 _lateralSpeedFilter.Update(
                     rawBodyVyMetersPerSecond,
                     deltaTimeSeconds);
+            filteredBodyOmegaRadiansPerSecond =
+                _angularSpeedFilter.Update(
+                    rawBodyOmegaRadiansPerSecond,
+                    deltaTimeSeconds);
         }
 
     }