打通新版Stanley轨迹跟踪闭环并补充实验测试与数据分析脚本

Co-authored-by: Cursor <cursoragent@cursor.com>

MultiWheelC/Experiments/NewControllerTrackingTests.cs

@@ -0,0 +1,297 @@
+using System;
+using System.Drawing;
+using System.Numerics;
+using System.Threading;
+using ClumsyCore;
+using ClumsyCore.DTools;
+using ClumsyCore.Interfaces;
+using ClumsyCore.Pilot;
+using CommonUsage.Chassis;
+using MultiWheelC.Control.Execution;
+using MultiWheelC.StateEstimation;
+using MyParking.Shared;
+
+namespace MultiWheelC
+{
+    /// <summary>
+    /// 从当前Detour位姿开始执行新版控制器4m直线跟踪并保存实验数据。
+    /// </summary>
+    [MovementTest(name = "新版控制器：4m直线轨迹跟踪")]
+    public sealed class NewControllerStraight4mTest
+        : MovementTest
+    {
+        private const float MillimetersPerMeter = 1000f;
+
+        private readonly Painter _painter =
+            UI.GetPainter("NewControllerStraight4m");
+
+        private DriveTask _task;
+        private TrackingExperimentRecorder _recorder;
+        private DetourVehicleStateProvider _stateProvider;
+
+        /// <summary>
+        /// 获取或设置本次测试编号，用于区分重复实验CSV。
+        /// </summary>
+        public int TrialNumber = 1;
+
+        /// <summary>
+        /// 获取或设置4m直线的巡航参考速度，单位为m/s。
+        /// </summary>
+        public double CruiseSpeedMetersPerSecond = 0.30;
+
+        /// <summary>
+        /// 获取或设置参考速度加速度，单位为m/s²。
+        /// </summary>
+        public double AccelerationMetersPerSecondSquared = 0.20;
+
+        /// <summary>
+        /// 获取或设置参考速度减速度，单位为m/s²。
+        /// </summary>
+        public double DecelerationMetersPerSecondSquared = 0.20;
+
+        /// <summary>
+        /// 获取或设置离散轨迹点间距，单位为m。
+        /// </summary>
+        public double PointSpacingMeters = 0.02;
+
+        /// <summary>
+        /// 读取当前位姿、绘制离散轨迹并启动新版轨迹跟踪动作。
+        /// </summary>
+        public override void Test()
+        {
+            if (_task != null)
+            {
+                Console.WriteLine(
+                    "新版4m直线轨迹测试已经在运行，请先停止当前测试。");
+                return;
+            }
+
+            if (!MovementTestPreparation.AreWheelsForward())
+            {
+                return;
+            }
+
+            var chassis =
+                PilotDefinition.Chassis as MultiWheelChassis;
+            if (chassis == null)
+            {
+                Console.WriteLine(
+                    "当前底盘不是MultiWheelChassis，无法执行新版轨迹测试。");
+                return;
+            }
+
+            _stateProvider =
+                new DetourVehicleStateProvider();
+            if (!_stateProvider.TryGetState(
+                    out var initialState))
+            {
+                Console.WriteLine(
+                    "无法读取有效Detour起点位姿：" +
+                    _stateProvider.LastFailureReason);
+                _stateProvider = null;
+                return;
+            }
+
+            var trajectory =
+                TestTrajectoryFactory.CreateStraight4Meters(
+                    initialState.PoseInWorld,
+                    CruiseSpeedMetersPerSecond,
+                    AccelerationMetersPerSecondSquared,
+                    DecelerationMetersPerSecondSquared,
+                    PointSpacingMeters);
+
+            DrawTrajectory(trajectory);
+
+            var referenceStart = ToMillimeterVector(
+                trajectory.StartPoint.PoseInWorld);
+            var referenceEnd = ToMillimeterVector(
+                trajectory.EndPoint.PoseInWorld);
+            var recorder =
+                new TrackingExperimentRecorder(
+                    controllerName: "NewStanleyPid",
+                    trajectoryName: "ProfiledStraight4m",
+                    trialNumber: TrialNumber,
+                    referenceStart: referenceStart,
+                    referenceEnd: referenceEnd,
+                    referenceSpeed:
+                        (float)CruiseSpeedMetersPerSecond,
+                    sampleIntervalMs: 50,
+                    referenceAccelerationMetersPerSecondSquared:
+                        (float)AccelerationMetersPerSecondSquared,
+                    referenceDecelerationMetersPerSecondSquared:
+                        (float)DecelerationMetersPerSecondSquared);
+            _recorder = recorder;
+
+            var controlPointRadiusMeters =
+                chassis.ControlPointRadius /
+                MillimetersPerMeter;
+            var movement =
+                new TrajectoryTrackingMovement
+                {
+                    Trajectory = trajectory,
+                    StateProvider = _stateProvider,
+                    MaximumCommandSpeedMetersPerSecond = 0.50,
+                    CycleObserver = controller =>
+                        RecordControlCycle(
+                            recorder,
+                            controller,
+                            controlPointRadiusMeters)
+                };
+
+            recorder.Start();
+
+            try
+            {
+                _task = new DriveTask(movement.Get());
+                _task.Wait();
+
+                // 保留少量停车后原始Detour数据，并生成一帧处理后的静止状态。
+                Thread.Sleep(400);
+                recorder.UpdateCommand(0f, 0f);
+                if (_stateProvider.TryGetState(
+                        out var stoppedState))
+                {
+                    recorder.UpdateProcessedState(
+                        stoppedState);
+                }
+            }
+            finally
+            {
+                _task?.Stop();
+                recorder.UpdateCommand(0f, 0f);
+                recorder.StopAndSave();
+                _painter.Clear();
+                _task = null;
+                _recorder = null;
+                _stateProvider = null;
+            }
+        }
+
+        /// <summary>
+        /// 停止正在运行的测试、保存已有数据并清除Clumsy轨迹可视化。
+        /// </summary>
+        public override void TestStop()
+        {
+            _task?.Stop();
+            _recorder?.UpdateCommand(0f, 0f);
+            _recorder?.StopAndSave();
+            _painter.Clear();
+            _task = null;
+            _recorder = null;
+            _stateProvider = null;
+        }
+
+        /// <summary>
+        /// 将离散轨迹点和相邻线段从SI单位转换为Clumsy毫米坐标后绘制。
+        /// </summary>
+        private void DrawTrajectory(
+            Trajectory.Trajectory2D trajectory)
+        {
+            _painter.Clear();
+
+            for (var index = 0;
+                 index < trajectory.Count;
+                 index++)
+            {
+                var point = ToMillimeterVector(
+                    trajectory[index].PoseInWorld);
+                _painter.DrawDot(
+                    Color.Cyan,
+                    point.X,
+                    point.Y,
+                    3f);
+
+                if (index == 0)
+                {
+                    continue;
+                }
+
+                var previousPoint = ToMillimeterVector(
+                    trajectory[index - 1].PoseInWorld);
+                _painter.DrawLine(
+                    Color.DeepSkyBlue,
+                    previousPoint.X,
+                    previousPoint.Y,
+                    point.X,
+                    point.Y,
+                    width: 2);
+            }
+
+            var start = ToMillimeterVector(
+                trajectory.StartPoint.PoseInWorld);
+            var end = ToMillimeterVector(
+                trajectory.EndPoint.PoseInWorld);
+            _painter.DrawDot(
+                Color.LimeGreen,
+                start.X,
+                start.Y,
+                8f);
+            _painter.DrawDot(
+                Color.OrangeRed,
+                end.X,
+                end.Y,
+                8f);
+        }
+
+        /// <summary>
+        /// 将控制器本周期使用的状态和最终GCP命令同步给实验记录器。
+        /// </summary>
+        private static void RecordControlCycle(
+            TrackingExperimentRecorder recorder,
+            ParkingGeometricController controller,
+            double controlPointRadiusMeters)
+        {
+            if (controller.LastVehicleState.HasValue)
+            {
+                recorder.UpdateProcessedState(
+                    controller.LastVehicleState.Value);
+            }
+
+            if (!controller.LastCommand.HasValue)
+            {
+                return;
+            }
+
+            if (controller.LastProjection.HasValue &&
+                controller.LastReferenceSpeedMetersPerSecond.HasValue)
+            {
+                var projection =
+                    controller.LastProjection.Value;
+                recorder.UpdateControlReference(
+                    projection.ArcLengthMeters,
+                    controller.LastReferenceSpeedMetersPerSecond.Value,
+                    projection.LateralErrorMeters,
+                    projection.HeadingErrorRadians,
+                    projection.DistanceToTrajectoryMeters,
+                    projection.RemainingDistanceMeters);
+            }
+
+            var command = controller.LastCommand.Value;
+            var curvaturePerMeter = Math.Tan(
+                command.FrontAngleRadians) /
+                controlPointRadiusMeters;
+            var angularSpeedRadiansPerSecond =
+                command.SpeedMetersPerSecond *
+                curvaturePerMeter;
+
+            recorder.UpdateCommand(
+                (float)command.SpeedMetersPerSecond,
+                (float)angularSpeedRadiansPerSecond);
+        }
+
+        /// <summary>
+        /// 将Shared世界坐标系米制位姿转换为Clumsy绘图和旧记录器使用的毫米坐标。
+        /// </summary>
+        private static Vector2 ToMillimeterVector(
+            Pose2D poseInWorld)
+        {
+            return new Vector2(
+                (float)(
+                    poseInWorld.XMeters *
+                    MillimetersPerMeter),
+                (float)(
+                    poseInWorld.YMeters *
+                    MillimetersPerMeter));
+        }
+    }
+}

