Optimal Control — the theory, the practice, the rules of thumb.

The canonical continuous-time problem

Given dynamics $\dot{x} = f(x,u,t)$ with initial state $x(t_0) = x_0$, find the input trajectory $u(\cdot)$ on $[t_0, t_f]$ that minimizes the Bolza cost functional:

Subject to path constraints: $g(x,u,t) \le 0$, terminal constraints: $\psi(x(t_f), t_f) = 0$, and possibly state/control bounds: $u(t) \in \mathcal{U}$, $x(t) \in \mathcal{X}$.

Three common degenerate forms you'll see named in the wild:

Lagrange problem

Only the running term. $J = \int L\,d\tau$. Typical for tracking with control effort: $L = \|x - x_{\text{ref}}\|_Q^2 + \|u\|_R^2$.

Mayer problem

Only the terminal term. $J = \phi(x(t_f))$. Example: maximize final battery charge, final projectile range, or miss-distance.

Minimum-time problem

$L \equiv 1$, so $J = t_f - t_0$. The free terminal time makes this structurally different — the optimal control is almost always bang-bang.

What "optimal" actually means

You do not get a closed-form $u^\star(t)$ by magic. You get one of three things, depending on the approach:

• An open-loop trajectory $u^\star(t)$ — a schedule. Calculus of variations and Pontryagin give you this. It is optimal only for the nominal model and the exact initial condition.
• A feedback policy $u^\star(x, t) = \pi(x,t)$ — a rule that maps state to input. Dynamic programming (HJB) gives you this. It's robust to disturbances insofar as the model is correct.
• A numerical approximation of either, computed once (offline) or re-computed at each control step (online, i.e. MPC).

Core objects you need to be fluent with

State $x \in \mathbb{R}^n$

The minimal information required to predict future behavior given future inputs. Not outputs — measurements live elsewhere.

Control $u \in \mathbb{R}^m$

What you get to choose. Usually constrained (actuator limits, safety bounds).

Costate / Adjoint $\lambda \in \mathbb{R}^n$

Shadow price of the state. $\lambda_i(t)$ = marginal change in $J^\star$ per unit change in $x_i$ at time $t$. Central to Pontryagin; dual variable to the dynamics constraint.

Hamiltonian $H(x,u,\lambda,t)$

$H = L + \lambda^\top f$. Pontryagin's whole principle reduces to "minimize $H$ in $u$, pointwise in time."

Value function $V(x,t)$

The optimal cost-to-go from state $x$ at time $t$. Bellman's object. Satisfies the HJB PDE.

Policy $\pi(x,t)$

The feedback law. For LQR: $\pi(x) = -Kx$ with constant $K$ (infinite horizon).

Consider minimizing $J[x(\cdot)] = \int_{t_0}^{t_f} L(x(t), \dot x(t), t)\, dt$ over smooth trajectories $x(\cdot)$ with fixed endpoints. A necessary condition for a minimizer is the Euler–Lagrange equation:

Derivation sketch: perturb $x \to x + \epsilon \eta$ with $\eta$ vanishing at endpoints. Require $\left.\tfrac{d}{d\epsilon}\right|_{\epsilon=0} J = 0$. Integrate by parts. The "arbitrariness" of $\eta$ gives the equation pointwise.

Transversality at free endpoints

If $t_f$ or $x(t_f)$ is free, you get boundary conditions for free:

• Free final state $x(t_f)$: $\left.\frac{\partial L}{\partial \dot x}\right|_{t=t_f} = 0$ (or $\lambda(t_f) = \nabla_x \phi$ in optimal-control form).
• Free final time $t_f$: $\left.\left(L - \dot x^\top \frac{\partial L}{\partial \dot x}\right)\right|_{t=t_f} = 0$ (the Hamiltonian vanishes at the free endpoint).

From variations to control

Optimal control generalizes this by splitting $\dot x$ into "dynamics $f(x,u,t)$", introducing $u$ as a separate variable, and handling constraints explicitly. The costate $\lambda$ plays the role that $\partial L / \partial \dot x$ played in the classical problem.

Statement

Define the Hamiltonian

Then along the optimal trajectory:

The first two give you a two-point boundary value problem (TPBVP): $x$ known at $t_0$, $\lambda$ known at $t_f$. The third is the point-wise optimality. The difficulty of the problem lives almost entirely in the TPBVP — it's forward-backward, which is why indirect methods are numerically tricky.

