Consider a robot manipulator with joint coordinates \( q \in \mathbb{R}^n \) and joint torques \( u \in \mathbb{R}^n \). With state \( x = \begin{bmatrix} q \\ \dot q \end{bmatrix} \), after discretization with sampling period \( T_s \) we have a nonlinear model

\[ x_{k+1} = f(x_k, u_k), \quad k = 0,\dots,N-1. \]

A typical finite-horizon tracking optimal control problem (OCP) is

\[ \min_{\{u_k\}_{k=0}^{N-1}} J_N(x_0) = \sum_{k=0}^{N-1} \ell(x_k,u_k) + V_f(x_N) \]

\[ \text{s.t.}\quad x_{k+1} = f(x_k,u_k),\; x_k \in \mathcal{X},\; u_k \in \mathcal{U}. \]

For manipulators, a standard quadratic tracking cost is

\[ \ell(x_k,u_k) = (x_k - x_k^{\text{ref}})^{\top} Q (x_k - x_k^{\text{ref}}) + (u_k - u_k^{\text{ref}})^{\top} R (u_k - u_k^{\text{ref}}), \]

with positive semidefinite \( Q \succeq 0 \) and positive definite \( R \succ 0 \). The optimal control sequence \( \{u_k^\star\} \) minimizes \( J_N(x_0) \). In real-time control, however, the available computation time per sample, \( T_s \), is limited: the optimization must be completed (or truncated) before the next control update.

Denote by \( t_{\text{solve}}(N, n_x, n_u, n_{\text{iter}}) \) the solver time as a function of horizon length \( N \), state dimension \( n_x \), control dimension \( n_u \), and number of iterations \( n_{\text{iter}} \). Real-time feasibility requires

\[ t_{\text{solve}}(N, n_x, n_u, n_{\text{iter}}) < T_s. \]

The central design question of this lesson is: how much optimality are we willing to sacrifice (shorter horizon, simpler model, fewer solver iterations) to satisfy this inequality without losing stability and good tracking?

flowchart TD
  T["Task: track trajectory / stabilize"] --> F["Formulate finite-horizon OCP"]
  F --> C["Choose model, horizon N, constraints"]
  C --> S["Pick solver: LQR / iLQR / SQP / QP"]
  S --> RT["Estimate t_solve(N, n_x, n_u, n_iter)"]
  RT --> D{"t_solve < T_s ?"}
  D -->|yes| DEP["Deploy controller"]
  D -->|no| ADJ["Adjust: shorten N, simplify model, \nreduce iterations"]
  ADJ --> S
        

For sufficiently small deviations around a reference trajectory \( (x_k^{\text{ref}}, u_k^{\text{ref}}) \), the manipulator dynamics can be linearized:

\[ \delta x_{k+1} = A_k \,\delta x_k + B_k \,\delta u_k, \]

with \( \delta x_k = x_k - x_k^{\text{ref}} \) and \( \delta u_k = u_k - u_k^{\text{ref}} \). The quadratic cost becomes

\[ J_N(\delta x_0) = \sum_{k=0}^{N-1} \left( \delta x_k^{\top} Q_k \delta x_k + \delta u_k^{\top} R_k \delta u_k \right) + \delta x_N^{\top} P_N \delta x_N. \]

Dynamic programming yields the discrete-time Riccati recursion (backward in time)

\[ P_k = Q_k + A_k^{\top} P_{k+1} A_k - A_k^{\top} P_{k+1} B_k \left(R_k + B_k^{\top} P_{k+1} B_k \right)^{-1} B_k^{\top} P_{k+1} A_k, \]

\[ K_k = -\left(R_k + B_k^{\top} P_{k+1} B_k \right)^{-1} B_k^{\top} P_{k+1} A_k, \]

where \( n_x = \dim(x_k) \) and \( n_u = \dim(u_k) \). A naive estimate of the per-step complexity of the Riccati update is

\[ \mathcal{O}(n_x^3 + n_x^2 n_u + n_x n_u^2 + n_u^3), \]

