feat: 发布 EM 轨迹规划首个版本

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs

@@ -1,5 +1,6 @@
 using System;
 using System.Collections.Generic;
+using System.Globalization;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
@@ -16,7 +17,15 @@ public sealed class LongitudinalConstraintBuilder
     public bool TryBuild(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate iterate,
         out QuadraticProgram problem, out string failureReason)
     {
-        return TryBuildCore(input, speedLimit, iterate, false, out problem, out failureReason);
+        return TryBuildCore(input, speedLimit, iterate, false, null, null, 0d, out problem, out failureReason);
+    }
+
+    internal bool TryBuildTrusted(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
+        LongitudinalCandidate anchor, LongitudinalEnvelopeTrustRegion trustRegion, double strictTolerance,
+        out QuadraticProgram problem, out string failureReason)
+    {
+        return TryBuildCore(input, speedLimit, anchor, false, trustRegion, anchor, strictTolerance,
+            out problem, out failureReason);
     }
 
     /// <summary>Builds the bounded full-scope feasibility projection before objective optimization.</summary>
@@ -38,12 +47,14 @@ public sealed class LongitudinalConstraintBuilder
             failureReason = "Initial feasibility projection is only defined for full-direction exact-stop planning.";
             return false;
         }
-        return TryBuildCore(input, speedLimit, linearizationIterate, true, out problem,
-            out failureReason);
+        return TryBuildCore(input, speedLimit, linearizationIterate, true, null, null, 0d,
+            out problem, out failureReason);
     }
 
     private bool TryBuildCore(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate iterate,
-        bool useScheduleReferenceObjective, out QuadraticProgram problem, out string failureReason)
+        bool useScheduleReferenceObjective, LongitudinalEnvelopeTrustRegion trustRegion,
+        LongitudinalCandidate trustedAnchor, double strictTolerance, out QuadraticProgram problem,
+        out string failureReason)
     {
         problem = null;
         failureReason = string.Empty;
@@ -63,6 +74,15 @@ public sealed class LongitudinalConstraintBuilder
             {
                 throw new ArgumentException("The ST iterate does not match the configured knot layout.");
             }
+            if ((trustRegion == null) != (trustedAnchor == null))
+                throw new ArgumentException("Trusted QP construction requires both a trust region and anchor.");
+            if (trustRegion != null && (trustRegion.MinimumPathS.Count != layout.KnotCount ||
+                trustRegion.MaximumPathS.Count != layout.KnotCount ||
+                trustRegion.SpeedSlope.Count != layout.KnotCount ||
+                trustRegion.SpeedIntercept.Count != layout.KnotCount))
+            {
+                throw new ArgumentException("The trust region does not match the configured knot layout.");
+            }
             if (!PathSpeedLimitBuilder.TryGetLimits(input, out double directionMaximum, out double maximumAcceleration,
                     out double maximumDeceleration, out double maximumJerk, out _, out _, out failureReason))
             {
@@ -84,12 +104,14 @@ public sealed class LongitudinalConstraintBuilder
                 _objectiveBuilder.AddTerms(input, speedLimit, layout, iterate, hessian, linearCost);
             int stabilizationStart = GetStabilizationStart(input, expectedTimes, layout.KnotCount);
             int stationaryKnotCount = layout.KnotCount - stabilizationStart;
-            int expectedRows = 9 * layout.KnotCount - 3 + 3 * stationaryKnotCount;
+            int expectedRows = 10 * layout.KnotCount - 3 + 3 * stationaryKnotCount;
             var constraints = new SparseTripletBuilder(expectedRows, layout.VariableCount);
             var lower = new List<double>(expectedRows);
             var upper = new List<double>(expectedRows);
             int row = 0;
-            AddVariableBounds(input, speedLimit, iterate, layout, maximumAcceleration, maximumDeceleration, maximumJerk,
+            AddVariableBounds(input, speedLimit, iterate, trustRegion, layout, maximumAcceleration,
+                maximumDeceleration, maximumJerk, constraints, lower, upper, ref row);
+            AddLowSpeedDecelerationReleaseEnvelope(layout, iterate, maximumJerk,
                 constraints, lower, upper, ref row);
             AddMonotonicProgress(layout, constraints, lower, upper, ref row);
             AddExactDynamics(expectedTimes, layout, constraints, lower, upper, ref row);
@@ -99,6 +121,20 @@ public sealed class LongitudinalConstraintBuilder
             if (row != expectedRows)
                 throw new InvalidOperationException("ST constraint row accounting is inconsistent.");
             problem = new QuadraticProgram(hessian.Build(), linearCost, constraints.Build(), lower, upper);
+            if (trustedAnchor != null)
+            {
+                LongitudinalQpAuditResult audit = LongitudinalQpFeasibilityAudit.Evaluate(problem, trustedAnchor,
+                    strictTolerance, layout, stabilizationStart);
+                if (!audit.IsFeasible)
+                {
+                    problem = null;
+                    failureReason = "Planner invariant failure: strict anchor is outside trusted QP" +
+                        ";row=" + audit.WorstRow + ";category=" + audit.Category +
+                        ";residual=" + audit.MaximumResidual.ToString("R", CultureInfo.InvariantCulture) +
+                        audit.Unit + ";tolerance=" + strictTolerance.ToString("R", CultureInfo.InvariantCulture);
+                    return false;
+                }
+            }
             return true;
         }
         catch (ArgumentException exception)
@@ -145,7 +181,8 @@ public sealed class LongitudinalConstraintBuilder
     }
 
     private static void AddVariableBounds(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
-        LongitudinalCandidate iterate, LongitudinalVariableLayout layout, double maximumAcceleration,
+        LongitudinalCandidate iterate, LongitudinalEnvelopeTrustRegion trustRegion,
+        LongitudinalVariableLayout layout, double maximumAcceleration,
         double maximumDeceleration, double maximumJerk, SparseTripletBuilder constraints, IList<double> lower,
         IList<double> upper, ref int row)
     {
@@ -153,14 +190,32 @@ public sealed class LongitudinalConstraintBuilder
         {
             if (iterate.S[index] < 0d || iterate.S[index] > input.PathUpperBoundS)
                 throw new ArgumentException("The ST iterate progress lies outside actual PathS bounds.");
-            AddSingleVariableRow(constraints, lower, upper, layout.S(index), 0d, input.PathUpperBoundS, ref row);
+            if (trustRegion == null)
+                AddSingleVariableRow(constraints, lower, upper, layout.S(index), 0d, input.PathUpperBoundS, ref row);
+            else
+                AddSingleVariableRow(constraints, lower, upper, layout.S(index),
+                    trustRegion.MinimumPathS[index], trustRegion.MaximumPathS[index], ref row);
             double maximumSpeed = index == 0
                 ? input.DirectionMaximumSpeedMetersPerSecond
                 : input.DirectionMaximumSpeedMetersPerSecond;
             AddSingleVariableRow(constraints, lower, upper, layout.U(index), 0d, maximumSpeed, ref row);
             if (index > 0)
-                AddLinearizedSpeedEnvelopeRow(speedLimit, iterate.S[index], layout.S(index), layout.U(index),
-                    constraints, lower, upper, ref row);
+            {
+                if (trustRegion == null)
+                {
+                    AddLinearizedSpeedEnvelopeRow(speedLimit, iterate.S[index], layout.S(index), layout.U(index),
+                        constraints, lower, upper, ref row);
+                }
+                else
+                {
+                    AddRow(constraints, lower, upper, row, new[]
+                    {
+                        new Coefficient(layout.U(index), 1d),
+                        new Coefficient(layout.S(index), -trustRegion.SpeedSlope[index]),
+                    }, -QuadraticProgram.MaximumFiniteBound, trustRegion.SpeedIntercept[index]);
+                    row++;
+                }
+            }
             AddSingleVariableRow(constraints, lower, upper, layout.A(index), -maximumDeceleration, maximumAcceleration,
                 ref row);
         }
@@ -209,6 +264,34 @@ public sealed class LongitudinalConstraintBuilder
         }
     }
 
