An Unscented Kalman Filter Based on the Adams–Bashforth Method with Applications to the State Estimation of Osprey-Type Drones Composed of Tiltable Rotor Mechanisms

Abstract

In the state estimation problem for nonlinear systems, the Unscented Kalman Filter (UKF) has gained attention as an algorithm capable of accurate state estimation based on high-fidelity discretization for strongly nonlinear systems. Furthermore, for applying the UKF to continuous-time state–space models, a method employing the Runge–Kutta method in the time-update equation for sigma points has already been proposed to achieve high-precision state estimation. While this method uses high-order numerical approximations, the associated decrease in computational efficiency due to processing time becomes problematic. It is thus unsuitable for the state estimation of relatively fast-moving objects, such as autonomous vehicles and drones, which require high sampling frequencies. In this study, to reduce computational load while achieving relatively high estimation accuracy, we newly apply the Adams–Bashforth method to the UKF algorithm. The effectiveness of the proposed method is demonstrated by first explaining a low-dimensional model’s state estimation problem, followed by a comparison of estimation accuracy and computation time in state estimation simulations for the UAV model of an Osprey-type drone.

1. Introduction

The navigation problem for moving objects is a fundamental challenge with important engineering applications [1]. The problem of position estimation is inherently formulated as a nonlinear filtering problem, and the theory of optimal filtering has been established by researchers such as [2]. However, in practice, only the linear Kalman filter (KF) proposed by Kalman [3] and its nonlinear extensions are feasible for implementation. This is because optimal nonlinear filters are computationally too expensive to implement.

The motion models of many real-world systems, particularly autonomous mobile robots, are described in continuous time. However, since observation information from sensors is obtained discretely through sampling, the observation model becomes discrete time. Such a system is called a continuous-discrete (CD) system [4]. While the approach that directly applies the Kalman filter to this CD system is referred to as CD-KF, in practice, it is common to discretize or numerically integrate the continuous-time motion model and construct a discrete-time Kalman filter. A Kalman filter in which both the motion model and the observation model are discrete-time is called a discrete–discrete Kalman filter (DD-KF). For nonlinear dynamics or nonlinear observation systems, the CD-EKF (continuous-discrete extended Kalman filter) or DD-EKF (discrete–discrete extended Kalman filter), which apply the Kalman filter to their linearly approximated systems, are widely used [5,6,7,8]. Furthermore, as a stable algorithm that does not require linear approximation via Jacobians, the Unscented Kalman Filter (UKF) has been proposed for DD nonlinear systems [9,10,11]. This will be referred to as DD-UKF hereafter. The UKF employs a nonlinear transformation called the Unscented Transform (UT). For a system of order n, it generates $(2n+1)$ sample points called sigma points, applies a nonlinear transformation to each, and then calculates the conditional expectation of the state vector and the covariance matrix by taking the sample average of the transformed points. By using this UT, the UKF enables state estimation based on high-fidelity discretization.

Since the proposal of the UKF, many researchers have advanced its development and improvement. Särkkä [12] derived continuous-time and continuous-discrete versions of the UKF and applied them to nonlinear continuous-time filtering and re-entry vehicle tracking problems. van der Merwe [13] proposed the square-root UKF to improve numerical stability, and Ito and Xiong [14] analyzed the convergence of Gaussian filters, including the UKF. Julier [15] proposed the spherical simplex UT to reduce the number of sigma points. Meanwhile, Haykin [16] advanced research on the integration of neural networks and Kalman filtering, demonstrating new possibilities for nonlinear adaptive filtering. Elliott [17] contributed to the foundations of filtering theory through the theory of hidden Markov models and stochastic control. Recently, Singh [18] comprehensively reviewed the development of Gaussian filtering, including the UKF. In particular, recent studies have combined the UKF with outlier handling and event-triggered mechanisms. For example, Ref. [19] proposed a dual-event-triggering ANFIS-based UKF for cluster cooperative navigation with measurement anomalies, demonstrating potential applications to navigation problems in complex environments.

From the perspective of numerical integration methods, Takeno and Katayama [20] applied the Runge–Kutta (RK) method to the time-update equations for sigma points and demonstrated that high-precision state estimation is possible for several models. Furthermore, Takeno and Katayama [21] applied Heun’s method, an improved version of the Euler method, to the prediction step. Kulikov and Kulikova [22,23] have advanced research on high-precision numerical implementation of the continuous-discrete UKF. However, discussions on these UKFs have focused primarily on estimation accuracy, with little consideration given to computational efficiency. Due to its characteristic of performing nonlinear transformations on each of the $(2n+1)$ sigma points in the UT algorithm, the UKF has the drawback of increased computation time as the system order n grows. For state estimation of large-scale real-world models, both accuracy and real-time performance are required, making the improvement of computational efficiency in UKF algorithms critically important [24]. In recent years, research addressing this computational efficiency challenge has begun to emerge. Wang et al. [25] proposed an adaptive step-size UKF based on the degree of nonlinearity, achieving a balance between accuracy and computational cost by dynamically adjusting the step size in highly nonlinear regions. Researchers in the aerospace domain [26] introduced a high-efficiency UKF utilizing parallel computation for sigma point propagation, demonstrating significant speedup for multi-target trajectory estimation. The integration of machine learning techniques with UKF has also been explored; a GAN-enhanced UKF framework [27] dynamically predicts and updates filter parameters in real-time, improving estimation accuracy without sacrificing computational performance. In the realm of numerical implementation, Kulikova and Kulikov [28] developed square-root information-type methods for continuous-discrete extended Kalman filtering, enhancing numerical stability which is crucial for efficient computation. Furthermore, theoretical advances in sensor scheduling for continuous-discrete systems [29] provide insights into optimizing the trade-off between resource allocation and estimation accuracy. A comparative evaluation of nonlinear filters [30] further contextualizes the performance of the UKF against other methods in practical tracking applications. These recent developments underscore the growing recognition of computational efficiency as a critical factor in the practical deployment of nonlinear Kalman filters.

Regarding Unmanned Aerial Vehicle (UAV) navigation, comprehensive reviews exist on topics such as autonomous navigation in GPS-denied environments [31], UAV positioning using GNSS [32], and sensor-based autonomous localization [33]. These reviews highlight the importance of filtering techniques in UAV navigation. Therefore, in this study, to improve the computational efficiency of the UKF, we propose a UKF that applies the Adams–Bashforth (AB) method, instead of the RK method, to the time-update equations for sigma points. The RK method is a single-step numerical integration method; it calculates the next numerical point $(x\left(t_{k+1}\right),t_{k+1})$ based solely on the current point $x\left(t_k\right)$. Furthermore, when the order of accuracy is $s (s<5)$, s function evaluations are required to generate the next numerical point. On the other hand, the AB method is a multi-step numerical integration method. For an accuracy order of 4, the next numerical point $x\left(t_{k+1}\right)$ is calculated based on the past four points $x\left(t_{k-3}\right),x\left(t_{k-2}\right),x\left(t_{k-1}\right),x\left(t_k\right)$, and only one function evaluation is needed to generate the next numerical point. Thus, regardless of the accuracy order s of the AB method, the number of function evaluations is always one. Therefore, compared to the fixed-step RK method, computation with the AB method is more efficient.

It should be noted, as a related study focusing on the degradation of computational efficiency of the RK method when applying numerical integration to nonlinear Kalman filters, that He et al. [34] have already conducted research applying a multi-step method based on the Adams–Bashforth–Moulton (ABM) method to the EKF. The objective of this study is to demonstrate that the UKF applying the AB method to the time-update equations for sigma points in the UT can achieve estimation with computational efficiency superior to that of the RK method, while maintaining comparable estimation accuracy. In addition to comparison with the RK method, we also compare the differences in estimation accuracy and computation time resulting from varying orders (2nd to 6th) of the AB method itself.

The effectiveness of the proposed method is demonstrated through state estimation in a MATLAB 2022a simulation environment using two nonlinear models. Specifically, as a preliminary experiment, we first apply the method to a low-dimensional falling object model used in previous research and compare estimation performance under identical conditions. Subsequently, as an application to a more complex model, we perform a similar comparison of estimation performance using a UAV model of an Osprey-type drone.

The structure of this paper is as follows. Section 2 formulates the continuous-discrete system and the state estimation problem. Section 3 discusses the integration of the UT with the Runge–Kutta method and the Adams–Bashforth method, presenting the time-update equations for sigma points based on each method. Section 4 details the modeling of the Osprey-type drone. After defining the coordinate systems, the rotational and translational motions are explained in detail. Section 5 describes the control law and control allocation. Specifically, a controller based on the computed torque method and the control input allocation problem are detailed. Section 6, as a preliminary experiment, applies the sigma-point time-update equations presented in Section 3 to the falling object model used in previous research, comparing estimation accuracy and computation time. Section 7 first derives the state equations and sigma-point time-update equations for the Osprey-type UAV model, then performs comparisons of estimation accuracy and computation time similar to those in Section 6. Furthermore, we provide discussions based on a broader range of estimation results, including not only comparisons with the RK method but also differences in estimation performance due to the order of the AB method and comparisons of estimation performance under different sampling periods. Section 6 summarizes the paper and presents concluding remarks.

2. Continuous-Discrete System and Problem Formulation

2.1. Continuous-Discrete System

The motion of many real-world systems, particularly moving objects such as UAVs, is described in continuous time, while observations from sensors are obtained discretely through sampling. Such systems are called CD systems, and the foundations of their filtering theory are detailed in Jazwinski [4].

In this study, we consider a system described by the following nonlinear continuous-time state equation and discrete-time observation equation:

$$\begin{array}{cc}\hfill \dot{x}\left(t\right)& =f_c(x\left(t\right),u\left(t\right))+w_c\left(t\right),\hfill \end{array}$$

(1)

$$\begin{array}{cc}\hfill y\left(t_k\right)& =h_m\left(x\left(t_k\right)\right)+v\left(t_k\right).\hfill \end{array}$$

(2)

Here, t denotes continuous time, $t_k=k\Delta t$ ($k=0,1,\dots$) denotes sampling instants, and $\Delta t$ is the sampling interval. $x\left(t\right)\in {\mathbb{R}}^n$ is the state vector, $u\left(t\right)\in {\mathbb{R}}^m$ is the input vector, and $y\left(t_k\right)\in {\mathbb{R}}^p$ is the output vector. $f_c:{\mathbb{R}}^n\times {\mathbb{R}}^m\to {\mathbb{R}}^n$ and $h_m:{\mathbb{R}}^n\to {\mathbb{R}}^p$ are nonlinear functions. Furthermore, $w_c\left(t\right)\in {\mathbb{R}}^n$ is the system noise, and $v\left(t_k\right)\in {\mathbb{R}}^p$ is the observation noise, both assumed to be Gaussian white noises with zero mean and covariance matrices $Q_c$ and R, respectively.

2.2. Discrete–Discrete Unscented Kalman Filter

Discretizing the continuous-time state Equation (1) using some numerical integration method yields the following discrete-time state equation:

$$\begin{array}{c}\hfill x\left(t_{k+1}\right)=f_d(x\left(t_k\right),u\left(t_k\right))+w\left(t_k\right)\end{array}$$

(3)

where $f_d$ is an approximation function obtained through the numerical integration method, and $w\left(t_k\right)$ is the system noise arising from discretization. The UKF can be applied to the DD system composed of Equations (2) and (3).

The UKF is an algorithm that performs state estimation for nonlinear systems using the Unscented Transform (UT) [9,10,11]. Hereafter, the filtered estimate is denoted as $\widehat{x}\left(t_k\right|t_k)$, its covariance matrix as $P\left(t_k\right|t_k)$, the one-step predicted estimate as $\widehat{x}\left(t_{k+1}\right|t_k)$, and its covariance matrix as $P\left(t_{k+1}\right|t_k)$. The initial estimate $\widehat{x}\left(0\right|0)$ and initial covariance matrix $P\left(0\right|0)$ are assumed to be known.

The UKF algorithm consists of a prediction step and an update step. The detailed derivation and specific equations are provided in Appendix A; here, we present only an overview.

Prediction Step: From the estimate $\widehat{x}\left(t_k\right|t_k)$ and covariance matrix $P\left(t_k\right|t_k)$ at time $t_k$, $(2n+1)$ sigma points ${\mathcal{X}}_i\left(t_k\right|t_k)$ and corresponding weights $W_i$ are generated. These sigma points are propagated through the nonlinear function $f_d$, and the one-step predicted estimate $\widehat{x}\left(t_{k+1}\right|t_k)$ and its covariance matrix $P\left(t_{k+1}\right|t_k)$ are computed by taking the weighted average.

Update Step: The predicted sigma points ${\mathcal{X}}_i\left(t_{k+1}\right|t_k)$ are substituted into the observation function $h_m$ to compute the predicted output $\widehat{y}\left(t_{k+1}\right|t_k)$, its covariance $P_{yy}$, and the cross-covariance between state and output $P_{x\nu }$. The innovation $\nu \left(t_{k+1}\right)$ is computed using the actual observation $y\left(t_{k+1}\right)$, and the estimate $\widehat{x}\left(t_{k+1}\right|t_{k+1})$ and covariance matrix $P\left(t_{k+1}\right|t_{k+1})$ are updated via the Kalman gain $K\left(t_{k+1}\right)$.

For the specific computational procedure and complete set of equations for the UKF, please refer to Appendix A.

3. Unscented Transform and Discretization Methods

3.1. Discrete-Time State–Space Model

Consider the following continuous-time nonlinear system:

$$\begin{array}{cc}\hfill {\displaystyle \frac{dx}{dt}}=f_c\left(x\right),& x\left(0\right)=x_0\hfill \end{array}$$

(4)

where $x\in {\mathbb{R}}^n$ is the state vector and $x_0$ is the initial value. Discretizing this continuous-time system using some numerical integration method yields the following time-update equation:

$$\begin{array}{cc}\hfill x\left(t_{k+1}\right)=f_d\left(x\left(t_k\right)\right),& k=0,1,\dots \hfill \end{array}$$

(5)

The observation equation is given similarly to Equation (2) as

$$\begin{array}{cc}\hfill y\left(t_k\right)=h_m\left(x\left(t_k\right)\right)+v\left(t_k\right),& k=0,1,\dots \hfill \end{array}$$

(6)

where v is Gaussian white noise with zero mean and covariance matrix R. Equations (5) and (6) constitute the discrete-time state–space model.

The objective of this study is to propose a nonlinear filtering algorithm that sequentially estimates the state vector $x\left(t_k\right)$ based on the observation data $y\left(t_k\right)$. Takeno and Katayama [20] demonstrated that combining the UKF with the RK method enables high-precision state estimation. In this study, we show that by combining the AB method with the UT instead of the RK method, relatively high-precision state estimation can be achieved while reducing computational load.