dominated by \( \mathcal{O}(n_x^3) \) matrix multiplications and \( \mathcal{O}(n_u^3) \) matrix inversions. Over a horizon of length \( N \), full finite-horizon LQR planning has complexity roughly

\[ \mathcal{O}\big( N (n_x^3 + n_u^3) \big). \]

For nonlinear optimal control, iterative methods such as iLQR (iterative LQR) or DDP perform multiple forward and backward passes. Each iteration consists of:

• Forward rollout: simulate \( x_{k+1} = f(x_k,u_k) \) for \( k=0,\dots,N-1 \).
• Linearization: compute \( A_k = \partial f / \partial x \), \( B_k = \partial f / \partial u \).
• Backward Riccati-like pass: compute local gains \( K_k \), feedforward terms, and updated control sequence.

Assuming the cost of one dynamics evaluation is \( c_f \) floating-point operations, one iLQR iteration has cost

\[ \mathcal{O}\big(N c_f + N (n_x^3 + n_u^3)\big). \]

With \( n_{\text{iter}} \) iterations, the total complexity becomes

\[ \mathcal{O}\big(n_{\text{iter}} N c_f + n_{\text{iter}} N (n_x^3 + n_u^3)\big). \]

Thus horizon length, state dimension, and iteration count are the major levers for controlling \( t_{\text{solve}} \).

For linear time-invariant (LTI) dynamics \( x_{k+1} = A x_k + B u_k \), with quadratic cost

\[ J_N(x_0) = \sum_{k=0}^{N-1} \big(x_k^{\top} Q x_k + u_k^{\top} R u_k\big) + x_N^{\top} P_N x_N, \]

the finite-horizon optimal value can be written as

\[ J_N^{\star}(x_0) = x_0^{\top} P_0^{(N)} x_0, \]

where \( P_0^{(N)} \) is obtained from the Riccati recursion with terminal condition \( P_N^{(N)} \). For an infinite-horizon problem with the same cost, the optimal value has the form

\[ J_{\infty}^{\star}(x_0) = x_0^{\top} P_{\infty} x_0, \]

where \( P_{\infty} \) is the stabilizing solution of the discrete algebraic Riccati equation (DARE).

Under standard assumptions (\( (A,B) \) stabilizable and \( (Q^{1/2},A) \) detectable), one can show

\[ P_0^{(1)} \succeq P_0^{(2)} \succeq \dots \succeq P_{\infty}, \]

and hence

\[ J_1^{\star}(x_0) \geq J_2^{\star}(x_0) \geq \dots \geq J_{\infty}^{\star}(x_0), \]

so longer horizons never increase the optimal cost (for the same model and constraints). However, larger \( N \) increases computational cost and memory usage.

For manipulators with moderately large state dimension (\( n_x = 2 n_{\text{joints}} \) or more if additional states are used), it is often necessary to select a moderate horizon (\( N \) on the order of 20–50 samples) even when a longer horizon would yield better performance, precisely to keep \( t_{\text{solve}} \) below \( T_s \).

In practice, a common strategy is:

• Design an infinite-horizon LQR (or steady-state iLQR) to obtain a stabilizing feedback.
• Use a shorter finite horizon for trajectory tracking, with terminal cost \( P_N = P_{\infty} \) to approximate the infinite tail.
• Choose \( N \) as small as possible while keeping tracking performance within specifications.

The exact manipulator dynamics are

\[ M(q)\ddot q + C(q,\dot q)\dot q + g(q) = \tau + \tau_d, \]

where \( M(q) \) is the inertia matrix, \( C(q,\dot q) \) the Coriolis/centrifugal terms, \( g(q) \) gravity, and \( \tau_d \) disturbances. High-fidelity models include:

• Joint friction (static/Coulomb + viscous + Stribeck).
• Flexible joints/links, actuator dynamics, and motor models.
• Contact forces, joint limits, and saturation effects.

