## 4 Trajectory Optimization Problem
### Model Predictive Control (MPC) Optimization  
In addition to time complexities associated with 3D obstacle-free trajectory design with valid LoS to IoVs, there is another factor that makes the trajectory optimization problem challenging. Since the goal point of the AV trajectory is uncertain, the solution to \(\mathcal{P}1\) can be suboptimal. The reason is that \(\mathcal{P}1\) is solved for each sample time, where the solutions of the receding time steps depend on the solution of the preceding time steps. This implies that after each time step, as the UAV moves to a new position, it loses a degree of freedom to explore the global optimal trajectory because the UAV trajectory is restricted to motion constraints. Figure 3 shows a naive illustration for two consecutive time slots. Suppose that \(X_{opt}[k+2]\) is a waypoint of the global optimum trajectory indicated by the green trajectory. On the other hand, the solution to \(\mathcal{P}1\) at sample time \([k+1]\) is denoted by \(X[k+1]\), which can be an energy-efficient and optimal solution. However, \(\mathcal{P}1\) cannot converge \(X[k+1]\) to \(X_{opt}[k+2]\) due to the speed/acceleration as well as the heading change constraints, so the solution lands upon \(X[k+2]\). This results in the divergence of the trajectory from the global optimum trajectory after each sampling time. Therefore, the whole trajectory including all sample times should be optimized at the same time. To this end, the trajectory optimization problem can be modeled as a finite horizon MPC optimization where the UAV trajectory across all sample times is optimized in one step so that global optimization is achieved. The MPC optimization can be formulated as \(\mathcal{P}2\) in the following. \[ \begin{array}{ll}\text{OF2.}&\min_{\mathfrak{U}[k]}\;\forall \mathfrak{K}\in\mathcal{K}\;\sum\limits_{k=1}^{K}\left(\underbrace{\omega_{2_ {i}}\left\|\mathfrak{U}[k]\right\|_{2}^{2}}_{\text{OF2}}+\omega_{2_{i}} \underbrace{\sum\limits_{n}^{N}\left\|\mathfrak{K}[k]-p_{n}[k]\right\|_{2}^{2} }_{\text{OP2}}\right)\\ \text{s. t.}&\\ \text{C2}_{1.}&X[k+1]=G\;X[k]+H\;\mathfrak{U}[k];&\forall k\in\mathcal{K}\\ \text{C2}_{2.}&X[k+1]=\mathcal{G}_{k}\;X[0]+\mathcal{H}_{k}\;\mathbb{U}[k],& \forall k\in\mathcal{K}\\ \text{C2}_{3.}&\mathbb{X}=\mathbb{G}X[0]+\mathbb{U}[K-1],&\\ \text{C2}_{4.}&\text{C1}_{1\sim 7}.&\forall k\in\mathcal{K}\end{array} \]  
where, e.g., for \(\mathcal{D}\mathcal{K}\mathcal{M}1\) in terms of matrix dimensions, we have:  
\[\mathbb{X}=X[1\sim K]=\left[X^{\top}[1],\;X^{\top}[2],\ldots,X^{\top}[K]\right] ^{\top}\in\mathbb{R}^{3K\times 1}; \tag{19}\]  
\[\mathbb{U}[k]=\left[\mathfrak{U}^{\top}[0],\;\mathfrak{U}^{\top}[1],\; \mathfrak{U}^{\top}[2],\ldots,\mathfrak{U}^{\top}[k]\right]^{\top}\in\mathbb{ R}^{3(k+1)\times 1}; \tag{20}\]  
\[\mathcal{G}_{k}=G^{k+1}\in\mathbb{R}^{3\times 3}; \tag{21}\]  
\[\mathbb{G}=\left[G^{1},\;G^{2},\ldots,G^{K}\right]^{\top}\in\mathbb{R}^{3K \times 3}; \tag{22}\]  
Figure 3: A naive illustration of the imposed limitations by motion constraints for converging to the global optimum trajectory by solving \(\mathcal{P}1\) for each sample time. \[\mathcal{H}_{k}=\left[G^{k}H,\ G^{k-1}H,\ldots,GH,H\right]^{\top}\in\mathbb{R}^{3(k+ 1)\times 3}; \tag{23}\]  
\[\mathbb{H}=\left[ \begin{array}{cccc}H&0&0&0\\ GH&H&\cdots&0&0\\ G^{2}H&GH&&0&0\\ \vdots&&\ddots&&\vdots\\ G^{K-1}H&G^{K-2}H&\cdots&GH&H\end{array} \right]\in\mathbb{R}^{3K\times 3K}. \tag{24}\]  
C\(2_{1}\) is the same as C\(1_{1}\), which models the linearized motion and state transition in consecutive time slots; through C\(2_{2}\), the state of the system can be obtained from the initial position \(X[0]\) (start point) by having \(\mathbb{U}[k]\), which provides motion inputs from the initial state to state \(k+1\). Lastly, using C\(2_{3}\), all states of the system \((\mathbb{X})\), i.e., UAV consecutive positions/velocity and accelerations, are obtained from the initial point up to the last sample time, provided that \(\mathbb{U}[K-1]\) (i.e., all inputs from \(k=0\ \sim K-1\)) are given.