For the general theory of numerical solutions for stochastic differential equations, please refer to Kloeden and Platen [35].

3.2. Integration of UT and the Runge–Kutta Method

When applying the RK method to the time update of sigma points in the UKF, numerical integration is performed for each sigma point. The sigma point matrix ${\mathcal{X}}_k$ at time k with its element ${\mathcal{X}}_{i,j,k}$ is defined as follows:

$${\mathcal{X}}_k=\left[\begin{array}{ccc}{\mathcal{X}}_{1,1,k}& \dots & {\mathcal{X}}_{1,2n+1,k}\\ ⋮& \ddots & ⋮\\ {\mathcal{X}}_{n,1,k}& \dots & {\mathcal{X}}_{n,2n+1,k}\end{array}\right].$$

(7)

Here, $i=1,\dots ,n$ denotes the element of the state vector, and $j=1,\dots ,2n+1$ denotes the index of the sigma point. Let the step size be $\Delta t=h$ and the time instant be $t_k=kh$.

When applying the 4th-order Runge–Kutta method, the time-update equation for the sigma points is expressed in the following general form:

$$\begin{array}{c}\hfill {\mathcal{X}}_{i,j,k+1}^{-}={\mathcal{X}}_{i,j,k}+{\displaystyle \frac{h}{6}}(a_{1,ij}+2a_{2,ij}+2a_{3,ij}+a_{4,ij}).\end{array}$$

(8)

Here, $a_{1,ij},a_{2,ij},a_{3,ij},a_{4,ij}$ are the function evaluations at each stage of the RK method. The specific definitions and detailed derivations are provided in Appendix B.

By taking the weighted average of these updated sigma points, the predicted value of the state vector (Equation (A10)) and the predicted covariance matrix (Equation (A11)) are obtained.

3.3. Integration of UT and the Adams–Bashforth Method

This section describes the integration of the AB method with the UT, which is the core of this study. The ordinary AB method is a multi-step method that calculates the next point using information from multiple past points. In this study, we extend and apply this method to the sigma point matrix of the UKF.

First, the result of substituting each column (each sigma point) of the sigma point matrix ${\mathcal{X}}_k$ at time k into the nonlinear function $f_c$ is defined as the following $n\times (2n+1)$ matrix:

$${\mathcal{F}}_c\left({\mathcal{X}}_k\right)=\left[\begin{array}{ccc}{f}_1\left({\mathcal{X}}_{1,1,k}\right)& \dots & f_1\left({\mathcal{X}}_{1,2n+1,k}\right)\\ ⋮& \ddots & ⋮\\ f_n\left({\mathcal{X}}_{n,1,k}\right)& \dots & f_n\left({\mathcal{X}}_{n,2n+1,k}\right)\end{array}\right].$$

(9)

This ${\mathcal{F}}_c\left({\mathcal{X}}_k\right)$ is a matrix that stores the results of evaluating the nonlinear function for all sigma points at time k. While the ordinary AB method uses function evaluation values for past state vectors, the feature of the proposed method is that it stores function evaluation matrices for past sigma point matrices ${\mathcal{F}}_c\left({\mathcal{X}}_{k-1}\right),{\mathcal{F}}_c\left({\mathcal{X}}_{k-2}\right),\dots$ and uses them as “past information.”

Below, we present the time-update equations for sigma points using the 2nd- to 6th-order AB methods [36,37].

• 2nd-order Adams–Bashforth method:

  $$\begin{array}{c}\hfill {\mathcal{X}}_{k+1}^{-}={\mathcal{X}}_k+{\displaystyle \frac{h}{2}}\left(3{\mathcal{F}}_c\left({\mathcal{X}}_k\right)-{\mathcal{F}}_c\left({\mathcal{X}}_{k-1}\right)\right).\end{array}$$

  (10)
• 3rd-order Adams–Bashforth method:

  $$\begin{array}{c}\hfill {\mathcal{X}}_{k+1}^{-}={\mathcal{X}}_k+{\displaystyle \frac{h}{12}}\left(23{\mathcal{F}}_c\left({\mathcal{X}}_k\right)-16{\mathcal{F}}_c\left({\mathcal{X}}_{k-1}\right)+5{\mathcal{F}}_c\left({\mathcal{X}}_{k-2}\right)\right).\end{array}$$

  (11)
• 4th-order Adams–Bashforth method:

  $$\begin{array}{c}\hfill {\mathcal{X}}_{k+1}^{-}={\mathcal{X}}_k+{\displaystyle \frac{h}{24}}\left(55{\mathcal{F}}_c\left({\mathcal{X}}_k\right)-59{\mathcal{F}}_c\left({\mathcal{X}}_{k-1}\right)+37{\mathcal{F}}_c\left({\mathcal{X}}_{k-2}\right)-9{\mathcal{F}}_c\left({\mathcal{X}}_{k-3}\right)\right).\end{array}$$

  (12)
• 5th-order Adams–Bashforth method:

  $$\begin{array}{cc}\hfill {\mathcal{X}}_{k+1}^{-}={\mathcal{X}}_k+{\displaystyle \frac{h}{720}}(& 1901{\mathcal{F}}_c\left({\mathcal{X}}_k\right)-2774{\mathcal{F}}_c\left({\mathcal{X}}_{k-1}\right)\hfill \\ \hfill & +2616{\mathcal{F}}_c\left({\mathcal{X}}_{k-2}\right)-1274{\mathcal{F}}_c\left({\mathcal{X}}_{k-3}\right)+251{\mathcal{F}}_c\left({\mathcal{X}}_{k-4}\right)).\hfill \end{array}$$

  (13)
• 6th-order Adams–Bashforth method:

$$\begin{array}{cc}\hfill {\mathcal{X}}_{k+1}^{-}={\mathcal{X}}_k+{\displaystyle \frac{h}{1440}}(& 4277{\mathcal{F}}_c\left({\mathcal{X}}_k\right)-7923{\mathcal{F}}_c\left({\mathcal{X}}_{k-1}\right)\hfill \\ \hfill & +9982{\mathcal{F}}_c\left({\mathcal{X}}_{k-2}\right)-7298{\mathcal{F}}_c\left({\mathcal{X}}_{k-3}\right)\hfill \\ \hfill & +2877{\mathcal{F}}_c\left({\mathcal{X}}_{k-4}\right)-475{\mathcal{F}}_c\left({\mathcal{X}}_{k-5}\right)).\hfill \end{array}$$

(14)

Here, ${\mathcal{X}}_k$ and ${\mathcal{X}}_{k+1}^{-}$ are $n\times (2n+1)$ matrices. These equations are an extension of the ordinary AB method formulas to the sigma point matrix, representing an original formulation for integrating the numerical integration method into the specific filtering method of UKF. In particular, the method of storing and utilizing the results of function evaluations for past sigma point matrices is a novel aspect of this approach not found in existing research.

3.4. Comparison of Computational Complexity: Runge–Kutta Method vs. Adams–Bashforth Method

We compare the computational complexity of the RK and AB methods in the UT from the following two perspectives.

3.4.1. Computational Complexity Comparison Based on “Number of Stages”

The difference width $\Delta t=h$ in discretization is generally called the step size in numerical computation. The RK method computes the derivative $f_i\left(\mathcal{X}\right)$ multiple times while advancing one step (this is referred to as the “number of stages”). For example, the 4th-order RK method requires four stages of computation, and its global truncation error is $O\left(h^4\right)$.

On the other hand, since the AB method utilizes previously computed sigma point matrices ${\mathcal{F}}_c\left({\mathcal{X}}_{k-1}\right),{\mathcal{F}}_c\left({\mathcal{X}}_{k-2}\right),\dots$, only one computation of ${\mathcal{F}}_c\left({\mathcal{X}}_k\right)$ is sufficient to derive the time-update equation for the current sigma points. That is, regardless of the accuracy order of the AB method, the number of stages is always $1/4$ that of the RK method.

3.4.2. Computational Complexity Comparison Based on “Number of Elements in the Sigma Point Set”

Generally, when simulating large-scale models in real space, the number of state variables n can become very large (millions to tens of millions). In the UKF, since the size of the sigma point set is $(2n+1)$, this effect becomes pronounced. The RK method performs four stages of derivative computation for each sigma point, for each element of the n-dimensional state vector and for each element of the sigma point set.

In contrast, the AB method requires only one stage of function computation for each element of the sigma point set. Furthermore, for past sigma point sets, it only involves multiplying the entire matrix by coefficients according to the accuracy order. Therefore, the impact of an increase in the number of state variables is significantly smaller compared to the RK method.

This theoretical comparison of computational complexity will be verified through numerical experiments presented later in Section 6 and Section 7.

4. Modeling of the Osprey-Type Drone

4.1. Definition of Coordinate Systems

Figure 1 shows the coordinate systems used in this study for the Osprey-type drone. The world coordinate system $F_W$ is a right-handed coordinate system with origin ${\mathit{O}}_W$ and axes $X_W$, $Y_W$, $Z_W$, where $Z_W$ is positive vertically downward. The body coordinate system $F_B$ has its origin ${\mathit{O}}_B$ at the center of gravity of the vehicle. It is also a right-handed coordinate system with axes $X_B$, $Y_B$, $Z_B$ and $Z_B$ positive vertically downward. The positive direction of the $X_B$ axis is designated as the forward direction of the vehicle.

The coordinate system for the first coaxial rotor, $F_{P_1}$, has origin ${\mathit{O}}_{P_1}$ and axes $X_{P_1}$, $Y_{P_1}$, $Z_{P_1}$. Similarly, the coordinate system for the second coaxial rotor, $F_{P_2}$, has origin ${\mathit{O}}_{P_2}$ and axes $X_{P_2}$, $Y_{P_2}$, $Z_{P_2}$. The coordinate system for the i-th ($i=1,2$) coaxial rotor is integrated into $F_{P_i}$. In the body coordinate system $F_B$, the angular velocities about the $X_B$, $Y_B$, $Z_B$ axes are $(p,q,r)$, respectively. Furthermore, in the rotor coordinate system $F_{P_i}$, the tilt angles about the $X_{P_i}$, $Y_{P_i}$ axes are $({\alpha }_i,{\beta }_i)$. The initial tilt angles ${\alpha }_i$ and ${\beta }_i$ are 0. Due to servo motor characteristics, the range of ${\alpha }_i$ is $-2/\pi \le {\alpha }_i\le 2/\pi$, while ${\beta }_i$ is unlimited.

Let the Euler angles in the world coordinate system be $\mathit{\eta }={\left[\varphi \theta \psi \right]}^T$. The rotation matrices about the x, y, z axes, ${\mathit{R}}_x$, ${\mathit{R}}_y$, ${\mathit{R}}_z$, can be expressed as follows (S denotes sin, and C denotes cos):

$$\begin{array}{cc}\hfill {\mathit{R}}_x& =\left[\begin{array}{ccc}1& 0& 0\\ 0& C_{\varphi }& -S_{\varphi }\\ 0& S_{\varphi }& C_{\varphi }\end{array}\right],\hfill \end{array}$$

(15)

$$\begin{array}{cc}\hfill {\mathit{R}}_y& =\left[\begin{array}{ccc}{C}_{\theta }& 0& S_{\theta }\\ 0& 1& 0\\ -S_{\theta }& 0& C_{\theta }\end{array}\right],\hfill \end{array}$$

(16)

$$\begin{array}{cc}\hfill {\mathit{R}}_z& =\left[\begin{array}{ccc}{C}_{\psi }& -S_{\psi }& 0\\ S_{\psi }& C_{\psi }& 0\\ 0& 0& 1\end{array}\right].\hfill \end{array}$$

(17)

From these ${\mathit{R}}_x$, ${\mathit{R}}_y$, ${\mathit{R}}_z$, the transformation matrix from the body coordinate system to the world coordinate system, ${}^W{\mathit{R}}_B$, can be expressed as

$$\begin{array}{cc}\hfill {}^W{\mathit{R}}_B& ={\mathit{R}}_z\left(\psi \right){\mathit{R}}_y\left(\theta \right){\mathit{R}}_x\left(\varphi \right)\hfill \\ \hfill & =\left[\begin{array}{ccc}{C}_{\theta }{C}_{\psi }& S_{\varphi }{S}_{\theta }{C}_{\psi }-C_{\varphi }{S}_{\psi }& C_{\varphi }{S}_{\theta }{C}_{\psi }+S_{\varphi }{S}_{\psi }\\ C_{\theta }{S}_{\psi }& S_{\varphi }{S}_{\theta }{S}_{\psi }+C_{\varphi }{C}_{\psi }& C_{\varphi }{S}_{\theta }{S}_{\psi }-S_{\varphi }{C}_{\psi }\\ -S_{\theta }& S_{\varphi }{C}_{\theta }& C_{\varphi }{C}_{\theta }\end{array}\right].\hfill \end{array}$$

(18)

Also, let the transformation matrix for angular velocity from the world coordinate system to the body coordinate system be $W_{\mathit{\eta }}$. Then, ${\mathit{\omega }}_B\triangleq {\left[pqr\right]}^T$ is

$$\begin{array}{cc}\hfill {\mathit{\omega }}_B& =W_{\mathit{\eta }}\dot{\mathit{\eta }}\hfill \end{array}$$

(19)

and its inverse is given by

$$\begin{array}{cc}\hfill \dot{\mathit{\eta }}& =W_{\eta }^{-1}{\mathit{\omega }}_B.\hfill \end{array}$$

(20)

Here, it is found in [38,39] that

$$\begin{array}{cc}\hfill W_{\eta }& =\left[\begin{array}{ccc}1& 0& -S_{\theta }\\ 0& C_{\varphi }& C_{\theta }{S}_{\varphi }\\ 0& -S_{\varphi }& C_{\theta }{C}_{\varphi }\end{array}\right],\hfill \end{array}$$

(21)

$$\begin{array}{cc}\hfill W_{\eta }^{-1}& =\left[\begin{array}{ccc}1& S_{\varphi }{T}_{\theta }& C_{\varphi }{T}_{\theta }\\ 0& C_{\varphi }& -S_{\varphi }\\ 0& S_{\varphi }/C_{\theta }& C_{\varphi }/C_{\theta }\end{array}\right]\hfill \end{array}$$

(22)

where $T_x=tan\left(x\right)$. Note that $W_{\eta }$ is invertible unless $\theta =(2k-1)\varphi /2,(k\in \mathbb{Z})$. That is, it is invertible as long as it does not take the specific $\varphi =\pm \pi /(2k-1)$ where gimbal lock occurs.

From the tilt angles ${\alpha }_i$ and ${\beta }_i$, where $(i=1,2)$ in each rotor coordinate system, the transformation matrix from each rotor coordinate system to the body coordinate system, ${}^B{\mathit{R}}_{P_i}$, can be expressed as