+    internal static void CalculateLowSpeedDecelerationReleaseTangent(
+        double anchorAcceleration, double maximumJerk,
+        out double accelerationCoefficient, out double lowerBound)
+    {
+        if (!IsFinite(anchorAcceleration) || !IsFinite(maximumJerk) || maximumJerk <= 0d)
+            throw new ArgumentOutOfRangeException(nameof(anchorAcceleration));
+        double a0 = Math.Min(0d, anchorAcceleration);
+        accelerationCoefficient = -a0 / maximumJerk;
+        lowerBound = -(a0 * a0) / (2d * maximumJerk);
+    }
+
+    private static void AddLowSpeedDecelerationReleaseEnvelope(LongitudinalVariableLayout layout,
+        LongitudinalCandidate iterate, double maximumJerk, SparseTripletBuilder constraints,
+        IList<double> lower, IList<double> upper, ref int row)
+    {
+        for (int index = 0; index < layout.KnotCount; index++)
+        {
+            CalculateLowSpeedDecelerationReleaseTangent(iterate.A[index], maximumJerk,
+                out double accelerationCoefficient, out double lowerBound);
+            AddRow(constraints, lower, upper, row, new[]
+            {
+                new Coefficient(layout.U(index), 1d),
+                new Coefficient(layout.A(index), accelerationCoefficient),
+            }, lowerBound, QuadraticProgram.MaximumFiniteBound);
+            row++;
+        }
+    }
+
     private static void AddExactDynamics(IReadOnlyList<double> times, LongitudinalVariableLayout layout,
         SparseTripletBuilder constraints, IList<double> lower, IList<double> upper, ref int row)
     {

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalContinuousProfileValidator.cs

@@ -0,0 +1,136 @@
+using System;
+using System.Collections.Generic;
+using System.Globalization;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Proves the constant-jerk profile used by publication is continuously forward-progressing.</summary>
+internal static class LongitudinalContinuousProfileValidator
+{
+    public static bool TryValidate(LongitudinalCandidate candidate, double tolerance, out string failureReason)
+    {
+        failureReason = string.Empty;
+        if (candidate == null)
+        {
+            failureReason = "A longitudinal candidate is required.";
+            return false;
+        }
+        if (!IsFinite(tolerance) || tolerance < 0d)
+        {
+            failureReason = "A finite nonnegative continuous-profile tolerance is required.";
+            return false;
+        }
+
+        double highWater = candidate.S[0];
+        for (int interval = 0; interval < candidate.J.Count; interval++)
+        {
+            double duration = candidate.KnotTimes[interval + 1] - candidate.KnotTimes[interval];
+            double initialS = candidate.S[interval];
+            double initialU = candidate.U[interval];
+            double initialA = candidate.A[interval];
+            double jerk = candidate.J[interval];
+            var evaluationTimes = new List<double>(5) { 0d, duration };
+            if (jerk != 0d)
+                AddIfInside(evaluationTimes, -initialA / jerk, duration);
+            AddSpeedRoots(evaluationTimes, initialU, initialA, jerk, duration);
+            evaluationTimes.Sort();
+
+            double previousTime = double.NegativeInfinity;
+            for (int point = 0; point < evaluationTimes.Count; point++)
+            {
+                double localTime = evaluationTimes[point];
+                if (localTime == previousTime)
+                    continue;
+                previousTime = localTime;
+                Evaluate(initialS, initialU, initialA, jerk, localTime,
+                    out double progress, out double speed, out double acceleration);
+                double regression = highWater - progress;
+                if (!IsFinite(progress) || !IsFinite(speed) || !IsFinite(acceleration) ||
+                    speed < -tolerance || regression > tolerance)
+                {
+                    string kind = !IsFinite(progress) || !IsFinite(speed) || !IsFinite(acceleration)
+                        ? "non-finite"
+                        : speed < -tolerance ? "negative-speed" : "PathS-regression";
+                    failureReason = FormatFailure(kind, interval, localTime, progress, speed,
+                        acceleration, jerk, regression, string.Empty);
+                    return false;
+                }
+                if (progress > highWater)
+                    highWater = progress;
+            }
+
+            Evaluate(initialS, initialU, initialA, jerk, duration,
+                out double integratedS, out double integratedU, out double integratedA);
+            double sMismatch = Math.Abs(integratedS - candidate.S[interval + 1]);
+            double uMismatch = Math.Abs(integratedU - candidate.U[interval + 1]);
+            double aMismatch = Math.Abs(integratedA - candidate.A[interval + 1]);
+            if (!IsFinite(sMismatch) || !IsFinite(uMismatch) || !IsFinite(aMismatch) ||
+                sMismatch > tolerance || uMismatch > tolerance || aMismatch > tolerance)
+            {
+                string detail = ";endpointMismatchS=" + Invariant(sMismatch) +
+                    ";endpointMismatchU=" + Invariant(uMismatch) +
+                    ";endpointMismatchA=" + Invariant(aMismatch);
+                failureReason = FormatFailure("endpoint-mismatch", interval, duration, integratedS,
+                    integratedU, integratedA, jerk, highWater - integratedS, detail);
+                return false;
+            }
+        }
+        return true;
+    }
+
+    private static void AddSpeedRoots(ICollection<double> times, double initialU, double initialA,
+        double jerk, double duration)
+    {
+        if (jerk == 0d)
+        {
+            if (initialA != 0d)
+                AddIfInside(times, -initialU / initialA, duration);
+            return;
+        }
+
+        double discriminant = initialA * initialA - 2d * jerk * initialU;
+        if (!IsFinite(discriminant) || discriminant < 0d)
+            return;
+        double rootTerm = Math.Sqrt(discriminant);
+        AddIfInside(times, (-initialA - rootTerm) / jerk, duration);
+        AddIfInside(times, (-initialA + rootTerm) / jerk, duration);
+    }
+
+    private static void AddIfInside(ICollection<double> times, double localTime, double duration)
+    {
+        if (IsFinite(localTime) && localTime >= 0d && localTime <= duration)
+            times.Add(localTime);
+    }
+
+    private static void Evaluate(double initialS, double initialU, double initialA, double jerk,
+        double localTime, out double progress, out double speed, out double acceleration)
+    {
+        acceleration = initialA + jerk * localTime;
+        speed = initialU + initialA * localTime + 0.5d * jerk * localTime * localTime;
+        progress = initialS + initialU * localTime + 0.5d * initialA * localTime * localTime +
+            jerk * localTime * localTime * localTime / 6d;
+    }
+
+    private static string FormatFailure(string kind, int interval, double localTime, double progress,
+        double speed, double acceleration, double jerk, double regression, string detail)
+    {
+        return "Continuous ST profile rejected: kind=" + kind +
+            ";interval=" + interval.ToString(CultureInfo.InvariantCulture) +
+            ";localTime=" + Invariant(localTime) +
+            ";S=" + Invariant(progress) +
+            ";U=" + Invariant(speed) +
+            ";A=" + Invariant(acceleration) +
+            ";J=" + Invariant(jerk) +
+            ";regression=" + Invariant(regression) + detail + ".";
+    }
+
+    private static string Invariant(double value)
+    {
+        return value.ToString("R", CultureInfo.InvariantCulture);
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalEnvelopeTrustRegion.cs

@@ -0,0 +1,274 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal sealed class LongitudinalEnvelopeTrustRegion
+{
+    internal LongitudinalEnvelopeTrustRegion(IReadOnlyList<double> minimumPathS, IReadOnlyList<double> maximumPathS,
+        IReadOnlyList<double> speedSlope, IReadOnlyList<double> speedIntercept,
+        IReadOnlyList<int> envelopeSegmentIndex, double scale)
+    {
+        MinimumPathS = Copy(minimumPathS, nameof(minimumPathS));
+        MaximumPathS = Copy(maximumPathS, nameof(maximumPathS));
+        SpeedSlope = Copy(speedSlope, nameof(speedSlope));
+        SpeedIntercept = Copy(speedIntercept, nameof(speedIntercept));
+        EnvelopeSegmentIndex = Copy(envelopeSegmentIndex, nameof(envelopeSegmentIndex));
+        Scale = scale;
+    }
+
+    internal IReadOnlyList<double> MinimumPathS { get; }
+
+    internal IReadOnlyList<double> MaximumPathS { get; }
+
+    internal IReadOnlyList<double> SpeedSlope { get; }
+
+    internal IReadOnlyList<double> SpeedIntercept { get; }
+
+    internal IReadOnlyList<int> EnvelopeSegmentIndex { get; }
+
+    internal double Scale { get; }
+
+    internal bool CanShrinkTo(double nextScale, double minimumActiveWidthMeters, out string failureReason)
+    {
+        failureReason = string.Empty;
+        if (!IsFinite(nextScale) || nextScale <= 0d || nextScale >= Scale)
+        {
+            failureReason = "Next trust-region scale must be finite, positive, and smaller than the current scale.";
+            return false;
+        }
+        if (!IsFinite(minimumActiveWidthMeters) || minimumActiveWidthMeters <= 0d)
+        {
+            failureReason = "Minimum active width must be finite and positive.";
+            return false;
+        }
+        double ratio = nextScale / Scale;
+        for (int index = 0; index < MinimumPathS.Count; index++)
+        {
+            double currentWidth = MaximumPathS[index] - MinimumPathS[index];
+            if (currentWidth > 0d && currentWidth * ratio < minimumActiveWidthMeters)
+            {
+                failureReason = "The next trust-region scale would fall below the minimum width at knot " + index + ".";
+                return false;
+            }
+        }
+        return true;
+    }
+
+    private static IReadOnlyList<double> Copy(IReadOnlyList<double> source, string parameterName)
+    {
+        if (source == null)
+            throw new ArgumentNullException(parameterName);
+        var copy = new List<double>(source.Count);
+        for (int index = 0; index < source.Count; index++)
+            copy.Add(source[index]);
+        return new ReadOnlyCollection<double>(copy);
+    }
+
+    private static IReadOnlyList<int> Copy(IReadOnlyList<int> source, string parameterName)
+    {
+        if (source == null)
+            throw new ArgumentNullException(parameterName);
+        var copy = new List<int>(source.Count);
+        for (int index = 0; index < source.Count; index++)
+            copy.Add(source[index]);
+        return new ReadOnlyCollection<int>(copy);
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}
+
+internal sealed class LongitudinalEnvelopeTrustRegionBuilder
+{
+    private const double ScheduleProgressTolerance = 1e-12d;
+    private const double StationSelectionTolerance = 1e-12d;
+
+    internal bool TryBuild(PathSpeedLimit speedLimit, LongitudinalCandidate anchor,
+        IReadOnlyList<double> referencePathS, int terminalHoldStartIndex, double scale,
+        double minimumActiveWidthMeters, out LongitudinalEnvelopeTrustRegion region,
+        out string failureReason)
+    {
+        region = null;
+        failureReason = string.Empty;
+        if (speedLimit == null || anchor == null || referencePathS == null)
+        {
+            failureReason = "Trust region inputs must be present.";
+            return false;
+        }
+        if (!IsSupportedScale(scale))
+        {
+            failureReason = "Trust-region scale must be one of 1, 0.5, 0.25, or 0.125.";
+            return false;
+        }
+        if (!IsFinite(minimumActiveWidthMeters) || minimumActiveWidthMeters <= 0d)
+        {
+            failureReason = "Minimum active width must be finite and positive.";
+            return false;
+        }
+        if (referencePathS.Count != anchor.S.Count)
+        {
+            failureReason = "Reference PathS count must match the anchor knot count.";
+            return false;
+        }
+        if (terminalHoldStartIndex == -1)
+            terminalHoldStartIndex = anchor.S.Count;
+        if (terminalHoldStartIndex < 1 || terminalHoldStartIndex > anchor.S.Count)
+        {
+            failureReason = "Terminal-hold start index is outside the anchor knot range.";
+            return false;
+        }
+        if (!TryValidateAnchorAndReference(speedLimit, anchor, referencePathS, out failureReason))
+            return false;
+
+        int knotCount = anchor.S.Count;
+        var minimumPathS = new double[knotCount];
+        var maximumPathS = new double[knotCount];
+        var speedSlope = new double[knotCount];
+        var speedIntercept = new double[knotCount];
+        var segmentIndex = new int[knotCount];
+        for (int index = 0; index < knotCount; index++)
+        {
+            double anchorS = anchor.S[index];
+            bool fixedKnot = index == 0 || index >= terminalHoldStartIndex;
+            int segment = SelectSegment(speedLimit, anchorS, referencePathS, index, fixedKnot);
+            FindMaximalExactAffineRun(speedLimit, segment, out int firstSegment, out int lastSegment,
+                out double slope, out double intercept);
+            double lower = speedLimit.PathS[firstSegment];
+            double upper = speedLimit.PathS[lastSegment + 1];
+            if (anchorS < lower && lower - anchorS <= StationSelectionTolerance)
+                lower = anchorS;
+            if (anchorS > upper && anchorS - upper <= StationSelectionTolerance)
+                upper = anchorS;
+            double trustedLower = fixedKnot ? anchorS : anchorS - scale * (anchorS - lower);
+            double trustedUpper = fixedKnot ? anchorS : anchorS + scale * (upper - anchorS);
+            if (!fixedKnot && scale < 1d && trustedUpper - trustedLower < minimumActiveWidthMeters)
+            {
+                failureReason = "Active trust-region interval is narrower than the minimum width at knot " + index + ".";
+                return false;
+            }
+
+            minimumPathS[index] = trustedLower;
+            maximumPathS[index] = trustedUpper;
+            speedSlope[index] = slope;
+            speedIntercept[index] = intercept;
+            segmentIndex[index] = segment;
+        }
+
+        region = new LongitudinalEnvelopeTrustRegion(minimumPathS, maximumPathS, speedSlope, speedIntercept,
+            segmentIndex, scale);
+        return true;
+    }
+
+    private static bool TryValidateAnchorAndReference(PathSpeedLimit speedLimit, LongitudinalCandidate anchor,
+        IReadOnlyList<double> referencePathS, out string failureReason)
+    {
+        if (anchor.S[0] != 0d)
+        {
+            failureReason = "The anchor must begin at exact PathS zero.";
+            return false;
+        }
+
+        double minimumPathS = speedLimit.PathS[0];
+        double maximumPathS = speedLimit.PathS[speedLimit.PathS.Count - 1];
+        double previousAnchorS = double.NegativeInfinity;
+        for (int index = 0; index < anchor.S.Count; index++)
+        {
+            double anchorS = anchor.S[index];
+            double referenceS = referencePathS[index];
+            if (!IsFinite(anchorS) || anchorS < minimumPathS || anchorS > maximumPathS)
+            {
+                failureReason = "Anchor PathS is outside the speed-limit range at knot " + index + ".";
+                return false;
+            }
+            if (anchorS < previousAnchorS)
+            {
+                failureReason = "Anchor PathS must be nondecreasing.";
+                return false;
+            }
+            if (!IsFinite(referenceS))
+            {
+                failureReason = "Reference PathS must be finite.";
+                return false;
+            }
+            previousAnchorS = anchorS;
+        }
+
+        failureReason = string.Empty;
+        return true;
+    }
+
+    private static int SelectSegment(PathSpeedLimit speedLimit, double anchorS, IReadOnlyList<double> referencePathS,
+        int knotIndex, bool terminalHold)
+    {
+        int lastSegment = speedLimit.PathS.Count - 2;
+        if (Math.Abs(anchorS - speedLimit.PathS[0]) <= StationSelectionTolerance)
+            return 0;
+        if (Math.Abs(anchorS - speedLimit.PathS[speedLimit.PathS.Count - 1]) <= StationSelectionTolerance)
+            return lastSegment;
+        for (int index = 1; index < speedLimit.PathS.Count - 1; index++)
+        {
+            if (Math.Abs(anchorS - speedLimit.PathS[index]) <= StationSelectionTolerance)
+            {
+                if (terminalHold)
+                    return index;
+                double scheduleDelta = referencePathS[knotIndex] - referencePathS[knotIndex - 1];
+                if (scheduleDelta > ScheduleProgressTolerance)
+                    return index;
+                if (scheduleDelta < -ScheduleProgressTolerance)
+                    return index - 1;
+                double leftWidth = speedLimit.PathS[index] - speedLimit.PathS[index - 1];
+                double rightWidth = speedLimit.PathS[index + 1] - speedLimit.PathS[index];
+                return rightWidth >= leftWidth ? index : index - 1;
+            }
+            if (anchorS < speedLimit.PathS[index])
+                return index - 1;
+        }
+        return lastSegment;
+    }
+
+    private static void GetAffineLine(PathSpeedLimit limit, int segment,
+        out double slope, out double intercept)
+    {
+        double lower = limit.PathS[segment];
+        double upper = limit.PathS[segment + 1];
+        slope = (limit.MaximumSpeedMetersPerSecond[segment + 1] -
+            limit.MaximumSpeedMetersPerSecond[segment]) / (upper - lower);
+        intercept = limit.MaximumSpeedMetersPerSecond[segment] - slope * lower;
+    }
+
+    private static void FindMaximalExactAffineRun(PathSpeedLimit limit, int selectedSegment,
+        out int firstSegment, out int lastSegment, out double slope, out double intercept)
+    {
+        GetAffineLine(limit, selectedSegment, out slope, out intercept);
+        firstSegment = selectedSegment;
+        while (firstSegment > 0)
+        {
+            GetAffineLine(limit, firstSegment - 1, out double candidateSlope, out double candidateIntercept);
+            if (candidateSlope != slope || candidateIntercept != intercept)
+                break;
+            firstSegment--;
+        }
+        lastSegment = selectedSegment;
+        while (lastSegment < limit.PathS.Count - 2)
+        {
+            GetAffineLine(limit, lastSegment + 1, out double candidateSlope, out double candidateIntercept);
+            if (candidateSlope != slope || candidateIntercept != intercept)
+                break;
+            lastSegment++;
+        }
+    }
+
+    private static bool IsSupportedScale(double scale)
+    {
+        return scale == 1d || scale == 0.5d || scale == 0.25d || scale == 0.125d;
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalQpFeasibilityAudit.cs

@@ -0,0 +1,217 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal sealed class LongitudinalQpAuditResult
+{
+    internal LongitudinalQpAuditResult(bool isFeasible, double maximumResidual, int worstRow,
+        string category, string unit)
+    {
+        IsFeasible = isFeasible;
+        MaximumResidual = maximumResidual;
+        WorstRow = worstRow;
+        Category = category;
+        Unit = unit;
+    }
+
+    internal bool IsFeasible { get; }
+
+    internal double MaximumResidual { get; }
+
+    internal int WorstRow { get; }
+
+    internal string Category { get; }
+
+    internal string Unit { get; }
+}
+
+internal static class LongitudinalQpFeasibilityAudit
+{
+    internal static LongitudinalQpAuditResult Evaluate(QuadraticProgram problem,
+        LongitudinalCandidate candidate, double tolerance, LongitudinalVariableLayout layout,
+        int stabilizationStart)
+    {
+        if (problem == null)
+            throw new ArgumentNullException(nameof(problem));
+        if (candidate == null)
+            throw new ArgumentNullException(nameof(candidate));
+        if (layout == null)
+            throw new ArgumentNullException(nameof(layout));
+        if (problem.VariableCount != layout.VariableCount || candidate.S.Count != layout.KnotCount ||
+            candidate.U.Count != layout.KnotCount || candidate.A.Count != layout.KnotCount ||
+            candidate.J.Count != layout.KnotCount - 1)
+        {
+            throw new ArgumentException("The QP, candidate, and longitudinal layout must have matching dimensions.");
+        }
+
+        double[] primal = ToPrimal(candidate, layout);
+        var activity = new double[problem.ConstraintCount];
+        bool allFinite = IsFinite(tolerance);
+        SparseCscMatrix matrix = problem.ConstraintMatrix;
+        for (int column = 0; column < matrix.ColumnCount; column++)
+        {
+            double value = primal[column];
+            allFinite &= IsFinite(value);
+            for (int entry = matrix.ColumnPointers[column]; entry < matrix.ColumnPointers[column + 1]; entry++)
+                activity[matrix.RowIndices[entry]] += matrix.Values[entry] * value;
+        }
+
+        double maximumResidual = 0d;
+        int worstRow = problem.ConstraintCount == 0 ? -1 : 0;
+        for (int row = 0; row < problem.ConstraintCount; row++)
+        {
+            double residual;
+            if (!IsFinite(activity[row]))
+            {
+                allFinite = false;
+                residual = double.PositiveInfinity;
+            }
+            else
+            {
+                residual = Math.Max(0d, Math.Max(
+                    problem.LowerBounds[row] - activity[row],
+                    activity[row] - problem.UpperBounds[row]));
+            }
+            if (row == 0 || residual > maximumResidual)
+            {
+                maximumResidual = residual;
+                worstRow = row;
+            }
+        }
+
+        DescribeRow(worstRow, layout.KnotCount, stabilizationStart, out string category, out string unit);
+        return new LongitudinalQpAuditResult(allFinite && maximumResidual <= tolerance,
+            maximumResidual, worstRow, category, unit);
+    }
+
+    private static double[] ToPrimal(LongitudinalCandidate candidate, LongitudinalVariableLayout layout)
+    {
+        var primal = new double[layout.VariableCount];
+        for (int index = 0; index < layout.KnotCount; index++)
+        {
+            primal[layout.S(index)] = candidate.S[index];
+            primal[layout.U(index)] = candidate.U[index];
+            primal[layout.A(index)] = candidate.A[index];
+        }
+        for (int index = 0; index < layout.KnotCount - 1; index++)
+            primal[layout.J(index)] = candidate.J[index];
+        return primal;
+    }
+
+    private static void DescribeRow(int targetRow, int knotCount, int stabilizationStart,
+        out string category, out string unit)
+    {
+        int row = 0;
+        for (int index = 0; index < knotCount; index++)
+        {
+            if (targetRow == row++)
+            {
+                category = "PathS trust";
+                unit = "m";
+                return;
+            }
+            if (targetRow == row++)
+            {
+                category = "speed";
+                unit = "m/s";
+                return;
+            }
+            if (index > 0 && targetRow == row++)
+            {
+                category = "speed envelope";
+                unit = "m/s";
+                return;
+            }
+            if (targetRow == row++)
+            {
+                category = "acceleration";
+                unit = "m/s^2";
+                return;
+            }
+        }
+
+        for (int index = 0; index < knotCount - 1; index++)
+        {
+            if (targetRow == row++)
+            {
+                category = "jerk";
+                unit = "m/s^3";
+                return;
+            }
+        }
+        for (int index = 0; index < knotCount; index++)
+        {
+            if (targetRow == row++)
+            {
+                category = "low-speed deceleration release";
+                unit = "m/s";
+                return;
+            }
+        }
+        for (int index = 0; index < knotCount - 1; index++)
+        {
+            if (targetRow == row++)
+            {
+                category = "monotonic progress";
+                unit = "m";
+                return;
+            }
+        }
+        for (int index = 0; index < knotCount - 1; index++)
+        {
+            if (targetRow == row++)
+            {
+                category = "exact dynamics";
+                unit = "m/s^2";
+                return;
+            }
+            if (targetRow == row++)
+            {
+                category = "exact dynamics";
+                unit = "m/s";
+                return;
+            }
+            if (targetRow == row++)
+            {
+                category = "exact dynamics";
+                unit = "m";
+                return;
+            }
+        }
+
+        string[] stateUnits = { "m", "m/s", "m/s^2" };
+        for (int state = 0; state < stateUnits.Length; state++)
+        {
+            if (targetRow == row++)
+            {
+                category = "exact start";
+                unit = stateUnits[state];
+                return;
+            }
+        }
+
+        int stopKnotCount = stabilizationStart >= 0 && stabilizationStart < knotCount
+            ? knotCount - stabilizationStart
+            : 0;
+        for (int index = 0; index < stopKnotCount; index++)
+        {
+            for (int state = 0; state < stateUnits.Length; state++)
+            {
+                if (targetRow == row++)
+                {
+                    category = "exact stop";
+                    unit = stateUnits[state];
+                    return;
+                }
+            }
+        }
+
+        category = "row accounting";
+        unit = string.Empty;
+    }
+
+    private static bool IsFinite(double value)
+    {
+        return !double.IsNaN(value) && !double.IsInfinity(value);
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs

@@ -1,5 +1,6 @@
 using System;
 using System.Collections.Generic;
+using System.Globalization;
 
 namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 
@@ -48,6 +49,8 @@ public sealed class LongitudinalSolutionValidator
                 failureReason = "ST candidate violates exact constant-jerk dynamics.";
                 return false;
             }
+            if (!LongitudinalContinuousProfileValidator.TryValidate(candidate, tolerance, out failureReason))
+                return false;
             if (!AreClose(candidate.S[0], 0d, tolerance) ||
                 !AreClose(candidate.U[0], input.InitialProgressSpeedMetersPerSecond, tolerance) ||
                 !AreClose(candidate.A[0], input.InitialAccelerationMetersPerSecondSquared, tolerance))
@@ -56,6 +59,9 @@ public sealed class LongitudinalSolutionValidator
                 return false;
             }
 
+            var canonicalS = new double[candidate.S.Count];
+            var canonicalU = new double[candidate.U.Count];
+            var canonicalA = new double[candidate.A.Count];
             for (int index = 0; index < candidate.S.Count; index++)
             {
                 double progress = candidate.S[index];
@@ -83,6 +89,11 @@ public sealed class LongitudinalSolutionValidator
                     failureReason = "ST candidate PathS decreases at knot " + index + ".";
                     return false;
                 }
+                canonicalS[index] = progress;
+                canonicalU[index] = speed < 0d ? 0d : speed;
+                canonicalA[index] = speed < 0d && acceleration < 0d && acceleration >= -tolerance
+                    ? 0d
+                    : acceleration;
             }
             for (int index = 0; index < candidate.J.Count; index++)
             {
@@ -122,25 +133,29 @@ public sealed class LongitudinalSolutionValidator
             {
                 for (int index = 0; index < candidate.S.Count; index++)
                 {
-                    if (!JerkLimitedStoppingMath.TryCalculate(candidate.U[index], candidate.A[index],
-                            maximumDeceleration, maximumJerk, out JerkLimitedStoppingProfile stop, out _) ||
-                        candidate.S[index] + stop.DistanceMeters > input.StopBoundaryPathS + tolerance)
+                    bool hasStop = JerkLimitedStoppingMath.TryCalculate(canonicalU[index], canonicalA[index],
+                        maximumDeceleration, maximumJerk, out JerkLimitedStoppingProfile stop,
+                        out string stoppingFailure);
+                    double stopDistance = hasStop ? stop.DistanceMeters : double.NaN;
+                    double margin = hasStop
+                        ? input.StopBoundaryPathS - canonicalS[index] - stopDistance
+                        : double.NaN;
+                    if (!hasStop || canonicalS[index] + stopDistance > input.StopBoundaryPathS + tolerance)
                     {
-                        failureReason = "ST candidate leaves the jerk-limited stoppable set at knot " + index + ".";
+                        failureReason = "ST candidate leaves the jerk-limited stoppable set: knot=" +
+                            index.ToString(CultureInfo.InvariantCulture) +
+                            ";S=" + Invariant(canonicalS[index]) +
+                            ";U=" + Invariant(canonicalU[index]) +
+                            ";A=" + Invariant(canonicalA[index]) +
+                            ";stopDistance=" + Invariant(stopDistance) +
+                            ";stopBoundary=" + Invariant(input.StopBoundaryPathS) +
+                            ";margin=" + Invariant(margin) +
+                            (hasStop ? string.Empty : ";stoppingReason=" + stoppingFailure) + ".";
                         return false;
                     }
                 }
             }
 
-            var canonicalS = new double[candidate.S.Count];
-            var canonicalU = new double[candidate.U.Count];
-            var canonicalA = new double[candidate.A.Count];
-            for (int index = 0; index < candidate.S.Count; index++)
-            {
-                canonicalS[index] = candidate.S[index];
-                canonicalU[index] = candidate.U[index];
-                canonicalA[index] = candidate.A[index];
-            }
             canonicalS[0] = 0d;
             canonicalU[0] = input.InitialProgressSpeedMetersPerSecond;
             canonicalA[0] = input.InitialAccelerationMetersPerSecondSquared;
@@ -202,6 +217,11 @@ public sealed class LongitudinalSolutionValidator
         return Math.Abs(actual - expected) <= tolerance;
     }
 
+    private static string Invariant(double value)
+    {
+        return value.ToString("R", CultureInfo.InvariantCulture);
+    }
+
     private static double RequireNonnegative(double value, string parameterName)
     {
         if (!IsFinite(value) || value < 0d)

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolveTrace.cs

@@ -0,0 +1,47 @@
+using System;
+using System.Collections.Generic;
+using System.Globalization;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal sealed class LongitudinalSolveTrace
+{
+    private const int MaximumEntries = 12;
+    private readonly List<string> _entries = new List<string>(MaximumEntries);
+
+    internal void Add(string phase, int callOrdinal, int anchorUpdateIndex, double trustScale,
+        TimeSpan remainingBudget, TimeSpan remainingAfterReserve, TimeSpan elapsed, QpSolveResult result,
+        double anchorObjective, double candidateObjective, string rejection)
+    {
+        if (_entries.Count >= MaximumEntries)
+            throw new InvalidOperationException("Longitudinal solve trace exceeded the twelve-call cap.");
+        string status = result == null ? "null" : result.Status.ToString();
+        int iterations = result == null ? -1 : result.Iterations;
+        double primal = result == null ? double.NaN : result.PrimalResidual;
+        double dual = result == null ? double.NaN : result.DualResidual;
+        _entries.Add("phase:" + phase +
+            ",call:" + callOrdinal.ToString(CultureInfo.InvariantCulture) +
+            ",anchor:" + anchorUpdateIndex.ToString(CultureInfo.InvariantCulture) +
+            ",scale:" + trustScale.ToString("R", CultureInfo.InvariantCulture) +
+            ",budgetMs:" + remainingBudget.TotalMilliseconds.ToString("F3", CultureInfo.InvariantCulture) +
+            ",remainingAfterReserveMs:" + remainingAfterReserve.TotalMilliseconds.ToString(
+                "F3", CultureInfo.InvariantCulture) +
+            ",elapsedMs:" + elapsed.TotalMilliseconds.ToString("F3", CultureInfo.InvariantCulture) +
+            ",status:" + status +
+            ",iterations:" + iterations.ToString(CultureInfo.InvariantCulture) +
+            ",primal:" + primal.ToString("R", CultureInfo.InvariantCulture) +
+            ",dual:" + dual.ToString("R", CultureInfo.InvariantCulture) +
+            ",anchorObj:" + anchorObjective.ToString("R", CultureInfo.InvariantCulture) +
+            ",candidateObj:" + candidateObjective.ToString("R", CultureInfo.InvariantCulture) +
+            ",rejection:" + Sanitize(rejection));
+    }
+
+    internal string Format() => string.Join("|", _entries);
+
+    private static string Sanitize(string text)
+    {
+        if (string.IsNullOrEmpty(text))
+            return "none";
+        return text.Replace(';', '/').Replace('|', '/').Replace('\r', '/').Replace('\n', '/');
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs

@@ -10,13 +10,21 @@ namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
 /// <summary>Bounded ST envelope iteration retaining only independently validated physical candidates.</summary>
 public sealed class SequentialLongitudinalOptimizer
 {
-    private const int MaximumEnvelopeIterations = 5;
-    private const double OrdinaryEnvelopeProbeLookaheadSteps = 1d;
-    private const double OrdinaryTerminalProbeFraction = 0.5d;
+    private const int MaximumAcceptedAnchorUpdates = 5;
+    private const int MaximumQpSolveCalls = 12;
+    private const double MinimumTrustRegionWidthMeters = 0.001d;
+    private const double ObjectiveAcceptanceRelativeTolerance = 1e-9d;
+    private const double HighPrecisionRetryTolerance = 1e-7d;
+    private const double StaticStartSeedBudgetFraction = 0.10d;
+    private static readonly TimeSpan MaximumStaticStartSeedBudget = TimeSpan.FromMilliseconds(250d);
+    private static readonly TimeSpan PublicationReserve = TimeSpan.FromMilliseconds(250d);
+    private static readonly double[] TrustRegionScales = { 1d, 0.5d, 0.25d, 0.125d };
     private readonly IQpSolver _qpSolver;
     private readonly PathSpeedLimitBuilder _speedLimitBuilder;
     private readonly LongitudinalConstraintBuilder _constraintBuilder;
     private readonly LongitudinalSolutionValidator _solutionValidator;
+    private readonly LongitudinalEnvelopeTrustRegionBuilder _trustRegionBuilder =
+        new LongitudinalEnvelopeTrustRegionBuilder();
 
     public SequentialLongitudinalOptimizer(IQpSolver qpSolver)
         : this(qpSolver, new PathSpeedLimitBuilder(), new LongitudinalConstraintBuilder(new LongitudinalObjectiveBuilder()),
@@ -65,133 +73,418 @@ public sealed class SequentialLongitudinalOptimizer
             return Failed(speedStatus, speedFailure);
 
         var stopwatch = Stopwatch.StartNew();
-        LongitudinalCandidate iterate;
-        LongitudinalCandidate lastStrictCandidate = null;
-        int remainingObjectiveIterations = iterationLimit;
+        var solveTrace = new LongitudinalSolveTrace();
+        LongitudinalCandidate initialCandidate;
+        int projectionSolveCount = 0;
         if (input.PlanningScope == EmPlanningScope.FullDirectionSegment &&
             input.Mode == EmLongitudinalMode.ExactStopAtBoundary)
         {
             if (!TryCreateInitialFeasibleCandidate(input, speedLimit, settings, totalBudget, convergenceTolerance,
-                    iterationLimit, stopwatch, cancellationToken, out iterate, out int projectionSolveCount,
-                    out EmPlanningStatus projectionStatus, out string projectionFailure))
+                    iterationLimit, stopwatch, solveTrace, cancellationToken, out initialCandidate,
+                    out int usedProjectionSolveCount, out EmPlanningStatus projectionStatus,
+                    out string projectionFailure))
             {
-                return Failed(projectionStatus, projectionFailure);
-            }
-            lastStrictCandidate = CopyCandidate(iterate);
-            remainingObjectiveIterations -= projectionSolveCount;
-            if (remainingObjectiveIterations <= 0)
-            {
-                return new LongitudinalPlanningResult(EmPlanningStatus.SuccessWithFallback, lastStrictCandidate,
-                    "The strict initial feasibility projection consumed the configured outer-iteration budget.");
+                string projectionTrace = solveTrace.Format();
+                return Failed(projectionStatus, projectionFailure +
+                    (string.IsNullOrEmpty(projectionTrace) ? string.Empty : ";solveTrace=" + projectionTrace));
             }
+            projectionSolveCount = usedProjectionSolveCount;
         }
         else
         {
-            iterate = CreateInitialIterate(input, speedLimit);
-            if (!_solutionValidator.TryValidate(input, speedLimit, iterate, out lastStrictCandidate, out _))
-                lastStrictCandidate = null;
+            LongitudinalCandidate seed = CreateInitialIterate(input, speedLimit);
+            if (!_solutionValidator.TryValidate(input, speedLimit, seed, out initialCandidate,
+                    out EmPlanningStatus initializationStatus, out string initializationFailure))
+            {
+                return Failed(initializationStatus, "No strictly validated longitudinal candidate was found. " +
+                    initializationFailure);
+            }
         }
-        double[] warmStart = ToPrimal(iterate);
-        bool hasDynamicsConsistentInitialWarmStart = iterate.SatisfiesExactDiscreteDynamics(1e-12d);
-        string lastCandidateRejection = string.Empty;
-        bool hasPreviousObjective = false;
-        double previousObjective = 0d;
 
-        for (int iteration = 0; iteration < remainingObjectiveIterations; iteration++)
+        LongitudinalCandidate anchor = CopyCandidate(initialCandidate);
+        int stabilizationStart = GetStabilizationStart(input);
+        int qpSolveCount = projectionSolveCount;
+        int trustShrinkCount = 0;
+        int acceptedAnchorCount = 0;
+        int acceptedUpdateLimit = Math.Min(MaximumAcceptedAnchorUpdates, iterationLimit);
+        double finalTrustScale = 1d;
+        string lastRejection = string.Empty;
+
+        while (acceptedAnchorCount < acceptedUpdateLimit && qpSolveCount < MaximumQpSolveCalls)
         {
+            bool promoted = false;
+            for (int scaleIndex = 0; scaleIndex < TrustRegionScales.Length; scaleIndex++)
+            {
+                double scale = TrustRegionScales[scaleIndex];
+                finalTrustScale = scale;
+                if (cancellationToken.IsCancellationRequested || totalBudget - stopwatch.Elapsed <= TimeSpan.Zero)
+                {
+                    return FinishFromAnchor(anchor, acceptedAnchorCount, qpSolveCount, trustShrinkCount,
+                        finalTrustScale, cancellationToken.IsCancellationRequested, lastRejection, solveTrace);
+                }
+                if (!_trustRegionBuilder.TryBuild(speedLimit, anchor, input.KnotSchedule.ReferencePathS,
+                        stabilizationStart, scale, MinimumTrustRegionWidthMeters,
+                        out LongitudinalEnvelopeTrustRegion region, out string regionFailure))
+                {
+                    lastRejection = regionFailure;
+                    break;
+                }
+                if (!_constraintBuilder.TryBuildTrusted(input, speedLimit, anchor, region,
+                        convergenceTolerance, out QuadraticProgram problem, out string buildFailure))
+                {
+                    lastRejection = buildFailure;
+                    break;
+                }
+                TrustedSolveAttempt attempt = SolveTrustedProblem(problem, input, speedLimit, anchor, settings,
+                    totalBudget, stopwatch, convergenceTolerance, stabilizationStart,
+                    MaximumQpSolveCalls - qpSolveCount, solveTrace, qpSolveCount, acceptedAnchorCount,
+                    scale, acceptedAnchorCount > 0, cancellationToken);
+                qpSolveCount += attempt.SolveCount;
+                lastRejection = attempt.FailureReason;
+                if (attempt.Status == EmPlanningStatus.Cancelled)
+                {
+                    return Failed(EmPlanningStatus.Cancelled, CreateRunDiagnostic(qpSolveCount,
+                        trustShrinkCount, acceptedAnchorCount, finalTrustScale, lastRejection, solveTrace));
+                }
+                if (attempt.Accepted)
+                {
+                    anchor = CopyCandidate(attempt.Candidate);
+                    acceptedAnchorCount++;
+                    promoted = true;
+                    break;
+                }
+                if (attempt.Status != EmPlanningStatus.SuccessWithFallback)
+                {
+                    return FinishFromAnchor(anchor, acceptedAnchorCount, qpSolveCount, trustShrinkCount,
+                        finalTrustScale, false, lastRejection, solveTrace);
+                }
+                if (scaleIndex + 1 < TrustRegionScales.Length)
+                {
+                    double nextScale = TrustRegionScales[scaleIndex + 1];
+                    if (!region.CanShrinkTo(nextScale, MinimumTrustRegionWidthMeters, out string shrinkFailure))
+                    {
+                        lastRejection = shrinkFailure;
+                        break;
+                    }
+                    trustShrinkCount++;
+                }
+            }
+            if (!promoted)
+                break;
+        }
+
+        return FinishFromAnchor(anchor, acceptedAnchorCount, qpSolveCount, trustShrinkCount,
+            finalTrustScale, cancellationToken.IsCancellationRequested, lastRejection, solveTrace);
+    }
+
+    private TrustedSolveAttempt SolveTrustedProblem(QuadraticProgram problem,
+        LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate anchor,
+        QpSolverSettings settings, TimeSpan totalBudget, Stopwatch stopwatch, double strictTolerance,
+        int stabilizationStart, int remainingCallCount, LongitudinalSolveTrace solveTrace,
+        int solveOrdinalOffset, int anchorUpdateIndex, double trustScale,
+        bool optionalImprovement, CancellationToken cancellationToken)
+    {
+        int solveCount = 0;
+        double anchorObjective = EvaluateObjective(problem, ToPrimal(anchor));
+        if (!IsFinite(anchorObjective))
+        {
+            return new TrustedSolveAttempt(EmPlanningStatus.LongitudinalInfeasible, null, solveCount, false,
+                "The strict anchor objective is non-finite for the trusted QP.");
+        }
+
+        IReadOnlyList<double> warmStart = ToPrimal(anchor);
+        string lastFailure = string.Empty;
+        for (int attemptIndex = 0; attemptIndex < 2; attemptIndex++)
+        {
+            if (solveCount >= remainingCallCount)
+            {
+                return new TrustedSolveAttempt(EmPlanningStatus.SolverTimedOut, null, solveCount, false,
+                    string.IsNullOrWhiteSpace(lastFailure) ? "The longitudinal QP solve-call cap was reached." : lastFailure);
+            }
             if (cancellationToken.IsCancellationRequested)
-                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Cancelled, "Longitudinal optimization was cancelled.");
+            {
+                return new TrustedSolveAttempt(EmPlanningStatus.Cancelled, null, solveCount, false,
+                    "Longitudinal optimization was cancelled before the trusted QP solve.");
+            }
             TimeSpan remainingBudget = totalBudget - stopwatch.Elapsed;
             if (remainingBudget <= TimeSpan.Zero)
             {
-                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.SolverTimedOut,
-                    "Longitudinal optimization exhausted its solve budget.");
+                return new TrustedSolveAttempt(EmPlanningStatus.SolverTimedOut, null, solveCount, false,
+                    "Longitudinal optimization exhausted its shared solve budget.");
             }
-            if (!_constraintBuilder.TryBuild(input, speedLimit, iterate, out QuadraticProgram problem, out string buildFailure))
+            bool hasOptionalSolveBudget = TryGetOptionalSolveBudget(remainingBudget,
+                out TimeSpan remainingAfterReserve);
+            if (optionalImprovement && !hasOptionalSolveBudget)
             {
-                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.LongitudinalInfeasible,
-                    "Longitudinal constraints are infeasible: " + buildFailure);
+                return new TrustedSolveAttempt(EmPlanningStatus.SolverTimedOut, null, solveCount, false,
+                    CreatePublicationReserveSkipDiagnostic(remainingAfterReserve));
+            }
+            TimeSpan solveBudget = optionalImprovement ? remainingAfterReserve : remainingBudget;
+
+            bool highPrecision = attemptIndex == 1;
+            double absoluteTolerance = highPrecision
+                ? Math.Min(settings.AbsoluteTolerance, HighPrecisionRetryTolerance)
+                : settings.AbsoluteTolerance;
+            double relativeTolerance = highPrecision
+                ? Math.Min(settings.AbsoluteTolerance, HighPrecisionRetryTolerance)
+                : settings.RelativeTolerance;
+            var solveStopwatch = Stopwatch.StartNew();
+            QpSolveResult solved = _qpSolver.Solve(problem,
+                new QpSolverSettings(settings.MaximumIterations, absoluteTolerance, relativeTolerance,
+                    solveBudget, settings.EnableWarmStart, settings.EnablePolishing,
+                    settings.EnableNativeVerboseOutput), warmStart, cancellationToken);
+            solveStopwatch.Stop();
+            solveCount++;
+            double candidateObjective = double.NaN;
+            void AddSolveTrace(string rejection)
+            {
+                solveTrace.Add(attemptIndex == 0 ? "normal" : "retry",
+                    solveOrdinalOffset + solveCount, anchorUpdateIndex, trustScale,
+                    solveBudget, remainingAfterReserve, solveStopwatch.Elapsed, solved, anchorObjective,
+                    candidateObjective, rejection);
             }
 
-            QpSolveResult solved = _qpSolver.Solve(problem,
-                new QpSolverSettings(settings.MaximumIterations, settings.AbsoluteTolerance, settings.RelativeTolerance,
-                    remainingBudget, settings.EnableWarmStart && (iteration > 0 || hasDynamicsConsistentInitialWarmStart),
-                    settings.EnablePolishing,
-                    settings.EnableNativeVerboseOutput),
-                warmStart, cancellationToken);
             if (cancellationToken.IsCancellationRequested)
-                return Failed(EmPlanningStatus.Cancelled, "Longitudinal optimization was cancelled after the QP solve.");
-            if (solved == null)
-                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Failed, "The longitudinal QP solver returned no result.");
-            if (solved.Status == QpSolveStatus.TimeLimit || solved.Status == QpSolveStatus.MaximumIterations)
             {
-                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.SolverTimedOut,
-                    "The longitudinal QP solver timed out (status=" + solved.NativeStatus +
-                    ", iterations=" + solved.Iterations + ", primal=" + solved.PrimalResidual +
-                    ", dual=" + solved.DualResidual + "): " + solved.Diagnostic);
+                const string rejection = "Longitudinal optimization was cancelled after the trusted QP solve.";
+                AddSolveTrace(rejection);
+                return new TrustedSolveAttempt(EmPlanningStatus.Cancelled, null, solveCount, false,
+                    rejection);
+            }
+            if (solved == null)
+            {
+                const string rejection = "The longitudinal QP solver returned no result.";
+                AddSolveTrace(rejection);
+                return new TrustedSolveAttempt(EmPlanningStatus.Failed, null, solveCount, false,
+                    rejection);
             }
             if (solved.Status == QpSolveStatus.Cancelled)
-                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Cancelled,
-                    "The longitudinal QP solver was cancelled: " + solved.Diagnostic);
+            {
+                string rejection = "The longitudinal QP solver was cancelled: " + solved.Diagnostic;
+                AddSolveTrace(rejection);
+                return new TrustedSolveAttempt(EmPlanningStatus.Cancelled, null, solveCount, false,
+                    rejection);
+            }
+            if (solved.Status == QpSolveStatus.TimeLimit || solved.Status == QpSolveStatus.MaximumIterations)
+            {
+                string rejection = "The longitudinal QP solver timed out (status=" + solved.NativeStatus +
+                    ", iterations=" + solved.Iterations + ", primal=" + solved.PrimalResidual +
+                    ", dual=" + solved.DualResidual + "): " + solved.Diagnostic;
+                AddSolveTrace(rejection);
+                return new TrustedSolveAttempt(EmPlanningStatus.SolverTimedOut, null, solveCount, false,
+                    rejection);
+            }
             if (solved.Status == QpSolveStatus.PrimalInfeasible || solved.Status == QpSolveStatus.DualInfeasible)
             {
-                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.LongitudinalInfeasible,
-                    "The longitudinal QP solver reported infeasibility: " + solved.Diagnostic);
+                string rejection = "The preflight-feasible longitudinal QP solver reported infeasibility;" +
+                    "solverNumericalAnomaly=true;status=" + solved.NativeStatus + ": " + solved.Diagnostic;
+                AddSolveTrace(rejection);
+                return new TrustedSolveAttempt(EmPlanningStatus.LongitudinalInfeasible, null, solveCount, false,
+                    rejection);
             }
             if (solved.Status == QpSolveStatus.SolverUnavailable)
-                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.SolverUnavailable,
-                    "The longitudinal QP solver is unavailable: " + solved.Diagnostic);
+            {
+                string rejection = "The longitudinal QP solver is unavailable: " + solved.Diagnostic;
+                AddSolveTrace(rejection);
+                return new TrustedSolveAttempt(EmPlanningStatus.SolverUnavailable, null, solveCount, false,
+                    rejection);
+            }
             if (solved.Status != QpSolveStatus.Solved && solved.Status != QpSolveStatus.SolvedInaccurate)
             {
-                return FallbackOrFailure(lastStrictCandidate, EmPlanningStatus.Failed,
-                    "The longitudinal QP solver failed: " + solved.Diagnostic);
+                string rejection = "The longitudinal QP solver failed (status=" + solved.NativeStatus + "): " +
+                    solved.Diagnostic;
+                AddSolveTrace(rejection);
+                return new TrustedSolveAttempt(EmPlanningStatus.Failed, null, solveCount, false,
+                    rejection);
             }
-            if (solved.Status == QpSolveStatus.SolvedInaccurate && !HasStrictResiduals(solved, convergenceTolerance))
+
+            if (!TryCreateCandidate(anchor.KnotTimes, solved.Primal, out LongitudinalCandidate candidate))
             {
-                lastCandidateRejection = "SolvedInaccurate residuals exceed the strict acceptance tolerance" +
+                const string rejection =
+                    "The solver primal does not match the ST variable layout or contains non-finite values.";
+                AddSolveTrace(rejection);
+                return new TrustedSolveAttempt(EmPlanningStatus.SuccessWithFallback, null, solveCount, false,
+                    rejection);
+            }
+
+            bool accepted = true;
+            if (solved.Status == QpSolveStatus.SolvedInaccurate && !HasStrictResiduals(solved, strictTolerance))
+            {
+                accepted = false;
+                lastFailure = "SolvedInaccurate residuals exceed the strict acceptance tolerance" +
                     " (primal=" + solved.PrimalResidual + ", dual=" + solved.DualResidual + ").";
-                if (TryCreateCandidate(iterate.KnotTimes, solved.Primal, out LongitudinalCandidate inaccurateCandidate))
-                    warmStart = ToPrimal(inaccurateCandidate);
-                continue;
             }
-            if (!TryCreateCandidate(iterate.KnotTimes, solved.Primal, out LongitudinalCandidate candidate))
+            if (accepted && !TryFastValidateCandidate(problem, candidate, strictTolerance,
+                    stabilizationStart, out lastFailure))
             {
-                lastCandidateRejection = "The solver primal does not match the ST variable layout.";
-                continue;
+                accepted = false;
             }
-            if (!_solutionValidator.TryValidate(input, speedLimit, candidate, out LongitudinalCandidate validated,
+            LongitudinalCandidate validated = null;
+            if (accepted && !_solutionValidator.TryValidate(input, speedLimit, candidate, out validated,
                     out string validationFailure))
             {
-                string rejection = validationFailure + CreateEnvelopeDiagnostic(speedLimit, iterate, candidate,
-                    iteration + 1);
-                lastCandidateRejection = string.IsNullOrEmpty(lastCandidateRejection)
-                    ? rejection
-                    : lastCandidateRejection + " | " + rejection;
-                if (TryCreateEnvelopeIterate(input, iterate, candidate, out LongitudinalCandidate nextIterate))
+                accepted = false;
+                lastFailure = validationFailure;
+            }
+            if (accepted)
+            {
+                candidateObjective = EvaluateObjective(problem, ToPrimal(validated));
+                if (!IsObjectiveAccepted(anchorObjective, candidateObjective))
                 {
-                    iterate = nextIterate;
-                    warmStart = ToPrimal(candidate);
+                    accepted = false;
+                    lastFailure = "The strictly valid candidate worsens the current trusted-QP objective" +
+                        " (anchor=" + anchorObjective.ToString("R", CultureInfo.InvariantCulture) +
+                        ", candidate=" + candidateObjective.ToString("R", CultureInfo.InvariantCulture) + ").";
                 }
-                continue;
+            }
+            if (accepted && cancellationToken.IsCancellationRequested)
+            {
+                const string rejection = "Longitudinal optimization was cancelled before strict-anchor promotion.";
+                AddSolveTrace(rejection);
+                return new TrustedSolveAttempt(EmPlanningStatus.Cancelled, null, solveCount, false,
+                    rejection);
+            }
+            if (accepted && totalBudget - stopwatch.Elapsed <= TimeSpan.Zero)
+            {
+                const string rejection =
+                    "Longitudinal optimization exhausted its shared solve budget before strict-anchor promotion.";
+                AddSolveTrace(rejection);
+                return new TrustedSolveAttempt(EmPlanningStatus.SolverTimedOut, null, solveCount, false,
+                    rejection);
+            }
+            if (accepted)
+            {
+                AddSolveTrace(string.Empty);
+                return new TrustedSolveAttempt(EmPlanningStatus.Success, validated, solveCount, true, string.Empty);
+            }
+            if (highPrecision)
+            {
+                AddSolveTrace(lastFailure);
+                return new TrustedSolveAttempt(EmPlanningStatus.SuccessWithFallback, null, solveCount, false,
+                    lastFailure);
             }
 
-            double maximumChange = MaximumProgressOrSpeedChange(iterate, validated);
-            double relativeObjectiveImprovement = hasPreviousObjective
-                ? RelativeObjectiveImprovement(previousObjective, solved.Objective)
-                : double.PositiveInfinity;
-            lastStrictCandidate = CopyCandidate(validated);
-            iterate = validated;
-            warmStart = ToPrimal(validated);
-            previousObjective = solved.Objective;
-            hasPreviousObjective = true;
-            if (maximumChange <= convergenceTolerance && relativeObjectiveImprovement <= convergenceTolerance)
-                return new LongitudinalPlanningResult(EmPlanningStatus.Success, lastStrictCandidate, string.Empty);
+            AddSolveTrace(lastFailure);
+            warmStart = ToPrimal(candidate);
         }
 
-        return lastStrictCandidate == null
-            ? Failed(EmPlanningStatus.LongitudinalInfeasible, "No strictly validated longitudinal candidate was found. " +
-                lastCandidateRejection)
-            : new LongitudinalPlanningResult(EmPlanningStatus.Success, lastStrictCandidate, string.Empty);
+        return new TrustedSolveAttempt(EmPlanningStatus.SuccessWithFallback, null, solveCount, false,
+            lastFailure);
+    }
+
+    private static double EvaluateObjective(QuadraticProgram problem, IReadOnlyList<double> primal)
+    {
+        if (problem == null || primal == null || primal.Count != problem.VariableCount)
+            return double.NaN;
+        double objective = 0d;
+        SparseCscMatrix hessian = problem.UpperTriangularP;
+        for (int column = 0; column < hessian.ColumnCount; column++)
+        {
+            double columnValue = primal[column];
+            if (!IsFinite(columnValue))
+                return double.NaN;
+            for (int entry = hessian.ColumnPointers[column]; entry < hessian.ColumnPointers[column + 1]; entry++)
+            {
+                int row = hessian.RowIndices[entry];
+                double term = hessian.Values[entry] * primal[row] * columnValue;
+                objective += row == column ? 0.5d * term : term;
+                if (!IsFinite(objective))
+                    return double.NaN;
+            }
+            objective += problem.LinearCost[column] * columnValue;
+            if (!IsFinite(objective))
+                return double.NaN;
+        }
+        return objective;
+    }
+
+    private static bool IsObjectiveAccepted(double anchorObjective, double candidateObjective)
+    {
+        if (!IsFinite(anchorObjective) || !IsFinite(candidateObjective))
+            return false;
+        double tolerance = ObjectiveAcceptanceRelativeTolerance * Math.Max(1d, Math.Abs(anchorObjective));
+        return candidateObjective <= anchorObjective + tolerance;
+    }
+
+    private static int GetStabilizationStart(LongitudinalPlanningInput input)
+    {
+        if (input.Mode != EmLongitudinalMode.ExactStopAtBoundary)
+            return input.KnotSchedule.KnotTimes.Count;
+        if (input.PlanningScope == EmPlanningScope.FullDirectionSegment)
+            return input.KnotSchedule.TerminalHoldStartIndex;
+        return LongitudinalTerminalSchedule.GetStabilizationStartIndex(input.KnotSchedule.KnotTimes,
+            input.Configuration.Scheduling.OutputTimeStepSeconds);
+    }
+
+    private static bool TryFastValidateCandidate(QuadraticProgram problem, LongitudinalCandidate candidate,
+        double strictTolerance, int stabilizationStart, out string failureReason)
+    {
+        failureReason = string.Empty;
+        var layout = new LongitudinalVariableLayout(candidate.KnotTimes.Count);
+        LongitudinalQpAuditResult audit;
+        try
+        {
+            audit = LongitudinalQpFeasibilityAudit.Evaluate(problem, candidate, strictTolerance,
+                layout, stabilizationStart);
+        }
+        catch (ArgumentException exception)
+        {
+            failureReason = "The candidate cannot be audited against the current trusted QP: " + exception.Message;
+            return false;
+        }
+        if (audit.IsFeasible)
+            return true;
+        failureReason = "The candidate violates the current trusted QP" +
+            ";row=" + audit.WorstRow + ";category=" + audit.Category +
+            ";residual=" + audit.MaximumResidual.ToString("R", CultureInfo.InvariantCulture) + audit.Unit +
+            ";tolerance=" + strictTolerance.ToString("R", CultureInfo.InvariantCulture);
+        return false;
+    }
+
+    private static string CreateRunDiagnostic(int qpSolveCount, int trustShrinkCount,
+        int acceptedAnchorCount, double finalTrustScale, string lastRejection,
+        LongitudinalSolveTrace solveTrace)
+    {
+        return "qpSolves=" + qpSolveCount +
+            ",trustShrinks=" + trustShrinkCount +
+            ",acceptedAnchors=" + acceptedAnchorCount +
+            ",trustScale=" + finalTrustScale.ToString("R", CultureInfo.InvariantCulture) +
+            (string.IsNullOrWhiteSpace(lastRejection) ? string.Empty : ";lastRejection=" + lastRejection) +
+            (string.IsNullOrEmpty(solveTrace.Format()) ? string.Empty : ";solveTrace=" + solveTrace.Format());
+    }
+
+    internal static bool TryGetOptionalSolveBudget(TimeSpan remaining, out TimeSpan solveBudget)
+    {
+        solveBudget = remaining - PublicationReserve;
+        if (solveBudget <= TimeSpan.Zero)
+        {
+            solveBudget = TimeSpan.Zero;
+            return false;
+        }
+        return true;
+    }
+
+    private static string CreatePublicationReserveSkipDiagnostic(TimeSpan remainingAfterReserve)
+    {
+        return "remainingAfterReserveMs=" + remainingAfterReserve.TotalMilliseconds.ToString(
+                "F3", CultureInfo.InvariantCulture) +
+            ";publicationReserveMs=" + PublicationReserve.TotalMilliseconds.ToString(
+                "F0", CultureInfo.InvariantCulture) +
+            ";optionalImprovement=skipped";
+    }
+
+    private static LongitudinalPlanningResult FinishFromAnchor(LongitudinalCandidate anchor,
+        int acceptedAnchorCount, int qpSolveCount, int trustShrinkCount, double finalTrustScale,
+        bool cancelled, string lastRejection, LongitudinalSolveTrace solveTrace)
+    {
+        string diagnostic = CreateRunDiagnostic(qpSolveCount, trustShrinkCount, acceptedAnchorCount,
+            finalTrustScale, lastRejection, solveTrace);
+        if (cancelled)
+            return Failed(EmPlanningStatus.Cancelled, diagnostic);
+        EmPlanningStatus status = acceptedAnchorCount > 0
+            ? EmPlanningStatus.Success
+            : EmPlanningStatus.SuccessWithFallback;
+        return new LongitudinalPlanningResult(status, CopyCandidate(anchor), diagnostic);
     }
 
     private static bool TryCreateSettings(LongitudinalPlanningInput input, out QpSolverSettings settings,
@@ -222,7 +515,7 @@ public sealed class SequentialLongitudinalOptimizer
             settings = new QpSolverSettings(solver.MaximumOsqpIterations, solver.AbsoluteTolerance, solver.RelativeTolerance,
                 totalBudget, solver.WarmStart, solver.Polish, solver.NativeVerbose);
             convergenceTolerance = solver.StrictResidualTolerance;
-            iterationLimit = Math.Min(MaximumEnvelopeIterations, solver.MaximumOuterIterations);
+            iterationLimit = Math.Min(MaximumAcceptedAnchorUpdates, solver.MaximumOuterIterations);
             return true;
         }
         catch (ArgumentException exception)
@@ -234,8 +527,9 @@ public sealed class SequentialLongitudinalOptimizer
 
     private bool TryCreateInitialFeasibleCandidate(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
         QpSolverSettings settings, TimeSpan totalBudget, double convergenceTolerance, int iterationLimit,
-        Stopwatch stopwatch, CancellationToken cancellationToken, out LongitudinalCandidate candidate,
-        out int projectionSolveCount, out EmPlanningStatus failureStatus, out string failureReason)
+        Stopwatch stopwatch, LongitudinalSolveTrace solveTrace, CancellationToken cancellationToken,
+        out LongitudinalCandidate candidate, out int projectionSolveCount,
+        out EmPlanningStatus failureStatus, out string failureReason)
     {
         candidate = null;
         projectionSolveCount = 0;
@@ -248,8 +542,9 @@ public sealed class SequentialLongitudinalOptimizer
                 input.Configuration.Validation.KinematicTolerance;
         bool staticStartSeedUsed = false;
         string staticStartSeedFailure = string.Empty;
-        if (staticStartEligible && TryCreateStaticStartSeed(input, speedLimit, out LongitudinalCandidate staticStartSeed,
-                out staticStartSeedFailure))
+        TimeSpan staticStartSeedDeadline = stopwatch.Elapsed + GetStaticStartSeedBudget(totalBudget);
+        if (staticStartEligible && TryCreateStaticStartSeed(input, speedLimit, stopwatch, staticStartSeedDeadline,
+                out LongitudinalCandidate staticStartSeed, out staticStartSeedFailure))
         {
             staticStartSeedUsed = true;
             candidate = staticStartSeed;
@@ -289,60 +584,93 @@ public sealed class SequentialLongitudinalOptimizer
 
             double projectionTolerance = Math.Min(settings.AbsoluteTolerance,
                 input.Configuration.Validation.KinematicTolerance * 0.1d);
+            double anchorObjective = double.NaN;
+            var solveStopwatch = Stopwatch.StartNew();
             QpSolveResult solved = _qpSolver.Solve(problem,
                 new QpSolverSettings(settings.MaximumIterations, projectionTolerance, projectionTolerance,
                     remainingBudget, settings.EnableWarmStart && linearizationIterate.SatisfiesExactDiscreteDynamics(1e-12d),
                     settings.EnablePolishing, settings.EnableNativeVerboseOutput),
                 ToPrimal(linearizationIterate), cancellationToken);
+            solveStopwatch.Stop();
             projectionSolveCount++;
+            int projectionCallOrdinal = projectionSolveCount;
+            double candidateObjective = double.NaN;
+            void AddProjectionTrace(string rejection)
+            {
+                TryGetOptionalSolveBudget(remainingBudget, out TimeSpan remainingAfterReserve);
+                solveTrace.Add("projection", projectionCallOrdinal, 0, 1d, remainingBudget,
+                    remainingAfterReserve, solveStopwatch.Elapsed, solved, anchorObjective,
+                    candidateObjective, rejection);
+            }
             if (cancellationToken.IsCancellationRequested)
             {
+                const string rejection =
+                    "Initial full-direction feasibility projection was cancelled after the QP solve.";
+                AddProjectionTrace(rejection);
                 failureStatus = EmPlanningStatus.Cancelled;
-                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection was cancelled after the QP solve.");
+                failureReason = WithStaticSeedDiagnostic(rejection);
                 return false;
             }
             if (solved == null)
             {
+                const string rejection = "The initial full-direction feasibility solver returned no result.";
+                AddProjectionTrace(rejection);
                 failureStatus = EmPlanningStatus.Failed;
-                failureReason = WithStaticSeedDiagnostic("The initial full-direction feasibility solver returned no result.");
+                failureReason = WithStaticSeedDiagnostic(rejection);
                 return false;
             }
             if (solved.Status == QpSolveStatus.TimeLimit || solved.Status == QpSolveStatus.MaximumIterations)
             {
-                failureStatus = EmPlanningStatus.SolverTimedOut;
-                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection timed out (status=" + solved.NativeStatus +
+                string rejection = "Initial full-direction feasibility projection timed out (status=" + solved.NativeStatus +
                     ", iterations=" + solved.Iterations + ", primal=" + solved.PrimalResidual + ", dual=" +
-                    solved.DualResidual + "): " + solved.Diagnostic);
+                    solved.DualResidual + "): " + solved.Diagnostic;
+                AddProjectionTrace(rejection);
+                failureStatus = EmPlanningStatus.SolverTimedOut;
+                failureReason = WithStaticSeedDiagnostic(rejection);
                 return false;
             }
             if (solved.Status == QpSolveStatus.Cancelled)
             {
+                string rejection =
+                    "Initial full-direction feasibility projection was cancelled: " + solved.Diagnostic;
+                AddProjectionTrace(rejection);
                 failureStatus = EmPlanningStatus.Cancelled;
-                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection was cancelled: " + solved.Diagnostic);
+                failureReason = WithStaticSeedDiagnostic(rejection);
                 return false;
             }
             if (solved.Status == QpSolveStatus.PrimalInfeasible || solved.Status == QpSolveStatus.DualInfeasible)
             {
+                string rejection =
+                    "Initial full-direction feasibility projection is infeasible: " + solved.Diagnostic;
+                AddProjectionTrace(rejection);
                 failureStatus = EmPlanningStatus.LongitudinalInfeasible;
-                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection is infeasible: " + solved.Diagnostic);
+                failureReason = WithStaticSeedDiagnostic(rejection);
                 return false;
             }
             if (solved.Status == QpSolveStatus.SolverUnavailable)
             {
+                string rejection =
+                    "Initial full-direction feasibility solver is unavailable: " + solved.Diagnostic;
+                AddProjectionTrace(rejection);
                 failureStatus = EmPlanningStatus.SolverUnavailable;
-                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility solver is unavailable: " + solved.Diagnostic);
+                failureReason = WithStaticSeedDiagnostic(rejection);
                 return false;
             }
             if (solved.Status != QpSolveStatus.Solved && solved.Status != QpSolveStatus.SolvedInaccurate)
             {
+                string rejection = "Initial full-direction feasibility solver failed: " + solved.Diagnostic;
+                AddProjectionTrace(rejection);
                 failureStatus = EmPlanningStatus.Failed;
-                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility solver failed: " + solved.Diagnostic);
+                failureReason = WithStaticSeedDiagnostic(rejection);
                 return false;
             }
             if (!TryCreateCandidate(input.KnotSchedule.KnotTimes, solved.Primal, out LongitudinalCandidate projected))
             {
+                const string rejection =
+                    "Initial full-direction feasibility solver primal does not match the ST layout.";
+                AddProjectionTrace(rejection);
                 failureStatus = EmPlanningStatus.LongitudinalInfeasible;
-                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility solver primal does not match the ST layout.");
+                failureReason = WithStaticSeedDiagnostic(rejection);
                 return false;
             }
             if (solved.Status == QpSolveStatus.Solved || HasStrictResiduals(solved, convergenceTolerance))
@@ -350,11 +678,13 @@ public sealed class SequentialLongitudinalOptimizer
                 if (_solutionValidator.TryValidate(input, speedLimit, projected, out LongitudinalCandidate strict,
                         out EmPlanningStatus validationStatus, out string validationFailure))
                 {
+                    AddProjectionTrace(string.Empty);
                     candidate = strict;
                     return true;
                 }
                 if (validationStatus == EmPlanningStatus.NoProgress)
                 {
+                    AddProjectionTrace(validationFailure);
                     failureStatus = validationStatus;
                     failureReason = WithStaticSeedDiagnostic(validationFailure);
                     return false;
@@ -365,8 +695,11 @@ public sealed class SequentialLongitudinalOptimizer
             if (!TryCreateFeasibilityEnvelopeIterate(input, projected,
                     out LongitudinalCandidate nextLinearization))
             {
+                const string rejection =
+                    "Initial full-direction feasibility candidate could not be relinearized against the PathS envelope.";
+                AddProjectionTrace(rejection);
                 failureStatus = EmPlanningStatus.LongitudinalInfeasible;
-                failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility candidate could not be relinearized against the PathS envelope.");
+                failureReason = WithStaticSeedDiagnostic(rejection);
                 return false;
             }
             linearizationIterate = nextLinearization;
@@ -374,6 +707,7 @@ public sealed class SequentialLongitudinalOptimizer
                 lastRejection = "Initial feasibility projection residuals exceed the strict acceptance tolerance.";
             else if (string.IsNullOrEmpty(lastRejection))
                 lastRejection = "Initial feasibility projection violated the strict physical validator.";
+            AddProjectionTrace(lastRejection);
         }
         failureStatus = EmPlanningStatus.LongitudinalInfeasible;
         failureReason = WithStaticSeedDiagnostic("Initial full-direction feasibility projection exhausted the configured outer iterations. " +
@@ -503,7 +837,7 @@ public sealed class SequentialLongitudinalOptimizer
     }
 
     private bool TryCreateStaticStartSeed(LongitudinalPlanningInput input, PathSpeedLimit speedLimit,
-        out LongitudinalCandidate candidate, out string failureReason)
+        Stopwatch stopwatch, TimeSpan deadline, out LongitudinalCandidate candidate, out string failureReason)
     {
         candidate = null;
         failureReason = "unknown";
@@ -515,9 +849,11 @@ public sealed class SequentialLongitudinalOptimizer
         }
 
         IReadOnlyList<double> times = input.KnotSchedule.KnotTimes;
-        if (TryCreateExactJerkSeed(input, times, stabilizationStart, speedLimit, out candidate))
+        if (TryCreateStaticStartScurveSeed(input, times, stabilizationStart, speedLimit, stopwatch, deadline, out candidate))
             return true;
-        failureReason = "exactJerkSeed=failed";
+        if (TryCreateExactJerkSeed(input, times, stabilizationStart, speedLimit, stopwatch, deadline, out candidate))
+            return true;
+        failureReason = "scurveSeed=failed; exactJerkSeed=failed";
         double firstDuration = times[1] - times[0];
         double secondDuration = times[2] - times[1];
         LongitudinalConfiguration configuration = input.Configuration.Longitudinal;
@@ -526,6 +862,8 @@ public sealed class SequentialLongitudinalOptimizer
                 configuration.MaximumJerkMetersPerSecondCubed * secondDuration / firstDuration));
         for (int sample = -256; sample <= 256; sample++)
         {
+            if (HasReachedDeadline(stopwatch, deadline))
+                return false;
             if (sample == 0)
                 continue;
             double firstJerk = maximumFirstJerk * sample / 256d;
@@ -545,18 +883,13 @@ public sealed class SequentialLongitudinalOptimizer
                 return true;
             }
         }
