Practical NLP Implementation of LGR Optimal Control

The previous sessions developed the mathematical foundations of Legendre pseudospectral transcription and costate recovery. This section explains how to implement an LGR transcription inside a general nonlinear programming solver.

The central practical issue is that an optimal-control transcription is naturally expressed using matrices of state and control values, whereas an NLP solver expects a single decision vector. Therefore, the implementation must provide a systematic mapping between:

1. the continuous optimal-control problem;

2. the LGR-discretized state and control arrays;

3. the NLP decision vector;

4. the scalar objective function;

5. the stacked equality and inequality constraints; and

6. the initial guess supplied to the solver.

Standard Nonlinear Programming Form¶

A general nonlinear program may be written as

Here:

• $\boldsymbol{z}$ is the NLP decision vector;

• $f(\boldsymbol{z})$ is the scalar objective;

• $\boldsymbol{g}(\boldsymbol{z})$ contains equality constraints;

• $\boldsymbol{h}(\boldsymbol{z})$ contains inequality constraints; and

• $\boldsymbol{z}_{\min}$ and $\boldsymbol{z}_{\max}$ are variable bounds.

For implementation, the equality and inequality constraints may be stacked into one vector,

with corresponding lower and upper bounds.

LGR-Discretized Bolza Problem¶

Consider a Bolza problem on $[t_0,t_f]$:

After mapping the interval to $[-1,1]$ and applying LGR collocation, the objective is approximated by

The defect equations are

Endpoint constraints are

Path constraints are imposed at the $N$ LGR points:

Dimensions of the Discrete Variables¶

Let:

• $n_x$ be the number of states;

• $n_u$ be the number of controls;

• $N$ be the number of LGR collocation points.

Then

because the state is stored at the $N$ LGR points plus the noncollocated terminal point.

Similarly,

because the control is represented only at the collocation points.

If both $t_0$ and $t_f$ are optimization variables, the total number of NLP variables is

Packing the NLP Decision Vector¶

The NLP solver expects a single column vector. A standard column-major packing is

The state block is

and the control block is

The time variables occupy the final two entries.

Index Ranges¶

The state portion is

The control portion is

The time variables are

and

Unpacking and Reshaping¶

Inside the objective and constraint functions, reshape the vector blocks back into matrices:

and

In MATLAB:

X = reshape(zx, N+1, nx);
U = reshape(zu, N,   nu);

The state values at the collocation points are

The endpoint state values are

Physical Time at the LGR Points¶

The affine time transformation gives

In vector form,

These time values are used to evaluate the dynamics, Lagrange integrand, and path constraints.

Evaluating the Dynamics¶

Evaluate the dynamics at the LGR points:

Stacking these rows produces

The dynamics evaluation is problem dependent.

A MATLAB-style implementation is:

Xlgr = X(1:N,:);
F = dynamics(Xlgr, U, t);

Defect Constraints¶

The LGR defect matrix is

Its dimension is

An NLP solver requires a vector, so vectorize the matrix:

In MATLAB:

defect = D*X - 0.5*(tf-t0)*F;
defect_col = defect(:);

The expression

A(:)

stacks the columns of a matrix into one column vector.

Evaluating the Objective¶

Separate the objective into Mayer and Lagrange terms.

Mayer Term¶

Evaluate

Lagrange Term¶

Evaluate the integrand at all LGR points:

Let

Then

The total cost is

A MATLAB-style implementation is:

mayer = endpointCost(X(1,:), t0, X(end,:), tf);
ell   = runningCost(X(1:N,:), U, t);
cost  = mayer + 0.5*(tf-t0)*(w.'*ell);

Boundary Constraints¶

Suppose the endpoint constraints are described by

Evaluate

These constraints are usually already returned as a vector and do not require additional reshaping.

Path Constraints¶

Suppose there are $n_c$ path-constraint functions. Evaluating them at all $N$ LGR points produces

The matrix is vectorized as

In MATLAB:

C = pathConstraints(X(1:N,:), U, t);
Ccol = C(:);

Stacking the Complete Constraint Vector¶

The full NLP constraint vector can be assembled as

The corresponding lower and upper bounds are stacked in the same order:

and

This arrangement is convenient for solvers such as SNOPT that permit general lower and upper bounds on constraint functions.

Objective Function Template¶

A practical MATLAB-style objective function is:

function J = objective(z, data)

N  = data.N;
nx = data.nx;
nu = data.nu;
w  = data.w;
tau = data.tau;

nX = (N+1)*nx;
nU = N*nu;

zx = z(1:nX);
zu = z(nX+1:nX+nU);
t0 = z(nX+nU+1);
tf = z(nX+nU+2);

X = reshape(zx, N+1, nx);
U = reshape(zu, N, nu);

t = 0.5*(tf-t0)*tau + 0.5*(tf+t0);

JM = endpointCost(X(1,:), t0, X(end,:), tf);
L  = runningCost(X(1:N,:), U, t);

J = JM + 0.5*(tf-t0)*(w.'*L);
end

Constraint Function Template¶

A practical constraint function is:

function K = constraints(z, data)

N  = data.N;
nx = data.nx;
nu = data.nu;
D  = data.D;
tau = data.tau;

nX = (N+1)*nx;
nU = N*nu;

zx = z(1:nX);
zu = z(nX+1:nX+nU);
t0 = z(nX+nU+1);
tf = z(nX+nU+2);

X = reshape(zx, N+1, nx);
U = reshape(zu, N, nu);

Xlgr = X(1:N,:);
t = 0.5*(tf-t0)*tau + 0.5*(tf+t0);

F = dynamics(Xlgr, U, t);

defect = D*X - 0.5*(tf-t0)*F;
defect_col = defect(:);

BC = endpointConstraints(X(1,:), t0, X(end,:), tf);

C = pathConstraints(Xlgr, U, t);
Ccol = C(:);

K = [defect_col; Ccol; BC];
end

Solver-Specific Treatment of Inequalities¶

Different NLP solvers accept constraints in different formats.

Bounded Constraint Form¶

Some solvers allow

This is natural for path constraints of the form

MATLAB fmincon Form¶

The standard fmincon interface uses

A two-sided bound

must be converted to

and

Thus:

c = [Ccol - Cmax_col;
     Cmin_col - Ccol];

ceq = [defect_col;
       BC];

Constructing the Initial Guess¶

An NLP solver requires an initial guess

A simple and widely used procedure is:

1. guess the initial and final state;

2. linearly interpolate the state over the LGR grid plus the terminal point;

3. guess the control at the first and last collocation points;

4. linearly interpolate the control over the LGR points;

5. guess $t_0$ and $t_f$;

6. vectorize and stack all quantities.

State Guess¶

Given guesses

construct

by straight-line interpolation at

Control Guess¶

Given endpoint or representative control guesses, interpolate over the LGR points.

Packing the Guess¶

Finally,

A MATLAB-style implementation is:

Xguess = interp1([-1; 1], [X0guess; Xfguess], ...
                 [tau; 1], 'linear');

Uguess = interp1([-1; 1], [U0guess; Ufguess], ...
                 tau, 'linear');

zguess = [Xguess(:);
          Uguess(:);
          t0guess;
          tfguess];

Variable and Constraint Bounds¶

Bounds must be packed in exactly the same order as the decision vector:

and similarly for $\boldsymbol{z}_{\max}$.

The defect constraints have zero lower and upper bounds:

Endpoint equality constraints also receive identical lower and upper bounds.

Recovering the Solution¶

After the NLP solver returns

unpack it using the same indexing convention:

Then reconstruct the physical time vector and inspect:

• state trajectories;

• control trajectories;

• endpoint residuals;

• defect residuals;

• path-constraint residuals;

• solver optimality measures;

• KKT multipliers and costate estimates.

Common Implementation Errors¶

1. Using inconsistent row-major and column-major packing.

2. Reshaping the state to $N\times n_x$ instead of $(N+1)\times n_x$.

3. Evaluating the control or dynamics at the noncollocated terminal point.

4. Omitting the time-scaling factor $(t_f-t_0)/2$.

5. Using $\boldsymbol{X}_N$ instead of $\boldsymbol{X}_{N+1}$ as the terminal state.

6. Stacking constraint values in an order inconsistent with their bounds.

7. Using the wrong sign convention for the defects.

8. Providing a differentiation matrix of the wrong size.

9. Forgetting to remove the row associated with a noncollocated point when a general differentiation routine returns a square matrix.

10. Starting directly with a complex research problem before validating the implementation on a known problem.

Computing LGR Points, Weights, and Differentiation Matrices¶

The implementation requires:

• the LGR nodes $\tau_i$;

• the quadrature weights $w_i$;

• the rectangular differentiation matrix $\boldsymbol{D}$.

A general differentiation routine may return a square matrix on the grid

For left-LGR collocation, the final point is noncollocated. Therefore, the row corresponding to $\tau_{N+1}=1$ must be removed so that

The columns must not be removed because all $N+1$ state values participate in the interpolation.

Recommended Validation Workflow¶

A robust development sequence is:

1. Implement a scalar problem with a known analytical solution.

2. Verify variable packing and unpacking.

3. Verify dimensions of every matrix and vector.

4. Check the objective independently.

5. Evaluate the constraints at a known feasible trajectory.

6. Solve the problem at low polynomial order.

7. Increase $N$ and verify convergence.

8. Compare the state and control against the known solution.

9. Recover and compare the costate when available.

10. Only then adapt the implementation to the research problem.

Minimal End-to-End Workflow¶

The complete computational process is:

1. Choose $N$.

2. Compute LGR points, weights, and $\boldsymbol{D}$.

3. Define state, control, and time bounds.

4. Construct $\boldsymbol{z}_{\mathrm{guess}}$.

5. Call the NLP solver.

6. Inside the objective function:

   1. unpack $\boldsymbol{z}$;

   2. reshape $\boldsymbol{X}$ and $\boldsymbol{U}$;

   3. compute physical time;

   4. evaluate the Mayer and Lagrange costs.

7. Inside the constraint function:

   1. unpack and reshape;

   2. evaluate the dynamics;

   3. form the defect matrix;

   4. vectorize defects;

   5. evaluate and vectorize path constraints;

   6. evaluate endpoint constraints;

   7. stack all constraints.

8. Recover the optimized trajectories.

9. Check residuals and convergence.

10. Extract multipliers and estimate costates if needed.

Summary¶

1. The NLP solver operates on one decision vector, not state and control matrices.

2. The discrete state and control arrays must be vectorized and stacked.

3. Inside each NLP function, the vector is unpacked and reshaped.

4. The LGR defect matrix is

5. Defect and path-constraint matrices must be converted into column vectors.

6. The objective consists of a Mayer term plus an LGR quadrature approximation.

7. Endpoint and path constraints are stacked with the defects.

8. A straight-line state and control profile is often sufficient as an initial guess for simple problems.

9. Solver interfaces differ mainly in how they accept equality and inequality constraints.

10. Implementation should first be validated on simple benchmark problems.