$$\begin{array}{cc}\hfill {}^B{\mathit{R}}_{P_1}& ={\mathit{R}}_z\left(0\right){\mathit{R}}_y\left({\beta }_1\right){\mathit{R}}_x\left({\alpha }_1\right)\hfill \\ & =\left[\begin{array}{ccc}{C}_{{\beta }_1}& S_{{\alpha }_1}{S}_{{\beta }_1}& C_{{\alpha }_1}{S}_{{\beta }_1}\\ 0& C_{{\alpha }_1}& -S_{{\alpha }_1}\\ -S_{{\beta }_1}& S_{{\alpha }_1}{C}_{{\beta }_1}& C_{{\alpha }_1}{C}_{{\beta }_1}\end{array}\right],\hfill \end{array}$$

(23)

$$\begin{array}{cc}{}^B{\mathit{R}}_{P_2}\hfill & ={\mathit{R}}_z\left(0\right){\mathit{R}}_y\left({\beta }_2\right){\mathit{R}}_x\left({\alpha }_2\right)\hfill \\ & =\left[\begin{array}{ccc}{C}_{{\beta }_2}& S_{{\alpha }_2}{S}_{{\beta }_2}& C_{{\alpha }_2}{S}_{{\beta }_2}\\ 0& C_{{\alpha }_2}& -S_{{\alpha }_2}\\ -S_{{\beta }_2}& S_{{\alpha }_2}{C}_{{\beta }_2}& C_{{\alpha }_2}{C}_{{\beta }_2}\end{array}\right].\hfill \end{array}$$

(24)

4.2. Rotational Motion

First, the angular velocity ${\mathit{\omega }}_{P_i}$ and angular acceleration ${\dot{\mathit{\omega }}}_{P_i}$ of the i-th ($i=1,2$) coaxial rotor can be expressed as