MultiWheelC/Experiments/TestTrajectoryFactory.cs

@@ -0,0 +1,163 @@
+using System;
+using System.Collections.Generic;
+using MultiWheelC.Trajectory;
+using MyParking.Shared;
+
+namespace MultiWheelC
+{
+    /// <summary>
+    /// 为新版控制器实验生成不依赖正式规划层的简单世界坐标系参考轨迹。
+    /// </summary>
+    public static class TestTrajectoryFactory
+    {
+        private const double StraightLengthMeters = 4.0;
+
+        /// <summary>
+        /// 从给定车体中心位姿沿当前航向生成带梯形速度规划的4m直线轨迹。
+        /// </summary>
+        public static Trajectory2D CreateStraight4Meters(
+            Pose2D startPoseInWorld,
+            double cruiseSpeedMetersPerSecond = 0.30,
+            double accelerationMetersPerSecondSquared = 0.20,
+            double decelerationMetersPerSecondSquared = 0.20,
+            double pointSpacingMeters = 0.02)
+        {
+            EnsureFinitePose(
+                startPoseInWorld,
+                nameof(startPoseInWorld));
+            EnsureFinitePositive(
+                cruiseSpeedMetersPerSecond,
+                nameof(cruiseSpeedMetersPerSecond));
+            EnsureFinitePositive(
+                accelerationMetersPerSecondSquared,
+                nameof(accelerationMetersPerSecondSquared));
+            EnsureFinitePositive(
+                decelerationMetersPerSecondSquared,
+                nameof(decelerationMetersPerSecondSquared));
+            EnsureFinitePositive(
+                pointSpacingMeters,
+                nameof(pointSpacingMeters));
+
+            if (pointSpacingMeters > StraightLengthMeters)
+            {
+                throw new ArgumentOutOfRangeException(
+                    nameof(pointSpacingMeters),
+                    "直线轨迹点间距不能大于轨迹总长度。");
+            }
+
+            var segmentCount = (int)Math.Ceiling(
+                StraightLengthMeters /
+                pointSpacingMeters);
+            var points = new List<TrajectoryPoint>(
+                segmentCount + 1);
+            var directionX = Math.Cos(
+                startPoseInWorld.YawRadians);
+            var directionY = Math.Sin(
+                startPoseInWorld.YawRadians);
+
+            for (var index = 0;
+                 index <= segmentCount;
+                 index++)
+            {
+                // 均分后最后一个点严格落在4m终点，避免浮点累加越界。
+                var arcLengthMeters =
+                    StraightLengthMeters *
+                    index /
+                    segmentCount;
+                var remainingDistanceMeters =
+                    StraightLengthMeters -
+                    arcLengthMeters;
+                var referenceSpeedMetersPerSecond =
+                    CalculateReferenceSpeed(
+                        arcLengthMeters,
+                        remainingDistanceMeters,
+                        cruiseSpeedMetersPerSecond,
+                        accelerationMetersPerSecondSquared,
+                        decelerationMetersPerSecondSquared);
+
+                points.Add(
+                    new TrajectoryPoint(
+                        arcLengthMeters,
+                        new Pose2D(
+                            startPoseInWorld.XMeters +
+                                directionX * arcLengthMeters,
+                            startPoseInWorld.YMeters +
+                                directionY * arcLengthMeters,
+                            startPoseInWorld.YawRadians),
+                        curvaturePerMeter: 0.0,
+                        referenceSpeedMetersPerSecond:
+                            referenceSpeedMetersPerSecond));
+            }
+
+            return new Trajectory2D(points);
+        }
+
+        /// <summary>
+        /// 根据起步、巡航和制动能力计算指定弧长位置允许的参考速度。
+        /// </summary>
+        private static double CalculateReferenceSpeed(
+            double arcLengthMeters,
+            double remainingDistanceMeters,
+            double cruiseSpeedMetersPerSecond,
+            double accelerationMetersPerSecondSquared,
+            double decelerationMetersPerSecondSquared)
+        {
+            // 由v²=2as分别得到从静止起步和到终点静止允许的速度上限。
+            var accelerationLimitedSpeed = Math.Sqrt(
+                2.0 *
+                accelerationMetersPerSecondSquared *
+                Math.Max(0.0, arcLengthMeters));
+            var brakingLimitedSpeed = Math.Sqrt(
+                2.0 *
+                decelerationMetersPerSecondSquared *
+                Math.Max(0.0, remainingDistanceMeters));
+
+            return Math.Min(
+                cruiseSpeedMetersPerSecond,
+                Math.Min(
+                    accelerationLimitedSpeed,
+                    brakingLimitedSpeed));
+        }
+
+        /// <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 EnsureFinitePositive(
+            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/Experiments/TrackingExperimentRecorder.cs

@@ -8,6 +8,7 @@ using System.Numerics;
 using System.Text;
 using System.Threading;
 using MyParking.Shared;
+using MultiWheelC.StateEstimation;
 
 namespace MultiWheelC
 {
@@ -26,6 +27,26 @@ namespace MultiWheelC
         public float CommandVx;
         public float CommandVy;
         public float CommandAngularSpeed;
+
+        // 新版状态估计统一使用SI单位；无有效控制器状态时HasProcessedState为false。
+        public bool HasProcessedState;
+        public double StateTimestampSeconds;
+        public double StateXmeters;
+        public double StateYMeters;
+        public double StateYawRadians;
+        public double StateWorldVxMetersPerSecond;
+        public double StateWorldVyMetersPerSecond;
+        public double StateBodyVxMetersPerSecond;
+        public double StateBodyVyMetersPerSecond;
+        public double StateAngularSpeedRadiansPerSecond;
+        public bool StateVelocityEstimateValid;
+        public bool HasControlReference;
+        public double ControlReferenceArcLengthMeters;
+        public double ControlReferenceSpeedMetersPerSecond;
+        public double ControlLateralErrorMeters;
+        public double ControlHeadingErrorRadians;
+        public double ControlDistanceToTrajectoryMeters;
+        public double ControlRemainingDistanceMeters;
     }
 
     // C层实验工具：统一采集并保存轨迹跟踪实验数据。
@@ -39,6 +60,8 @@ namespace MultiWheelC
         private readonly float _referenceSpeed;
         private readonly float _referenceAngularSpeed;
         private readonly float _referenceMotionFrameYawDegrees;
+        private readonly float _referenceAccelerationMetersPerSecondSquared;
+        private readonly float _referenceDecelerationMetersPerSecondSquared;
         private readonly int _sampleIntervalMs;
 
         private readonly List<TrackingSample> _samples =
@@ -50,6 +73,9 @@ namespace MultiWheelC
         private readonly object _commandSyncRoot =
             new object();
 
+        private readonly object _stateSyncRoot =
+            new object();
+
         private readonly Stopwatch _stopwatch =
             new Stopwatch();
 
@@ -63,6 +89,14 @@ namespace MultiWheelC
         private float _externalCommandVx;
         private float _externalCommandVy;
         private float _externalCommandAngularSpeed;
+        private VehicleState? _latestProcessedState;
+        private bool _hasControlReference;
+        private double _controlReferenceArcLengthMeters;
+        private double _controlReferenceSpeedMetersPerSecond;
+        private double _controlLateralErrorMeters;
+        private double _controlHeadingErrorRadians;
+        private double _controlDistanceToTrajectoryMeters;
+        private double _controlRemainingDistanceMeters;
 
         public TrackingExperimentRecorder(
             string controllerName,
@@ -73,7 +107,9 @@ namespace MultiWheelC
             float referenceSpeed,
             float referenceAngularSpeed = 0f,
             int sampleIntervalMs = 50,
-            float referenceMotionFrameYawDegrees = 0f)
+            float referenceMotionFrameYawDegrees = 0f,
+            float referenceAccelerationMetersPerSecondSquared = 0f,
+            float referenceDecelerationMetersPerSecondSquared = 0f)
         {
             if (string.IsNullOrWhiteSpace(controllerName))
                 throw new ArgumentException(
@@ -99,6 +135,10 @@ namespace MultiWheelC
             _referenceAngularSpeed = referenceAngularSpeed;
             _referenceMotionFrameYawDegrees =
                 referenceMotionFrameYawDegrees;
+            _referenceAccelerationMetersPerSecondSquared =
+                referenceAccelerationMetersPerSecondSquared;
+            _referenceDecelerationMetersPerSecondSquared =
+                referenceDecelerationMetersPerSecondSquared;
             _sampleIntervalMs = sampleIntervalMs;
         }
 
@@ -162,6 +202,46 @@ namespace MultiWheelC
             }
         }
 