-        if (TryCreateStaticStartScurveSeed(input, times, stabilizationStart, speedLimit, out candidate))
-        {
-            failureReason = string.Empty;
-            return true;
-        }
-        failureReason = "exactJerkSeed=failed; sampledSeeds=failed; scurveSeeds=failed";
+        failureReason = "scurveSeed=failed; exactJerkSeed=failed; sampledSeeds=failed";
         return false;
     }
 
     private bool TryCreateStaticStartScurveSeed(LongitudinalPlanningInput input,
         IReadOnlyList<double> times, int stabilizationStart, PathSpeedLimit speedLimit,
-        out LongitudinalCandidate candidate)
+        Stopwatch stopwatch, TimeSpan deadline, out LongitudinalCandidate candidate)
     {
         candidate = null;
         int intervalCount = stabilizationStart;
@@ -569,6 +902,8 @@ public sealed class SequentialLongitudinalOptimizer
         {
             for (int plateau = 0; 4 * ramp + 2 * plateau <= intervalCount; plateau++)
             {
+                if (HasReachedDeadline(stopwatch, deadline))
+                    return false;
                 int cruise = intervalCount - 4 * ramp - 2 * plateau;
                 var basisAccel = new double[intervalCount];
                 var basisBrake = new double[intervalCount];
@@ -688,6 +1023,14 @@ public sealed class SequentialLongitudinalOptimizer
 
     private bool TryCreateExactJerkSeed(LongitudinalPlanningInput input, IReadOnlyList<double> times,
         int stabilizationStart, PathSpeedLimit speedLimit, out LongitudinalCandidate candidate)
+    {
+        return TryCreateExactJerkSeed(input, times, stabilizationStart, speedLimit, Stopwatch.StartNew(),
+            TimeSpan.MaxValue, out candidate);
+    }
+
+    private bool TryCreateExactJerkSeed(LongitudinalPlanningInput input, IReadOnlyList<double> times,
+        int stabilizationStart, PathSpeedLimit speedLimit, Stopwatch stopwatch, TimeSpan deadline,
+        out LongitudinalCandidate candidate)
     {
         candidate = null;
         int intervalCount = stabilizationStart;
@@ -747,6 +1090,8 @@ public sealed class SequentialLongitudinalOptimizer
         {
             for (int basisIndex = 0; basisIndex < intervalCount; basisIndex++)
             {
+                if (HasReachedDeadline(stopwatch, deadline))
+                    return false;
                 double[] direction = CreateEndpointNullspaceDirection(influence, gram, basisIndex);
                 if (direction == null)
                     continue;
@@ -755,6 +1100,8 @@ public sealed class SequentialLongitudinalOptimizer
                 double bestViolation = currentViolation;
                 for (int sample = -256; sample <= 256; sample++)
                 {
+                    if (HasReachedDeadline(stopwatch, deadline))
+                        return false;
                     double scale = maximumJerk * sample / 256d;
                     var probeJerk = new double[intervalCount];
                     for (int interval = 0; interval < intervalCount; interval++)
@@ -776,6 +1123,18 @@ public sealed class SequentialLongitudinalOptimizer
         return false;
     }
 
+    private static TimeSpan GetStaticStartSeedBudget(TimeSpan totalBudget)
+    {
+        double milliseconds = Math.Min(MaximumStaticStartSeedBudget.TotalMilliseconds,
+            Math.Max(1d, totalBudget.TotalMilliseconds * StaticStartSeedBudgetFraction));
+        return TimeSpan.FromMilliseconds(milliseconds);
+    }
+
+    private static bool HasReachedDeadline(Stopwatch stopwatch, TimeSpan deadline)
+    {
+        return stopwatch.Elapsed >= deadline;
+    }
+
     private bool TryValidateExactSeed(LongitudinalPlanningInput input, IReadOnlyList<double> times,
         int stabilizationStart, PathSpeedLimit speedLimit, IReadOnlyList<double> jerk,
         out LongitudinalCandidate candidate)
@@ -1065,72 +1424,6 @@ public sealed class SequentialLongitudinalOptimizer
         return primal;
     }
 
-    private static bool TryCreateEnvelopeIterate(LongitudinalPlanningInput input, LongitudinalCandidate previous,
-        LongitudinalCandidate candidate, out LongitudinalCandidate nextIterate)
-    {
-        nextIterate = null;
-        if (candidate.S.Count != previous.S.Count)
-            return false;
-        int stabilizationStart = input.Mode != EmLongitudinalMode.ExactStopAtBoundary
-            ? candidate.S.Count
-            : input.PlanningScope == EmPlanningScope.FullDirectionSegment
-                ? input.KnotSchedule.TerminalHoldStartIndex
-                : LongitudinalTerminalSchedule.GetStabilizationStartIndex(candidate.KnotTimes,
-                    input.Configuration.Scheduling.OutputTimeStepSeconds);
-        var candidateProgressSamples = new double[candidate.S.Count];
-        double priorProgress = double.NegativeInfinity;
-        double priorPreviousProgress = double.NegativeInfinity;
-        for (int index = 0; index < candidate.S.Count; index++)
-        {
-            double candidateProgress = candidate.S[index];
-            double previousProgress = previous.S[index];
-            if (!IsFinite(previousProgress) || previousProgress < 0d || previousProgress > input.PathUpperBoundS ||
-                previousProgress < priorPreviousProgress)
-            {
-                return false;
-            }
-            if (!IsFinite(candidateProgress))
-            {
-                candidateProgress = previousProgress;
-            }
-            candidateProgress = Math.Max(0d, Math.Min(input.PathUpperBoundS, candidateProgress));
-            if (input.Mode == EmLongitudinalMode.ExactStopAtBoundary && index >= stabilizationStart)
-                candidateProgress = input.StopBoundaryPathS;
-            candidateProgress = Math.Max(priorProgress, candidateProgress);
-            candidateProgressSamples[index] = candidateProgress;
-            priorProgress = candidateProgress;
-            priorPreviousProgress = previousProgress;
-        }
-        var progress = new double[candidate.S.Count];
-        double previousNextProgress = 0d;
-        for (int index = 0; index < progress.Length; index++)
-        {
-            double candidateProgress = candidateProgressSamples[index];
-            if (index == 0 || index == progress.Length - 1 || (input.Mode == EmLongitudinalMode.ExactStopAtBoundary &&
-                index >= stabilizationStart) || candidateProgress >= input.PathUpperBoundS)
-            {
-                progress[index] = candidateProgress;
-            }
-            else
-            {
-                double timeStep = candidate.KnotTimes[index + 1] - candidate.KnotTimes[index];
-                double iterationAdvance = Math.Max(0d, candidateProgress - previous.S[index]);
-                double candidateSpeed = IsFinite(candidate.U[index]) ? Math.Max(0d, candidate.U[index]) : 0d;
-                double lookaheadAdvance = IsFinite(candidate.U[index])
-                    ? OrdinaryEnvelopeProbeLookaheadSteps * candidateSpeed * timeStep
-                    : 0d;
-                double terminalLimitedAdvance = OrdinaryTerminalProbeFraction *
-                    (input.PathUpperBoundS - candidateProgress);
-                double advance = Math.Min(Math.Max(iterationAdvance, lookaheadAdvance), terminalLimitedAdvance);
-                progress[index] = candidateProgress + advance;
-            }
-            progress[index] = Math.Max(previousNextProgress, progress[index]);
-            previousNextProgress = progress[index];
-        }
-        nextIterate = new LongitudinalCandidate(candidate.KnotTimes, progress, previous.U, previous.A, previous.J);
-        return true;
-    }
-
     private static bool TryCreateFeasibilityEnvelopeIterate(LongitudinalPlanningInput input,
         LongitudinalCandidate candidate, out LongitudinalCandidate nextIterate)
     {
@@ -1161,52 +1454,27 @@ public sealed class SequentialLongitudinalOptimizer
             result.PrimalResidual <= tolerance && result.DualResidual <= tolerance;
     }
 
-    private static string CreateEnvelopeDiagnostic(PathSpeedLimit speedLimit, LongitudinalCandidate iterate,
-        LongitudinalCandidate candidate, int iteration)
+    private readonly struct TrustedSolveAttempt
     {
-        int worstIndex = -1;
-        double worstExcess = double.NegativeInfinity;
-        for (int index = 0; index < candidate.S.Count; index++)
+        internal TrustedSolveAttempt(EmPlanningStatus status, LongitudinalCandidate candidate,
+            int solveCount, bool accepted, string failureReason)
         {
-            if (!IsFinite(candidate.S[index]) || !IsFinite(candidate.U[index]))
-                continue;
-            double candidateProgress = Math.Max(0d, Math.Min(speedLimit.PathUpperBoundS, candidate.S[index]));
-            double limit = speedLimit.MaximumSpeedAt(candidateProgress);
-            double excess = candidate.U[index] - limit;
-            if (excess > worstExcess)
-            {
-                worstExcess = excess;
-                worstIndex = index;
-            }
+            Status = status;
+            Candidate = candidate;
+            SolveCount = solveCount;
+            Accepted = accepted;
+            FailureReason = failureReason ?? string.Empty;
         }
-        if (worstIndex < 0)
-            return "";
-        return " Envelope iteration " + iteration + " used PathS=" + iterate.S[worstIndex] +
-            " and produced PathS=" + candidate.S[worstIndex] + " at its largest speed-envelope excess.";
-    }
 
-    private static double MaximumProgressOrSpeedChange(LongitudinalCandidate previous, LongitudinalCandidate current)
-    {
-        double maximum = 0d;
-        for (int index = 0; index < previous.S.Count; index++)
-        {
-            maximum = Math.Max(maximum, Math.Abs(current.S[index] - previous.S[index]));
-            maximum = Math.Max(maximum, Math.Abs(current.U[index] - previous.U[index]));
-        }
-        return maximum;
-    }
+        internal EmPlanningStatus Status { get; }
 
-    private static double RelativeObjectiveImprovement(double previous, double current)
-    {
-        return Math.Abs(previous - current) / Math.Max(1d, Math.Abs(previous));
-    }
+        internal LongitudinalCandidate Candidate { get; }
 
-    private static LongitudinalPlanningResult FallbackOrFailure(LongitudinalCandidate candidate,
-        EmPlanningStatus failureStatus, string failureReason)
-    {
-        return failureStatus == EmPlanningStatus.Cancelled || candidate == null
-            ? Failed(failureStatus, failureReason)
-            : new LongitudinalPlanningResult(EmPlanningStatus.SuccessWithFallback, candidate, failureReason);
+        internal int SolveCount { get; }
+
+        internal bool Accepted { get; }
+
+        internal string FailureReason { get; }
     }
 
     private static LongitudinalPlanningResult Failed(EmPlanningStatus status, string reason)