Unconstrained $u$: the stationarity condition

If $u$ is unconstrained and $H$ is differentiable in $u$, then "minimize in $u$" becomes $\nabla_u H = 0$. For $L = \tfrac{1}{2} u^\top R u + \cdots$ with $R \succ 0$ and $f$ control-affine ($f = a(x) + B(x) u$), this gives:

This is the control-affine shortcut that powers LQR, iLQR, and most of classical optimal control.

Constrained $u$: bang-bang emerges

If $u \in [u_{\min}, u_{\max}]$ and $H$ is linear in $u$ (which happens for $L$ independent of $u$, e.g. minimum-time), then minimizing $H$ picks $u$ at a corner of the constraint set whenever the "switching function" $\sigma(t) = \lambda^\top B$ is nonzero:

When $\sigma(t) \equiv 0$ on an interval, you have a singular arc, and $u$ is determined by differentiating $\sigma$ until $u$ appears — a delicate case.

The principle of optimality

Bellman's insight, crudely: whatever you did to get to state $x$ at time $t$, the optimal continuation from $(x,t)$ only depends on $(x,t)$. Formally:

Taking $\Delta t \to 0$ and expanding $V$:

Notice the clean correspondence: $\lambda = \nabla_x V$. The costate is the gradient of the value function. This is exactly why PMP and HJB agree where both are applicable — they're two views of the same optimality.

Discrete version (Bellman equation)

Solvable by backward recursion when the state/action spaces are finite and small. For continuous states, you must parametrize $V$ — linear combinations of basis functions, neural networks, tensor trains, etc. That's where Approximate Dynamic Programming lives.

Curse of dimensionality

A grid of $N$ points per dimension over $n$ state dimensions is $N^n$ cells. For $n=10$ and $N=100$ that's $10^{20}$ — hopeless. This is why HJB is rarely solved directly outside $n \le 4$ or so. The exceptions:

• Linear-quadratic problems: $V$ is quadratic in $x$, so it's parametrized by a symmetric matrix $P$. The HJB collapses to a Riccati equation. See §5.
• Structured problems: hybrid systems, decomposable dynamics, reachability (HJI).
• Approximation: use a function approximator, iterate. This is value iteration / ADP / deep RL depending on which community you hang out with.

Setup

With $Q \succeq 0$, $Q_f \succeq 0$, $R \succ 0$. (Strict positive definiteness of $R$ is required — otherwise the minimization is ill-posed.)

The finite-horizon solution

Write $V(x,t) = \tfrac{1}{2} x^\top P(t) x$. Plug into HJB. Match quadratic coefficients. You get:

Integrate backward in time from $t_f$ to $0$. Then the optimal controller is

Feedback gain $K(t) = R^{-1} B^\top P(t)$ is time-varying.

Infinite horizon ($t_f \to \infty$)

If $(A,B)$ is stabilizable and $(A, Q^{1/2})$ is detectable, $P(t)$ converges to a constant $P_\infty$ satisfying:

with the stabilizing solution $P \succ 0$ (the unique one such that $A - BK$ is Hurwitz, $K = R^{-1}B^\top P$). That's your constant-gain feedback controller.

Discrete-time LQR

For $x_{k+1} = A_d x_k + B_d u_k$ and cost $\sum_{k=0}^{N-1}(x_k^\top Q x_k + u_k^\top R u_k) + x_N^\top Q_f x_N$:

Infinite horizon: iterate until $P_k$ converges — that's the Discrete Algebraic Riccati Equation (DARE) solution.

Gain margin and stability margins

LQR has beautiful classical guarantees for the full state feedback case:

• Infinite gain margin in the stable direction — you can multiply the gain by any number $\ge 1/2$ and still be stable.
• Gain margin of $\ge 6$ dB in reduction (i.e., down to $K/2$).
• Phase margin of $\ge 60°$.

These are for SISO loop-at-a-time. MIMO has multivariable equivalents (e.g., return difference inequality: $|I + L(j\omega)| \ge I$).

Backward iteration (the textbook method)

Just run the discrete Riccati backward from $P_N = Q_f$ until $\|P_{k+1} - P_k\|$ is below tolerance. Simple, robust, no linear algebra beyond matrix multiply and small inverse. Convergence is linear — doubling the precision takes a fixed number of iterations, typically 20–100 for well-conditioned problems.

