LQR Optimization problem

In general, a control optimization problem consists of a cost function, \(\mathcal{J}\), and a dynamical constraint, \(f_k\), that describes how the state evolves over time,

\[\min \mathcal{J}(x, u) = \ell_f(x_T) + \sum_{t=0}^{T-1} \ell_t(x_t, u_t)\]

subject to

\[ \begin{align}\begin{aligned}x_{t+1} = f_t(x_t, u_t), \quad t = 0, \ldots, T-1\\x_0 = x_{\text{init}}\end{aligned}\end{align} \]

where \(\ell_k(\cdot, \cdot)\) is the momentary cost function at time \(t\) for a given input \(u_t \in \mathbb{R}^m\) and state \(x_t \in \mathbb{R}^n\). The goal is to find an optimal state feedback law \(u_t^*(x_t)\), that minimizes the cost function \(\mathcal{J}\) given the control sequence \(u^* = \{u_0^*, \ldots, u_{T-1}^*\}\).

Some optimal control problems can be solved using the Dynamic Programming (DP) approach. This method consists of defining a Value function, \(\mathcal{V}_t(x, t)\),

\[\mathcal{V}_t(x, t) = \min_{u_t \in \mathcal{U}} \{ \ell_t(x_t, u_t) + \mathcal{V}_{t+1}(x, t+1) \}\]

This is the optimal (minimal) cost-to-go from time \(t\) to the terminal time \(T\). In order to initialize the Value function, the terminal boundary condition is defined as \(\mathcal{V}_T(x_T) = \ell_T(x_T)\). Therefore, we can dynamically sweep back through time.

Using the Value function, we can find the optimal control law by finding the argmin of the Value function,

\[u_t(x_t) = \arg\min_{u_t \in \mathcal{U}} \{ \ell_t(x_t, u_t) + \mathcal{V}_{t+1}(x, t+1) \}\]

and successively compute the optimal trajectory \(\{x_s^*:T\}\) in a single forward pass,

\[x_{t+1}^* = f_t(x_t^*, u_t^*(x_t^*)) = f_t(x_t^*).\]

Linear quadratic program

Linear Quadratic Regulators (LQR) are a particular class of optimal control problems that are well-structured and can be solved efficiently using quadratic programming. For the LQR problem, the \(\ell_k\) term in Equation (2.1) is defined as

\[\begin{split}\ell_t(x_t, u_t) = \begin{bmatrix} x_t \\ u_t \end{bmatrix}^T \begin{bmatrix} Q_t & S_t \\ S_t^T & R_t \end{bmatrix} \begin{bmatrix} x_t \\ u_t \end{bmatrix} + \begin{bmatrix} x_t \\ u_t \end{bmatrix}^T \begin{bmatrix} q_t \\ r_t \end{bmatrix}\end{split}\]

and the terminal cost term,

\[\ell_T(x_T) = x_T^T Q_T x_T + x_T^T q_T.\]