Including all these in \( f(x,u) \) improves open-loop prediction but increases:

• computation per dynamics evaluation \( c_f \), and
• nonlinearity and stiffness, which may slow numerical convergence.

A common trade-off is to design control-oriented models:

• Use rigid-body dynamics with simplified friction.
• Neglect lightly damped flexible modes when the bandwidth is low.
• Approximate constraints (e.g., soft penalties for joint limits).

These simplifications reduce \( c_f \) and improve solver robustness, but introduce model mismatch. From the perspective of optimal control, model mismatch appears as an additive disturbance or parameter uncertainty; the resulting controller is optimal for the approximate model but only near-optimal (or suboptimal) for the real plant.

Later robust and adaptive control chapters formalize how such mismatches affect stability and performance; here we simply note that moderate model simplifications are often necessary to reach real-time feasibility on embedded hardware.

In many manipulator applications, solving the OCP exactly at each sampling instant is impossible. Instead, we use suboptimal schemes:

• Warm-start with the previous optimal sequence, shifted by one step.
• Perform a small, fixed number of iLQR or SQP iterations.
• Apply only the first control input \( u_0 \) in the new sequence.

Let \( V_N(x) \) denote the finite-horizon value function (\( V_N(x_0) = J_N^{\star}(x_0) \) for the exact solution). Suppose our suboptimal controller produces an input sequence \( \tilde u(\cdot) \) with closed-loop cost \( \tilde J_N(x_0) \). We define a relative suboptimality factor \( \gamma \geq 1 \) by

\[ \tilde J_N(x_0) \leq \gamma \, J_N^{\star}(x_0). \]

If \( \gamma \) is moderately close to 1 (e.g., \( \gamma \leq 1.1 \)), then the degradation in tracking performance is typically acceptable in practice.

In receding-horizon control (the conceptual basis of MPC), a sufficient condition for stability of the origin is the decrease condition

\[ V_N(x_{k+1}) - V_N(x_k) \leq -\ell(x_k,u_k) \]

along the closed-loop trajectory. For suboptimal controllers, one can show that if the terminal cost and constraints are chosen appropriately and each optimization step yields a sufficient cost decrease, then the origin remains asymptotically stable.

The high-level real-time iteration scheme is:

flowchart TD
  X["Measure x_k"] --> W["Warm-start with shifted previous solution"]
  W --> I["Run few iLQR / SQP iterations"]
  I --> U["Extract first input u_k"]
  U --> A["Apply u_k to robot"]
  A --> P["Predict next state / shift horizon"]
  P --> X
        

The key trade-off is between:

• More iterations: better approximation to the optimal sequence, larger cost decrease, but higher \( t_{\text{solve}} \).
• Fewer iterations: guaranteed real-time execution, but potentially worse tracking.

In Python, typical choices for optimal control and manipulator dynamics are:

• numpy, scipy for linear algebra and integration.
• casadi for symbolic/algorithmic differentiation and nonlinear OCPs.
• roboticstoolbox-python or pin (Pinocchio) for kinematics and dynamics.

Below we implement a minimal iLQR-like routine for a simple 2-DOF manipulator model with limited iterations per control step. The manipulator dynamics are abstracted in a function f(x,u) (could be implemented via roboticstoolbox).

import numpy as np

# Dimensions
nq = 2
nx = 2 * nq
nu = nq

dt = 0.005  # sampling time

def manipulator_dynamics(x, u):
    """
    Simple control-oriented dynamics for a 2-DOF manipulator:
    x = [q1, q2, dq1, dq2]
    M(q) ddq + C(q,dq) dq + g(q) = u
    Here we use a toy model with diagonal M and simple damping.
    In practice, call a robotics library (e.g., Pinocchio) here.
    """
    q = x[:nq]
    dq = x[nq:]
    # Inertia matrix (approximate)
    M = np.diag(1.0 + 0.3 * np.cos(q))
    # Damping
    D = 0.05 * np.eye(nq)
    # Gravity
    g = np.array([9.81 * np.cos(q[0]),
                  9.81 * np.cos(q[1])])
    # Solve for ddq: M ddq = u - D dq - g
    ddq = np.linalg.solve(M, u - D @ dq - g)
    xdot = np.concatenate([dq, ddq])
    return xdot

