Part of World Models Handbook
Covers classical planning and optimal control: state-space search with A*, dynamic programming, LQR.
Choose your expertise level to adjust how many terms are explained. Beginners see more tooltips, experts see fewer to maintain reading flow. Hover over underlined terms for instant definitions.
Article links
Make inline references clickable
Classical Planning and Optimal Control
Imagine a warehouse robot that must fetch a pallet from the far corner of a cluttered aisle. Chapters 1 through 3 of this part have shown you how to build and sharpen the pieces that make such prediction possible: a world model that captures the environment's dynamics, belief states that summarize what the robot knows, and system identification that fits those dynamics from data. But a model that predicts is not yet an agent that acts. Rolling the model forward under one fixed candidate policy evaluates the futures that policy induces; planning must compare or improve the available actions and policies. It must weigh competing futures, decide which are desirable, and commit before the world reveals what will happen. Closing that gap between predicting and choosing is the subject of this chapter.
This chapter develops classical answers to that choice problem. We will work through four families of methods that share a model and an objective but differ in how they represent decisions and whether they use a finite horizon, an infinite horizon, or goal termination:
- State-space search treats the world as a graph and finds a path through it.
- Dynamic programming solves for a value function over all states at once, using the Bellman equations.
- Linear-quadratic control exploits linear dynamics and quadratic cost to obtain an analytic feedback law after solving a Riccati equation.
- Trajectory optimization optimizes a control trajectory and, in direct transcription, may optimize state and control variables jointly under task constraints.
These methods are decades old, but they remain useful reference points. Many later methods approximate or extend them, or address one of their failure modes. When their assumptions fit, they supply concrete baselines: would exact search find a shorter path, would tabular value iteration converge, and does a learned controller stabilize the system under the same conditions as LQR? Those comparisons make later claims about learned planners easier to assess.
To preview one of these design decisions concretely, consider the discount factor. It is a modeling choice, but in discounted value iteration it is also the worst-case contraction modulus. The following one-state continuing problem isolates that effect.
import numpy as np
## One-state continuing MDP: reward -1 and a deterministic self-transition.
## The Bellman residual contracts by exactly gamma on each sweep.
gamma_values = np.array([0.5, 0.7, 0.8, 0.9, 0.95, 0.99])
sweeps_needed = []
for g in gamma_values:
v = 0.0
for sweep in range(1, 5001):
v_new = -1.0 + g * v
if abs(v_new - v) < 1e-6:
v = v_new
break
v = v_new
sweeps_needed.append(sweep)
Here the initial Bellman residual is one, and the residual after updates is . Reaching tolerance therefore takes about sweeps. In a larger MDP, the observed count also depends on the dynamics, initialization, norm, and stopping rule, but still controls the worst-case geometric bound.
There is also a practical reason to master these methods before we complicate them. Search shows the computational value of heuristic information through expansions saved. Dynamic programming exposes the cost of enumerating a state space. In LQR, the matrices and jointly shape the trade-off between state regulation and control effort. Nonlinear trajectory optimization with a local solver exposes sensitivity to initialization and constraints. These same design choices remain when hand-built models are replaced by learned ones, but their effects become harder to isolate.
From Prediction to Decision
Let's separate the two roles cleanly, because blurring them causes endless confusion.
The transition model answers a predictive question: given a state and an action, what happens next? In notation we have used throughout Part II, it provides . A learned reward model may also predict the numeric value of an externally defined objective. The transition dynamics are descriptive; the reward or cost supplies the normative criterion used by the planner. A perfect weather simulator predicts well but does not, by itself, specify what an agent should prefer.
The controller or planner answers a normative question: given that predictive machinery, which actions should we take to accumulate the most reward (or the least cost) over some horizon? The planner is prescriptive. It consumes predictions and produces commitments. The two components can be developed and unit-tested separately, but they must be validated jointly because planning changes which parts of the model matter.
Given an initial state , a model of the environment, a reward or cost function, and a planning horizon, find an action sequence or a policy that maximizes expected cumulative reward. For an open-loop sequence, we seek
where:
- : the open-loop action sequence being optimized, one action per time step
- : the planning horizon, the number of steps into the future
- : the reward assigned by the planning objective to state-action pair , possibly supplied by a separate learned reward model
- : the discount factor, weighing future rewards against immediate ones
- : the expectation over successor-state randomness under trajectories consistent with the planning transition model; the reward or cost may be specified separately from that model.
The formulation above hides an important choice. An open-loop plan commits to an action sequence before seeing where the actions land, while a closed-loop policy can react to new state measurements or estimates. The basic search and trajectory-optimization forms used here produce open-loop plans; tabular dynamic programming and LQR produce feedback policies. Receding-horizon methods partially close the loop by replanning from new information, which can limit error accumulation but does not guarantee robustness to a bad model.
Two distinctions from earlier chapters matter immediately. First, open-loop prediction quality and closed-loop decision usefulness can diverge. A model can fit one-step transitions in a validation set yet remain biased along the directions a planner exploits. Conversely, a controller can tolerate sizable task-irrelevant or feedback-correctable errors when the resulting closed loop remains stable. Predictive fidelity, representation quality, uncertainty quality, and decision usefulness therefore need separate tests. Second, planning from a model means reasoning about futures that have not been experienced, which is valuable precisely where multi-step model error becomes dangerous.
A useful mental split for this whole chapter:
- Search asks: what is the best single path from here to there?
- Dynamic programming asks: what is the best action from every state?
- LQR asks: what is the optimal feedback law for known linear dynamics and quadratic cost?
- Trajectory optimization asks: what state-and-control trajectory best satisfies the dynamics, objective, and any task constraints?
These four questions sit on a spectrum of scope. The A* example answers one start-goal query. Tabular dynamic programming amortizes work over every represented state. LQR gives a feedback formula under linear-quadratic assumptions. Nonlinear trajectory optimization accepts broader dynamics and costs, but still relies on a finite parameterization and on solver assumptions such as differentiability or constraint regularity.
We now take these in turn, using one running example, a small grid robot, for the discrete methods, and a double integrator plus a pendulum for the continuous ones.
State-Space Search
When the world is deterministic and fully observable and the reachable state-action space is represented as a discrete graph, planning can be posed as graph search. States become nodes, actions become edges, and action costs become edge weights. The planner's job is to find a minimum-cost path from the current state to a goal state.
This is a transparent relationship between model and decision: the model is the graph itself, and a rollout is a walk along edges. There is no expectation to average over, no discounting to tune, and no value function to estimate. The planning problem is combinatorial: which of the potentially exponentially many simple paths from start to goal is cheapest?
This abstraction covers route planning, puzzles, symbolic task planning, and motion planning on discretized configuration spaces. Other tasks can be approximated as search when a finite state representation and known transitions are available. Exact search is then a useful baseline when the graph size, costs, and query structure make it tractable.
Dijkstra's algorithm and uniform-cost search
If all we know is the cost of each action, the right baseline is Dijkstra's algorithm. It maintains a priority queue of frontier states ordered by the cheapest known cost to reach them, and repeatedly expands the cheapest frontier state. When a state is expanded, we can be certain that no cheaper route to it exists, because any other route would have to pass through the frontier, and everything on the frontier is already at least as expensive. When the goal is expanded, the path is therefore guaranteed optimal, provided all costs are nonnegative.
The price is that Dijkstra is uninformed about where the goal lies. On the uniform grid used below, its shortest-distance sublevel set grows like a blob around the start until it reaches the goal. On an irregular weighted graph that set need not resemble a disk, and the same work may be reusable for other queries.
A* and the value of a heuristic
A* fixes this by adding heuristic knowledge. Each frontier state is ranked not by cost-so-far alone, but by
where:
- : the cheapest known cost to reach from the start, accumulated along the best path found so far
- : an estimate of the remaining cost from to the goal
- : the estimated total cost of a solution path passing through , the priority used to rank frontier states
If were the true remaining cost, A* would avoid nodes that cannot belong to an optimal solution, although ties among equally optimal alternatives can still cause extra expansions. In practice is a cheap lower bound, such as straight-line distance for a robot or Manhattan distance on a grid. These estimates ignore walls, detours, and dynamics, which makes them optimistic.
A heuristic is admissible if it never overestimates the true remaining cost: for every state . It is consistent (or monotone) if for every edge. Admissibility is enough for A* tree search, while graph-search implementations with a permanent closed set require consistency or must reopen nodes when a cheaper path appears. Manhattan distance on this unit-step grid is both admissible and consistent.
Why does optimism translate into optimality? Let be the optimal solution cost. Before the optimal goal is popped, at least one frontier entry on an optimal path has ; admissibility then gives . A suboptimal goal has and therefore . A* cannot pop that goal while the optimal-prefix entry remains. For graph search, consistency or node reopening preserves this invariant when paths merge.
The heuristic is encoded task knowledge that accelerates planning. It approximates remaining cost without simulating the full dynamics. Manhattan distance knows nothing about the walls in our grid, yet it reduces search effort because cost tends to grow with distance. The same pattern, an exact method guided by an approximation of cost-to-go, reappears in learned latent-space planning.
Let's build a small grid world with two walls, each with a gap, and compare A* against Dijkstra.
from heapq import heappop, heappush
import numpy as np
grid = np.zeros((8, 12), dtype=int)
grid[0:6, 4] = 1 # first wall, gap along the bottom two rows
grid[2:, 8] = 1 # second wall, gap along the top two rows
start, goal = (6, 0), (1, 11)
def astar(grid, start, goal, heuristic):
"""4-connected grid search. heuristic=None reduces A* to Dijkstra."""
rows, cols = grid.shape
h = heuristic if heuristic is not None else (lambda s: 0)
frontier = [(h(start), 0, start)]
came_from = {start: None}
cost_so_far = {start: 0}
expanded = set()
while frontier:
_, g, current = heappop(frontier)
if current == goal:
break
if current in expanded:
continue
expanded.add(current)
for dr, dc in ((-1, 0), (1, 0), (0, -1), (0, 1)):
r, c = current[0] + dr, current[1] + dc
if 0 <= r < rows and 0 <= c < cols and grid[r, c] == 0:
nxt = (r, c)
new_cost = g + 1
if nxt not in cost_so_far or new_cost < cost_so_far[nxt]:
cost_so_far[nxt] = new_cost
came_from[nxt] = current
heappush(frontier, (new_cost + h(nxt), new_cost, nxt))
path, node = [], goal
while node is not None:
path.append(node)
node = came_from[node]
return path[::-1], expandedThe key data structures are the priority queue over frontier states and the came_from map, which lets us reconstruct the optimal path once the goal is popped. States land in expanded only after being popped with their best cost, so the set records where the search spent its effort. Passing heuristic=None reduces A* to Dijkstra with no other change, because a heuristic of zero is the uninformed special case. This keeps the encoded heuristic as the only difference in the comparison.
Now run both variants and compare how much work each does.
manhattan = lambda s: abs(s[0] - goal[0]) + abs(s[1] - goal[1])
path_a, expanded_a = astar(grid, start, goal, manhattan)
path_d, expanded_d = astar(grid, start, goal, None)A* : 16 steps, 46 states expanded Dijkstra : 16 steps, 65 states expanded Heuristic saved 19 expansions for the same optimal path
Both searches find a path of identical length because Manhattan distance is admissible and consistent on this grid. Excluding the goal pop, A* expands substantially fewer states than Dijkstra for the same optimal path, as the printed counts above show. In problems where expanding a state requires an expensive simulation or collision check, reducing that count can materially extend the planning horizon, although queue work, memory, and other costs may also dominate.

