## 3 UAV Kinodynamic Models
### Kinodynamic Model 1  
The first kinodynamic motion model (\(\mathcal{KM}1\)) is derived from the popular nonlinear but simple motion model that is used for fixed-wing UAVs [51; 52], as follows:  
\[\mathcal{KM}1:=\left\{\begin{array}{l}\dot{x}(t)=v_{xy}(t)\text{cos}\,\psi( t)\\ \dot{y}(t)=v_{xy}(t)\text{sin}\,\psi(t)\\ m\dot{z}(t)=u_{z}(t)-m\,\mathbf{g}\\ \dot{\psi}(t)=\omega_{z}(t)\end{array}\right. \tag{8}\]  
where the notation of parameters is the same as the earlier formulation in the paper; to recap, \(v_{xy}\) and \(u_{z}\) denote horizontal and vertical linear input speeds, and \(\omega_{z}\) is the angular velocity that specifies the UAV heading \(\psi\) in 2D horizontal space. Through the following analogy, \(\mathcal{KM}1\) can be used for quadrotors:  
\[u\ :\ \left\{\begin{array}{l}u_{1}=u_{z}\\ u_{2}=v_{xy}\text{sin}\,\psi=v_{y}\\ u_{3}=v_{xy}\text{cos}\,\psi=v_{x}\\ u_{4}=\omega_{z}\end{array}\right. \tag{9}\]  
with \(\left\{\begin{array}{l}v_{xy}=\left\|u_{2},u_{3}\right\|_{2}\\ \psi=\tan^{-1}(u_{2}/u_{3})\end{array}\right.\) where the operator \(\left\|\.\right\|_{2}\) denotes the Euclidean (\(\mathcal{L}_{2}\)) norm. The transitional and angular accelerations as well as the dynamics of rotational velocities are ignored, which is reasonable because of the use of high bandwidth controllers with very fast responses (considering the sampling time). However, this model is still nonlinear, making computationally efficient real-time trajectory optimization challenging, particularly for long-receding horizon MPC optimization [17]. The linearized form of \(\mathcal{KM}1\) in discrete time with sampling time \(h\) is presented as:  
\[\mathcal{DK}\mathcal{M}1:=\left\{\begin{array}{c}\underbrace{\xi[k+1]}_{x_{1}[k +1]}=\underbrace{\begin{bmatrix}1&0&0\\ 0&1&0\\ 0&0&1\end{bmatrix}}_{C_{1}}\underbrace{\xi[k]}_{x_{1}[k]}+h\underbrace{ \begin{bmatrix}1&0&0\\ 0&1&0\\ 0&0&1/m\end{bmatrix}}_{H_{1}}\left(\underbrace{\begin{bmatrix}u_{3}[k]\\ u_{2}[k]\\ u_{1}[k]\end{bmatrix}-\begin{bmatrix}0\\ 0\\ m\mathbf{g}\end{bmatrix}}_{\text{\tiny{$\mathfrak{A}$}}[k]}\right)\end{array} \right. \tag{10}\]  
where \(\xi=\begin{bmatrix}x&y&z\end{bmatrix}^{\top}\in\mathbb{R}^{3}\) denotes the 3D coordinates (position) of the UAV; \(X_{1}\in\mathbb{R}^{3}\) denotes state vector (3D coordinates), \(G_{1}\in\mathbb{R}^{3\times 3}\) denotes discrete state matrix, and \(H_{1}\in\mathbb{R}^{3\times 3}\) denotes discrete input matrix.  
**Remark 2**: \(u_{4}=\omega_{z}\)_, associated with the UAV lateral movement (i.e., turning around the \(z\)-axis), is eliminated in the input vector, as it does not impact the linear motion [53] and the quadrotor's heading change is controlled by controlling horizontal speeds in \(x\) and \(y\) directions (\(v_{x}\) and \(v_{y}\)). Nevertheless, we add a rate of heading change (i.e., nonholonomic) constraint to the trajectory optimization problem, with appropriate convexification presented in Lemma 1._  
The rate of heading change constraint is modeled as:  
\[\overrightarrow{\psi}[k]=\left(\tan^{-1\left(v_{y}[k]/\tau_{\text{\tiny{$ \mathfrak{A}$}}}[k]\right)}-\tan^{-1\left(v_{y}[k-1]/\tau_{\text{\tiny{$ \mathfrak{A}$}}}[k-1]\right)}\right)<\left(h*\psi_{max}\right)\left/\left(1+ \gamma\tau_{\text{\tiny{$\mathfrak{A}$}}}y\right)\right. \tag{11}\]  
where \(\psi_{max}\) denotes the maximum heading change that can be achieved in rad/s, and \(\gamma\) is a hyperparameter to limit heading change based on horizontal speed. For example, if the horizontal speed is zero (i.e., the quadrotor is hovering) the maximum heading change can be achieved, but by increasing the horizontal speed, its heading change is limited for a stable maneuver. However, embedding the rate of the heading change constraint to model nonholonomic limitations makes the optimization problem non-convex.