def f_discrete(x, u):
    # Forward Euler discretization
    return x + dt * manipulator_dynamics(x, u)

# Quadratic stage cost
Q = np.diag([100.0, 100.0, 1.0, 1.0])
R = 0.01 * np.eye(nu)

def stage_cost(x, u, x_ref, u_ref):
    dx = x - x_ref
    du = u - u_ref
    return dx.T @ Q @ dx + du.T @ R @ du

def terminal_cost(x, x_ref):
    P_N = 10.0 * Q  # crude approximation of P_infinity
    dx = x - x_ref
    return dx.T @ P_N @ dx

def ilqr_step(x0, x_ref_traj, u_init, n_iter_rt=3):
    """
    Run a small, fixed number of iLQR iterations.
    x_ref_traj: array of shape (N+1, nx)
    u_init: initial guess, shape (N, nu)
    n_iter_rt: maximum iterations (real-time budget)
    """
    N = u_init.shape[0]
    u = u_init.copy()
    alpha = 0.5  # line-search parameter (fixed here)

    for it in range(n_iter_rt):
        # Forward rollout
        x_traj = np.zeros((N + 1, nx))
        x_traj[0] = x0
        cost = 0.0
        for k in range(N):
            x_traj[k + 1] = f_discrete(x_traj[k], u[k])
            cost += stage_cost(x_traj[k], u[k], x_ref_traj[k], np.zeros(nu))
        cost += terminal_cost(x_traj[-1], x_ref_traj[-1])

        # Backward pass (very simplified, no second-order terms)
        K = np.zeros((N, nu, nx))
        du_ff = np.zeros((N, nu))
        P = 10.0 * Q  # terminal cost matrix (same as in terminal_cost)

        for k in reversed(range(N)):
            # Linearize dynamics numerically around (x_k, u_k)
            xk = x_traj[k]
            uk = u[k]
            Ak = np.zeros((nx, nx))
            Bk = np.zeros((nx, nu))
            eps = 1e-5
            for i in range(nx):
                dx = np.zeros(nx); dx[i] = eps
                Ak[:, i] = (f_discrete(xk + dx, uk) - f_discrete(xk - dx, uk)) / (2.0 * eps)
            for j in range(nu):
                du_j = np.zeros(nu); du_j[j] = eps
                Bk[:, j] = (f_discrete(xk, uk + du_j) - f_discrete(xk, uk - du_j)) / (2.0 * eps)

            # Quadratic approximation of cost
            Qx = 2.0 * Q @ (xk - x_ref_traj[k])
            Qu = 2.0 * R @ (uk - np.zeros(nu))
            Qxx = 2.0 * Q
            Quu = 2.0 * R
            Qux = np.zeros((nu, nx))

            # Compute Q-function derivatives
            Qx = Qx + Ak.T @ P @ (f_discrete(xk, uk) - x_traj[k + 1])
            Qu = Qu + Bk.T @ P @ (f_discrete(xk, uk) - x_traj[k + 1])
            Qxx = Qxx + Ak.T @ P @ Ak
            Quu = Quu + Bk.T @ P @ Bk
            Qux = Qux + Bk.T @ P @ Ak

            # Feedback and feedforward gains
            Quu_reg = Quu + 1e-4 * np.eye(nu)
            K[k] = -np.linalg.solve(Quu_reg, Qux)
            du_ff[k] = -np.linalg.solve(Quu_reg, Qu)

            # Update P
            P = Qxx + K[k].T @ Quu @ K[k] + \
                K[k].T @ Qux + Qux.T @ K[k]

        # Control update with simple line search
        u_new = u.copy()
        x_new = x0.copy()
        for k in range(N):
            du = du_ff[k] + K[k] @ (x_new - x_traj[k])
            u_new[k] = u[k] + alpha * du
            x_new = f_discrete(x_new, u_new[k])

        u = u_new

    return u
      