Hamiltonian matrix / Schur method

Form the $2n \times 2n$ Hamiltonian matrix:

It has $n$ stable and $n$ unstable eigenvalues (in mirror-image symmetry across the imaginary axis for CT, unit circle for DT). Compute the real Schur form, reorder so stable eigenvalues are in the top-left, extract the stable invariant subspace:

This is what MATLAB's care/dare actually does. Numerically stable, one-shot.

Doubling algorithm (structured DARE)

For DARE, the structured doubling algorithm (SDA) doubles the horizon per step: $P_k \to P_{2k}$ at each iteration. Quadratic convergence, very fast, preserves structure. Method of choice for large-scale DAREs.

Numerical tips, the ones that matter

• Check the problem is well-posed: $(A,B)$ stabilizable, $(A, Q^{1/2})$ detectable. If detectability fails, the algebraic equation has non-stabilizing solutions too, and iteration can land on a wrong fixed point.
• Balance the system first for ill-conditioned $A$. balreal in MATLAB, or equivalent scaling in SciPy.
• Use the square-root form for ill-conditioned $P$: propagate $S$ with $P = S S^\top$ using QR updates. Avoids loss of symmetry/positive-definiteness.
• Don't compute $R^{-1}$ directly. Solve the linear system $(R + B^\top P B) X = B^\top P A$ for $X = K$ via Cholesky. Halves the cost and better conditioned.
• Offline is fine for LTI. For embedded real-time with an LTI plant, solve DARE offline once, hardcode $K$ as a constant. Spend zero cycles at runtime.

Stochastic problem setup

Minimize $E\!\left[ \int_0^\infty (x^\top Q x + u^\top R u)\, dt \right]$.

The separation principle

The optimal controller is:

1. Estimate $\hat x(t)$ via the Kalman filter (the optimal linear unbiased estimator under Gaussian noise).
2. Apply $u(t) = -K \hat x(t)$ where $K$ is the deterministic LQR gain from the CARE.

The "filter Riccati" dual to the control Riccati gives the Kalman gain $L$:

Observer dynamics: $\dot{\hat x} = A\hat x + Bu + L(y - C\hat x)$.

Discrete Kalman filter (the one you actually code)

Use the Joseph form for $\Sigma_{k|k}$ (shown above) — symmetric and positive semi-definite under roundoff. The textbook form $(I - L_k C)\Sigma_{k|k-1}$ is algebraically equivalent but loses symmetry.

Extended and Unscented variants for nonlinear systems

• EKF: linearize $f$, $h$ around current estimate each step. Reuses the Kalman equations with $A_k = \partial f/\partial x|_{\hat x_k}$. Cheap. Can diverge if nonlinearity is severe or initial guess is poor.
• UKF: propagate sigma points through the nonlinear map, reconstruct mean/covariance. Better statistics for moderately nonlinear systems. ~$2n+1$ state propagations per step.
• Particle filter: for non-Gaussian, heavily multimodal, or highly nonlinear problems. Expensive, not typically real-time unless state dimension is tiny.

The MPC loop

At time $k$, given current state estimate $\hat x_k$, solve:

Apply $u_k \leftarrow u_0^\star$. Discard the rest. Advance $k$. Repeat.

Why receding horizon (not just open-loop)?

• Disturbance rejection: re-measuring the state and re-optimizing corrects for model mismatch and disturbances.
• Implicit feedback: the closed loop $k \mapsto u_k(\hat x_k)$ is a state-feedback policy, even though each instance is an open-loop optimization.
• Explicit constraints: $\mathcal{U}, \mathcal{X}$ enter the QP directly. No anti-windup hacks.

Constraints and the QP

For linear dynamics with polytopic constraints (box bounds on $u$ and $x$, linear inequalities), the MPC subproblem is a quadratic program:

where $z = (u_0, \ldots, u_{N-1})$ (condensed form) or $z = (x_0, u_0, x_1, u_1, \ldots)$ (sparse form). Sparse form has better asymptotic complexity for long horizons; condensed form is often faster for $N \lesssim 20$.

Stability and recursive feasibility

These are the two things you must verify — not just hope for.

Recursive feasibility

If the MPC QP is feasible at step $k$, will it still be feasible at $k+1$? Not automatic with state constraints. Guaranteed by terminal set + control-invariance of that set.