+        /// <summary>
+        /// 保存新版控制器本周期实际使用的校验后车辆状态，供后台采样线程写入CSV。
+        /// </summary>
+        public void UpdateProcessedState(VehicleState state)
+        {
+            lock (_stateSyncRoot)
+            {
+                _latestProcessedState = state;
+            }
+        }
+
+        /// <summary>
+        /// 保存新版控制器本周期实际使用的轨迹投影、误差和参考速度。
+        /// </summary>
+        public void UpdateControlReference(
+            double arcLengthMeters,
+            double referenceSpeedMetersPerSecond,
+            double lateralErrorMeters,
+            double headingErrorRadians,
+            double distanceToTrajectoryMeters,
+            double remainingDistanceMeters)
+        {
+            lock (_stateSyncRoot)
+            {
+                _controlReferenceArcLengthMeters =
+                    arcLengthMeters;
+                _controlReferenceSpeedMetersPerSecond =
+                    referenceSpeedMetersPerSecond;
+                _controlLateralErrorMeters =
+                    lateralErrorMeters;
+                _controlHeadingErrorRadians =
+                    headingErrorRadians;
+                _controlDistanceToTrajectoryMeters =
+                    distanceToTrajectoryMeters;
+                _controlRemainingDistanceMeters =
+                    remainingDistanceMeters;
+                _hasControlReference = true;
+            }
+        }
+
         // 停止采样并将本次实验保存为CSV；重复调用只保存一次。
         public void StopAndSave()
         {
@@ -224,6 +304,14 @@ namespace MultiWheelC
                 float commandVx;
                 float commandVy;
                 float commandAngularSpeed;
+                VehicleState? processedState;
+                bool hasControlReference;
+                double controlReferenceArcLengthMeters;
+                double controlReferenceSpeedMetersPerSecond;
+                double controlLateralErrorMeters;
+                double controlHeadingErrorRadians;
+                double controlDistanceToTrajectoryMeters;
+                double controlRemainingDistanceMeters;
 
                 lock (_commandSyncRoot)
                 {
@@ -257,6 +345,26 @@ namespace MultiWheelC
                     }
                 }
 