Increasing n_iter_rt and implementing a more accurate backward pass (with full second-order terms) improves optimality but also increases computation. In a real-time controller, n_iter_rt is chosen such that the computation fits in the sampling period \( T_s \) on the target hardware.

In C++, typical libraries include:

• Eigen for linear algebra.
• RBDL or Pinocchio for rigid-body dynamics.
• Dedicated OCP libraries such as ACADO or HPIPM for real-time optimal control.

The snippet below implements a finite-horizon LQR Riccati recursion using Eigen. Reducing the horizon N or reusing precomputed matrices is a common way to reduce computation time.

#include <Eigen/Dense>
#include <vector>

using Eigen::MatrixXd;
using Eigen::VectorXd;

struct LQRParams {
  std::vector<MatrixXd> A;
  std::vector<MatrixXd> B;
  MatrixXd Q;
  MatrixXd R;
  MatrixXd P_N;
};

struct LQRGains {
  std::vector<MatrixXd> K;
  std::vector<VectorXd> kff;  // feedforward (if needed)
};

LQRGains finiteHorizonLQR(const LQRParams& p) {
  const int N = static_cast<int>(p.A.size());
  const int nx = static_cast<int>(p.Q.rows());
  const int nu = static_cast<int>(p.R.rows());

  std::vector<MatrixXd> P(N + 1);
  P[N] = p.P_N;

  LQRGains gains;
  gains.K.resize(N);
  gains.kff.resize(N);

  for (int k = N - 1; k >= 0; --k) {
    const MatrixXd& Ak = p.A[k];
    const MatrixXd& Bk = p.B[k];
    const MatrixXd& Pkp1 = P[k + 1];

    MatrixXd S = p.R + Bk.transpose() * Pkp1 * Bk;
    MatrixXd Kk = -S.ldlt().solve(Bk.transpose() * Pkp1 * Ak);

    MatrixXd Pk = p.Q
      + Ak.transpose() * Pkp1 * Ak
      + Ak.transpose() * Pkp1 * Bk * Kk
      + Kk.transpose() * Bk.transpose() * Pkp1 * Ak
      + Kk.transpose() * p.R * Kk;

    gains.K[k] = Kk;
    gains.kff[k] = VectorXd::Zero(nu);
    P[k] = Pk;
  }

  return gains;
}

// In real-time loop:
// 1. Linearize dynamics to get A_k, B_k around current trajectory.
// 2. Call finiteHorizonLQR with modest N (e.g. N=20).
// 3. Apply u_k = K_k * (x_k - x_ref_k).
      

For nonlinear problems, the same Riccati routine is used inside iLQR or DDP iterations. Real-time feasibility is achieved by choosing a small horizon and limiting the number of linearization/iteration steps per control update.

Java is less common for low-level robot control but can be used for higher-level optimal control. A typical linear algebra library is EJML. The following example sketches a discrete-time LQR solver using a fixed, small horizon to ensure real-time feasibility.

import org.ejml.simple.SimpleMatrix;

public class LQR {
    public static class LQRResult {
        public SimpleMatrix[] K;
    }