Closed-loop stability

Standard sufficient conditions: (a) terminal cost $P$ is a Lyapunov function on a terminal set $\mathcal{X}_f$, (b) $\mathcal{X}_f$ is positively invariant under the terminal controller, (c) running cost is positive definite. Take $P$ = DARE solution for the unconstrained terminal controller, $\mathcal{X}_f$ = maximal positively invariant set under the LQR law.

Warm-starting

At step $k+1$, use the shifted solution from step $k$ (drop $u_0^\star$, append a terminal input) as initial guess. Huge speedup for active-set and interior-point QP solvers.

Solver landscape (what you'd actually use)

iLQR — iterative LQR

Given a nominal trajectory $(\bar x_k, \bar u_k)$, linearize the dynamics and quadratize the cost around it. The resulting time-varying LQR sub-problem has a closed-form solution via the discrete Riccati recursion. Apply the update, relinearize, repeat. Converges superlinearly near a local optimum.

# Given nominal (x̄, ū), cost ℓ, dynamics f
for iter in range(max_iter):
    # backward pass: build quadratic Q, solve Riccati
    V_x, V_xx = ∂ℓ_N/∂x, ∂²ℓ_N/∂x²
    for k in reversed(range(N)):
        # Q-function expansion around (x̄_k, ū_k)
        A_k, B_k = ∂f/∂x, ∂f/∂u
        Q_x  = ℓ_x  + A_k.T @ V_x
        Q_u  = ℓ_u  + B_k.T @ V_x
        Q_xx = ℓ_xx + A_k.T @ V_xx @ A_k
        Q_uu = ℓ_uu + B_k.T @ V_xx @ B_k
        Q_ux = ℓ_ux + B_k.T @ V_xx @ A_k
        # feedforward + feedback gains
        k_ff =  -Q_uu⁻¹ Q_u
        K_fb =  -Q_uu⁻¹ Q_ux
        # value update
        V_x  = Q_x  + K_fb.T @ Q_uu @ k_ff + K_fb.T @ Q_u  + Q_ux.T @ k_ff
        V_xx = Q_xx + K_fb.T @ Q_uu @ K_fb + K_fb.T @ Q_ux + Q_ux.T @ K_fb
    # forward pass with line search on α ∈ (0,1]
    for k in range(N):
        u_k = ū_k + α·k_ff_k + K_fb_k @ (x_k - x̄_k)
        x_{k+1} = f(x_k, u_k)
    if converged: break
    x̄, ū = x, u

DDP vs iLQR

DDP (Differential Dynamic Programming) also includes second-order dynamics terms $f_{xx}, f_{uu}, f_{xu}$ in the backward pass. iLQR drops them (Gauss–Newton style). In practice:

• iLQR is far easier to implement, needs only first-order dynamics derivatives.
• DDP converges faster near the optimum but is more sensitive to indefinite Hessians.
• For 90% of robotics/aerospace use cases, iLQR is the right default.

Direct collocation and shooting

Instead of solving the TPBVP, discretize $(x_0, u_0, \ldots, x_N, u_N)$ as NLP variables and impose dynamics as equality constraints between knots:

• Multiple shooting: integrate dynamics within each interval, enforce matching at knots. Good for stiff ODEs, parallelizable.
• Trapezoidal collocation: approximate $\dot x$ on each segment with trapezoid rule, enforce as equality. Second-order accurate, very common.
• Hermite–Simpson or LGR pseudo-spectral: higher-order, sparser in time, great for long-horizon problems with smooth dynamics.

Feed the NLP to IPOPT, SNOPT, or a custom SQP. This is the workhorse of trajectory optimization for aircraft, spacecraft, and robots.

Policy iteration (the Rosetta stone)

For a fixed policy $\pi$, the value function $V^\pi$ satisfies the policy evaluation equation (linear):

Given $V^\pi$, improve the policy greedily:

Alternating these two steps converges to $\pi^\star$. In control: this is Howard's policy iteration. In RL: same thing, but with sampled trajectories replacing exact expectations and function approximation replacing exact $V$.

The LQR–RL bridge

For LQR problems, policy iteration on $V(x) = x^\top P x$ with $\pi(x) = -Kx$ converges to the CARE solution in closed form. Every iteration solves a Lyapunov equation (linear in $P$, given $K$), which is trivial. This is known as Kleinman's algorithm (1968) — the direct ancestor of modern actor-critic methods.

Quadratic convergence to CARE solution. Each step is a Lyapunov equation. Foundational.

Q-learning and the discrete-time dual

Define the state-action value $Q^\pi(x,u) = L(x,u) + \gamma V^\pi(f(x,u))$. Then the optimal $Q^\star$ satisfies:

This is what Q-learning approximates. For LQR, $Q^\star(x,u) = [x;u]^\top H [x;u]$ is quadratic — and the greedy policy $u^\star = -H_{uu}^{-1} H_{ux}\,x$ is exactly the LQR gain. You can learn $H$ without knowing $(A,B)$ by solving least-squares on observed $(x,u,x')$ tuples. This is model-free LQR — a staple of adaptive dynamic programming.

Tuning Q and R

Discretization

Sanity checks before deployment

MPC-specific

Numerical hygiene

Offline vs. online computation

For LTI plants, push everything offline that possibly can be:

• Solve CARE/DARE at design time, hardcode $K$ as a constant matrix.
• For time-varying $K(t)$ (gain scheduling, finite horizon), precompute a lookup table indexed by operating point or time, interpolate online.
• For explicit MPC (small state/input dims, polytopic constraints): compute the explicit piecewise-affine solution offline via multi-parametric QP. Online cost: region lookup + affine eval. Microsecond execution.

Fixed-point vs. floating-point

Automotive and aerospace platforms often mandate fixed-point to avoid floating-point exceptions and determinism issues (and because AUTOSAR-generated C from TargetLink frequently targets fixed-point integer types). Implications:

• Scale state and control to fit in int16/int32. Pick scalings that leave headroom for $|Q_{\max}|$ dB of transient overshoot.
• Precompute $K$ in fixed-point with appropriate binary-point placement. Multiplying $K \cdot x$ produces a wider intermediate — manage overflow explicitly.
• Saturation vs. wrap: always saturate, never wrap. Unsigned wrap in a control signal has broken more vehicles than any other bug class.
• Avoid division in the loop: precompute $R^{-1}$ or its factorization. Divisions are slow and often non-constant-time on embedded ALUs.

MPC on a microcontroller

Yes, it's feasible. Here's the reality:

• 100–1000 Hz rates are routine for $n \le 10$, $N \le 20$ on an Infineon TriCore or ARM Cortex-M7. 10 kHz is achievable for condensed QPs with $\sim 50$ decision variables using OSQP code-gen or HPIPM with custom BLAS.
• Fixed number of solver iterations. Cap the solver at $k_{\max}$ iterations regardless of convergence. This bounds worst-case execution time (WCET) deterministically, at the cost of sub-optimal solutions occasionally. In practice, with warm-starting, the cap is almost never binding.
• Watch memory. Condensed QP Hessian for $N=20, m=2$ is $40 \times 40$ = 6.4 KB in float32. Fine. Sparse MPC at $N=50$, $n=10$ needs more care.
• WCET analysis is mandatory. If you can't certify a bound, you can't ship it on a safety-critical target (ASIL-B and above).

State estimator: covariance propagation tricks

• Square-root / UD forms: propagate factors of $\Sigma$, never $\Sigma$ itself. Bierman/Thornton UDU factorization is the classic, used by aerospace nav filters for decades. Halves dynamic range required, guarantees positive-definiteness.
• Stationary Kalman filter: if $A, W, C, V$ are time-invariant, $L$ converges to a constant. Precompute offline, deploy as a static gain. Same speed as Luenberger observer, optimal noise rejection.
• Delta updates for sparse $C$: when only one sensor arrives asynchronously, update one scalar at a time. Avoid the full matrix inversion.

Validation workflow

1. Model-in-the-loop (MIL): Simulink/numerical, bit-exact to design. Full coverage including corner cases.
2. Software-in-the-loop (SIL): generated C against plant simulation. Verifies code-gen hasn't introduced bugs.
3. Processor-in-the-loop (PIL): C running on target MCU, plant on host. Exposes timing and fixed-point effects.
4. Hardware-in-the-loop (HIL): full controller + real sensors/actuators against plant model. This is where you find integration bugs (message ordering, timer drift, CAN bus jitter).
5. Bench test → rig test → vehicle test. Measure closed-loop frequency response. Compare to LTI model to catch unmodeled dynamics before they bite.