+                lock (_stateSyncRoot)
+                {
+                    processedState =
+                        _latestProcessedState;
+                    hasControlReference =
+                        _hasControlReference;
+                    controlReferenceArcLengthMeters =
+                        _controlReferenceArcLengthMeters;
+                    controlReferenceSpeedMetersPerSecond =
+                        _controlReferenceSpeedMetersPerSecond;
+                    controlLateralErrorMeters =
+                        _controlLateralErrorMeters;
+                    controlHeadingErrorRadians =
+                        _controlHeadingErrorRadians;
+                    controlDistanceToTrajectoryMeters =
+                        _controlDistanceToTrajectoryMeters;
+                    controlRemainingDistanceMeters =
+                        _controlRemainingDistanceMeters;
+                }
+
                 var sample = new TrackingSample
                 {
                     ElapsedSeconds =
@@ -268,9 +376,50 @@ namespace MultiWheelC
                     CommandVx = commandVx,
                     CommandVy = commandVy,
                     CommandAngularSpeed =
-                        commandAngularSpeed
+                        commandAngularSpeed,
+                    HasProcessedState =
+                        processedState.HasValue,
+                    HasControlReference =
+                        hasControlReference,
+                    ControlReferenceArcLengthMeters =
+                        controlReferenceArcLengthMeters,
+                    ControlReferenceSpeedMetersPerSecond =
+                        controlReferenceSpeedMetersPerSecond,
+                    ControlLateralErrorMeters =
+                        controlLateralErrorMeters,
+                    ControlHeadingErrorRadians =
+                        controlHeadingErrorRadians,
+                    ControlDistanceToTrajectoryMeters =
+                        controlDistanceToTrajectoryMeters,
+                    ControlRemainingDistanceMeters =
+                        controlRemainingDistanceMeters
                 };
 