    public static LQRResult finiteHorizonLQR(SimpleMatrix[] A,
                                             SimpleMatrix[] B,
                                             SimpleMatrix Q,
                                             SimpleMatrix R,
                                             SimpleMatrix P_N) {
        int N = A.length;
        int nx = Q.numRows();
        int nu = R.numRows();

        SimpleMatrix[] P = new SimpleMatrix[N + 1];
        P[N] = P_N.copy();

        SimpleMatrix[] K = new SimpleMatrix[N];

        for (int k = N - 1; k >= 0; --k) {
            SimpleMatrix Ak = A[k];
            SimpleMatrix Bk = B[k];
            SimpleMatrix Pkp1 = P[k + 1];

            SimpleMatrix S = R.plus(Bk.transpose().mult(Pkp1).mult(Bk));
            SimpleMatrix Kk = S.invert().negative().mult(Bk.transpose()).mult(Pkp1).mult(Ak);

            SimpleMatrix Pk = Q.plus(
                    Ak.transpose().mult(Pkp1).mult(Ak))
                .plus(Ak.transpose().mult(Pkp1).mult(Bk).mult(Kk))
                .plus(Kk.transpose().mult(Bk.transpose()).mult(Pkp1).mult(Ak))
                .plus(Kk.transpose().mult(R).mult(Kk));

            K[k] = Kk;
            P[k] = Pk;
        }

        LQRResult result = new LQRResult();
        result.K = K;
        return result;
    }

    // Real-time loop snippet (pseudo-code):
    // SimpleMatrix x = ...;      // current state
    // SimpleMatrix xRef = ...;   // reference
    // SimpleMatrix u = K[0].mult(x.minus(xRef));
}
      

In a Java-based controller, the horizon N and matrix sizes are chosen such that the entire LQR computation fits within the control cycle, possibly with precomputed A and B from linearization.

In MATLAB, robotics and control toolboxes provide:

• robotics.RigidBodyTree for manipulator kinematics and dynamics.
• lqr for infinite-horizon LQR on linear systems.
• fmincon, Optimization Toolbox, and Model Predictive Control Toolbox for OCP/MPC.

Below we compare an infinite-horizon LQR with a finite-horizon LQR approximation using a short horizon N to satisfy real-time constraints.

% Linearized manipulator dynamics: x_{k+1} = A_d x_k + B_d u_k
A_d = ...;  % from c2d(linearized continuous-time model, Ts)
B_d = ...;

Q = diag([100 100 1 1]);
R = 0.01 * eye(2);

% Infinite-horizon LQR
[K_inf, P_inf, ~] = dlqr(A_d, B_d, Q, R);

% Finite-horizon LQR with horizon N
N = 30;        % choose small N to reduce computation
P = cell(N+1, 1);
K = cell(N, 1);
P{N+1} = P_inf;    % terminal cost approximation

