增加GCP运动学并完善车体平面速度反馈分析

MultiWheelC/Experiments/NewControllerTrackingTests.cs

@@ -224,6 +224,9 @@ namespace MultiWheelC
                     referenceSpeed:
                         (float)CruiseSpeedMetersPerSecond,
                     sampleIntervalMs: 50,
+                    referenceMotionFrameYawDegrees:
+                        (float)AngleMath.RadiansToDegrees(
+                            MotionDirectionInBodyRadians),
                     referenceAccelerationMetersPerSecondSquared:
                         (float)AccelerationMetersPerSecondSquared,
                     referenceDecelerationMetersPerSecondSquared:
@@ -432,6 +435,8 @@ namespace MultiWheelC
                     out var detourVelocityValid,
                     out var rawWheelBodyVx,
                     out var filteredWheelBodyVx,
+                    out var rawWheelBodyVy,
+                    out var filteredWheelBodyVy,
                     out var wheelVelocityValid))
             {
                 return;
@@ -442,6 +447,8 @@ namespace MultiWheelC
                 detourVelocityValid,
                 rawWheelBodyVx,
                 filteredWheelBodyVx,
+                rawWheelBodyVy,
+                filteredWheelBodyVy,
                 wheelVelocityValid);
         }
 
@@ -855,6 +862,8 @@ namespace MultiWheelC
                     out var detourVelocityValid,
                     out var rawWheelBodyVx,
                     out var filteredWheelBodyVx,
+                    out var rawWheelBodyVy,
+                    out var filteredWheelBodyVy,
                     out var wheelVelocityValid))
             {
                 return;
@@ -865,6 +874,8 @@ namespace MultiWheelC
                 detourVelocityValid,
                 rawWheelBodyVx,
                 filteredWheelBodyVx,
+                rawWheelBodyVy,
+                filteredWheelBodyVy,
                 wheelVelocityValid);
         }