The picture also shows the heuristic's limits. Many states tie on the same value, so A* still expands a broad region to the left of the first wall. A wall-aware cost-to-go estimate could reduce that extra work, but tie-breaking and multiple optimal routes still matter.
What search assumes, and what it costs
The finite deterministic shortest-path methods used here are exact under their cost and heuristic conditions. Their basic formulations also make several simplifying assumptions:
- Determinism. Each action has exactly one outcome. Stochasticity does not remove the state graph, but it breaks the single-successor shortest-path formulation and requires expectations or contingent policies over transition distributions.
- Full observability. The planner knows the current node. With partial observability we search over belief states instead, exactly the objects Chapter 2 of this part built for us. Note that the belief space is continuous even when the underlying state space is discrete, so partial observability quietly destroys the discreteness assumption as well.
- Discreteness. Exact Dijkstra or A* needs an explicitly enumerable or lazily generated discrete graph whose successors can be enumerated at each expansion. Sampling-based continuous planners use different constructions and may offer probabilistic-completeness or asymptotic-optimality guarantees; the exact guarantee depends on the algorithm.
- Query scope. The A* run here answers one start-goal query. Dijkstra can compute single-source distances to all reachable nodes, and roadmap methods can amortize preprocessing across queries. Tabular DP goes further by computing a policy for every represented state under stochastic dynamics.
For this example, A* is a one-shot calculation and tabular DP is an amortized investment: it spends more work up front, then supplies an action at every represented state. Neither dominates the other. The right choice depends on query volume, stochasticity, graph size, and whether the model stays fixed.
Dynamic Programming and Bellman Equations
Dynamic programming (DP) flips the question. Instead of "how do we get from here to the goal?", it asks "what is the value of being in each state, assuming we act optimally from now on?" In a tabular model, the resulting policy is a lookup for every represented state. DP handles stochastic transitions without changing its Bellman structure, whereas ordinary deterministic graph search must be generalized.
The central object is the optimal value function. Recall from Chapter 1 of this part that a policy assigns actions to states and that we score policies by expected discounted return. The optimal value of a state is the best return achievable from it:
where:
- : the optimal value of state , the best expected discounted return achievable starting from
- : a policy assigning an action distribution to each state, with selecting the best one; in a finite discounted MDP, a deterministic stationary optimum exists
- : the expectation over trajectories generated by following under the model's transition dynamics
- : the condition that the trajectory starts in state
- : the reward received at step , discounted by one factor of per step into the future
One subtlety worth pausing on: the maximum is over policies, which map states to action distributions; deterministic policies are the action-valued special case. The Bellman equation decomposes this global optimization into linked per-state action comparisons.
The Bellman optimality equation
What makes DP work is that the value function satisfies a self-referential equation. If we act optimally, our first action must be the one that maximizes immediate reward plus the discounted optimal value of wherever we land:
where:
- : a candidate action available in state , with selecting the best one
- : the immediate reward for taking action in state
- : a possible successor state after taking action in state
- : the model's transition probability, its prediction of where action from state lands
- : the discounted expected optimal value of the successor state
This is the Bellman optimality equation. Read it as a statement about consistency across time: the value of being somewhere equals the best one-step payoff plus the value of the future. In a Markov problem with an additive return, the principle of optimality says that an optimal continuation must itself be optimal from the successor state. That is what licenses the replacement of "the best policy from here on" with "the best action now, assuming optimality thereafter." The recursion turns a global policy problem into local action comparisons linked through the value function. Many methods in model-based RL solve this equation, or an approximation to it, but not every planner is organized around an explicit Bellman equation.
For a fixed policy , the maximization drops out and we get the Bellman expectation equation, a linear system in :
where:
- : the value of state when the agent follows the fixed policy from onward
- : the probability that policy assigns to choosing action in state
- , , and : the same reward, transition model, and discount factor as in the optimality equation above
The linearity of this equation is more useful than it may first appear. A finite-state version can be solved by linear algebra or converged to by iteration. With sampled transitions, Monte Carlo and temporal-difference methods instead estimate the same policy value or its Bellman fixed point without forming the full transition matrix. Learned critics extend that idea with function approximation.
Why iteration converges
For bounded rewards and , the Bellman optimality operator is a -contraction in the max-norm: applying it to any two bounded value functions leaves them at most times as far apart. Two value functions that differ by at most in every state therefore cannot differ by more than after one backup. The contraction mapping theorem gives a unique fixed point, . We can start from any bounded guess, say , and repeatedly apply the operator:
where is the value estimate after sweeps of the operator, with the starting guess , and is the result of one more application.
This is value iteration, and the contraction property guarantees geometric convergence to . With , the worst-case value error after a backup is at most 95 percent of the preceding error. To reduce an initial error bound by a factor , the usual worst-case count scales like . A large discount factor therefore expresses a long planning horizon and also gives a slower worst-case contraction. It does not, by itself, set a planner's noise floor.
Value iteration improves the value first and reads the policy out afterwards. Policy iteration alternates the two steps explicitly: (1) policy evaluation, solve the linear Bellman expectation equation for exactly; (2) policy improvement, set . Each improvement step is nondecreasing in value and is strictly better wherever it selects an action with strict positive advantage. With consistent tie handling, a finite discounted MDP reaches an optimal policy after finitely many distinct improvements. Modified variants truncate the evaluation step. Actor-critic methods use the related pattern of approximate evaluation plus policy improvement, though they are not simply exact policy iteration with fewer sweeps.
A worked Bellman backup
Let's make the iteration concrete on our grid world, where rewards are per step, the goal is terminal with value , and . Consider a free cell exactly one step from the goal. Its best action reaches the goal, so one sweep of the operator gives
A cell three steps away needs three sweeps before the goal's influence propagates back, and its converged value is the geometric sum
You can already see the mechanism: value flows backward from the goal through the Bellman operator, one step of lookahead per sweep. After sweeps, every state within steps of the goal has a correct value, and after convergence, every state does. This backward flow is the discrete cousin of the Riccati recursion we will meet in the LQR section. It is also related to backpropagation through time in trajectory optimization. A forward rollout computes states from controls; the reverse-mode gradient pass then propagates cost sensitivities backward through those dynamics. Bellman backups propagate value information backward across predecessor relationships. The objects differ, but both algorithms exploit temporal composition.
Now implement it. The environment transition is embedded in a small step function that bumps the agent back if it would leave the grid or hit a wall.
gamma = 0.95
step_reward = -1.0
actions = ((-1, 0), (1, 0), (0, -1), (0, 1)) # up, down, left, right
def step(s, a):
r, c = s[0] + a[0], s[1] + a[1]
inside = 0 <= r < grid.shape[0] and 0 <= c < grid.shape[1]
return (r, c) if inside and grid[r, c] == 0 else s
free_cells = [
(r, c)
for r in range(grid.shape[0])
for c in range(grid.shape[1])
if grid[r, c] == 0 and (r, c) != goal
]
V = np.zeros(grid.shape)
sweeps = 0
for _ in range(500):
delta = 0.0
V_new = V.copy()
for s in free_cells:
V_new[s] = step_reward + gamma * max(V[step(s, a)] for a in actions)
delta = max(delta, abs(V_new[s] - V[s]))
V = V_new
sweeps += 1
if delta < 1e-6:
break
policy = -np.ones(grid.shape, dtype=int)
for s in free_cells:
q_values = [step_reward + gamma * V[step(s, a)] for a in actions]
policy[s] = int(np.argmax(q_values))This is the entire algorithm: apply the Bellman operator until the change is small, then read the policy off as the argmax. We never specified a start state; the table covers the whole grid. The stopping statistic is the Bellman-update residual, not the value error itself. For a discounted contraction it supplies a bound, , so a tolerance of here implies a correspondingly small bounded value error.
s = start
walk = [s]
for _ in range(60):
if s == goal:
break
s = step(s, actions[policy[s]])
walk.append(s)Converged after 23 sweeps Value at start: -11.197 Greedy policy walks from start to goal in 16 steps
The greedy policy reproduces one of the same optimal routes that A* found. Value iteration did more work for this single query, sweeping every free cell repeatedly, but it now supplies an action for every represented free state. If the wheels can slip, the Bellman backup can average over a transition distribution without changing its basic structure. A deterministic shortest-path graph would need to be replaced by an appropriate stochastic planning formulation. With at most nonzero successors per state-action pair, a sweep costs ; a dense transition table gives .