for k = N:-1:1
    Ak = A_d;
    Bk = B_d;
    Pkp1 = P{k+1};

    S = R + Bk' * Pkp1 * Bk;
    Kk = -S \ (Bk' * Pkp1 * Ak);

    Pk = Q + Ak' * Pkp1 * Ak ...
           + Ak' * Pkp1 * Bk * Kk ...
           + Kk' * Bk' * Pkp1 * Ak ...
           + Kk' * R * Kk;

    K{k} = Kk;
    P{k} = Pk;
end

% Real-time implementation:
% At each sample, use K{1} as feedback gain (or time-varying K{k} along a trajectory).
% Simulink implementation can embed these gains in a discrete-time state-feedback block.
      

In Simulink, one can implement the manipulator dynamics using RigidBodyTree and a discrete-time LQR block. To emulate real-time feasibility, the optimization (computation of K) is typically done offline; online, only matrix-vector multiplications are performed.

Wolfram Mathematica provides symbolic and numeric tools for optimal control. A simple finite-horizon OCP for a low-dimensional manipulator can be implemented with NDSolve and NMinimize. Real-time feasibility is emulated by limiting iteration counts and accuracy.

(* Simple 1-DOF manipulator (pendulum-like) *)
g = 9.81;
l = 1.0;
m = 1.0;

(* Continuous-time dynamics *)
xDot[{q_, dq_}, u_] := {dq, (u - m g l Sin[q])/(m l^2)};

(* Discrete-time dynamics via Euler approximation *)
dt = 0.01;
fDiscrete[{q_, dq_}, u_] := Module[{xd = xDot[{q, dq}, u]},
  {q, dq} + dt * xd
];

(* Horizon and cost *)
N = 40;
xRef = Table[{0.0, 0.0}, {k, 0, N}];
R = 0.01;

(* Decision variables: input sequence u[0],...,u[N-1] *)
vars = Table[u[k], {k, 0, N - 1}];

simulateTrajectory[x0_, uSeq_List] := Module[{x = x0, traj = {x0}},
  Do[
    x = fDiscrete[x, uSeq[[k + 1]]];
    traj = Append[traj, x];
    ,
    {k, 0, N - 1}
  ];
  traj
];

costFunctional[x0_, uSeq_List] := Module[{traj, cost = 0.0},
  traj = simulateTrajectory[x0, uSeq];
  Do[
    Module[{xk = traj[[k + 1]], xr = xRef[[k + 1]]},
      cost += (xk - xr).(xk - xr) + R * uSeq[[k + 1]]^2;
    ],
    {k, 0, N - 1}
  ];
  cost
];

x0 = {0.2, 0.0};

(* Real-time style: limit MaxIterations and AccuracyGoal *)
sol = NMinimize[
   costFunctional[x0, Table[u[k], {k, 0, N - 1}]],
   vars,
   Method -> {"NelderMead"},
   MaxIterations -> 80,
   AccuracyGoal -> 4
];

uStar = Table[u[k] /. sol[[2]], {k, 0, N - 1}];
      

Reducing MaxIterations and AccuracyGoal yields faster but more suboptimal solutions. For actual real-time deployment, Mathematica is usually combined with code generation or off-line computation of feedback laws.

Problem 1 (Monotonicity of Finite-Horizon LQR Costs).
Consider an LTI system \( x_{k+1} = A x_k + B u_k \) with \( Q \succeq 0 \), \( R \succ 0 \). Let \( P_0^{(N)} \) be the initial Riccati matrix for horizon length \( N \) and zero terminal cost \( P_N^{(N)} = 0 \). Show that

\[ P_0^{(N+1)} \preceq P_0^{(N)}. \]

Solution.
For horizon \( N \), the value function is \( V_N(x_0) = x_0^{\top} P_0^{(N)} x_0 \). For horizon \( N+1 \) with the same system and cost, one feasible strategy is: apply zero input at time \( k = N \), and otherwise use the optimal policy of the \( N \)-step problem. This yields

\[ J_{N+1}^{\star}(x_0) \leq J_{N}^{\star}(x_0) \quad\Rightarrow\quad x_0^{\top} P_0^{(N+1)} x_0 \leq x_0^{\top} P_0^{(N)} x_0 \quad\forall x_0. \]

Hence \( P_0^{(N+1)} \preceq P_0^{(N)} \). This formalizes that longer horizons cannot increase the optimal LQR cost.

Problem 2 (Rough Real-Time Budget Estimation).
A 6-DOF manipulator has state dimension \( n_x = 12 \) and control dimension \( n_u = 6 \). Assume a single iLQR iteration costs roughly

\[ c_{\text{iter}} \approx \alpha N (n_x^3 + n_u^3), \]

with \( \alpha = 2 \times 10^{-7} \) seconds per floating-point operation, \( N = 30 \). Estimate \( t_{\text{solve}} \) for \( n_{\text{iter}} = 5 \), and determine whether a sampling period \( T_s = 5 \) ms is plausible.

Solution.
First compute \( n_x^3 = 12^3 = 1728 \), \( n_u^3 = 6^3 = 216 \), so

\[ n_x^3 + n_u^3 = 1728 + 216 = 1944. \]

Then

\[ c_{\text{iter}} \approx \alpha N (n_x^3 + n_u^3) = 2\times10^{-7} \cdot 30 \cdot 1944 \approx 0.011664\ \text{s}. \]

With \( n_{\text{iter}} = 5 \), we obtain

\[ t_{\text{solve}} \approx 5 \times 0.011664 \approx 0.0583\ \text{s} = 58.3\ \text{ms}. \]

This is much larger than \( T_s = 5 \) ms, so the chosen horizon and iteration count are not real-time feasible on the assumed hardware. One must reduce \( N \), reduce \( n_{\text{iter}} \), or optimize the implementation.

Problem 3 (Suboptimal Value Function Decrease).
Let \( V_N(x) \) be the optimal finite-horizon value function for a stabilizable system. Suppose a suboptimal real-time controller yields the inequality

\[ V_N(x_{k+1}) - V_N(x_k) \leq -\sigma \ell(x_k,u_k), \]

for some constant \( \sigma \in (0,1] \) and all \( k \). Argue that if \( \ell(x,u) \) is positive definite in \( x \), then the origin is asymptotically stable.

Solution.
The inequality yields

\[ V_N(x_{k+1}) \leq V_N(x_k) - \sigma \ell(x_k,u_k). \]

Since \( \ell(x,u) \) is positive definite in \( x \), there exists \( \alpha_1 > 0 \) such that \( \ell(x,u) \geq \alpha_1 \|x\|^2 \) for all admissible \( u \). Moreover, \( V_N(x) \) is positive definite and radially unbounded in \( x \) under standard assumptions. Therefore

\[ V_N(x_{k+1}) - V_N(x_k) \leq -\sigma \alpha_1 \|x_k\|^2, \]

so \( V_N(x_k) \) is a Lyapunov function with negative definite difference along trajectories. By the discrete-time Lyapunov theorem, the origin is asymptotically stable. The factor \( \sigma \) quantifies how much suboptimality (relative decrease) can be tolerated while preserving stability.

Problem 4 (Horizon Truncation with Terminal Cost).
For an LTI manipulator model with infinite-horizon LQR solution \( P_{\infty} \), consider the finite-horizon cost

\[ J_N(x_0) = \sum_{k=0}^{N-1} \big(x_k^{\top} Q x_k + u_k^{\top} R u_k\big) + x_N^{\top} P_{\infty} x_N. \]

Show that the corresponding finite-horizon value function satisfies

\[ J_N^{\star}(x_0) \geq J_{\infty}^{\star}(x_0) \]

and explain why \( J_N^{\star}(x_0) \) converges to \( J_{\infty}^{\star}(x_0) \) as \( N \to \infty \).

Solution.
The infinite-horizon optimal cost is

\[ J_{\infty}^{\star}(x_0) = \sum_{k=0}^{\infty} \big(x_k^{\top} Q x_k + u_k^{\top} R u_k\big), \]

with closed-loop dynamics under \( u_k = K_{\infty} x_k \). The finite-horizon cost with terminal weight \( P_{\infty} \) includes the tail cost approximation

\[ x_N^{\top} P_{\infty} x_N = \sum_{k=N}^{\infty} \big(x_k^{\top} Q x_k + u_k^{\top} R u_k\big) \]

when the infinite-horizon optimal feedback is applied from time \( N \) onward. Therefore, the infinite-horizon optimal policy is feasible for the finite-horizon problem and attains cost exactly \( J_{\infty}^{\star}(x_0) \), so

\[ J_N^{\star}(x_0) \leq J_{\infty}^{\star}(x_0) + \varepsilon_N, \]

where \( \varepsilon_N \) represents the extra flexibility the finite-horizon optimizer has over the first \( N \) steps. As \( N \to \infty \), the effect of the finite initial segment becomes negligible, and we obtain \( J_N^{\star}(x_0) \to J_{\infty}^{\star}(x_0) \). Monotonicity arguments similar to Problem 1 show that \( J_N^{\star}(x_0) \) is nonincreasing in \( N \), so it converges from above to the infinite-horizon optimum.

In this lesson we examined how optimal control formulations for manipulators interact with real-time constraints. We quantified the complexity of LQR and iLQR in terms of horizon length, state dimension, and iteration count, and showed how finite-horizon value functions approximate infinite-horizon optimality. We discussed the trade-offs between model fidelity and computational burden, and formalized suboptimal real-time iterations via value-function decrease conditions. Finally, we demonstrated how Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica implementations can exploit these trade-offs to yield real-time feasible yet near-optimal robot controllers.