## 7 Receding Horizon Trajectory Planning
### LQR-Based Trajectory Generation  
DDP [87; 88] is one of the first techniques proposed for solving optimal control problems. Let \(\mathbf{x}_{k+1}=\mathbf{f}_{d}(\mathbf{x}_{k},\mathbf{u}_{k})\) be the discrete-time system dynamics; the total cost of the trajectory can be formulated for a given control policy, i.e., \(\pi_{k+i}\), for all \(i=\{0,1,\ldots,N-1\}\). \[\sum_{i=0}^{N-1}c(\mathbf{x}_{k+i},\mathbf{u}_{k+i})+c_{goal}(\mathbf{x}_{k+N}). \tag{50}\]  
The optimal control input, i.e., \(\mathbf{u}_{k+i}=\pi_{k+i}(\mathbf{x}_{k+i})\), for a given time index, i.e., \(i+k\), can be obtained by minimizing the (50). Thus, cost (cost-to-go), which was proposed in [89], is fully determined by:  
\[V_{k+i}(\mathbf{x}_{k+i})=\underset{\mathbf{u}_{k+i}}{\text{min}}\quad(c( \mathbf{x}_{k+i},\mathbf{u}_{k+i})+V_{k+1}(\mathbf{f}_{d}(\mathbf{x}_{k+i}, \mathbf{u}_{k+i})). \tag{51}\]  
The same procedure can be applied recursively in a backward direction for seeking the optimal \(\pi_{k+i}(\mathbf{x}_{k+i})=\arg\underset{\mathbf{u}_{k+i}}{\text{min}}(c( \mathbf{x}_{k+i},\mathbf{u}_{k+i})+V_{k+i}(\mathbf{f}_{d}(\mathbf{x}_{k+i}, \mathbf{u}_{k+i})))\). DDP yields almost the same behavior: first estimate optimal control and then apply a forward pass to determine the updated nominal trajectory. Consequently, LQR is a simplified version of DDP. LQR is one of the fundamental ways to obtain a closed-form solution for a given optimal control problem under which system dynamics is assumed to be linear. Let us assume the system dynamics is defined as in (4). The intuition of LQR is to estimate the optimal control sequence for maneuvering the quadrotor from an initial position to the desired pose. Let \(N\) be the receding horizon whose optimal trajectory is to be determined. The total cost, i.e., \(J_{k}(\mathbf{x}_{k},\pi_{N})\), consists of three parts: initial, intermediate, and final costs, where \(\pi_{N}=\{\pi_{k},\pi_{k+1},\ldots,\pi_{k+i},\ldots,\pi_{N-1}\}\):  
\[J_{k}(\mathbf{x}_{k},\pi_{N})=c_{start}(\mathbf{x}_{k})+\sum_{i=0}^{N-1}c( \mathbf{x}_{k+i},\mathbf{u}_{k+i})dt+c_{end}(\mathbf{x}_{k+N}), \tag{52}\]  
where \(\frac{\partial^{2}C_{start}(x_{k})}{\partial x\partial x}\leq 0\), \(\frac{\partial^{2}C_{goal}(x_{k+N})}{\partial x\partial x}\leq 0\), \(\frac{\partial^{2}C}{\partial u}\Big{|}\frac{\partial^{2}C}{\partial u}\leq 0\), and \(\frac{\partial^{2}C}{\partial u\partial u}\leq 0\) are positive semi-definite Hessians to guarantee the minimizing of the total cost. The total cost can be formulated in various ways. In LQR, the total cost is defined as Quadratic costs as follows:  
\[c_{start}(\mathbf{x}_{k})=\frac{1}{2}\mathbf{x}_{k}^{T}Q_{start}\mathbf{x}_{k }+\mathbf{x}_{k}^{T}q_{start},\]  
\[c_{goal}(\mathbf{x}_{k+N})=\frac{1}{2}\mathbf{x}_{k+N}^{T}Q_{goal}\mathbf{x}_ {k+N}+\mathbf{x}_{k+N}^{T}q_{goal},\]  
\[c(\mathbf{x}_{k+i},\mathbf{u}_{k+i})=\frac{1}{2}\mathbf{x}_{k+i}^{T}Q\mathbf{x }_{k+i}+\frac{1}{2}\mathbf{u}_{k+i}^{T}\mathbf{u}_{k+i}+\mathbf{u}_{k+i}^{T} P\mathbf{x}_{k+i}+\mathbf{x}_{k+i}^{T}p \tag{53}\]  
where \(i\in\{0,1,\ldots,N-1\}\), \(Q_{start}\in\mathbb{R}^{n_{x}\times n_{x}}Q_{goal}\in\mathbb{R}^{n_{x}\times n _{x}},Q\in\mathbb{R}^{n_{x}\times n_{x}},R\in\mathbb{R}^{n_{u}\times n_{u}}\), \(P\in\mathbb{R}^{n_{u}\times n_{x}},q_{start}\in\mathbb{R}^{n_{x}},q_{goal}\in \mathbb{R}^{n_{x}},p\in\mathbb{R}^{n_{x}}\), \(r\in\mathbb{R}^{n_{u}}\), and \(\tilde{\zeta}\in\mathbb{R}\) are predefined in which \(Q_{start},Q_{goal},Q\), and \(R\) are positive definite, whereas \(J_{k}\geq 0\) and \(j_{k}\geq 0\) assumed to be positive semi-definite. LQR problem (52) and (53) provides an optimal \(\pi_{N}\) in close form solution as expressed in (51); the cost-to-go function, i.e., (51), can be reformulated as an explicit quadratic formulation as follows:  
\[V_{k+i}(\mathbf{x}_{k+i})=\frac{1}{2} \begin{bmatrix}\mathbf{x}_{k+i}\\ \mathbf{u}_{k+i}\end{bmatrix} ^{T}J_{k+i} \begin{bmatrix}\mathbf{x}_{k+i}\\ \mathbf{u}_{k+i}\end{bmatrix} + \begin{bmatrix}\mathbf{x}_{k+i}\\ \mathbf{u}_{k+i}\end{bmatrix}