+                if (processedState.HasValue)
+                {
+                    var state = processedState.Value;
+                    sample.StateTimestampSeconds =
+                        state.SampleTimestampSeconds;
+                    sample.StateXmeters =
+                        state.PoseInWorld.XMeters;
+                    sample.StateYMeters =
+                        state.PoseInWorld.YMeters;
+                    sample.StateYawRadians =
+                        state.PoseInWorld.YawRadians;
+                    sample.StateWorldVxMetersPerSecond =
+                        state.TwistInWorld.VxMetersPerSecond;
+                    sample.StateWorldVyMetersPerSecond =
+                        state.TwistInWorld.VyMetersPerSecond;
+                    sample.StateBodyVxMetersPerSecond =
+                        state.TwistInBody.VxMetersPerSecond;
+                    sample.StateBodyVyMetersPerSecond =
+                        state.TwistInBody.VyMetersPerSecond;
+                    sample.StateAngularSpeedRadiansPerSecond =
+                        state.TwistInBody.OmegaRadiansPerSecond;
+                    sample.StateVelocityEstimateValid =
+                        state.HasValidVelocityEstimate;
+                }
+
                 lock (_sampleSyncRoot)
                 {
                     _samples.Add(sample);
@@ -336,7 +485,27 @@ namespace MultiWheelC
                     "ReferenceEndY," +
                     "ReferenceSpeed," +
                     "ReferenceAngularSpeedRadPerSecond," +
-                    "ReferenceMotionFrameYawDegrees");
+                    "ReferenceMotionFrameYawDegrees," +
+                    "ReferenceAccelerationMetersPerSecondSquared," +
+                    "ReferenceDecelerationMetersPerSecondSquared," +
+                    "HasProcessedState," +
+                    "StateTimestampSeconds," +
+                    "StateXMeters," +
+                    "StateYMeters," +
+                    "StateYawRadians," +
+                    "StateWorldVxMetersPerSecond," +
+                    "StateWorldVyMetersPerSecond," +
+                    "StateBodyVxMetersPerSecond," +
+                    "StateBodyVyMetersPerSecond," +
+                    "StateAngularSpeedRadiansPerSecond," +
+                    "StateVelocityEstimateValid," +
+                    "HasControlReference," +
+                    "ControlReferenceArcLengthMeters," +
+                    "ControlReferenceSpeedMetersPerSecond," +
+                    "ControlLateralErrorMeters," +
+                    "ControlHeadingErrorRadians," +
+                    "ControlDistanceToTrajectoryMeters," +
+                    "ControlRemainingDistanceMeters");
 
                 foreach (var sample in snapshot)
                 {
@@ -363,7 +532,67 @@ namespace MultiWheelC
                         Format(_referenceEnd.Y),
                         Format(_referenceSpeed),
                         Format(_referenceAngularSpeed),
-                        Format(_referenceMotionFrameYawDegrees)));
+                        Format(_referenceMotionFrameYawDegrees),
+                        Format(
+                            _referenceAccelerationMetersPerSecondSquared),
+                        Format(
+                            _referenceDecelerationMetersPerSecondSquared),
+                        sample.HasProcessedState
+                            ? "1"
+                            : "0",
+                        FormatOptional(
+                            sample.HasProcessedState,
+                            sample.StateTimestampSeconds),
+                        FormatOptional(
+                            sample.HasProcessedState,
+                            sample.StateXmeters),
+                        FormatOptional(
+                            sample.HasProcessedState,
+                            sample.StateYMeters),
+                        FormatOptional(
+                            sample.HasProcessedState,
+                            sample.StateYawRadians),
+                        FormatOptional(
+                            sample.HasProcessedState,
+                            sample.StateWorldVxMetersPerSecond),
+                        FormatOptional(
+                            sample.HasProcessedState,
+                            sample.StateWorldVyMetersPerSecond),
+                        FormatOptional(
+                            sample.HasProcessedState,
+                            sample.StateBodyVxMetersPerSecond),
+                        FormatOptional(
+                            sample.HasProcessedState,
+                            sample.StateBodyVyMetersPerSecond),
+                        FormatOptional(
+                            sample.HasProcessedState,
+                            sample.StateAngularSpeedRadiansPerSecond),
+                        sample.HasProcessedState
+                            ? sample.StateVelocityEstimateValid
+                                ? "1"
+                                : "0"
+                            : string.Empty,
+                        sample.HasControlReference
+                            ? "1"
+                            : "0",
+                        FormatOptional(
+                            sample.HasControlReference,
+                            sample.ControlReferenceArcLengthMeters),
+                        FormatOptional(
+                            sample.HasControlReference,
+                            sample.ControlReferenceSpeedMetersPerSecond),
+                        FormatOptional(
+                            sample.HasControlReference,
+                            sample.ControlLateralErrorMeters),
+                        FormatOptional(
+                            sample.HasControlReference,
+                            sample.ControlHeadingErrorRadians),
+                        FormatOptional(
+                            sample.HasControlReference,
+                            sample.ControlDistanceToTrajectoryMeters),
+                        FormatOptional(
+                            sample.HasControlReference,
+                            sample.ControlRemainingDistanceMeters)));
                 }
             }
         }
@@ -392,6 +621,18 @@ namespace MultiWheelC
                 CultureInfo.InvariantCulture);
         }
 
+        /// <summary>
+        /// 在新版状态尚未产生时为空，否则按统一小数格式输出状态数值。
+        /// </summary>
+        private static string FormatOptional(
+            bool hasValue,
+            double value)
+        {
+            return hasValue
+                ? Format(value)
+                : string.Empty;
+        }
+
         // 对CSV文本字段进行引号和逗号转义。
         private static string EscapeCsv(string value)
         {