fix: accelerate EM trajectories from rest

2026-08-07 07:51:43 +08:00
parent 162f1a24e6
commit 0bba8d7e61
9 changed files with 273 additions and 20 deletions
@@ -26,7 +26,10 @@ public sealed class LongitudinalObjectiveBuilder
        for (int index = 0; index < layout.KnotCount; index++)
        {
            double iteratePathS = Math.Max(0d, Math.Min(input.PathUpperBoundS, iterate.S[index]));
-            AddSquaredResidual(hessian, linearCost, layout.U(index), speedLimit.MaximumSpeedAt(iteratePathS),
+            double referenceSpeed = input.PlanningScope == EmPlanningScope.FullDirectionSegment
+                ? input.KnotSchedule.ReferenceSpeedMetersPerSecond[index]
+                : speedLimit.MaximumSpeedAt(iteratePathS);
+            AddSquaredResidual(hessian, linearCost, layout.U(index), referenceSpeed,
                weights.ReferenceSpeed, speedScale);
            AddSquaredResidual(hessian, linearCost, layout.A(index), 0d, weights.Acceleration, accelerationScale);
            if (index < layout.KnotCount - 1 && index < input.PreviousPathS.Count)