The left panel shows value flowing toward the goal, brightest at the terminal state and darkest in the far corner. Notice how the contours bend around the walls: value is a distance-like field, but it is a distance through the navigable space, so the walls leave visible ridges in it. The right panel shows why value functions are so useful in practice: the policy is a vector field, a complete answer to "what should we do?" available at every state, computed once, queried forever. If the robot is picked up and dropped somewhere new, no replanning is needed; the field already covers that location. This "compute once, query forever" property is exactly what a learned world-model agent tries to approximate when it trains a policy network against rollouts from a latent dynamics model.
The curse of dimensionality
The catch is enumeration. If each state-action pair has at most possible successors, a Bellman sweep costs ; deterministic transitions give the special case, while a dense table costs . Our grid has 84 free states. A robot arm with 10 joints discretized at 10 values each has state combinations, and 50 binary variables give . This Cartesian growth is the curse of dimensionality.
At fixed resolution per coordinate, the table grows exponentially in the number of factored state dimensions, not in any direct measure of task difficulty. A ten-joint arm does not need qualitatively different behaviors; sensible behaviors may occupy a much smaller structured subset of that grid. Tabular representations cannot exploit such smoothness or structure, so the curse is partly representational. Function approximation attacks that problem, though it introduces new approximation and optimization errors.
Every escape route changes the problem. Discretization loses precision. Function approximation replaces the unconditional tabular contraction guarantee with guarantees that require additional assumptions about the architecture, objective, sampling, and update rule. Exploiting known structure, which is what LQR does next, restricts the problem class but buys exact and efficient solutions within it. Learned latent models make a different wager: that a compact representation preserves enough task structure for approximate planning to be useful.
The exponential character of the curse is easier to feel as a picture than as a sentence.
import numpy as np
## Tabular state counts for discretizing d dimensions at several resolutions.
## Purely combinatorial counting; no simulation or randomness involved.
dims = np.arange(1, 9)
resolutions = {"2 values/dim": 2, "5 values/dim": 5, "10 values/dim": 10}
state_counts = {name: res**dims for name, res in resolutions.items()}
A ten-dimensional system at ten values per dimension already sits at states, far beyond any table. This curve is the quantitative face of the curse, and it is the reason the next section's structural assumptions, and later chapters' learned compact representations, are not conveniences but necessities.
Linear-Quadratic Control
Finite tabular DP needs an enumerable state set. Continuous-state dynamic programming also exists, but representing an arbitrary value function is difficult. The linear-quadratic regulator (LQR) is a foundational, widely used special case in which the Bellman equation reduces to Riccati equations. Iterative LQR and differential dynamic programming, in turn, build local trajectory updates from LQR-like subproblems.
The setting is:
-
Linear dynamics (from Chapter 3's system identification, or first-principles modeling):
where is the state, the control input, the state-transition matrix describing how the current state carries into the next, and the input matrix describing how the control changes the next state.
-
Quadratic cost, penalizing deviation from zero and control effort:
where:
- : the per-step cost in state directions weighted by ; it is positive for every nonzero deviation only when
- : the per-step control cost, penalizing control effort
- : the terminal cost on the final state
- , , : symmetric cost matrices with and , so state costs are nonnegative while control is strictly penalized
Why quadratic? Squared error is a natural penalty for regulation, and, together with linear dynamics, it gives a value-function family that is closed under Bellman backups: if , the next backup is quadratic too. A probabilistic interpretation is available when the quadratic terms are proportional to negative log densities of Gaussian state preferences and control priors, but that interpretation needs those modeling assumptions; it is not intrinsic to every LQR problem. The closure property is the computational trick: we solve for the finitely many entries of rather than for an arbitrary function.
Deriving the Riccati equation
Guess that the value function is quadratic, , and run one step of the Bellman backup:
where:
- : the matrix of the current quadratic value estimate
- : the updated matrix after one backup, defining the next value iterate
- : the model's prediction of the next state under the linear dynamics
- : optimization over all candidate controls , selecting the one with the smallest one-step cost plus successor value
The expression inside the min is a quadratic in , so it can be minimized analytically by setting its gradient to zero:
Solving this linear equation for gives the optimal feedback law
Two things just happened that are worth savoring. First, the optimal policy came out linear in the state, with the gain matrix as the only unknown. We did not restrict the policy class; we allowed arbitrary (measurable) functions from state to action, and the optimizer chose a linear one because the problem's structure made it optimal. Second, the minimizing can be computed in closed form, no search over actions needed. Compare this with the grid world, where every action had to be enumerated and evaluated at every state. Here the entire maximization over a continuum of controls collapses to solving a small linear system. Structure bought us the same thing the heuristic bought A*: a way to skip most of the work. Now substitute back into the backup, so that :
Reading off the matrix and imposing the fixed-point condition yields
This is the discrete algebraic Riccati equation (DARE). The finite-horizon version is a backward recursion in time: start with and step the recursion backward to . The infinite-horizon equation has a unique positive semidefinite stabilizing solution under the usual stabilizability and detectability conditions, and the resulting modeled closed-loop matrix is Schur stable. Along its trajectories,
This is nonpositive in general and strictly negative for every nonzero when is positive definite. Under those conditions, is a Lyapunov certificate for the modeled closed loop. The theorem does not automatically certify a different true plant or an approximate nonlinear system.
LQR assumes known and . A common practice when they are estimated is certainty equivalence: fit the model, then design the controller as if the estimate were exact. In the standard partially observed LQG setting, the separation principle makes an optimal controller from a Kalman state estimate followed by the corresponding LQR feedback. That theorem does not cover generic parameter-estimation error or nonlinear learned models. In those cases certainty equivalence is a heuristic: an optimizer may seek regions where the fitted model is optimistic, so robustness margins and evaluation on the true plant or a credible uncertainty set matter.
Building an LQR on an identified model
Let's do this end to end. Following Chapter 3, we fit and of a double integrator from noisy transition data, solve the Riccati equation on the estimated model, and deploy the resulting gain on the true system. This identify-plan-deploy chain is one common model-based-control pattern. Later systems may also change the representation, uncertainty treatment, planner, data-collection loop, adaptation rule, and safety architecture.
First, generate data and identify the model by least squares.
rng = np.random.default_rng(3)
dt = 0.1
A_true = np.array([[1.0, dt], [0.0, 1.0]]) # position and velocity update
B_true = np.array([[0.0], [dt]]) # control is acceleration
n = 400
X = rng.normal(scale=1.0, size=(n, 2))
U = rng.normal(scale=1.5, size=(n, 1))
noise = rng.normal(scale=0.01, size=(n, 2))
X_next = X @ A_true.T + U @ B_true.T + noise
Z = np.hstack([X, U]) # regressors: state and action
Theta, *_ = np.linalg.lstsq(Z, X_next, rcond=None)
A_hat, B_hat = Theta[:2, :].T, Theta[2:, :].TTrue A: [[1. 0.1] [0. 1. ]] Estimated A: [[ 1.0005e+00 9.9900e-02] [-5.0000e-04 1.0000e+00]] True B: [[0. ] [0.1]] Estimated B: [[0.0004] [0.1003]]
The estimates land within a few thousandths of the true matrices in this seeded experiment. The regression succeeds because the synthetic state-action design matrix is full rank and well conditioned; white-noise controls alone would not establish identifiability of the state columns in . In a rollout dataset, input design and state visitation jointly determine the regressor's rank and conditioning. We will judge whether certainty-equivalent control worked here by checking the true closed loop below, rather than by treating a particular parameter-error magnitude as a universal threshold.
Now solve the Riccati equation by fixed-point iteration, which is nothing more than repeatedly applying the Bellman backup for quadratic value functions until stops moving.
Q = np.eye(2) # penalize state deviation
R = 0.5 * np.eye(1) # penalize control effort
P = Q.copy()
riccati_converged = False
for _ in range(5000):
K = np.linalg.solve(R + B_hat.T @ P @ B_hat, B_hat.T @ P @ A_hat)
P_new = Q + A_hat.T @ P @ (A_hat - B_hat @ K)
if np.max(np.abs(P_new - P)) < 1e-10:
P = P_new
riccati_converged = True
break
P = P_new
if not riccati_converged:
raise RuntimeError("Riccati iteration did not converge")
K = np.linalg.solve(R + B_hat.T @ P @ B_hat, B_hat.T @ P @ A_hat)Compare this loop with value iteration. Both initialize a value representation and iterate a Bellman-derived update toward a fixed point. Here the representation is a two-by-two matrix rather than a table, and analytic minimization replaces action enumeration. They are structurally analogous iterations with different operators, domains, and convergence assumptions.
closed_loop = A_true - B_true @ K
eig_mags = np.abs(np.linalg.eigvals(closed_loop))Gain K: [1.27244878 2.09436387] Closed-loop eigenvalue magnitudes: [0.8963 0.8963] Stabilizing: True
The gain came out positive on both state components, meaning the controller pushes acceleration opposite to position and velocity. We evaluated the gain on the true matrices and found both eigenvalue magnitudes below one. That is an empirical check for this identified example; the Riccati theorem itself guarantees stability for the estimated model on which the gain was designed. Finally, deploy the gain on the true dynamics, starting from a state with nonzero velocity, and compare against doing nothing.
T = 50
x0 = np.array([0.0, 1.5])
x_un = np.zeros((T + 1, 2))
x_cl = np.zeros((T + 1, 2))
x_un[0], x_cl[0] = x0, x0
for t in range(T):
u_t = -K @ x_cl[t]
x_cl[t + 1] = A_true @ x_cl[t] + B_true @ u_t
x_un[t + 1] = A_true @ x_un[t]
state_cost_un = float(sum(x @ Q @ x for x in x_un))
state_cost_cl = float(sum(x @ Q @ x for x in x_cl))Uncontrolled final state: position +7.500, velocity +1.500 LQR final state: position +0.011, velocity -0.016 Accumulated state cost, uncontrolled: 1080.6 Accumulated state cost, LQR: 10.87
The uncontrolled cart coasts with its initial velocity over this noiseless simulation, and its accumulated squared-state cost dwarfs the regulated one. This illustrates why feedback matters: the open-loop system has repeated eigenvalues at one, so velocity persists and position drifts, while the two-component gain makes this tested closed loop forget its initial condition exponentially.


The controller was designed on the estimated matrices but deployed on the true system. The explicit true-plant eigenvalue check shows a comfortable stability margin for this experiment. Certainty equivalence prescribes the design step; that numerical check supplies the robustness evidence here. On other plants, especially away from a valid local model, parameter error can change stability or performance substantially. The gap between the model used for planning and the world used for execution therefore needs direct evaluation.
The role of the R weight in that trade-off can be made concrete by re-solving the Riccati equation across a range of control penalties.
import numpy as np
## Re-solve the infinite-horizon Riccati equation on the identified double
## integrator model (A_hat, B_hat from above) for a sweep of R values.
## Deterministic fixed-point iteration, tolerance 1e-9, 20000-iteration cap.
r_values = np.logspace(-2, 1.5, 12)
gain_norms = []
eig_mags_best = []
Q_sweep = np.eye(2)
for r_val in r_values:
R_sweep = r_val * np.eye(1)
P_s = Q_sweep.copy()
for _ in range(20000):
K_s = np.linalg.solve(
R_sweep + B_hat.T @ P_s @ B_hat, B_hat.T @ P_s @ A_hat
)
P_new = Q_sweep + A_hat.T @ P_s @ (A_hat - B_hat @ K_s)
if np.max(np.abs(P_new - P_s)) < 1e-9:
P_s = P_new
break
P_s = P_new
gain_norms.append(np.linalg.norm(K_s))
eig_mags_best.append(
np.max(np.abs(np.linalg.eigvals(A_true - B_true @ K_s)))
)
gain_norms = np.array(gain_norms)
eig_mags_best = np.array(eig_mags_best)
As shrinks, control becomes cheap and the gain grows. The worst eigenvalue magnitude of the true plant is nonmonotone over this sweep, so gain size alone should not be read as a universal speed measure. Every tested true-plant eigenvalue remains below the dotted unit line, but that is a numerical result for this small identification error. The Riccati guarantee applies directly to each estimated-model design; transfer to the true plant requires a robustness margin or an explicit check such as this one.
When the structure is approximately right
LQR's limits mirror its strengths: its exact solution needs linear dynamics and quadratic costs. Iterative LQR (iLQR) and differential dynamic programming approximate nonlinear dynamics and costs around a nominal trajectory, compute local feedback updates, roll out a new nominal, and repeat. Model predictive control (MPC) solves a finite-horizon problem at each step, applies the first action, observes the next state, and replans. That feedback can correct some disturbances and model errors before they accumulate across many horizons, but it does not erase within-horizon error or guarantee robustness. Local approximation and receding-horizon replanning form a bridge to the planning agents of Part VII.
Trajectory Optimization and Constraints
LQR gives optimality by exploiting linear-quadratic structure. With nonlinear dynamics, non-quadratic costs, or constraints, trajectory optimization states a finite-horizon problem directly and solves it numerically. These methods still exploit substantial structure: dynamics couple only neighboring time steps, derivatives are sparse, and specialized shooting or collocation solvers use that organization. The result is a flexible framework used throughout robotics and aerospace, with guarantees that depend on the solver, formulation, initialization, and model.
The formulation is direct. Treat the entire future as decision variables:
subject to the dynamics acting as equality constraints,
and the initial condition
plus whatever task constraints apply, for example from actuator limits or obstacle-avoidance inequalities on the states. The symbols in these two expressions are:
- : the running cost incurred at step , encoding objectives such as tracking error and control effort
- : the terminal cost on the final state
- : the model's dynamics, predicting the next state from the current state and control
- : the optimized state-and-control trajectory; is fixed by the initial condition
Search and DP asked for a path or value function; LQR produced a feedback law. Open-loop trajectory optimization instead returns one nominal state-control trajectory. It can be used offline with a tracking controller, or online inside receding-horizon MPC when computation permits. Both pairings add feedback that the nominal trajectory alone does not provide.
For the nonlinear problems emphasized here, two widely used direct formulations are shooting and collocation.
- Direct shooting keeps only the controls as decision variables and simulates the model to produce the states. The dynamics constraints are satisfied by construction, and gradients are obtained by differentiating through the rollout. The downside: the map from early controls to late states becomes long, thin, and poorly conditioned, so gradients through long rollouts can vanish or explode. Intuitively, an early control affects a late state only through a long chain of intermediate states, and the Jacobian of that chain is a product of many Jacobians whose singular values multiply, so the gradient magnitude can shrink or grow exponentially in the horizon length.
- Direct collocation keeps states and controls as joint decision variables and enforces discretized dynamics as explicit constraints at collocation points. This exposes sparse structure and often improves conditioning, at the cost of a larger optimization problem. An NLP solver drives the discrete constraint residuals toward its numerical tolerances; whether the resulting continuous trajectory is feasible also depends on the transcription resolution and model fidelity.
A hard constraint belongs to the feasible set, whereas a soft constraint adds a penalty to the objective. A constrained solver enforces the discretized hard constraint only to numerical tolerance, and real-system satisfaction additionally depends on discretization and model error. A penalty merely discourages violation. This distinction matters in safety-critical settings, where robust margins and independent safety mechanisms may still be needed. We return to it in Part XII.
To make this concrete, let's solve a nonlinear task by direct shooting with automatic differentiation and Adam: swinging up a bounded-input pendulum. We use normalized angular dynamics
where:
- : the pendulum angle, with hanging down and the upright target
- , : the angular velocity and the angular acceleration
- : the normalized gravity coefficient, with units of inverse seconds squared; has angular-acceleration units
- : the damping rate
- : the commanded angular acceleration after normalizing motor torque by inertia
In the simulation below the normalized gravity coefficient is while the input bound is . A constant saturated input therefore cannot hold the pendulum at horizontal, and the optimizer must exploit momentum to reach the upright. We optimize by reverse-mode differentiation through the discretized rollout, with the input bound represented by clamping.
import torch
torch.manual_seed(0)
gravity_coeff, c_damp, dt_p, T_p = 3.0, 0.25, 0.05, 120
u_max = 2.0 # below the normalized gravity term at horizontal
def rollout(u_seq):
theta = torch.zeros(())
omega = torch.zeros(())
thetas, omegas = [theta], [omega]
for t in range(T_p):
tau = torch.clamp(u_seq[t], -u_max, u_max)
alpha = -gravity_coeff * torch.sin(theta) - c_damp * omega + tau
omega = omega + dt_p * alpha
theta = theta + dt_p * omega
thetas.append(theta)
omegas.append(omega)
return torch.stack(thetas), torch.stack(omegas)
u = torch.zeros(T_p, requires_grad=True)
opt = torch.optim.Adam([u], lr=0.05)
losses = []
for it in range(600):
opt.zero_grad()
th, om = rollout(u)
loss = (
((th[:-1] - torch.pi) ** 2 + 0.05 * om[:-1] ** 2).mean()
+ 10.0 * (th[-1] - torch.pi) ** 2
+ 1.0 * om[-1] ** 2
+ 0.001 * (u**2).mean()
)
loss.backward()
opt.step()
losses.append(loss.item())The cost penalizes angle and velocity along the trajectory, applies extra terminal penalties at , and mildly discourages large inputs. The running term provides gradient information at every simulated time step; a terminal-only objective would still have a gradient here, but it can be harder to optimize because all influence must pass through the full rollout. For this discretized, piecewise-differentiable PyTorch program, loss.backward() computes reverse-mode derivatives of the implemented objective with respect to all 120 controls. At clamp boundaries, the returned derivative follows PyTorch's chosen subgradient convention.
with torch.no_grad():
th_final, om_final = rollout(u)
th_np = th_final.numpy()
u_np = u.detach().numpy()Final angle: +3.1071 rad (target: pi = 3.1416) Final angular velocity: +0.16096 rad/s Initial loss: 108.57 final loss: 6.5452 Controls at torque limit: 65% of the time
The optimized trajectory ends near the upright, with a small residual angular velocity reported in the printed output above. The angle makes a backswing rather than rising monotonically because the bounded input cannot hold the pendulum at horizontal. That momentum-building maneuver is outside what an LQR linearized around the upright can discover from the downward initial state. The example shows how differentiating through a nonlinear model can expose a useful open-loop strategy without encoding the maneuver by hand.


The loss drops rapidly and then improves more gradually. The curve shows optimization progress, but it does not identify which physical strategy Adam discovered at each iteration; that would require inspecting intermediate trajectories. The later, slower phase is consistent with local refinement of a nonlinear shooting solution, not proof of a universal signature for shooting methods.
This example compresses the chapter's arc into one picture. We had a model, used it predictively to simulate rollouts, and obtained an open-loop control sequence by optimizing the model's outputs. No explicit value table or search tree was needed. The pendulum has one mechanical degree of freedom and a two-dimensional state . A comparable transcription gains variables as state and control dimension increase; conditioning, constraint difficulty, and local optima depend on the model, coordinates, objective, and constraint structure. When a nonconvex problem has separate basins, a local optimizer may settle on one gait even if a lower-cost gait lies in another. Better initialization, multiple starts, sampling-based exploration, and learned trajectory priors are common responses.
Key Parameters
The key parameters for the planning methods in this chapter are:
- gamma (): The discount factor used in value iteration, set to . For bounded discounted problems, it is the Bellman operator's worst-case contraction modulus. Values close to one put more weight on distant rewards and give a slower worst-case convergence bound.
- tolerance: The sweep-stopping threshold, set to
1e-6on the maximum update. It is a residual threshold; the contraction bound converts it to a value-error bound by dividing by . - Q, R: The state and control cost matrices for LQR, set to and . Their relative weighting changes the feedback gain and the trade-off between state regulation and control effort; the response need not be a simple proportional scaling.
- u_max: The normalized angular-acceleration input bound for pendulum swing-up, set to
2.0, below the gravity coefficient at horizontal. The optimized solution uses momentum to reach the target under this bound. - lr: The Adam learning rate, set to
0.05. A value that is too large can make parameter updates unstable, while a very small value may converge slowly. Its useful scale depends on the horizon, dynamics, parameterization, and objective.
Limitations and Impact
Many assumptions behind these methods motivate research questions later in the book.
The deepest shared assumption is that the model is adequate where the planner uses it. Once the model is learned, two new failure modes appear. First is compounding rollout error. Small one-step errors are propagated by the dynamics and can accumulate, cancel, or amplify depending on their bias, correlation, and the system's sensitivity. A systematic error of size can contribute a sum of propagated errors over an -step open-loop rollout. In feedback, corrupted states also change later actions. One-step accuracy therefore does not determine multi-step fidelity, so both should be evaluated at the horizons used for decisions.
The compounding mechanism can be illustrated with the double integrator itself.
import numpy as np
## Seeded Monte Carlo illustration on the double integrator. The two error
## models have matched one-step RMS magnitude: one is zero-mean independent
## noise, while the other has a fixed velocity bias.
dt_e = 0.1
A_true_e = np.array([[1.0, dt_e], [0.0, 1.0]])
x0_e = np.array([0.0, 1.0])
sigma_e = 0.02
n_rollouts = 2000
bias_step = np.array([0.0, np.sqrt(2.0) * sigma_e])
rng_e = np.random.default_rng(17)
horizons = np.arange(1, 51)
sq_err_unbiased = np.zeros((n_rollouts, len(horizons)))
sq_err_biased = np.zeros(len(horizons))
x_true = x0_e.copy()
unbiased_states = np.repeat(x0_e[None, :], n_rollouts, axis=0)
biased_state = x0_e.copy()
for i, H in enumerate(horizons):
x_true = A_true_e @ x_true
unbiased_states = unbiased_states @ A_true_e.T
unbiased_states += rng_e.normal(0.0, sigma_e, size=unbiased_states.shape)
biased_state = A_true_e @ biased_state + bias_step
sq_err_unbiased[:, i] = np.sum((unbiased_states - x_true) ** 2, axis=1)
sq_err_biased[i] = np.sum((biased_state - x_true) ** 2)
err_unbiased = np.sqrt(sq_err_unbiased.mean(axis=0))
err_biased = np.sqrt(sq_err_biased)
The injected one-step errors have matched RMS magnitude, yet their rollout errors differ markedly. Independent zero-mean disturbances still accumulate in this marginally stable system, while the fixed velocity bias compounds much faster. This controlled example does not establish a universal growth law; it shows why the temporal structure of model error matters and why rollout fidelity should be measured directly.
Second is model exploitation. Optimization can steer a planner toward inputs where an approximate model is optimistically wrong, especially outside the data distribution. Approximation creates this risk but does not imply that every model will be exploitable on every task. Classical optimizers do not remove the problem when they plan with an approximate model. Practical defenses include uncertainty-aware or robust objectives, conservative penalties, shorter horizons, feedback and replanning, and collecting new data in planner-visited regions. Chapter 6 develops this gap between predictive fidelity and decision usefulness directly.
The second limitation is scale. Cartesian tabular representations grow exponentially in the number of factored dimensions at fixed resolution, although a particular search problem may explore only a small reachable subset. LQR remains efficient by restricting dynamics and costs to the linear-quadratic class. Nonlinear trajectory optimization with a local solver is initialization-sensitive and may reach a local stationary point, an infeasible point, or fail; convex special cases have stronger global guarantees. Discretizing a continuous space exposes a familiar trade-off: fine grids improve resolution but grow combinatorially, while coarse grids may erase task-relevant structure. Part III turns to representations that aim to make approximate planning tractable.
A third limitation is that these planners typically assume full observability and a stationary, Markov environment. We know from Chapter 1 of this part how to restore Markovianity through belief states, and from Chapter 2 how to propagate those beliefs forward. But belief-augmented planning (search over beliefs, DP over beliefs) multiplies the state space again, since a belief is a distribution over states and the space of distributions is vastly larger than the space of states, and making that tractable is an open engineering frontier that connects directly to the memory architectures of Part III.
Under their stated assumptions, these methods do offer strong guarantees: Dijkstra and appropriately configured A* return optimal graph paths; discounted finite-MDP value iteration converges; and infinite-horizon LQR stabilizes its modeled plant under stabilizability and detectability conditions, with a quadratic certificate under the positivity conditions stated above. Learned controllers can also be analyzed or certified in restricted settings, but classical baselines make the assumptions unusually explicit. Hybrid systems often use learning to supply a model or representation and classical optimization to choose actions; how dominant that pattern is depends on the application domain.
The conceptual takeaway is the division of labor: the model predicts and the planner chooses. Search, DP, LQR, and trajectory optimization implement that split differently, trading off model class, planning horizon, computation, and available guarantees. Many later agents can be analyzed in the same trade space even when model and planner are implemented jointly.
Summary
- Planning turns a predictive model into a controller. Given and a reward, the planner finds actions that maximize expected return. Open-loop plans are action sequences; closed-loop policies are state-dependent and can react to observed outcomes.
- State-space search handles finite graph problems. Dijkstra is uninformed; A* uses a heuristic and is optimal under the admissibility and graph-search conditions stated in the chapter.
- For bounded rewards and , tabular dynamic programming solves the Bellman optimality equation and value iteration converges geometrically. Stochastic transitions fit naturally, but state enumeration causes the curse of dimensionality.
- LQR exploits linear dynamics and quadratic costs to obtain from the Riccati equation. Its stability guarantee applies to the modeled system under stabilizability and detectability conditions; certainty-equivalent deployment on an estimated plant needs a robustness check.
- Trajectory optimization computes finite-horizon state-control sequences by shooting or collocation. Numerical hard-constraint satisfaction is tolerance- and discretization-dependent, and local solvers are sensitive to initialization.
- All four methods depend on a model that is adequate in the region used for decisions. With learned models, rollout error and optimizer-induced model exploitation become first-class problems.
The next chapter develops a probabilistic framing of control. Control-as-inference methods introduce optimality or preference variables and infer a posterior over trajectories and actions. Exact inference equivalences are narrow, for example maximum-entropy formulations with suitable deterministic dynamics, while stochastic systems generally require approximations such as variational inference. Uncertainty drives epistemic exploration only when the objective includes terms such as information gain or expected free energy. Those conditions lead into control as inference and active inference.
Quiz
Ready to test your understanding? Take this quick quiz to reinforce what you've learned about classical planning and optimal control.
Classical Planning and Optimal Control
Reference
Citation details
Cite or share this article.
Continue with the full handbook
This chapter is part of World Models Handbook. Use the handbook page to browse the complete table of contents and continue reading in sequence.
Explore World Models HandbookStay up to date
Get articles, book updates, and news delivered to your inbox.
No spam, unsubscribe anytime.
Join the community
Sign in to remove popups, track your reading progress, and join the discussion.

Comments
No comments yet. Be the first to share your thoughts!