$$\left\{\begin{array}{c}{\mathit{\omega }}_{P_1}={\left[\begin{array}{ccc}{\dot{\alpha }}_1& {\dot{\beta }}_1& {\overline{\omega }}_2-{\overline{\omega }}_1\end{array}\right]}^T\hfill \\ {\mathit{\omega }}_{P_2}={\left[\begin{array}{ccc}{\dot{\alpha }}_2& {\dot{\beta }}_2& {\overline{\omega }}_3-{\overline{\omega }}_4\end{array}\right]}^T\hfill \end{array}\right.,$$

(25)

$$\left\{\begin{array}{c}{\dot{\mathit{\omega }}}_{P_1}={\left[\begin{array}{ccc}{\ddot{\alpha }}_1& {\ddot{\beta }}_1& {\dot{\overline{\omega }}}_2-{\dot{\overline{\omega }}}_1\end{array}\right]}^T\hfill \\ {\dot{\mathit{\omega }}}_{P_2}={\left[\begin{array}{ccc}{\ddot{\alpha }}_2& {\ddot{\beta }}_2& {\dot{\overline{\omega }}}_3-{\dot{\overline{\omega }}}_4\end{array}\right]}^T\hfill \end{array}\right.$$

(26)

where ${\overline{\omega }}_1$, ${\overline{\omega }}_2$ are the angular velocities of each brushless motor in the first coaxial rotor, and ${\overline{\omega }}_3$, ${\overline{\omega }}_4$ are those of the second coaxial rotor.

Next, the reaction torque ${\mathit{\tau }}_{c,i}$ of the i-th ($i=1,2$) coaxial rotor can be expressed as

$$\left\{\begin{array}{c}{\mathit{\tau }}_{c,1}={\left[\begin{array}{ccc}0& 0& k_t({\overline{{\omega }_2}}^2-{\overline{\omega }}_1^2)\end{array}\right]}^T\hfill \\ {\mathit{\tau }}_{c,2}={\left[\begin{array}{ccc}0& 0& k_t({\overline{{\omega }_3}}^2-{\overline{\omega }}_4^2)\end{array}\right]}^T\hfill \end{array}\right.$$

(27)

where $k_t>0$ represents the torque coefficient.

Also, the thrust ${\mathit{T}}_i$ of the i-th ($i=1,2$) coaxial rotor can be expressed as

$$\left\{\begin{array}{c}{\mathit{T}}_1={\left[\begin{array}{ccc}0& 0& -T_1\end{array}\right]}^T={\left[\begin{array}{ccc}0& 0& -k_f({\overline{\omega }}_1^2+{\overline{\omega }}_2^2)\end{array}\right]}^T\hfill \\ {\mathit{T}}_2={\left[\begin{array}{ccc}0& 0& -T_2\end{array}\right]}^T={\left[\begin{array}{ccc}0& 0& -k_f({\overline{\omega }}_3^2+{\overline{\omega }}_4^2)\end{array}\right]}^T\hfill \end{array}\right.$$

(28)

where $k_f>0$ represents the thrust coefficient of the coaxial rotor.

Using the Newton–Euler method, the torque ${\mathit{\tau }}_{P_i}$ generated by the coaxial rotor can be expressed as

$${\mathit{\tau }}_{P_i}={\mathit{I}}_P{\dot{\mathit{\omega }}}_{P_i}+{\mathit{\omega }}_{P_i}\times {\mathit{I}}_P{\mathit{\omega }}_{P_i}+{\mathit{\tau }}_{c,i}.$$

(29)

Here, since the angular velocity and angular acceleration caused by the servo motor tilt are instantaneous and minute, they can be expressed as

$$\begin{array}{c}{\dot{\alpha }}_i=0,{\ddot{\alpha }}_i=0,{\dot{\beta }}_i=0,{\ddot{\beta }}_i=0(i=1,2);{\dot{\mathit{\omega }}}_{P_j}=0(j=1,2,3,4).\end{array}$$

(30)

Therefore, ${\mathit{\omega }}_{P_i}$ and ${\mathit{\tau }}_{P_i}$ become as follows:

$$\left\{\begin{array}{c}{\mathit{\omega }}_{P_1}={\left[\begin{array}{ccc}0& 0& \overline{{\omega }_2}-{\overline{\omega }}_1\end{array}\right]}^T\hfill \\ {\mathit{\omega }}_{P_2}={\left[\begin{array}{ccc}0& 0& \overline{{\omega }_3}-{\overline{\omega }}_4\end{array}\right]}^T\hfill \end{array}\right.,$$

(31)

$$\left\{\begin{array}{c}{\mathit{\tau }}_{P_1}={\left[\begin{array}{ccc}0& 0& k_t({\overline{\omega }}_2^2-{\overline{\omega }}_1^2)\end{array}\right]}^T\hfill \\ {\mathit{\tau }}_{P_2}={\left[\begin{array}{ccc}0& 0& k_t({\overline{\omega }}_3^2-{\overline{\omega }}_4^2)\end{array}\right]}^T\hfill \end{array}\right..$$

(32)

4.2.1. Rotational Torque ${\mathit{\tau }}_B$ Generated by Rotor Thrust

The rotational torque ${\mathit{\tau }}_B$ generated on the vehicle by rotor thrust can be expressed using the position vector ${}^B{\mathit{O}}_{r_i}$ of each rotor in the body coordinate system as follows:

$$\begin{array}{cc}\hfill {\mathit{\tau }}_B& =\sum _{i=1}^2({}^B{\mathit{O}}_{P_i}\times {}^B{\mathit{R}}_{P_i}{\mathit{T}}_i),\hfill \end{array}$$

(33)

$$\begin{array}{cc}\hfill {}^B{\mathit{O}}_{P_i}& ={\left[\begin{array}{ccc}0& {(-1)}^{i}l& -h_o\end{array}\right]}^T.\hfill \end{array}$$

(34)

The calculation details for $i=1,2$ are shown below:

$$\begin{array}{cc}\hfill {}^B{\mathit{R}}_{P_1}{\mathit{T}}_1& =\left[\begin{array}{ccc}{C}_{{\beta }_1}& S_{{\alpha }_1}{S}_{{\beta }_1}& C_{{\alpha }_1}{S}_{{\beta }_1}\\ 0& C_{{\alpha }_1}& -S_{{\alpha }_1}\\ -S_{{\beta }_1}& S_{{\alpha }_1}{C}_{{\beta }_1}& C_{{\alpha }_1}{C}_{{\beta }_1}\end{array}\right]\left[\begin{array}{c}0\\ 0\\ -T_1\end{array}\right]=\left[\begin{array}{c}-C_{{\alpha }_1}{S}_{{\beta }_1}{T}_1\\ S_{{\alpha }_1}{T}_1\\ -C_{{\alpha }_1}{C}_{{\beta }_1}{T}_1\end{array}\right],\hfill \\ \hfill {}^B{\mathit{R}}_{r_2}{\mathit{T}}_2& =\left[\begin{array}{ccc}{C}_{{\beta }_2}& S_{{\alpha }_2}{S}_{{\beta }_2}& C_{{\alpha }_2}{S}_{{\beta }_2}\\ 0& C_{{\alpha }_2}& -S_{{\alpha }_2}\\ -S_{{\beta }_2}& S_{{\alpha }_2}{C}_{{\beta }_2}& C_{{\alpha }_2}{C}_{{\beta }_2}\end{array}\right]\left[\begin{array}{c}0\\ 0\\ -T_2\end{array}\right]=\left[\begin{array}{c}-C_{{\alpha }_2}{S}_{{\beta }_2}{T}_2\\ S_{{\alpha }_2}{T}_2\\ -C_{{\alpha }_2}{C}_{{\beta }_2}{T}_2\end{array}\right],\hfill \end{array}$$

$$\begin{array}{cc}\hfill {}^B{\mathit{O}}_{P_1}\times {}^B{\mathit{R}}_{P_1}{\mathit{T}}_1& =\left[\begin{array}{c}0\\ -l\\ -h_o\end{array}\right]\times \left[\begin{array}{c}-C_{{\alpha }_1}{S}_{{\beta }_1}{T}_1\\ S_{{\alpha }_1}{T}_1\\ -C_{{\alpha }_1}{C}_{{\beta }_1}{T}_1\end{array}\right]=\left[\begin{array}{c}l{C}_{{\alpha }_1}{C}_{{\beta }_1}{T}_1+h_o{S}_{{\alpha }_1}{T}_1\\ h_o{C}_{{\alpha }_1}{S}_{{\beta }_1}{T}_1\\ -l{C}_{{\alpha }_1}{S}_{{\beta }_1}{T}_1\end{array}\right],\hfill \\ \hfill {}^B{\mathit{O}}_{P_2}\times {}^B{\mathit{R}}_{P_2}{\mathit{T}}_2& =\left[\begin{array}{c}0\\ l\\ -h_o\end{array}\right]\times \left[\begin{array}{c}-C_{{\alpha }_2}{S}_{{\beta }_2}{T}_2\\ S_{{\alpha }_2}{T}_2\\ -C_{{\alpha }_2}{C}_{{\beta }_2}{T}_2\end{array}\right]=\left[\begin{array}{c}-l{C}_{{\alpha }_2}{C}_{{\beta }_2}{T}_2+h_o{S}_{{\alpha }_2}{T}_2\\ h_o{C}_{{\alpha }_2}{S}_{{\beta }_2}{T}_2\\ l{C}_{{\alpha }_2}{S}_{{\beta }_2}{T}_2\end{array}\right].\hfill \end{array}$$

Therefore, ${\mathit{\tau }}_B={\left[{\tau }_x^B{\tau }_y^B{\tau }_z^B\right]}^T$ can be expressed as

$$\begin{array}{cc}\hfill {\mathit{\tau }}_B=\left[\begin{array}{c}{\tau }_x^B\\ {\tau }_y^B\\ {\tau }_z^B\end{array}\right]& =\left[\begin{array}{c}(l{C}_{{\alpha }_1}{C}_{{\beta }_1}+h_o{S}_{{\alpha }_1})T_1-(l{C}_{{\alpha }_2}{C}_{{\beta }_2}-h_o{S}_{{\alpha }_2})T_2\\ h_o{C}_{{\alpha }_1}{S}_{{\beta }_1}{T}_1+h_o{C}_{{\alpha }_2}{S}_{{\beta }_2}{T}_2\\ -l{C}_{{\alpha }_1}{S}_{{\beta }_1}{T}_1+l{C}_{{\alpha }_2}{S}_{{\beta }_2}{T}_2\end{array}\right]\hfill \\ \hfill & =k_f\left[\begin{array}{c}\begin{array}{cc}\hfill & (l{C}_{{\alpha }_1}{C}_{{\beta }_1}+h_o{S}_{{\alpha }_1})({\overline{\omega }}_1^2+{\overline{\omega }}_2^2)\hfill \\ \hfill & -(l{C}_{{\alpha }_2}{C}_{{\beta }_2}-h_o{S}_{{\alpha }_2})({\overline{\omega }}_3^2+{\overline{\omega }}_4^2)\hfill \\ \hfill & h_o{C}_{{\alpha }_1}{S}_{{\beta }_1}({\overline{\omega }}_1^2+{\overline{\omega }}_2^2)+h_o{C}_{{\alpha }_2}{S}_{{\beta }_2}({\overline{\omega }}_3^2+{\overline{\omega }}_4^2)\hfill \\ \hfill & -l{C}_{{\alpha }_1}{S}_{{\beta }_1}({\overline{\omega }}_1^2+{\overline{\omega }}_2^2)+l{C}_{{\alpha }_2}{S}_{{\beta }_2}({\overline{\omega }}_3^2+{\overline{\omega }}_4^2)\hfill \end{array}\end{array}\right].\hfill \end{array}$$

(35)

4.2.2. Reaction Torque ${\mathit{\tau }}_c$ Generated by Rotor Rotation

The reaction torque ${\mathit{\tau }}_c$ generated by rotor rotation can be expressed using the reaction torque ${\mathit{\tau }}_{P_i}$ of each rotor as follows:

$$\begin{array}{cc}\hfill {\mathit{\tau }}_c& =\sum _{i=1}^2\left({}^B{\mathit{R}}_{P_i}{\mathit{\tau }}_{P_i}\right).\hfill \end{array}$$

(36)

The calculation details for $i=1,2$ are shown below:

$$\begin{array}{cc}\hfill {}^B{\mathit{R}}_{P_1}{\mathit{\tau }}_{P_1}& =\left[\begin{array}{ccc}{C}_{{\beta }_1}& S_{{\alpha }_1}{S}_{{\beta }_1}& C_{{\alpha }_1}{S}_{{\beta }_1}\\ 0& C_{{\alpha }_1}& -S_{{\alpha }_1}\\ -S_{{\beta }_1}& S_{{\alpha }_1}{C}_{{\beta }_1}& C_{{\alpha }_1}{C}_{{\beta }_1}\end{array}\right]\left[\begin{array}{c}0\\ 0\\ k_t({\overline{\omega }}_2^2-{\overline{\omega }}_1^2)\end{array}\right]\hfill \\ \hfill & =k_t\left[\begin{array}{c}{C}_{{\alpha }_1}{S}_{{\beta }_1}({\overline{\omega }}_2^2-{\overline{\omega }}_1^2)\\ -S_{{\alpha }_1}({\overline{\omega }}_2^2-{\overline{\omega }}_1^2)\\ C_{{\alpha }_1}{C}_{{\beta }_1}({\overline{\omega }}_2^2-{\overline{\omega }}_1^2)\end{array}\right],\hfill \end{array}$$

$$\begin{array}{cc}\hfill {}^B{\mathit{R}}_{P_2}{\mathit{\tau }}_{P_2}& =\left[\begin{array}{ccc}{C}_{{\beta }_2}& S_{{\alpha }_2}{S}_{{\beta }_2}& C_{{\alpha }_2}{S}_{{\beta }_2}\\ 0& C_{{\alpha }_2}& -S_{{\alpha }_2}\\ -S_{{\beta }_2}& S_{{\alpha }_2}{C}_{{\beta }_2}& C_{{\alpha }_2}{C}_{{\beta }_2}\end{array}\right]\left[\begin{array}{c}0\\ 0\\ k_t({\overline{\omega }}_3^2-{\overline{\omega }}_4^2)\end{array}\right]\hfill \\ \hfill & =k_t\left[\begin{array}{c}{C}_{{\alpha }_2}{S}_{{\beta }_2}({\overline{\omega }}_3^2-{\overline{\omega }}_4^2)\\ -S_{{\alpha }_2}({\overline{\omega }}_3^2-{\overline{\omega }}_4^2)\\ C_{{\alpha }_2}{C}_{{\beta }_2}({\overline{\omega }}_3^2-{\overline{\omega }}_4^2)\end{array}\right].\hfill \end{array}$$

Therefore, ${\mathit{\tau }}_c={\left[{\tau }_x^c{\tau }_y^c{\tau }_z^c\right]}^T$ can be expressed as

$$\begin{array}{cc}\hfill {\mathit{\tau }}_c& =\left[\begin{array}{c}{\tau }_x^c\\ {\tau }_y^c\\ {\tau }_z^c\end{array}\right]\hfill \\ \hfill & =k_t\left[\begin{array}{c}{C}_{{\alpha }_1}{S}_{{\beta }_1}({\overline{\omega }}_2^2-{\overline{\omega }}_1^2)+C_{{\alpha }_2}{S}_{{\beta }_2}({\overline{\omega }}_3^2-{\overline{\omega }}_4^2)\\ -S_{{\alpha }_1}({\overline{\omega }}_2^2-{\overline{\omega }}_1^2)-S_{{\alpha }_2}({\overline{\omega }}_3^2-{\overline{\omega }}_4^2)\\ C_{{\alpha }_1}{C}_{{\beta }_1}({\overline{\omega }}_2^2-{\overline{\omega }}_1^2)+C_{{\alpha }_2}{C}_{{\beta }_2}({\overline{\omega }}_3^2-{\overline{\omega }}_4^2)\end{array}\right].\hfill \end{array}$$

(37)

4.2.3. Vehicle Coriolis Force ${\mathit{\tau }}_{cori}$

The vehicle Coriolis force ${\mathit{\tau }}_{cori}$ can be expressed using the vehicle’s angular velocity ${\mathit{\omega }}_B$ and its inertia matrix ${\mathit{I}}_B$ as follows:

$${\mathit{\tau }}_{cori}={\mathit{\omega }}_B\times {\mathit{I}}_B{\mathit{\omega }}_B.$$

(38)

Here, since

$$\begin{array}{c}{\mathit{I}}_B=\left[\begin{array}{ccc}{I}_{Bxx}& 0& 0\\ 0& I_{Byy}& 0\\ 0& 0& I_{Bzz}\end{array}\right],{\mathit{\omega }}_B=\left[\begin{array}{c}p\\ q\\ r\end{array}\right]\end{array}$$

we have

$${\mathit{I}}_B{\mathit{\omega }}_B=\left[\begin{array}{ccc}{I}_{Bxx}& 0& 0\\ 0& I_{Byy}& 0\\ 0& 0& I_{Bzz}\end{array}\right]\left[\begin{array}{c}p\\ q\\ r\end{array}\right]=\left[\begin{array}{c}p{I}_{Bxx}\\ q{I}_{Byy}\\ r{I}_{Bzz}\end{array}\right].$$

Therefore, letting ${\mathit{\tau }}_{cori}={\left[{\tau }_x^{cori}{\tau }_y^{cori}{\tau }_z^{cori}\right]}^T$, we obtain

$$\begin{array}{cc}\hfill {\mathit{\tau }}_{cori}=\left[\begin{array}{c}{\tau }_x^{cori}\\ {\tau }_y^{cori}\\ {\tau }_z^{cori}\end{array}\right]& =\left[\begin{array}{c}p\\ q\\ r\end{array}\right]\times \left[\begin{array}{c}p{I}_{Bxx}\\ q{I}_{Byy}\\ r{I}_{Bzz}\end{array}\right]\hfill \\ \hfill & =\left[\begin{array}{c}qr(I_{Bzz}-I_{Byy})\\ pr(I_{Bxx}-I_{Bzz})\\ pq(I_{Byy}-I_{Bxx})\end{array}\right].\hfill \end{array}$$

(39)

4.2.4. Vehicle Angular Acceleration ${\dot{\mathit{\omega }}}_B$

Using the equations above, the vehicle’s angular acceleration ${\dot{\mathit{\omega }}}_B={\left[\dot{p}\dot{q}\dot{r}\right]}^T$ can be expressed as

$$\begin{array}{cc}\hfill {\dot{\mathit{\omega }}}_B& ={\mathit{I}}_B^{-1}({\mathit{\tau }}_B+{\mathit{\tau }}_c-{\mathit{\tau }}_{cori}+{\mathit{\tau }}_{ext}).\hfill \end{array}$$

(40)

Furthermore, neglecting external disturbance torques ${\mathit{\tau }}_{ext}$ acting on the vehicle and gyroscopic effects (i.e., ${\mathit{\tau }}_{ext}=0$), we have

$$\begin{array}{cc}\hfill \left[\begin{array}{c}\dot{p}\\ \dot{q}\\ \dot{r}\end{array}\right]& =\left[\begin{array}{c}{\displaystyle \frac{1}{I_{Bxx}}}({\tau }_x^B+{\tau }_x^c)+qr\left({\displaystyle \frac{I_{Byy}-I_{Bzz}}{I_{Bxx}}}\right)\\ {\displaystyle \frac{1}{I_{Byy}}}({\tau }_y^B+{\tau }_y^c)+pr\left({\displaystyle \frac{I_{Bzz}-I_{Bxx}}{I_{Byy}}}\right)\\ {\displaystyle \frac{1}{I_{Bzz}}}({\tau }_z^B+{\tau }_z^c)+pq\left({\displaystyle \frac{I_{Bxx}-I_{Byy}}{I_{Bzz}}}\right)\end{array}\right].\hfill \end{array}$$

(41)

4.3. Translational Motion

The thrust ${\mathit{T}}_B$ in the body coordinate system can be expressed using the thrust $T_i$ of each rotor as

$${\mathit{T}}_B=\sum _{i=1}^2\left({}^B{\mathit{R}}_{P_i}{\mathit{T}}_i\right).$$

(42)

Therefore, letting ${\mathit{T}}_B={\left[T_x{T}_y{T}_z\right]}^T$, we obtain

$$\begin{array}{cc}\hfill {\mathit{T}}_B=\left[\begin{array}{c}{T}_x\\ T_y\\ T_z\end{array}\right]& =\left[\begin{array}{c}-C_{{\alpha }_1}{S}_{{\beta }_1}{T}_1-C_{{\alpha }_2}{S}_{{\beta }_2}{T}_2\\ S_{{\alpha }_1}{T}_1+S_{{\alpha }_2}{T}_2\\ -C_{{\alpha }_1}{C}_{{\beta }_1}{T}_1-C_{{\alpha }_2}{C}_{{\beta }_2}{T}_2\end{array}\right]\hfill \\ & =k_f\left[\begin{array}{c}-C_{{\alpha }_1}{S}_{{\beta }_1}({\overline{\omega }}_1^2+{\overline{\omega }}_2^2)-C_{{\alpha }_2}{S}_{{\beta }_2}({\overline{\omega }}_3^2+{\overline{\omega }}_4^2)\\ S_{{\alpha }_1}({\overline{\omega }}_1^2+{\overline{\omega }}_2^2)+S_{{\alpha }_2}({\overline{\omega }}_3^2+{\overline{\omega }}_4^2)\\ -C_{{\alpha }_1}{C}_{{\beta }_1}({\overline{\omega }}_1^2+{\overline{\omega }}_2^2)-C_{{\alpha }_2}{C}_{{\beta }_2}({\overline{\omega }}_3^2+{\overline{\omega }}_4^2)\end{array}\right].\hfill \end{array}$$

(43)

Using the vehicle mass m and gravitational acceleration $\mathit{g}={\left[00g\right]}^T$, the vehicle’s translational acceleration $\ddot{\mathit{\xi }}={\left[\ddot{x}\ddot{y}\ddot{z}\right]}^T$ can be expressed as

$$\ddot{\mathit{\xi }}=\mathit{g}+{\displaystyle \frac{1}{m}}({}^W{\mathit{R}}_B{\mathit{T}}_B+{\mathit{F}}_{ext}),$$

(44)

$$\begin{array}{cc}\hfill \left[\begin{array}{c}\ddot{x}\\ \ddot{y}\\ \ddot{z}\end{array}\right]& ={\displaystyle \frac{1}{m}}\left[\begin{array}{c}\begin{array}{c}{C}_{\theta }{C}_{\psi }{T}_x+(S_{\varphi }{S}_{\theta }{C}_{\psi }-C_{\varphi }{S}_{\psi })T_y\hfill \\ +(C_{\varphi }{S}_{\theta }{C}_{\psi }+S_{\varphi }{S}_{\psi })T_z\hfill \\ C_{\theta }{S}_{\psi }{T}_x+(S_{\varphi }{S}_{\theta }{S}_{\psi }+C_{\varphi }{C}_{\psi })T_y\hfill \\ +(C_{\varphi }{S}_{\theta }{S}_{\psi }-S_{\varphi }{C}_{\psi })T_z\hfill \\ \hfill -S_{\theta }{T}_x+S_{\varphi }{C}_{\theta }{T}_y+C_{\varphi }{C}_{\theta }{T}_z+mg\end{array}\end{array}\right]\hfill \end{array}$$

(45)

where ${\mathit{F}}_{ext}$ is the external disturbance force acting on the vehicle. Neglecting effects such as friction, we set ${\mathit{F}}_{ext}=0$.

5. Controller Design and Control Allocation

5.1. Computed Torque Method

The computed torque method is a technique to determine the generalized force (comprising thrust and torque in the body coordinate system) from the desired translational and angular accelerations in the world coordinate system, given that the vehicle’s physical information is known. Utilizing the relation $\dot{\mathit{\eta }}=W_{\eta }^{-1}{\mathit{\omega }}_B$ in the translational acceleration of Equation (44) and the rotational angular acceleration of Equation (40), and defining the generalized coordinates as $X\triangleq {\left[{\mathit{\xi }}^T{\mathit{\eta }}^T\right]}^T$, the system can be represented as

$$\begin{array}{cc}\hfill \ddot{X}& =F\left(\dot{X}\right)+G\left(X\right)\mathit{u}+D\left(X\right)\mathit{d}.\hfill \end{array}$$

(46)

This is because, from Equation (20),

$$\begin{array}{cc}\hfill \ddot{\mathit{\eta }}& ={\displaystyle \frac{d}{dt}}\left(W_{\eta }^{-1}\right){\mathit{\omega }}_B+W_{\eta }^{-1}{\dot{\mathit{\omega }}}_B\hfill \\ \hfill & ={\displaystyle \frac{d}{dt}}\left(W_{\eta }^{-1}\right)W_{\eta }\dot{\mathit{\eta }}+W_{\eta }^{-1}\left\{I_B^{-1}\left[({\tau }_B+{\tau }_c)-{\tau }_{cori}+{\tau }_{ext}\right]\right\}\hfill \\ \hfill & ={\displaystyle \frac{d}{dt}}\left(W_{\eta }^{-1}\right)W_{\eta }\dot{\mathit{\eta }}-{\left(I_B{W}_{\eta }\right)}^{-1}[\left(W_{\eta }\dot{\mathit{\eta }}\right)\times I_B\left(W_{\eta }\dot{\mathit{\eta }}\right)]+{\left(I_B{W}_{\eta }\right)}^{-1}({\tau }_B+{\tau }_c)+{\left(I_B{W}_{\eta }\right)}^{-1}{\tau }_{ext}\hfill \end{array}$$

(47)

and thus

$$\begin{array}{c}\hfill F\left(\dot{X}\right)=\left[\begin{array}{c}\mathit{g}\\ {\displaystyle \frac{d}{dt}}\left(W_{\eta }^{-1}\right)W_{\eta }\dot{\mathit{\eta }}-{\left(I_B{W}_{\eta }\right)}^{-1}[\left(W_{\eta }\dot{\mathit{\eta }}\right)\times I_B\left(W_{\eta }\dot{\mathit{\eta }}\right)]\end{array}\right],\end{array}$$

$$\begin{array}{c}\hfill G\left(X\right)=\left[\begin{array}{cc}{{\displaystyle \frac{1}{m}}}^W{R}_B& 0\\ 0& {\left(I_B{W}_{\eta }\right)}^{-1}\end{array}\right],D\left(X\right)=\left[\begin{array}{cc}{\displaystyle \frac{1}{m}}{I}_{3\times 3}& 0\\ 0& {\left(I_B{W}_{\eta }\right)}^{-1}\end{array}\right],\end{array}$$

$$\begin{array}{cc}\hfill \mathit{u}& ={\left[T_B^T{({\tau }_B+{\tau }_c)}^T\right]}^T,\mathit{d}={\left[F_{ext}^T{\tau }_{ext}^T\right]}^T.\hfill \end{array}$$

Here, since the rotation matrix ${}^W{R}_B$ is orthogonal, ${}^W{R}_B^{-1}{=}^W{R}_B^T$. Also, as each principal moment of inertia $I_{B_{ii}}\ne 0(i=x,y,z)$, $G^{-1}\left(X\right)=\mathrm{diag}(m^W{R}_B^T,W_{\eta }{I}_B)$. Then, the inverse system of Equation (46) is

$$\begin{array}{cc}\hfill \mathit{u}& =G^{-1}\left(X\right)[\ddot{X}-F\left(\dot{X}\right)-D\left(X\right)\mathit{d}].\hfill \end{array}$$

(48)

Let the target value for the generalized coordinate vector X be $X_d$. Constructing its augmented acceleration vector ${\ddot{X}}^{*}$ using a PD servo system yields

$$\begin{array}{cc}\hfill {\ddot{X}}^{*}& ={\ddot{X}}_d+K_d\dot{\mathit{e}}+K_p\mathit{e}\hfill \end{array}$$

(49)

where $\mathit{e}=X_d-X$, $K_d>0$ is the derivative gain, and $K_p>0$ is the proportional gain. Substituting ${\ddot{X}}^{*}$ from Equation (49) for $\ddot{X}$ in Equation (48) gives

$$\begin{array}{cc}\hfill {\mathit{u}}^{*}& =G^{-1}\left(X\right)[{\ddot{X}}_d+K_d\dot{\mathit{e}}+K_p\mathit{e}-F\left(\dot{X}\right)-D\left(X\right)\mathit{d}].\hfill \end{array}$$

(50)

Substituting this ${\mathit{u}}^{*}$ into the original plant Equation (46) yields

$$\begin{array}{c}\hfill \ddot{\mathit{e}}+K_d\dot{\mathit{e}}+K_p\mathit{e}=0.\end{array}$$

(51)

Here, we set $K_p=\mathrm{diag}(K_{p1},\dots ,K_{p6})$ and $K_d=\mathrm{diag}(K_{d1},\dots ,K_{d6})$. For application to actual vehicles, considering modeling errors, the condition $K_{di}=2\sqrt{K_{pi}}$, which ideally achieves a damping ratio of 1 (i.e., critical damping), is used as a base, but the damping condition for this error may need slight modification. In the simulations, the proportional gains for translational position and attitude angles are defined as $K_p=\mathrm{diag}(K_{px},K_{py},K_{pz},K_{p\varphi },K_{p\theta },K_{p\psi })$, and the derivative gains as $K_d=\mathrm{diag}(K_{dx},K_{dy},K_{dz},K_{d\varphi },K_{d\theta },K_{d\psi })$, with the following settings:

$$\begin{array}{cc}\hfill K_{px}& =K_{py}=3.0;K_{pz}=5;K_{p\varphi }=K_{p\theta }=K_{p\psi }=2.0;\hfill \\ \hfill K_{dx}& =K_{dy}=3.46;K_{dz}=5;K_{d\varphi }=K_{d\theta }=K_{d\psi }=2.83.\hfill \end{array}$$

5.2. Control Input Allocation Problem

Appropriate rotor speeds and tilt angles are allocated to each coaxial rotor to achieve the generalized force in the body coordinate system, determined earlier by the computed torque method. A single coaxial rotor constructed in this study utilizes two brushless motors. The thrust of coaxial rotor 1 is $T_1=k_f({\overline{\omega }}_1^2+{\overline{\omega }}_2^2)$, and that of coaxial rotor 2 is $T_2=k_f({\overline{\omega }}_3^2+{\overline{\omega }}_4^2)$. The reaction torque for the first rotor is ${\tau }_{c,1}=k_t({\overline{\omega }}_2^2-{\overline{\omega }}_1^2)$, and for the second rotor is ${\tau }_{c,2}=k_t({\overline{\omega }}_3^2-{\overline{\omega }}_4^2)$. However, directly solving for the rotation speeds ${\overline{\omega }}_1^2\sim {\overline{\omega }}_4^2$ and tilt angles ${\alpha }_i,{\beta }_i,(i=1,2)$ results in an underdetermined problem: six generalized forces versus eight unknown decision variables. This would yield an effectiveness matrix of size $6\times 8$, requiring solution via pseudo-inverse or numerical optimization such as constrained QP. Below, we present a simpler method based on experimental results for coaxial rotor thrust.

Referring to the experimental results on coaxial rotors by Itakura et al. [40], for a total thrust of $T_{total}=10.6$ N of the coaxial rotor, the thrust of the upstream propeller alone was $T_{upstream}=7.3$ N, and that of the downstream propeller alone was $T_{downstream}=8.0$ N. Therefore, the thrust efficiency is

$$\begin{array}{cc}\hfill {\eta }_{thrust}& ={\displaystyle \frac{T_{total}}{T_{upstream}+T_{downstream}}}\hfill \\ \hfill & ={\displaystyle \frac{10.6}{7.3+8.0}}=0.693.\hfill \end{array}$$

(52)

This serves as an indicator of deviation from theoretical ideal conditions. On the other hand, the interference efficiency, which is the increase rate of total thrust relative to the upstream propeller’s thrust, is

$$\begin{array}{cc}\hfill {\eta }_{if}& ={\displaystyle \frac{T_{total}}{T_{upstream}}}-1\hfill \\ \hfill & ={\displaystyle \frac{10.6}{7.3}}-1=0.452.\hfill \end{array}$$

(53)

Introducing this interference efficiency allows for the substitution of ${\overline{\omega }}_2^2$ with ${\eta }_{if}{\overline{\omega }}_1^2$ and ${\overline{\omega }}_4^2$ with ${\eta }_{if}{\overline{\omega }}_3^2$, thereby eliminating ${\overline{\omega }}_2^2$ and ${\overline{\omega }}_4^2$ from the unknown variables.

With this interference efficiency, the reaction torque ${\mathit{\tau }}_c$ from Equation (37) becomes

$$\begin{array}{cc}\hfill {\mathit{\tau }}_c& =({\eta }_{if}-1)k_t\left[\begin{array}{c}{C}_{{\alpha }_1}{S}_{{\beta }_1}{\overline{\omega }}_1^2-C_{{\alpha }_2}{S}_{{\beta }_2}{\overline{\omega }}_3^2\\ -S_{{\alpha }_1}{\overline{\omega }}_1^2+S_{{\alpha }_2}{\overline{\omega }}_3^2\\ C_{{\alpha }_1}{C}_{{\beta }_1}{\overline{\omega }}_1^2-C_{{\alpha }_2}{C}_{{\beta }_2}{\overline{\omega }}_3^2\end{array}\right].\hfill \end{array}$$

(54)

Meanwhile, the rotational torque due to thrust ${\mathit{\tau }}_B$ from Equation (35) becomes

$$\begin{array}{cc}\hfill {\mathit{\tau }}_B& =({\eta }_{if}+1)k_f\left[\begin{array}{c}\begin{array}{c}(l{C}_{{\alpha }_1}{C}_{{\beta }_1}+h_o{S}_{{\alpha }_1}){\overline{\omega }}_1^2\hfill \\ -(l{C}_{{\alpha }_2}{C}_{{\beta }_2}-h_o{S}_{{\alpha }_2}){\overline{\omega }}_3^2\hfill \\ h_o{C}_{{\alpha }_1}{S}_{{\beta }_1}{\overline{\omega }}_1^2+h_o{C}_{{\alpha }_2}{S}_{{\beta }_2}{\overline{\omega }}_3^2\hfill \\ -l{C}_{{\alpha }_1}{S}_{{\beta }_1}{\overline{\omega }}_1^2+l{C}_{{\alpha }_2}{S}_{{\beta }_2}{\overline{\omega }}_3^2\hfill \end{array}\end{array}\right].\hfill \end{array}$$

(55)

Similarly, the thrust ${\mathit{T}}_B$ from Equation (43) is rewritten as

$$\begin{array}{c}\hfill {\mathit{T}}_B=({\eta }_{if}+1)k_f\left[\begin{array}{c}-C_{{\alpha }_1}{S}_{{\beta }_1}{\overline{\omega }}_1^2-C_{{\alpha }_2}{S}_{{\beta }_2}{\overline{\omega }}_3^2\\ S_{{\alpha }_1}{\overline{\omega }}_1^2+S_{{\alpha }_2}{\overline{\omega }}_3^2\\ -C_{{\alpha }_1}{C}_{{\beta }_1}{\overline{\omega }}_1^2-C_{{\alpha }_2}{C}_{{\beta }_2}{\overline{\omega }}_3^2\end{array}\right].\end{array}$$

(56)

Therefore, by introducing an intermediate 6-dimensional vector $\mathit{n}={\left[n_{1a}{n}_{1b}{n}_{1c}{n}_{2a}{n}_{2b}{n}_{2c}\right]}^T$, defining ${\Omega }_1\triangleq {\overline{\omega }}_1^2$, ${\Omega }_2\triangleq {\overline{\omega }}_3^2$, and for $i=1,2$,

$$\left\{\begin{array}{c}{n}_{ia}={\Omega }_i{C}_{{\alpha }_i}{S}_{{\beta }_i}\hfill \\ n_{ib}={\Omega }_i{S}_{{\alpha }_i}\hfill \\ n_{ic}={\Omega }_i{C}_{{\alpha }_i}{C}_{{\beta }_i}\hfill \end{array}\right.,$$

(57)

the relation ${\left[T_B^T{({\mathit{\tau }}_B+{\mathit{\tau }}_c)}^T\right]}^T$ finally reduces to a linear equation:

$$\left[\begin{array}{c}{\mathit{T}}_B\\ {\mathit{\tau }}_B+{\mathit{\tau }}_c\end{array}\right]=A\mathit{n}({\Omega }_i,{\alpha }_i,{\beta }_i).$$

(58)

Here, the effectiveness matrix A is

$$\begin{array}{c}\hfill \mathit{A}=\left[\begin{array}{cccccc}-(1+{\eta }_{if})k_f& 0& 0& -(1+{\eta }_{if})k_f& 0& 0\\ 0& (1+{\eta }_{if})k_f& 0& 0& (1+{\eta }_{if})k_f& 0\\ 0& 0& -({\eta }_{if}+1)k_f& 0& 0& -(1+{\eta }_{if})k_f\\ ({\eta }_{if}-1)k_t& (1+{\eta }_{if})h_o{k}_f& (1+{\eta }_{if})l{k}_f& -(1-{\eta }_{if})k_t& (1+{\eta }_{if})h_o{k}_t& -(1+{\eta }_{if})l{k}_f\\ (1+{\eta }_{if})h_o{k}_f& (1-{\eta }_{if})k_t& 0& (1+{\eta }_{if})h_o{k}_f& ({\eta }_{if}-1)k_t& 0\\ -(1+{\eta }_{if})l{k}_f& 0& ({\eta }_{if}-1)k_t& (1+{\eta }_{if})k_f& 0& (1-{\eta }_{if})k_t\end{array}\right].\end{array}$$

(59)

Figure 2. Control allocation process.

Figure 2. Control allocation process.

To generate control inputs for the actual vehicle, the intermediate variable vector $\mathit{n}$ is obtained using $A^{-1}$, and then the squared rotation speeds ${\Omega }_1={\overline{\omega }}_1^2$, ${\Omega }_2={\overline{\omega }}_3^2$ and tilt angles $({\alpha }_1,{\beta }_1)$, $({\alpha }_2,{\beta }_2)$ for each rotor are generated via the following relations:

$$\left\{\begin{array}{c}{\Omega }_i=\sqrt{n_{ia}^2+n_{ib}^2+n_{ic}^2}\hfill \\ {\alpha }_i=arcsin(n_{ib}/{\Omega }_i)\hfill \\ {\beta }_i=arctan(n_{ia}/n_{ic})\hfill \end{array}\right..$$

(60)

Figure 2 illustrates the sequential calculation flow for this control allocation problem.

6. Preliminary Simulation: Application to the Falling Body Model

In this section, we describe state estimation for the falling body model [41] as a preliminary experiment. The falling body model is often used as a nonlinear model and was also employed as a simulation model by Takeno and Katayama [20]. The purpose here is to verify the basic effectiveness of the proposed method by performing state estimation under conditions similar to previous studies. Since the RK method used by Takeno et al. is a 4th-order discretization method, this section focuses on the 4th-order AB method for comparison.

6.1. Model Overview (Falling Body)

The differential equations for the falling body model are as follows [41]:

$$\begin{array}{cc}\hfill {\dot{x}}_1& =x_2,\hfill \end{array}$$

(61)

$$\begin{array}{cc}\hfill {\dot{x}}_2& ={\displaystyle \frac{{\rho }_0{e}^{-x_1/k_{\rho }}{x}_2^2}{2\beta }}-g.\hfill \end{array}$$

(62)

Here, $x_1$ is the altitude of the falling object, g is the gravitational acceleration, ${\rho }_0$ is the atmospheric density at sea level (0 m), $k_{\rho }$ is the decay coefficient, and $\beta$ is the ballistic coefficient.

The extended state vector is defined as follows:

$$\begin{array}{c}\hfill \mathit{x}=\left[\begin{array}{c}{x}_1\\ x_2\\ \beta \end{array}\right]=\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right]\left(\mathrm{3-dimensional}\mathrm{state}\mathrm{vector}\right).\end{array}$$

(63)

Letting $\mathit{f}={\left[f_1{f}_2{f}_3\right]}^T$, the state–space model becomes

$$\begin{array}{cc}\hfill {\displaystyle \frac{d{x}_1}{dt}}& \triangleq f_1=x_2,\hfill \end{array}$$

(64)

$$\begin{array}{cc}\hfill {\displaystyle \frac{d{x}_2}{dt}}& \triangleq f_2={\displaystyle \frac{{\rho }_0}{2x_3}}{e}^{-x_1/k_{\rho }}{x}_2^2-g,\hfill \end{array}$$

(65)

$$\begin{array}{cc}\hfill {\displaystyle \frac{d{x}_3}{dt}}& \triangleq f_3=0.\hfill \end{array}$$

(66)

The observation equation is nonlinear and is assumed to be as follows:

$$y=\sqrt{M_1^2+{(x_1-M_2)}^2}+v.$$

(67)

Here, $M_1$ is the horizontal distance between the falling object and the radar, and $M_2$ is the radar altitude. Furthermore, v is the observation noise, assumed to be Gaussian white noise with zero mean and variance R.

6.2. Estimation Algorithms (Falling Body)

In this study, we compare three types of numerical integration methods integrated with the UKF: the Euler method, the RK method, and the AB method. Hereafter, the UKF using the Euler method is called “Euler-UKF”, that using the RK method “RK-UKF”, and that using the AB method “AB-UKF”.

The detailed derivation and specific equations for the sigma point time update of each method are presented in Appendix C. This section provides only an overview of each method.

6.2.1. Euler-UKF (Falling Body)

In Euler-UKF, the Euler method is applied to Equations (64)–(66). The Euler method has first-order accuracy and requires one function evaluation per update step. The specific sigma point time-update equations are given in Appendix C.1.

6.2.2. RK-UKF (Falling Body)

In RK-UKF, the 4th-order RK method (hereafter denoted as RK4) is applied to Equations (64)–(66) [20]. The RK4 method has fourth-order accuracy and requires four function evaluations per update step. The specific sigma point time-update equations are given in Appendix C.2.

6.2.3. AB-UKF (Falling Body)

In AB-UKF, the 4th-order AB method (hereafter denoted as AB4) is applied to Equations (64)–(66). The AB4 method achieves fourth-order accuracy while requiring only one function evaluation per update step. Instead, it stores and utilizes the function evaluation results for past sigma point matrices, ${\mathcal{F}}_c\left({\mathcal{X}}_{k-1}\right)$, ${\mathcal{F}}_c\left({\mathcal{X}}_{k-2}\right)$, and ${\mathcal{F}}_c\left({\mathcal{X}}_{k-3}\right)$. The specific sigma point time-update equations are given in Appendix C.3.

6.3. Estimation Conditions (Falling Body)

State estimation simulations are performed using the three methods described above. The physical parameters are as follows: $g=9.8$ [m/s²], ${\rho }_0=2.202$ [kg·s²/m⁴], $k_{\rho }=1000/0.1558$ [m]. The true ballistic coefficient is $\beta =2000$ [kg/m²]. The horizontal distance between the falling object and the radar is $M_1$ = 10,000 [m], and the radar altitude is $M_2=0$ [m]. The step size is $h=0.1$ [s], the total number of sampling points is $N=300$, and the UT parameter is $\lambda =2.0$. The initial state vector, system noise covariance, and observation noise covariance are as follows [20]:

$$\begin{array}{cc}\hfill \left[\begin{array}{c}{x}_1\left(0\right)\\ x_2\left(0\right)\\ x_3\left(0\right)\end{array}\right]& =\left[\begin{array}{c}40000\mathrm{m}\\ -3000\mathrm{m}/\mathrm{s}\\ 2000\mathrm{kg}/{\mathrm{m}}^2\end{array}\right],\hfill \end{array}$$

(68)

$$\begin{array}{cc}\hfill Q& =\mathrm{diag}(0,0,10{\mathrm{kg}}^2/{\mathrm{m}}^4),\hfill \end{array}$$

(69)

$$\begin{array}{cc}\hfill R& =3600{\mathrm{m}}^2.\hfill \end{array}$$

(70)

The initial state estimate and its covariance matrix are as follows:

$$\begin{array}{cc}\hfill \widehat{\mathit{x}}\left(0\right|0)& =\left[\begin{array}{c}42000\mathrm{m}\\ -3100\mathrm{m}/\mathrm{s}\\ 3000\mathrm{kg}/{\mathrm{m}}^2\end{array}\right],\hfill \end{array}$$

(71)

$$\begin{array}{cc}\hfill P\left(0\right|0)& =\mathrm{diag}(10000{\mathrm{m}}^2,10000{\mathrm{m}}^2/{\mathrm{s}}^2,10000{\mathrm{kg}}^2/{\mathrm{m}}^4).\hfill \end{array}$$

(72)

6.4. Simulation Results

6.4.1. State Estimation Accuracy

Figure 3a shows the estimation results for the ballistic coefficient $\beta$, and Figure 3b shows the time evolution of the state estimation error. The state estimation error was calculated using the following RMSE (Root Mean Squared Error) formula:

$$E\left(t_k\right)=\sqrt{\sum _{i=1}^2{(x_i\left(t_k\right)-{\widehat{x}}_i\left(t_k\right|t_k))}^2}.$$

(73)

The results are averages from 100 simulation runs for each method.

Figure 3a shows that the parameter $\beta$ converges well for all three methods: Euler-UKF, RK-UKF, and AB-UKF. On the other hand, Figure 3b indicates that after the convergence of $\beta$, the state estimation error is suppressed more effectively in the order of Euler-UKF, RK-UKF, and AB-UKF.

Table 1. Comparison of $\mathrm{RMSE}$ among the three methods.

	Method
	Euler-UKF	RK-UKF	AB4-UKF
RMSE	125.585	116.826	115.537

presents the average RMSE values of the state estimation error obtained from 100 simulation runs for each of the three methods. The computations were performed on a laptop with 16 GB RAM, an AMD Ryzen7 4800H with Radeon Graphics CPU, running Windows 10 and MATLAB 2022a.

As shown in Table 1, the Euler method exhibits the largest RMSE, while the RK and AB methods show smaller RMSE values than the Euler method. This result is attributed to the difference in discretization order: the Euler method has first-order accuracy, whereas the RK and AB methods have fourth-order accuracy.

6.4.2. Computation Time

To more clearly demonstrate the differences between the methods, the estimation conditions from Section 6.3 were modified: $h=0.01$ and the total number of sampling points $N=5000$.

Table 2. Comparison of computation time among the three methods.

	Method
	Euler-UKF	RK-UKF	AB4-UKF
Alg. total time ($\times {10}^{-2}$) [s]	8.4612	9.2542	8.7108
Pred. time only ($\times {10}^{-6}$) [s]	58.3842	77.8698	70.6750

shows the comparison results for the total computation time of the entire UKF algorithm (from Equations (A3) to (A18)) and the computation time for the prediction step only (Equation (A9)).

Table 2 shows that the computation time is shortest for the Euler method, followed by the AB method, and then the RK method. The particularly short prediction time of the Euler method stems from the difference in discretization order.

6.5. Discussion

The results presented in Section 6.4 can be attributed to the following two factors.

• Small number of state variables: The falling body model has three state variables, with $x_3$ representing a parameter estimation problem. Consequently, as is evident from the sigma point time-update equations, the discretization formulas are actually applied only to the state variables $x_1$ and $x_2$, making this a model where differences between the RK and AB methods are less likely to manifest.
• Small proportion of UT within the overall UKF algorithm: The falling body model has a relatively small number of state variables and simple discretization equations, making it difficult for differences in computational load to become pronounced. As a result, the time spent on UT constitutes a small proportion of the overall UKF estimation algorithm, and the differences in computation time due to the discretization method are less noticeable.

Considering the above two points, the next section, Section 7, addresses a state estimation problem for a UAV model, which is higher dimensional than the falling body model.

7. Application to the Osprey-Type Drone

7.1. Model Overview (Drone)

The UAV vehicle conditions are based on the Osprey-type drone with 2-DOF tiltable coaxial rotors by Itakura et al. [40]. The vehicle’s general appearance and the derivation of its dynamic model have already been detailed in Section 4.

The vehicle’s position in the world coordinate system, $\mathit{\xi }={\left[xyz\right]}^T$, follows the translational motion equation expressed by Equations (44) or (45).

On the other hand, the vehicle’s angular acceleration ${\dot{\mathit{\omega }}}_B={\left[\dot{p}\dot{q}\dot{r}\right]}^T$ is given by Equations (40) or (41). Assuming the Euler angle variations are relatively small, i.e., $W_{\eta }\simeq I$, then $p\simeq \dot{\varphi },q\simeq \dot{\theta },r\simeq \dot{\psi }$. Therefore, Equation (41) can be rewritten as

$$\left[\begin{array}{c}\ddot{\varphi }\\ \ddot{\theta }\\ \ddot{\psi }\end{array}\right]=\left[\begin{array}{c}{\displaystyle \frac{1}{I_{Bxx}}}({\tau }_x^B+{\tau }_x^c)+\dot{\theta }\dot{\psi }\left({\displaystyle \frac{I_{Byy}-I_{Bzz}}{I_{Bxx}}}\right)\\ {\displaystyle \frac{1}{I_{Byy}}}({\tau }_y^B+{\tau }_y^c)+\dot{\varphi }\dot{\psi }\left({\displaystyle \frac{I_{Bzz}-I_{Bxx}}{I_{Byy}}}\right)\\ {\displaystyle \frac{1}{I_{Bzz}}}({\tau }_z^B+{\tau }_z^c)+\dot{\varphi }\dot{\theta }\left({\displaystyle \frac{I_{Bxx}-I_{Byy}}{I_{Bzz}}}\right)\end{array}\right].$$

(74)

Now, defining a 12-dimensional state vector as $\mathit{x}={\left[x\dot{x}y\dot{y}z\dot{z}\varphi \dot{\varphi }\theta \dot{\theta }\psi \dot{\psi }\right]}^T={\left[x_1{x}_2{x}_3{x}_4{x}_5{x}_6{x}_7{x}_8{x}_9{x}_{10}{x}_{11}{x}_{12}\right]}^T$, the continuous-time state equation for the UAV model can be derived from Equations (45) and (74) as follows:

$$\begin{array}{c}\hfill {\displaystyle \frac{d}{dt}}\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\\ x_4\\ x_5\\ x_6\\ x_7\\ x_8\\ x_9\\ x_{10}\\ x_{11}\\ x_{12}\end{array}\right]=\left[\begin{array}{c}{x}_2\\ A_1\left(x\right)T_x+A_2\left(x\right)T_y+A_3\left(x\right)T_z\\ x_4\\ A_4\left(x\right)T_x+A_5\left(x\right)T_y+A_6\left(x\right)T_z\\ x_6\\ A_7\left(x\right)T_x+A_8\left(x\right)T_y+A_9\left(x\right)T_z+g\\ x_8\\ {\displaystyle \frac{1}{I_{Bxx}}}({\tau }_x^B+{\tau }_x^c)+x_{10}{x}_{12}\left({\displaystyle \frac{I_{Byy}-I_{Bzz}}{I_{Bxx}}}\right)\\ x_{10}\\ {\displaystyle \frac{1}{I_{Byy}}}({\tau }_y^B+{\tau }_y^c)+x_8{x}_{12}\left({\displaystyle \frac{I_{Bzz}-I_{Bxx}}{I_{Byy}}}\right)\\ x_{12}\\ {\displaystyle \frac{1}{I_{Bzz}}}({\tau }_z^B+{\tau }_z^c)+x_8{x}_{10}\left({\displaystyle \frac{I_{Bxx}-I_{Byy}}{I_{Bzz}}}\right)\end{array}\right].\end{array}$$

(75)

Here, $A_1\left(x\right),A_2\left(x\right),\dots ,A_9\left(x\right)$ in Equation (75) are defined as follows:

$$\left\{\begin{array}{c}{A}_1\left(x\right)=C_{x_9}{C}_{x_{11}}/m\hfill \\ A_2\left(x\right)=(S_{x_7}{S}_{x_9}{C}_{x_{11}}-C_{x_7}{S}_{x_{11}})/m\hfill \\ A_3\left(x\right)=(C_{x_7}{S}_{x_9}{C}_{x_{11}}+S_{x_7}{S}_{x_{11}})/m\hfill \\ A_4\left(x\right)=C_{x_9}{S}_{x_{11}}/m\hfill \\ A_5\left(x\right)=(S_{x_7}{S}_{x_9}{S}_{x_{11}}+C_{x_7}{C}_{x_{11}})/m\hfill \\ A_6\left(x\right)=(C_{x_7}{S}_{x_9}{C}_{x_{11}}-S_{x_7}{C}_{x_{11}})/m\hfill \\ A_7\left(x\right)=-S_{x_9}/m\hfill \\ A_8\left(x\right)=S_{x_7}{C}_{x_9}/m\hfill \\ A_9\left(x\right)=C_{x_7}{C}_{x_9}/m\hfill \end{array}\right..$$

(76)

7.2. Estimation Algorithms (Drone)

Similar to the falling body model, we compare three types of numerical integration methods integrated with the UKF for the UAV model: the Euler method, the RK method, and the AB method. The detailed derivation and specific equations for the sigma point time update of each method are presented in Appendix D. This section provides only an overview of each method.

7.2.1. Euler-UKF (Drone)

In Euler-UKF, the Euler method is applied to Equation (75). The Euler method has first-order accuracy and requires one function evaluation per update step. The specific sigma point time-update equations are given in Appendix D.1.

7.2.2. RK-UKF (Drone)

In RK-UKF, the RK4 method is applied to Equation (75). The RK4 method has fourth-order accuracy and requires four function evaluations per update step. The specific sigma point time-update equations are given in Appendix D.2.

7.2.3. AB-UKF (Drone)

In AB-UKF, the AB method is applied to Equation (75). Regardless of its order, the AB method requires only one function evaluation per update step and stores and utilizes the function evaluation results for past sigma point matrices. The specific forms of the sigma point time-update equations corresponding to the 2nd to 6th order AB methods are given in Appendix D.3.

7.3. Estimation Conditions (Drone)

This section describes the UAV physical parameters and the covariance of system and observation noise used in the estimation simulation.

7.3.1. Parameter Settings for the UAV Model

Similar to the falling body model, state estimation simulations are performed using the three methods described above.

Table 3. Simulation parameters of the UAV.

Variable	Definition	Value
m [kg]	Mass of UAV	1.5
g [m/s2]	Acceleration of gravity	9.81
$I_{Bxx}$ [kg·m2]	Moment of inertia around ${\mathrm{X}}_{\mathrm{B}}$ axis	0.01
$I_{Byy}$ [kg·m2]	Moment of inertia around ${\mathrm{Y}}_{\mathrm{B}}$ axis	0.01
$I_{Bzz}$ [kg·m2]	Moment of inertia around ${\mathrm{Z}}_{\mathrm{B}}$ axis	0.006
l [m]	${\mathrm{Y}}_{\mathrm{B}}$ axis distance between ${\mathrm{O}}_{\mathrm{B}}$ and ${\mathrm{O}}_{{\mathrm{P}}_i}$	0.24
$h_o$ [m]	${\mathrm{Z}}_{\mathrm{B}}$ axis distance between ${\mathrm{O}}_{\mathrm{B}}$ and ${\mathrm{O}}_{{\mathrm{P}}_i}$	0.045
$k_f$ [Ns2/rad2]	Thrust coefficient of the propeller	$1.784\times {10}^{-5}$
$k_t$ [Nms2/rad2]	Drag coefficient of the propeller	$4.379\times {10}^{-7}$

summarizes the definitions and values of each parameter.

All initial states are set to $\mathit{x}\left(0\right)={[0\dots 0]}^T$, and the initial estimation information is set to $\widehat{\mathit{x}}\left(0\right|0)={[0\dots 0]}^T$ and $P\left(0\right|0)=\mathrm{diag}(1,\dots ,1)$.

7.3.2. Determination of System Noise

In this simulation experiment, the system noise is set based on the global truncation error arising from discretization by the Euler, RK, and AB methods. When the step size is denoted by h, the global truncation error for the Euler method is proportional to $h^1$, and for the RK4 and AB4 methods, it is proportional to $h^4$. Therefore, letting the variance values of the respective system noises be ${\mathit{Q}}_{Euler}$ and ${\mathit{Q}}_{RK,AB4}$, they can be expressed as

$$\begin{array}{cc}\hfill {\mathit{Q}}_{Euler}& =h\times {\mathit{I}}_{12\times 12},\hfill \end{array}$$

(77)

$$\begin{array}{cc}\hfill {\mathit{Q}}_{RK,AB4}& =h^4\times {\mathit{I}}_{12\times 12}.\hfill \end{array}$$

(78)

Thus, for comparison purposes in this simulation experiment, the value $\mathit{Q}$ is set as follows:

$$\begin{array}{c}\hfill \mathit{Q}=h^{5/2}\times {\mathit{I}}_{12\times 12}.\end{array}$$

(79)

Here, the exponent $5/2$ signifies taking an intermediate order of magnitude between ${\mathit{Q}}_{Euler}$ and ${\mathit{Q}}_{RK,AB4}$.

7.3.3. Determination of Observation Noise

Typically, UAVs are equipped with GPS sensors for position observation and gyro sensors for attitude observation. Determining a precise noise value for GPS sensor error is difficult due to various influencing factors; thus, for simplicity, the noise value is set to 2 m here. For the gyro sensor, we assume the use of the MPU-9250, a 9-axis sensor module from InvenSense.

The MPU-9250 datasheet states a gyro sensor noise value of 0.01 [deg/$\sqrt{\mathrm{Hz}}$]. Using this noise value and the step size h [s], the observation noise variance $R_1$ [rad²] for attitude angles $\varphi ,\theta ,\psi$ and $R_2$ [rad²/s²] for attitude angular velocities $\dot{\varphi },\dot{\theta },\dot{\psi }$ are determined.

First, convert the dimension 0.01 [deg/$\sqrt{\mathrm{Hz}}$] to [rad/$\sqrt{\mathrm{Hz}}$] to obtain

$$\begin{array}{c}\hfill 1.0\times {10}^{-2}\left({\displaystyle \frac{\pi }{180}}\right)[\mathrm{rad}/\sqrt{\mathrm{Hz}}].\end{array}$$

(80)

Squaring the value obtained in Equation (80) yields

$$\begin{array}{c}\hfill 1.0\times {10}^{-4}{\left({\displaystyle \frac{\pi }{180}}\right)}^2[{\mathrm{rad}}^2/\mathrm{Hz}].\end{array}$$

(81)

Here, the dimension [Hz] can also be expressed as [1/s]; thus, the dimension [rad²/Hz] can also be expressed as [rad²·s]. Therefore, the observation noise variance $R_1$ [rad²] for attitude angles $\varphi ,\theta ,\psi$ is obtained by dividing Equation (81) by h [s]:

$$\begin{array}{c}\hfill R_1=1.0\times {10}^{-4}{\left({\displaystyle \frac{\pi }{180}}\right)}^2/h\left[{\mathrm{rad}}^2\right].\end{array}$$

(82)

Then, the observation noise variance $R_2$ [rad²/s²] for attitude angular velocities $\dot{\varphi },\dot{\theta },\dot{\psi }$ is obtained by dividing Equation (82) by $h^2$ [s]:

$$\begin{array}{c}\hfill R_2=1.0\times {10}^{-4}{\left({\displaystyle \frac{\pi }{180}}\right)}^2/h^3[{\mathrm{rad}}^2/{\mathrm{s}}^2].\end{array}$$

(83)

Consequently, the observation equation is

$$\begin{array}{c}\hfill y\left(t_k\right)=Hx\left(t_k\right)+v\left(t_k\right),\end{array}$$

(84)

$$\begin{array}{c}\hfill H=\left[\begin{array}{cc}{H}_{pos}& \mathbf{0}\\ \mathbf{0}& I_{6\times 6}\end{array}\right]\end{array}$$

(85)

where $H_{pos}$ is the observation matrix for position $(x,y,z)$:

$$\begin{array}{c}\hfill H_{pos}=\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 1& 0\end{array}\right].\end{array}$$

(86)

The covariance $\mathit{R}$ of the observation noise $v\left(t_k\right)$ is represented by the following diagonal matrix:

$$\begin{array}{c}\hfill \mathit{R}=\mathrm{diag}(2,2,2,R_1,R_2,R_1,R_2,R_1,R_2).\end{array}$$

(87)

Remark

The observation setting used in this paper is linear (direct observation of position and attitude angles), and the advantages of nonlinear filtering are not fully utilized. In actual UAV navigation, nonlinear observations such as radar observations in GPS-denied environments are common. While the main focus of this paper is the analysis of the impact of state equation complexity on computational efficiency, and the linearity of observations is not an essential problem, extension to nonlinear observations is an important future task.

7.4. Target Trajectory

The simulation starts from the initial position $\left[xyz\right]={\left[000\right]}^T$ and initial attitude angles $\left[\varphi \theta \psi \right]={\left[000\right]}^T$. First, over 10 s from the start time, the vehicle moves to the target position $\left[x_d{y}_d{z}_d\right]={\left[001\right]}^T$, while all target attitude angles remain zero. Subsequently, from 10 s to 70 s, the vehicle is commanded to follow the target position, velocity, and acceleration given below [42].

Target position ${\mathit{X}}_d$:

$$\begin{array}{c}\hfill {\mathit{X}}_d=\left[\begin{array}{c}{x}_d\left(t\right)\\ y_d\left(t\right)\\ z_d\left(t\right)\\ {\varphi }_d\left(t\right)\\ {\theta }_d\left(t\right)\\ {\psi }_d\left(t\right)\end{array}\right]=\left[\begin{array}{c}sin\left({\displaystyle \frac{\pi }{10}}(t-10)\right)\\ cos\left({\displaystyle \frac{\pi }{4}}\right)\left(-1+cos\left({\displaystyle \frac{\pi }{10}}(t-10)\right)\right)\\ -1+sin\left({\displaystyle \frac{\pi }{4}}\right)\left(-1+cos\left({\displaystyle \frac{\pi }{10}}(t-10)\right)\right)\\ {\displaystyle \frac{\pi }{4}}sin\left({\displaystyle \frac{\pi }{40}}(t-10)\right)\\ -{\displaystyle \frac{\pi }{4}}sin\left({\displaystyle \frac{\pi }{40}}(t-10)\right)\\ 0\end{array}\right].\end{array}$$

(88)

Target velocity ${\dot{\mathit{X}}}_d$:

$$\begin{array}{c}\hfill {\dot{\mathit{X}}}_d=\left[\begin{array}{c}{\dot{x}}_d\left(t\right)\\ {\dot{y}}_d\left(t\right)\\ {\dot{z}}_d\left(t\right)\\ {\dot{\varphi }}_d\left(t\right)\\ {\dot{\theta }}_d\left(t\right)\\ {\dot{\psi }}_d\left(t\right)\end{array}\right]=\left[\begin{array}{c}{\displaystyle \frac{\pi }{10}}cos\left({\displaystyle \frac{\pi }{10}}(t-10)\right)\\ -{\displaystyle \frac{\pi }{10}}cos\left({\displaystyle \frac{\pi }{4}}\right)sin\left({\displaystyle \frac{\pi }{10}}(t-10)\right)\\ -{\displaystyle \frac{\pi }{10}}sin\left({\displaystyle \frac{\pi }{4}}\right)sin\left({\displaystyle \frac{\pi }{10}}(t-10)\right)\\ {\displaystyle \frac{{\pi }^2}{160}}cos\left({\displaystyle \frac{\pi }{40}}(t-10)\right)\\ -{\displaystyle \frac{{\pi }^2}{160}}cos\left({\displaystyle \frac{\pi }{40}}(t-10)\right)\\ 0\end{array}\right].\end{array}$$

(89)

Target acceleration ${\ddot{\mathit{X}}}_d$:

$$\begin{array}{c}\hfill {\ddot{\mathit{X}}}_d=\left[\begin{array}{c}{\ddot{x}}_d\left(t\right)\\ {\ddot{y}}_d\left(t\right)\\ {\ddot{z}}_d\left(t\right)\\ {\ddot{\varphi }}_d\left(t\right)\\ {\ddot{\theta }}_d\left(t\right)\\ {\ddot{\psi }}_d\left(t\right)\end{array}\right]=\left[\begin{array}{c}-{\displaystyle \frac{{\pi }^2}{100}}sin\left({\displaystyle \frac{\pi }{10}}(t-10)\right)\\ -{\displaystyle \frac{{\pi }^2}{100}}cos\left({\displaystyle \frac{\pi }{4}}\right)cos\left({\displaystyle \frac{\pi }{10}}(t-10)\right)\\ -{\displaystyle \frac{{\pi }^2}{100}}sin\left({\displaystyle \frac{\pi }{4}}\right)cos\left({\displaystyle \frac{\pi }{10}}(t-10)\right)\\ -{\displaystyle \frac{{\pi }^3}{6400}}sin\left({\displaystyle \frac{\pi }{40}}(t-10)\right)\\ {\displaystyle \frac{{\pi }^3}{6400}}sin\left({\displaystyle \frac{\pi }{40}}(t-10)\right)\\ 0\end{array}\right].\end{array}$$

(90)

This target trajectory is a circular path with a radius of 1 m and center $(x,y)=(0,-1)$ in the $xy$-plane, rotated 45 deg about the x-axis, and translated −1 m along the z-axis. Since this vehicle is left-right symmetric, independent control in three degrees of freedom (x, y, z directions) can be verified by following this circular path. The vehicle follows this trajectory with a period of 20 s per revolution. Simultaneously, the target roll angle changes every revolution: from 0 deg to 45 deg, 45 deg to 0 deg, and 0 deg to −45 deg. The target pitch angle changes every revolution: from 0 deg to −45 deg, −45 deg to 0 deg, and 0 deg to 45 deg. The target yaw angle is always 0 deg. Following this trajectory verifies the possibility of independent control in all six degrees of freedom.

7.5. Description of Comparative Experiments

This comparative experiment focuses on two aspects of the UKF based on each discretization method: “estimation accuracy” and “computational efficiency.” Details of the comparison methods are described below. Comparisons are performed in two patterns: “comparison of three methods: Euler, RK4, and AB4” and “comparison of 2nd to 6th-order AB methods.” The results of this comparative experiment are averages from 50 Monte Carlo simulations.

7.5.1. Estimation Accuracy Comparison Experiment

The estimation accuracy comparison is performed by calculating the RMSE values between the estimates from each discretization-based UKF and the true values.

The RMSE value at estimation time k ($k=1,\dots ,N$) is calculated using the following equations:

$$\begin{array}{cc}\hfill {\mathrm{MSE}}_x\left(k\right)& ={\displaystyle \frac{1}{k}}\sum _{i=1}^k{\left(x\left(t_i\right)-\widehat{x}\left(t_i\right|t_i)\right)}^2,\hfill \end{array}$$

(91)

$$\begin{array}{cc}\hfill {\mathrm{RMSE}}_x\left(k\right)& =\sqrt{{\mathrm{MSE}}_x\left(k\right)}\hfill \end{array}$$

(92)

where $x\left(t_i\right)$ and $\widehat{x}\left(t_i\right|t_i)$ represent the true value and the estimate of position x at time i, respectively. Here, N is the total number of sampling points, determined from the simulation time of 70 s and the discretization period h [s] as $N=70/h$. For example, if $h=0.01$ [s], then $N=7000$. Equations (91) and (92) are similarly applied to other variables: positions $y,z$, velocities $\dot{x},\dot{y},\dot{z}$, attitude angles $\varphi ,\theta ,\psi$, and attitude angular velocities $\dot{\varphi },\dot{\theta },\dot{\psi }$.

Furthermore, when comparing estimation accuracy under different discretization periods ($h=0.01,0.02,\dots ,0.1$), the sum of the individual RMSE values ${\mathrm{RMSE}}_x\left(N\right),{\mathrm{RMSE}}_y\left(N\right),$$\dots ,{\mathrm{RMSE}}_{\dot{\psi }}\left(N\right)$ for the 12 state variables at the final discrete time $k=N$ is denoted as $\mathrm{RMSE}\left(N\right)$, and this value is used for comparison.

Here, we discuss the theoretical meaning of $\mathrm{RMSE}\left(N\right)$. In the UAV model, when the a priori error covariance matrix $P\left(t_k\right|t_k)$ at time $t_k$ is expressed as

$$\begin{array}{c}\hfill P\left(t_k\right|t_k)=\left[\begin{array}{cccc}{p}_{1,1}& p_{1,2}& \dots & p_{1,12}\\ p_{2,1}& p_{2,2}& \dots & p_{2,12}\\ ⋮& ⋮& \ddots & ⋮\\ p_{12,1}& p_{12,2}& \dots & p_{12,12}\end{array}\right],\end{array}$$

(93)

the trace of this error covariance matrix is denoted as $\mathrm{tr}\left(P\left(t_k\right|t_k)\right)$. The aforementioned $\mathrm{RMSE}\left(N\right)$ is the sum of RMSE values based on actual sampled estimates, averaged over time up to $k=N$ and averaged over the ensemble of 50 simulation results. Therefore, while transient values differ from the actual $\mathrm{tr}\left(P\left(t_k\right|t_k)\right)$, they can be treated as equivalent values in the steady state.

7.5.2. Computational Efficiency Comparison Experiment

The computational efficiency comparison experiment measures and compares computation times at the following three points:

• Measurement Time I: Computation time for sigma point time update (computation of Equation (A9)).
• Measurement Time II: Computation time for the entire UKF algorithm (computation from Equations (A3) to (A18)).
• Measurement Time III: Total computation time of the state estimation program for the UAV model.

Figure 4 shows the overall diagram of the state estimation program for the UAV model and the locations of “Measurement Time I”, “Measurement Time II”, and “Measurement Time III”.

The controller design for generating generalized forces follows the computed torque method described in Section 5. The target trajectory involves a circular path from 10 s to 70 s, with yaw angle always at 0 deg, and roll and pitch angles changing to specified angles each revolution. However, the control inputs are first transformed from the generalized forces (thrust and torque for the motion system) provided by the computed torque method into actuator commands (rotor speeds and tilt angles) via the control allocation law. These are then multiplied by the propeller thrust coefficient $k_f$ and torque coefficient $k_t$ to reproduce the thrust and torque (including reaction torque) from the actuators, which are applied as inputs to the plant.

Below, we detail the data collection methods for Measurement Times I, II, and III.

• Measurement Time I: Since one simulation yields N measurement data points due to the total number of sampling points N, the average of all these is taken to calculate the average computation time per loop. However, due to the nature of the AB method, the average is taken over $N-1$ data points for 2nd order, $N-2$ for 3rd order, $N-3$ for 4th order, $N-4$ for 5th order, and $N-5$ for 6th order.
• Measurement Time II: Similar to Measurement Time I, the average computation time per loop is calculated, and the total computation time for N loops is also calculated.
• Measurement Time III: This is the total program computation time, so one simulation yields a single measurement data point.

Using these measurement methods, 50 simulations are repeatedly executed, and their average computation times are calculated for each case.

Remark

The purpose of this study’s computational efficiency evaluation is to compare algorithm-specific computational loads using the MATLAB environment. The validity of this approach is based on the following points:

• Elimination of Hardware Dependence: MATLAB simulations eliminate hardware-dependent factors such as CPU architecture, memory bandwidth, and cache size, enabling a pure evaluation of the relative computational load differences between the proposed numerical integration method (the AB method) and the conventional method (the RK method).
• Indicator of Computational Amount: In an onboard environment (flight control computer), the sampling frequency is high and computing resources are limited, making it important to reduce the number of floating-point operations and execution time to achieve the same accuracy. The reduction in computation time measured in MATLAB serves as a basic indicator of the effectiveness of processing speed improvement on an actual aircraft.

However, final verification requires implementation on an actual autopilot (e.g., Pixhawk) and benchmarking, which is considered a future challenge. This simulation demonstrates the theoretical and algorithmic advantages prior to actual implementation.

7.6. Results and Discussion of Estimation Accuracy

7.6.1. Comparison of Three Methods in Estimation Accuracy: Euler, RK4, and AB4

First, to show the estimates for the UAV model and to verify whether the UKF based on the multi-step AB method can perform estimation as stably as the single-step Euler and RK methods, Figure 5 shows the time evolution of state estimates by UKF based on the three methods for $h=0.01$.

As seen from Figure 5, the UKF estimates based on the AB method converge stably and achieve high-precision estimation. Also, no significant deviation from the RK method estimates is observed.

Next, Figure 6 shows the time evolution of the RMSE values for the state estimates by UKF based on the three methods for $h=0.01$, over the 70 s simulation period. From Figure 6, under the condition $h=0.01$, although the RMSE differences among the methods are very small for all state variables, relatively larger differences are observed in the RMSE values for positions $x,y$, attitude angles $\varphi ,\theta ,\psi$, and attitude angular velocities $\dot{\varphi },\dot{\theta },\dot{\psi }$. Also, for all 12 state variables, the time variation of RMSE values becomes smaller after around 50 s of simulation time.

In addition, estimation accuracy is compared under different discretization periods: $h=0.01,0.02,\dots ,0.1$. Figure 7 shows the change in $\mathrm{RMSE}\left(N\right)$ with varying discretization period h. Figure 7 shows that as the discretization period h increases, the differences among the methods become larger, and ultimately the $\mathrm{RMSE}\left(N\right)$ value for the RK method is the smallest. Furthermore, for the AB4 method, estimation values could not be obtained (diverged) for h values greater than or equal 0.08. Details regarding this are discussed later.

The specific values for each point in Figure 7 are shown in

Table 4. Comparison of $\mathrm{RMSE}\left(N\right)$ with different step size for three methods.

	Method
	Euler	RK4	AB4
$h=0.01$	0.47961	0.47646	0.47304
$h=0.02$	0.67167	0.66121	0.66235
$h=0.03$	0.83102	0.82695	0.81186
$h=0.04$	0.96179	0.94971	0.94171
$h=0.05$	1.07992	1.07212	1.07913
$h=0.06$	1.19891	1.18197	1.19785
$h=0.07$	1.31153	1.28889	1.37018
$h=0.08$	1.41016	1.39465	N/A
$h=0.09$	1.52378	1.49280	N/A
$h=0.10$	1.62067	1.58091	N/A

N/A indicates that the computation could not be performed.

. The “N/A” in Table 4 indicates that estimation values could not be obtained due to divergence.

7.6.2. RMSE Comparison of AB Methods with Different Orders of Accuracy

The estimation values are omitted, as their differences from Figure 6 were very small. Figure 8 shows the time evolution of the RMSE values for UKF estimates based on the second- to sixth-order AB methods for $h=0.01$. From Figure 8, under the condition $h=0.01$, all AB methods from the second to sixth order converge to values comparable to the RMSE values for UKF estimates based on the Euler and RK methods shown in Figure 6, without significant deviation.

Next, estimation accuracy is compared under different discretization periods: $h=0.01,0.02,\dots ,0.1$. Figure 9 shows the change in $\mathrm{RMSE}\left(N\right)$ with varying discretization period h. Similar to Figure 7, Figure 9 shows that as the discretization period h increases, the differences among the methods become larger. Also, the sixth-order AB method diverged for h greater than or equal 0.03; the fifth-order, for h greater than or equal 0.05; and the fourth-order, for h greater than or equal 0.08, preventing estimation value acquisition.

The specific values for each point in Figure 9 are shown in

Table 5. $\mathrm{RMSE}\left(N\right)$ values for 2nd- to 6th-order AB-based UKFs with different step sizes.

	AB Method with Different Order
	AB2	AB3	AB4	AB5	AB6
$h=0.01$	0.47140	0.47891	0.47304	0.47878	0.47409
$h=0.02$	0.66619	0.66920	0.66235	0.66816	0.68082
$h=0.03$	0.81327	0.80840	0.81186	0.82628	N/A
$h=0.04$	0.94432	0.93755	0.94171	1.00880	N/A
$h=0.05$	1.06020	1.05421	1.07913	N/A	N/A
$h=0.06$	1.17243	1.17460	1.19785	N/A	N/A
$h=0.07$	1.27515	1.26857	1.37018	N/A	N/A
$h=0.08$	1.37661	1.37650	N/A	N/A	N/A
$h=0.09$	1.47709	1.49280	N/A	N/A	N/A
$h=0.10$	1.59363	1.60998	N/A	N/A	N/A

N/A: Not available (calculation could not be performed).

. Additionally, for the 2nd and 3rd-order AB methods, further verification confirmed that they similarly diverge for values greater than or equal $h=0.25$ and $h=0.15$, respectively. Based on these results and additional experiments, the maximum step sizes at which each order can perform stable estimation without divergence are summarized as follows:

• 2nd-order AB method: Stable up to $h=0.24$ (verified by additional experiments up to $h=0.24$). Divergence occurs at $h\ge 0.25$.
• 3rd-order AB method: Stable up to $h=0.14$. Divergence occurs at $h\ge 0.15$.
• 4th-order AB method: Stable up to $h=0.07$. Divergence occurs at $h\ge 0.08$.
• 5th-order AB method: Stable up to $h=0.04$. Divergence occurs at $h\ge 0.05$.
• 6th-order AB method: Stable up to $h=0.02$. Divergence occurs at $h\ge 0.03$.

These experimentally obtained stability limits are consistent with the theoretical property of linear multi-step methods that the absolute stability region shrinks as the order increases [36,37].

In practical applications, the choice of step size involves a trade-off between estimation accuracy and computational efficiency. A larger step size reduces computational load but may degrade accuracy or cause divergence, while a smaller step size improves stability and accuracy at the cost of increased computation time. Users should select the step size based on the specific requirements of their application, referring to the experimentally verified stability limits above as a guideline for the maximum allowable step size.

7.6.3. Comparative Analysis: RK Method vs. AB Method

Comparing the results in Table 4 (re-entry vehicle problem) and Table 5 (UAV problem) reveals the following:

• Accuracy Difference Between the RK and AB Methods: At the same step size (e.g., $h=0.01$), the RMSE of the RK4-UKF (UAV: 0.47646) is nearly identical to the RMSE of the AB4-UKF (UAV: 0.47304), indicating no significant difference in accuracy between the two. This indicates that the RK method achieves high-order accuracy through multiple evaluations within a single step, while the AB method achieves similar accuracy with fewer function evaluations by utilizing past information.
• Relationship between Order and Stability: For both problems, the higher the order of the AB algorithm, the smaller the upper limit of the step size for stable estimation. This is due to the theoretical property of numerical analysis that the absolute stability region shrinks as the order of the linear multi-step algorithm increases.
• Problem Complexity and Computational Efficiency: For the simple 3D reentry vehicle problem (Table 2), the computational time of the AB4-based UKF increased by approximately 2.9% compared to the Euler-based UKF, while for the complex 12D UAV model (

  Table 6. Comparison of computational time among Euler-based UKF, RK4-based UKF and AB4-based UKF ($h=0.01$).

  	Method
  	Euler	RK4	AB4
  Time I ($\times {10}^{-4}$) [s]	3.7593	5.6078	3.2978
  Time II ($\times {10}^{-4}$) [s]	6.0411	8.0064	5.6144
  Time III [s]	6.3785	7.6520	6.0550

  ), it was reduced by approximately 5.1%. This reversal phenomenon is thought to be due to the characteristics of matrix operations in high-dimensional models (regularity of memory access patterns, cache efficiency).

These results indicate a trade-off between computational efficiency and numerical stability. When applying this method to real problems, it is necessary to select an appropriate order based on the required estimation accuracy and acceptable step size.

7.7. Results and Discussion of Computational Efficiency

7.7.1. Comparison of Three Methods in Computational Efficiency: Euler, RK4, and AB4

Table 6 shows the computation times for Time I, Time II, and Time III for the three methods (Euler, RK4, and AB4) with step size $h=0.01$. From Table 6, the reduction rate in computation time for the 4th-order AB method is 12.3% compared to Euler and 41.1% compared to RK for Time I; 7.1% compared to Euler and 29.9% compared to RK4 for Time II; and 5.1% compared to Euler and 20.9% compared to RK4 for Time III.

Next, Figure 10 shows the change in computation time with varying discretization period h. Figure 10 shows that as h becomes smaller, the difference between the two methods increases. This is considered to be due to the influence of the increasing number of program loops as the total number of sampling points N becomes larger for smaller h.

7.7.2. Computational Time Comparison of AB Methods with Different Orders of Accuracy

Next, computation times for Time I, Time II, and Time III are compared under the same conditions for AB methods from 2nd to 6th order for $h=0.01$, not just the AB4 method. The result is given in

Table 7. Comparison of computational time for 2nd-order to 6th-order AB-based UKFs.

	AB Method with Different Order
	AB2	AB3	AB4	AB5	AB6
Time I ($\times {10}^{-4}$) [s]	3.2125	3.2839	3.3212	3.3478	3.4263
Time II ($\times {10}^{-4}$) [s]	5.5107	5.6166	5.6353	5.6081	5.7562
Time III [s]	6.1799	6.2990	6.1856	6.1427	6.3341

. The computational difference among AB methods from 2nd to 6th order lies only in the number of past sigma point matrices used as “previous information”. Therefore, the differences among methods are smaller than the results shown in Table 6.

Next, Figure 11 shows the change in computation time with varying discretization period h. From Figure 11, unlike the comparison between RK and AB methods, almost no difference is seen among the methods. The reason for this result is considered to be that the difference in computational load due to the order of the AB method stems only from the difference in the number of sigma point matrices handled, and the number of derivative calculations per step is the same for any order. Also, from this result, it can be understood that while estimation with the AB method diverges if the step size is too large, it does not diverge under conditions with h values smaller than 0.01, allowing estimation with higher-order accuracy and shorter computation time compared to the RK method under those conditions.

8. Conclusions

This paper proposed a UKF that newly integrates the Adams–Bashforth method into the sigma point time-update equation of the UKF (AB-UKF) as an alternative to the Runge–Kutta method. The proposed method maintains estimation accuracy comparable to the RK-UKF proposed by Takeno and Katayama [20] while enabling estimation with more efficient computation time.

The main findings obtained from the numerical simulation results are summarized below.

• Maintenance of estimation accuracy: For both models, AB-UKF achieved estimation accuracy comparable to RK-UKF. For the UAV model ($h=0.01$), no significant difference was observed between the RMSE of AB4-UKF (0.47304) and that of RK4-UKF (0.47646).
• Comparison of computational efficiency: Comparison of total computation time relative to the Euler-based UKF confirmed the effectiveness of the proposed method.

  –

  For the simple three-dimensional reentry vehicle problem (Table 2), the computation time of AB4-based UKF was approximately 2.9% longer than Euler-based UKF (slower than Euler), whereas RK4-based UKF was approximately 9.4% longer.

  –

  For the complex 12-dimensional UAV model (Table 6), the computation time of AB4-based UKF was approximately 5.1% shorter than Euler-based UKF (faster than Euler). In contrast, RK4-based UKF exhibited a more pronounced slowdown, with an approximately 20.0% increase.

  These results demonstrate that the computational time reduction effect achieved by the proposed AB method integration becomes more pronounced for models with more complex state equations and higher dimensions.
• Relationship between order and stability: Verification using the UAV model quantitatively showed that higher-order AB methods have a smaller upper limit on the step size that allows stable estimation. The maximum stable step sizes for each order are: AB2: $h\le 0.24$, AB3: $h\le 0.14$, AB4: $h\le 0.07$, AB5: $h\le 0.04$, and AB6: $h\le 0.02$. Divergence occurs at step sizes exceeding these limits.

These results indicate that high-precision numerical integration methods are not necessarily superior in terms of computational efficiency; the RK4-based UKF was consistently slower than the Euler-based UKF in both problems (reentry problem: +9.4%, UAV problem: +20.0%). The proposed AB-UKF is an effective option for improving computational efficiency while maintaining estimation accuracy, particularly for high-dimensional models.

8.1. Limitations of This Study

This study has the following limitations:

• Linearity of observations: The observations in the UAV model are linear (direct observation of position and attitude angles), and the advantages of nonlinear filtering are not fully utilized. In actual UAV navigation, nonlinear observations such as radar observations in GPS-denied environments are common.
• Simulation environment: The evaluation of computational efficiency is based on MATLAB simulations and has not been verified for real-time operation on actual hardware (flight control computers).
• Application to specific models: The effectiveness of this method has been verified on two models (the falling body model and the UAV model), but additional verification is required for generalization to other nonlinear systems.

8.2. Future Work

Based on the above limitations, future work includes the following:

• Extension to nonlinear observations: Verify the effectiveness of the proposed method under nonlinear observations using only range and azimuth. Specifically, we will tackle the problem of estimating a 12-dimensional state vector using only range and azimuth information from one or two radars, and verify whether AB-UKF can maintain its advantage in terms of computational efficiency even under nonlinear observations.
• Reduction of the number of sigma points: While the standard UKF uses $(2n+1)$ sigma points, the number can be reduced to $n+2$ or $n+1$ by using the Spherical Simplex Unscented Transformation [15,43,44,45] or the Simplex Unscented Transformation [46]. Combining these methods with the AB method is expected to further improve computational efficiency.
• Implementation on actual hardware: Implement the proposed algorithm on a microcontroller (e.g., Pixhawk) for the UAV model used in this simulation, and verify the degree of improvement in estimation accuracy and computational efficiency in actual flight environments.
• Adaptive step size control: Integrate the adaptive step size control based on the degree of nonlinearity proposed by Wang et al. [25] into AB-UKF to further optimize estimation accuracy and computational efficiency.

The proposed method provides a new option for the trade-off between computational efficiency and estimation accuracy in state estimation for high-dimensional nonlinear systems, and its future development is expected to lead to applications in practical UAV navigation systems.