FORC · Lecture 6

LQR and trajectory optimization

Simeon Nedelchev

Full notes, practice, and references

Tangent-state linearization

δxk+1=Aδxk+Bδuk\delta x_{k+1}=A\delta x_k+B\delta u_k

A free body's local state error has 12 coordinates.

Raw quaternion subtraction gives the wrong representation.

LQR objective

∑kδxk⊤Qδxk+δuk⊤Rδuk\sum_k \delta x_k^\top Q\delta x_k+\delta u_k^\top R\delta u_k

Weights encode units and priorities.

Feedback gain

K=(R+B⊤PB)−1B⊤PAK=(R+B^\top PB)^{-1}B^\top PA
δu=−Kδx\delta u=-K\delta x

The eigenvalues of A−BKA-BK provide a local discrete-time check.

Scalar example

xk+1=xk+ukx_{k+1}=x_k+u_k, Q=R=1Q=R=1

P≈1.618P\approx1.618, K≈0.618K\approx0.618

Closed-loop multiplier: 0.382

Robot examples

Cart-pole upright balance.

Quadrotor hover with scalar-first quaternions.

Large errors and saturation need nonlinear rollout tests.

Trajectory optimization

Shooting optimizes controls through a rollout.

Transcription introduces states and dynamics constraints.

Reading: MIT LQR MIT trajectory optimization

Reading

Return to the notes