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

1. Introduction

In recent years, research on autonomous mobile robots, such as self-driving cars and autonomous drones, has been actively pursued. Simultaneous Localization and Mapping (SLAM) is a fundamental technology for the autonomous movement of these robots. SLAM performs self-localization using sequential observation data obtained from multiple different sensors. However, since sensor data contain noise, stable and accurate estimation values cannot be obtained without appropriately integrating them. Therefore, in SLAM, the Kalman filter, a filtering algorithm for sequentially estimating time-varying quantities from discrete, error-contained observation data, is used.

Generally, the motion models of many real-world systems, including autonomous mobile robots, are 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. Directly applying the Kalman filter to this CD system is called CD-KF. However, sooner or later, during the implementation stage, the continuous-time motion model (i.e., the prediction model) will need to be discretized or numerically integrated, and a discrete-time Kalman filter will be constructed for that discrete-time motion model. This type of Kalman filter is called the discrete-discrete Kalman filter (DD-KF), as both the motion and observation models are discrete-time. Moreover, for nonlinear dynamics or nonlinear observation systems, it is well-known that the CD-EKF or DD-EKF formally applies the Kalman filter to their linearly approximated systems [1,2,3,4]. 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 [5,6,7]. 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 prepares $(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. Sarkka [8] derived continuous-time and continuous-discrete versions of DD-UKF and applied them to nonlinear continuous-time filtering and reentry vehicle tracking problems. Furthermore, in previous research, the Runge-Kutta (RK) method has been applied to the time-update equation for sigma points, demonstrating the possibility of high-precision state estimation for several models [9,10,11]. Additionally, Takeno & Katayama [12] applied Heun’s Method, an improved version of the Euler method, during the prediction step.

However, discussions on the proposed UKFs have been limited to estimation accuracy, with no 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 increasing computation time as the system order n grows. In practice, considering state estimation for larger-scale models in the real world, both accuracy and real-time performance are required, making the improvement of the UKF algorithm’s computational efficiency very important [13].

Recent studies have begun to address this computational challenge from various perspectives. For instance, Wang et al. [14] proposed an adaptive step-size UKF for continuous-discrete systems based on the degree of nonlinearity, achieving a balance between accuracy and computational cost by dynamically adjusting the step size in highly nonlinear regions. Another approach by researchers in the aerospace domain [15] introduced a high-efficiency UKF utilizing parallel computation for sigma point propagation, demonstrating significant speedup for multi-target trajectory estimation. Furthermore, the integration of machine learning techniques with UKF has been explored; a GAN-enhanced UKF framework [16] 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 [17] developed square-root information-type methods for continuous-discrete extended Kalman filtering, enhancing numerical stability which is crucial for efficient computation. Additionally, theoretical advances in sensor scheduling for continuous-discrete systems [18] provide insights into optimizing the trade-off between resource allocation and estimation accuracy. A comparative evaluation of nonlinear filters [19] further contextualizes the performance of 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.

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 equation 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)$, without using past information. Moreover, when the order of accuracy is $s(s<5)$, s computations 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 s of 4, the next numerical point $x\left(t_{k+1}\right)$ is calculated based on the last 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 computation is needed to generate the next numerical point. Thus, regardless of the accuracy order s of the AB method, the number of computations 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 computational efficiency degradation of the RK method in applying numerical integration to nonlinear Kalman filters, that Renke He et al. [20] have already conducted research applying a multi-step method based on the Adams-Bashforth-Moulton (ABM) method to the Extended Kalman Filter (EKF).

The objective of this study is to clarify that the UKF applying the AB method to the time-update equation 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 due to varying orders (2nd to 6th) of the AB method itself.

The effectiveness of the proposed method is demonstrated through state estimation in a MATLAB simulation environment using two nonlinear models. Specifically, we first verify applicability to a low-dimensional falling object model used in previous research as a preliminary experiment and compare estimation performance under the same conditions. Subsequently, as an application to a more complex model, we conduct a similar comparison of estimation performance using a UAV (Unmanned Aerial Vehicle) model of a tandem-rotor drone.

Section 2 reviews the UKF algorithm for discrete-time state-space models. Section 3 demonstrates the compatibility of UT with two discretization methods, the RK method and the AB method, and presents the time update formulas for the $\sigma$ point based on each. Section 4 describes the modeling of the Osprey-type drone. After defining the coordinate systems, the rotational and translational motions are explained in detail. Section 5 explains the controller design and control allocation. Specifically, a controller based on the computed torque method is described, and the control input allocation problem is detailed. Section 6 performs state estimation by applying the sigma-point time-update equations presented in Section 2 to the falling object model [21] used in previous research as a preliminary experiment, comparing estimation accuracy and computation time. Section 7 first derives the state equations and sigma-point time-update equations for the resulting tandem-rotor UAV model, then performs comparisons of estimation accuracy and computation time similar to Section 6. Furthermore, we provide discussions based on a wider range of estimation results, including not only comparison 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 8 summarizes the paper and presents concluding remarks.

2. Review of the Unscented Kalman Filter

Consider the following discrete-time state-space model.

$$\begin{array}{cc}\hfill x\left(t_{k+1}\right)& =f(x\left(t_k\right),u\left(t_k\right))+w\left(t_k\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)

where $t_k=k\Delta t$, $k=0,1,\dots$ ($\Delta t$: sampling interval). $x\left(t_k\right)\in {\mathbb{R}}^n$, $u\left(t_k\right)\in {\mathbb{R}}^m$, $y\left(t_k\right)\in {\mathbb{R}}^p$ are the state vector, input vector, and output vector, respectively, and $f\in {\mathbb{R}}^n$, $h_m\in {\mathbb{R}}^p$ are nonlinear functions. Also, $w\left(t_k\right)\in {\mathbb{R}}^n$, $v\left(t_k\right)\in {\mathbb{R}}^p$ are Gaussian white noises with mean 0 and covariance matrices Q and R, respectively.

Hereafter, the filtering value of the state vector and its covariance matrix are denoted as $\widehat{x}\left(t_k\right|t_k),P\left(t_k\right|t_k)$, and the one-step predicted value and its covariance matrix as $\widehat{x}\left(t_{k+1}\right|t_k),P\left(t_{k+1}\right|t_k)$. It is assumed that the initial filtering values $\widehat{x}\left(0\right|0),P\left(0\right|0)$ are given. The DD-UKF algorithm is then composed of a prediction step and an update step as follows [5,6,7].

Prediction Step

Step 1: Sigma-point vectors ${\mathcal{X}}_i(i=1,\dots ,2n+1)$ and scalar weights $W_i$

$$\begin{array}{cc}\hfill {\mathcal{X}}_0\left(t_k\right|t_k)& =\widehat{x}\left(t_k\right|t_k)\hfill \end{array}$$

(3)

$$\begin{array}{cc}\hfill {\mathcal{X}}_i\left(t_k\right|t_k)& =\widehat{x}\left(t_k\right|t_k)+{\left(\sqrt{(n+\lambda )P\left(t_k\right|t_k)}\right)}_i\hfill \end{array}$$

(4)

$$\begin{array}{cc}\hfill {\mathcal{X}}_{i+n}\left(t_k\right|t_k)& =\widehat{x}\left(t_k\right|t_k)-{\left(\sqrt{(n+\lambda )P\left(t_k\right|t_k)}\right)}_i\hfill \end{array}$$

(5)

$$\begin{array}{cc}\hfill W_0& ={\displaystyle \frac{\lambda }{n+\lambda }}\hfill \end{array}$$

(6)

$$\begin{array}{cc}\hfill W_i=W_{n+i}& ={\displaystyle \frac{1}{2(n+\lambda )}}(i=1,\dots ,n)\hfill \end{array}$$

(7)

Here, ${\left(\sqrt{(n+\lambda )P\left(t_k\right|t_k)}\right)}_i$ denotes the i-th column vector of the matrix square root of $P\left(t_k\right|t_k)$, scaled by $\sqrt{(n+\lambda )}$. $\lambda$ is a parameter used for computing higher-order moments, such as the covariance.

Step 2: One-step predicted state estimate and its covariance matrix via time update of sigma points

$$\begin{array}{cc}\hfill {\mathcal{X}}_i\left(t_{k+1}\right|t_k)& =f({\mathcal{X}}_i\left(t_k\right|t_k),u\left(t_k\right))(i=1,\dots ,2n+1)\hfill \end{array}$$

(8)

$$\begin{array}{cc}\hfill \widehat{x}\left(t_{k+1}\right|t_k)& =\sum _{i=0}^{2n}{W}_i{\mathcal{X}}_i\left(t_{k+1}\right|t_k)\hfill \end{array}$$

(9)

$$\begin{array}{cc}\hfill P\left(t_{k+1}\right|t_k)& =\sum _{i=0}^{2n}{W}_i\{{\mathcal{X}}_i\left(t_{k+1}\right|t_k)-\widehat{x}\left(t_{k+1}\right|t_k)\}{\{{\mathcal{X}}_i\left(t_{k+1}\right|t_k)-\widehat{x}\left(t_{k+1}\right|t_k)\}}^T+Q\hfill \end{array}$$

(10)

Update Step

Step 3: Predicted output estimate and its covariance matrix using updated sigma points

$$\begin{array}{cc}\hfill {\eta }_i\left(t_{k+1}\right|t_k)& =h_m\left({\mathcal{X}}_i\left(t_{k+1}\right|t_k)\right)(i=1,\dots ,2n+1)\hfill \end{array}$$

(11)

$$\begin{array}{cc}\hfill \widehat{y}\left(t_{k+1}\right|t_k)& =\sum _{i=0}^{2n}{W}_i{\eta }_i\left(t_{k+1}\right|t_k)\hfill \end{array}$$

(12)

$$\begin{array}{cc}\hfill P_{yy}\left(t_{k+1}\right|t_k)& =\sum _{i=0}^{2n}{W}_i\{{\eta }_i\left(t_{k+1}\right|t_k)-\widehat{y}\left(t_{k+1}\right|t_k)\}{\{{\eta }_i\left(t_{k+1}\right|t_k)-\widehat{y}\left(t_{k+1}\right|t_k)\}}^T\hfill \end{array}$$

(13)

Step 4: Output prediction error (innovation)

$$\begin{array}{c}\hfill \nu \left(t_{k+1}\right|t_k)=y\left(t_{k+1}\right)-\widehat{y}\left(t_{k+1}\right|t_k)\end{array}$$

(14)

Step 5: Auto-covariance matrix of $\nu \left(t_{k+1}\right)$ and cross-covariance matrix between $x\left(t_{k+1}\right)$ and $\nu \left(t_{k+1}\right)$

$$\begin{array}{cc}\hfill P_{\nu \nu }\left(t_{k+1}\right|t_k)& =R+P_{yy}\left(t_{k+1}\right|t_k)\hfill \end{array}$$

(15)

$$\begin{array}{cc}\hfill P_{x\nu }\left(t_{k+1}\right|t_k)& =\sum _{i=0}^{2n}{W}_i\{{\mathcal{X}}_i\left(t_{k+1}\right|t_k)-\widehat{x}\left(t_{k+1}\right|t_k)\}{\{{\nu }_i\left(t_{k+1}\right|t_k)-\widehat{y}\left(t_{k+1}\right|t_k)\}}^T\hfill \end{array}$$

(16)

Step 6: Observation update using $\nu \left(t_{k+1}\right)$ and the Kalman gain $K\left(t_{k+1}\right)$

$$\begin{array}{cc}\hfill K\left(t_{k+1}\right)& =P_{x\nu }\left(t_{k+1}\right|t_k)P_{\nu \nu }^{-1}\left(t_{k+1}\right|t_k)\hfill \end{array}$$

(17)

$$\begin{array}{cc}\hfill \widehat{x}\left(t_{k+1}\right|t_{k+1})& =\widehat{x}\left(t_{k+1}\right|t_k)+K\left(t_{k+1}\right)\nu \left(t_{k+1}\right)\hfill \end{array}$$

(18)

$$\begin{array}{cc}\hfill P\left(t_{k+1}\right|t_{k+1})& =P\left(t_{k+1}\right|t_k)-K\left(t_{k+1}\right)P_{\nu \nu }\left(t_{k+1}\right|t_k)K^T\left(t_{k+1}\right)\hfill \end{array}$$

(19)

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\left(x\right),& x\left(0\right)=x_0\hfill \end{array}$$

(20)

Here, $x\in {\mathbb{R}}^n$ is the state vector and $x_0$ is the initial value. Let $f_d$ be the approximate function obtained by discretizing Eq. (20). Then, we obtain the 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}$$

(21)

The observation equation is the same as Eq. (2),

$$\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}$$

(22)

where v is Gaussian white noise with mean 0 and covariance matrix R. Eqs. (21) and (22) constitute the discrete-time state-space model.

The problem is to propose a nonlinear filtering algorithm that sequentially estimates the system’s state vector $x\left(t_k\right)$ based on the observation data $y\left(t_k\right)$. Takeno et al. [9] demonstrated that combining the UKF with the RK method enables high-precision state estimation simulation.

Based on this, this paper shows that by newly combining the AB method with the UT instead of the RK method, relatively high-precision state estimation can be achieved while reducing computational load.

3.2. UT and the Runge-Kutta Method

Here, since the nonlinear function f in Eq. (20) is a continuous-time model before applying discretization, it is redefined as $f_c:=f$. First, we derive the time-update equation for the sigma points when applying the UT and the Runge-Kutta method to Eq. (20). For this, let the dimension of x in Eq. (20) be n, the i-th component of $f_c$ be $f_i$ ($i=1,\dots ,n$), and redefine the sigma point matrix ${\mathcal{X}}_k$ at time k with its elements ${\mathcal{X}}_{i,j,k}$, $(i=1,\dots ,n$, $j=1,\dots ,2n+1)$ as

$${\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]$$

(23)

where i is the subscript indicating the element of the n-dimensional state vector, and j is the subscript indicating the element of the sigma point. Let the difference width be $\Delta t=h$ and $t_k=kh,k=0,1,\dots$. Also, for simplicity, the sigma points in Eqs. (8) and (10) are denoted as ${\mathcal{X}}_{i,j,k+1}^{-}={\mathcal{X}}_{i,j}\left(t_{k+1}\right|t_k)$.

Applying the 4th-order RK method to each sigma point, we define the following [9].

$$\left\{\begin{array}{cc}\hfill a_{1,ij}& =f_i\left({\mathcal{X}}_{1,j,k},\dots ,{\mathcal{X}}_{n,j,k},t_k\right)\hfill \\ \hfill a_{2,ij}& =f_i\left({\mathcal{X}}_{1,j,k}+{\displaystyle \frac{h}{2}}{a}_{1,1j},\dots ,{\mathcal{X}}_{n,j,k}+{\displaystyle \frac{h}{2}}{a}_{1,nj},t_k+{\displaystyle \frac{h}{2}}\right)\hfill \\ \hfill a_{3,ij}& =f_i\left({\mathcal{X}}_{1,j,k}+{\displaystyle \frac{h}{2}}{a}_{2,1j},\dots ,{\mathcal{X}}_{n,j,k}+{\displaystyle \frac{h}{2}}{a}_{2,nj},t_k+{\displaystyle \frac{h}{2}}\right)\hfill \\ \hfill a_{4,ij}& =f_i\left({\mathcal{X}}_{1,j,k}+h{a}_{3,1j},\dots ,{\mathcal{X}}_{n,j,k}+h{a}_{3,nj},t_k+h\right)\hfill \end{array}\right.$$

(24)

Then, the time-update equation for the sigma points is

$$\begin{array}{cc}\hfill {\mathcal{X}}_{i,j,k+1}^{-}& ={\mathcal{X}}_{i,j,k}+{\displaystyle \frac{h}{6}}\left(a_{1,ij}+a_{2,ij}+a_{3,ij}+a_{4,ij}\right)\hfill \\ \hfill & (i=1,\dots ,n,j=1,\dots ,2n+1)\hfill \end{array}$$

(25)

Taking the weighted average of these updated sigma points yields the time-updated value (i.e., the predicted value) of the state vector estimate via Eq. (9). The corresponding time-updated covariance matrix is obtained via Eq. (10).

3.3. UT and the Adams-Bashforth Method

Let the j-th column ($j=1,\dots ,2n+1$) of the sigma point matrix ${\mathcal{X}}_k$ at time k be sequentially substituted into $f_c$. The resulting sigma point matrix ${\mathcal{F}}_c\left({\mathcal{X}}_k\right)$ can be represented by 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]$$

(26)

In this paper, focusing on the key feature of the AB method—its ability to utilize “previously computed information”—we propose a discretization method that uses the sigma point matrix represented by Eq. (26), computed at times $k-1$ and earlier, as “previously computed information.” Below, we present the time-update equations for the sigma points when applying 2nd to 6th order AB methods [22,23].

• Sigma point update equation using the 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}$$

  (27)
• Sigma point update equation using the 3rd-order Adams-Bashforth method

  $$\begin{array}{cc}\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)\hfill \end{array}$$

  (28)
• Sigma point update equation using the 4th-order Adams-Bashforth method

  $$\begin{array}{cc}\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)\hfill \end{array}$$

  (29)

• Sigma point update equation using the 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)+2616{\mathcal{F}}_c\left({\mathcal{X}}_{k-2}\right)-1274{\mathcal{F}}_c\left({\mathcal{X}}_{k-3}\right)\hfill \\ \hfill & +251{\mathcal{F}}_c\left({\mathcal{X}}_{k-4}\right))\hfill \end{array}$$

  (30)
• Sigma point update equation using the 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)+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}$$

  (31)

where ${\mathcal{X}}_k$ is considered an $n\times (2n+1)$ dimensional matrix.

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 is a discretization method that computes the derivative $f_i\left(\mathcal{X}\right)$ four times (this is called having 4 stages) while advancing one step size, and its global truncation error is proportional to $O\left(h^4\right)$, indicating 4th-order accuracy.

On the other hand, in the AB method, since it uses previously computed sigma point matrices ${\mathcal{F}}_c\left({\mathcal{X}}_{k-1}\right)$, ${\mathcal{F}}_c\left({\mathcal{X}}_{k-2}\right)$, ${\mathcal{F}}_c\left({\mathcal{X}}_{k-3}\right)$ to derive the time-update equation for the current sigma points, computing ${\mathcal{F}}_c\left({\mathcal{X}}_k\right)$ only once is sufficient to advance one step size. In other words, regardless of the order of the global truncation error, 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 two- or three-dimensional real space, the number of state variables becomes very large, with examples where the vector element count n reaches millions or tens of millions. The UKF is an example where this effect is pronounced because the sigma point set size is $(2n+1)$, leading to a vast number of elements. The RK method corresponds to each element of the n-dimensional state vector and each element of the sigma point set, performing derivative computations of 4 stages for each sigma point.

In contrast, the AB method requires only one stage of function computation per element of the sigma point set. Moreover, for past sigma point sets, it only involves multiplying the entire matrix by coefficients according to the order of accuracy. Therefore, it can be seen that the impact of increasing the number of state variables is significantly smaller compared to the RK method.

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 $\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, 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}$$

(32)

$$\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}$$

(33)

$$\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}$$

(34)

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}$$

(35)

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

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

(36)

and its inverse is given by

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

(37)

Here, it is found in [24,25] 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}$$

(38)

$$\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}$$

(39)

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$, ${\beta }_i$, $(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 \end{array}$$

$$\begin{array}{cc}\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 \\ \hfill {}^B\mathit{R}_{P_2}& ={\mathit{R}}_z\left(0\right){\mathit{R}}_y\left({\beta }_2\right){\mathit{R}}_x\left({\alpha }_2\right)\hfill \end{array}$$

(40)

$$\begin{array}{cc}\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}$$

(41)

4.2. Rotational Motion

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

$$\left\{\begin{array}{c}{\omega }_{P_1}={\left[\begin{array}{ccc}{\dot{\alpha }}_1& {\dot{\beta }}_1& {\overline{\omega }}_2-{\overline{\omega }}_1\end{array}\right]}^T\hfill \\ {\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.$$

(42)

$$\left\{\begin{array}{c}{\dot{\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{\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.$$

(43)

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 ${\tau }_{c,i}$ of the i-th ($i=1,2$) coaxial rotor can be expressed as

$$\left\{\begin{array}{c}{\tau }_{c,1}={\left[\begin{array}{ccc}0& 0& k_t({\overline{{\omega }_2}}^2-{\overline{\omega }}_1^2)\end{array}\right]}^T\hfill \\ {\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.$$

(44)

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.$$

(45)

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

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

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

(46)

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{\dot{\omega }}_{P_j}=0\\ (i=1,2)(j=1,2,3,4)\end{array}$$

(47)

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

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

(48)

$$\left\{\begin{array}{c}{\tau }_{P_1}={\left[\begin{array}{ccc}0& 0& k_t({\overline{\omega }}_2^2-{\overline{\omega }}_1^2)\end{array}\right]}^T\hfill \\ {\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.$$

(49)

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

The rotational torque ${\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 {\tau }_B& =\sum _{i=1}^2({}^B\mathit{O}_{P_i}\times {}^B\mathit{R}_{P_i}{\mathit{T}}_i)\hfill \end{array}$$

(50)

$$\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}$$

(51)

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, ${\tau }_B={\left[{\tau }_x^B{\tau }_y^B{\tau }_z^B\right]}^T$ can be expressed as

$$\begin{array}{cc}\hfill {\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}$$

(52)

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

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

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

(53)

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

$$\begin{array}{cc}\hfill {}^B\mathit{R}_{P_1}{\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}{\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, ${\tau }_c={\left[{\tau }_x^c{\tau }_y^c{\tau }_z^c\right]}^T$ can be expressed as

$$\begin{array}{cc}\hfill {\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}$$

(54)

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

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

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

(55)

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],{\omega }_B=\left[\begin{array}{c}p\\ q\\ r\end{array}\right]\end{array}$$

we have

$${\mathit{I}}_B{\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 ${\tau }_{cori}={\left[{\tau }_x^{cori}{\tau }_y^{cori}{\tau }_z^{cori}\right]}^T$, we obtain

$$\begin{array}{cc}\hfill {\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}$$

(56)

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

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

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

(57)

Furthermore, neglecting external disturbance torques ${\tau }_{ext}$ acting on the vehicle and gyroscopic effects (i.e., ${\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}$$

(58)

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).$$

(59)

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 \\ \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}$$

(60)

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

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

(61)

$$\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}{cc}& C_{\theta }{C}_{\psi }{T}_x+(S_{\varphi }{S}_{\theta }{C}_{\psi }-C_{\varphi }{S}_{\psi })T_y\hfill \\ \hfill & +(C_{\varphi }{S}_{\theta }{C}_{\psi }+S_{\varphi }{S}_{\psi })T_z\hfill \\ \hfill & C_{\theta }{S}_{\psi }{T}_x+(S_{\varphi }{S}_{\theta }{S}_{\psi }+C_{\varphi }{C}_{\psi })T_y\hfill \\ \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\hfill \end{array}\end{array}\right].\hfill \end{array}$$

(62)

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{\eta }=W_{\eta }^{-1}{\omega }_B$ in the translational acceleration of Eq. (61) and the rotational angular acceleration of Eq. (57), and defining the generalized coordinates as $X\triangleq {\left[{\xi }^T{\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}$$

(63)

This is because, from Eq. (37),

$$\begin{array}{cc}\hfill \ddot{\eta }& ={\displaystyle \frac{d}{dt}}\left(W_{\eta }^{-1}\right){\omega }_B+W_{\eta }^{-1}{\dot{\omega }}_B\hfill \\ \hfill & ={\displaystyle \frac{d}{dt}}\left(W_{\eta }^{-1}\right)W_{\eta }\dot{\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{\eta }-{\left(I_B{W}_{\eta }\right)}^{-1}[\left(W_{\eta }\dot{\eta }\right)\times I_B\left(W_{\eta }\dot{\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}$$

(64)

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{\eta }-{\left(I_B{W}_{\eta }\right)}^{-1}[\left(W_{\eta }\dot{\eta }\right)\times I_B\left(W_{\eta }\dot{\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 Eq. (63) 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}$$

(65)

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}$$

(66)

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 Eq. (66) for $\ddot{X}$ in Eq. (65) 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}$$

(67)

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

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

(68)

Here, we set $K_p=\mathrm{diag}(K_{p1},...,K_{p6})$ and $K_d=\mathrm{diag}(K_{d1},...,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: 6 generalized forces versus 8 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. [26], 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}$$

(69)

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}$$

(70)

Introducing this interference efficiency allows 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 ${\tau }_c$ from Eq. (54) becomes

$$\begin{array}{cc}\hfill {\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}$$

(71)

Meanwhile, the rotational torque due to thrust ${\tau }_B$ from Eq. (52) becomes

$$\begin{array}{cc}\hfill {\tau }_B& =({\eta }_{if}+1)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\hfill \\ \hfill & -(l{C}_{{\alpha }_2}{C}_{{\beta }_2}-h_o{S}_{{\alpha }_2}){\overline{\omega }}_3^2\hfill \\ \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 \\ \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}$$

(72)

Similarly, the thrust ${\mathit{T}}_B$ from Eq. (60) 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}$$

(73)

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 ${\mathsf{\Omega }}_1\triangleq {\overline{\omega }}_1^2$, ${\mathsf{\Omega }}_2\triangleq {\overline{\omega }}_3^2$, and for $i=1,2$,

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

(74)

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

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

(75)

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}$$

(76)

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 ${\mathsf{\Omega }}_1={\overline{\omega }}_1^2$, ${\mathsf{\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}{\mathsf{\Omega }}_i=\sqrt{n_{ia}^2+n_{ib}^2+n_{ic}^2}\hfill \\ {\alpha }_i=arcsin(n_{ib}/{\mathsf{\Omega }}_i)\hfill \\ {\beta }_i=arctan(n_{ia}/n_{ic})\hfill \end{array}\right.$$

(77)

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

6. Preliminary Simulation: Application to the Falling Body Model

This section describes state estimation for the falling body model [21] as a preliminary experiment. The falling body model, often used as a nonlinear model, was also employed as a simulation model by Takeno and Katayama [9]. The aim here is to compare with previous research by performing state estimation under similar conditions. Since the RK method by Takeno and Katayama is a 4th-order discretization method, this section discusses the 4th-order AB method for comparison.

6.1. Model Overview

The differential equations for the falling body model are as follows [21].

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

(78)

$$\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}$$

(79)

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

Defining the extended state vector as

$$\begin{array}{cc}\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(3-\mathrm{dimensional}\mathrm{state}\mathrm{vector}\right)\hfill \end{array}$$

and 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}$$

(80)

$$\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}$$

(81)

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

(82)

The observation equation is assumed to be nonlinear as shown below:

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

(83)

Here, $M_1$ is the horizontal distance between the falling object and the radar, $M_2$ is the radar height. Also, v is observation noise, which is Gaussian white noise with mean 0 and variance R.

6.2. Estimation Algorithms

We present the time-update equations for the sigma point matrix $\mathcal{X}$ when applying UKF with the Euler, RK, and AB methods. 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".

6.2.1. Euler-UKF

Applying the Euler method to Eqs. (80)–(82) yields the following sigma point time-update equations:

$$\begin{array}{cc}\hfill {\mathcal{X}}_{1,j,k+1}^{-}& ={\mathcal{X}}_{1,j,k}+h{\mathcal{X}}_{2,j,k}\hfill \end{array}$$

(84)

$$\begin{array}{cc}\hfill {\mathcal{X}}_{2,j,k+1}^{-}& ={\mathcal{X}}_{2,j,k}+h\left({\displaystyle \frac{{\rho }_0}{2{\mathcal{X}}_{3,j,k}}}{e}^{-{\mathcal{X}}_{1,j,k}/k_{\rho }}{\mathcal{X}}_{2,j,k}^2-g\right)\hfill \end{array}$$

(85)

$$\begin{array}{cc}\hfill {\mathcal{X}}_{3,j,k+1}^{-}& ={\mathcal{X}}_{3,j,k}\hfill \end{array}$$

(86)

Then, for the time-updated sigma points from Eqs. (84)–(86), the predicted state estimate is obtained by taking the weighted average via Eq. (9), and its covariance matrix via Eq. (10). The observation update proceeds similarly.

6.2.2. RK-UKF

Applying the RK method to Eqs. (80)–(82) and defining

$$\begin{array}{cc}\hfill c_1& ={\mathcal{X}}_{2,j,k}\hfill \end{array}$$

(87)

$$\begin{array}{cc}\hfill q_1& ={\displaystyle \frac{{\rho }_0}{2{\mathcal{X}}_{3,j,k}}}{e}^{-{\mathcal{X}}_{1,j,k}/k_{\rho }}{\mathcal{X}}_{2,j,k}^2-g\hfill \end{array}$$

(88)

$$\begin{array}{cc}\hfill c_2& ={\mathcal{X}}_{2,j,k}+{\displaystyle \frac{h}{2}}{q}_1\hfill \end{array}$$

(89)

$$\begin{array}{cc}\hfill q_2& ={\displaystyle \frac{{\rho }_0}{2{\mathcal{X}}_{3,j,k}}}{e}^{-({\mathcal{X}}_{1,j,k}+h{c}_1/2)/k_{\rho }}{({\mathcal{X}}_{2,j,k}+{\displaystyle \frac{h}{2}}{q}_1)}^2-g\hfill \end{array}$$

(90)

$$\begin{array}{cc}\hfill c_3& ={\mathcal{X}}_{2,j,k}+{\displaystyle \frac{h}{2}}{q}_2\hfill \end{array}$$

(91)

$$\begin{array}{cc}\hfill q_3& ={\displaystyle \frac{{\rho }_0}{2{\mathcal{X}}_{3,j,k}}}{e}^{-({\mathcal{X}}_{1,j,k}+h{c}_2/2)/k_{\rho }}{({\mathcal{X}}_{2,j,k}+{\displaystyle \frac{h}{2}}{q}_2)}^2-g\hfill \end{array}$$

(92)

$$\begin{array}{cc}\hfill c_4& ={\mathcal{X}}_{2,j,k}+h{q}_3\hfill \end{array}$$

(93)

$$\begin{array}{cc}\hfill q_4& ={\displaystyle \frac{{\rho }_0}{2{\mathcal{X}}_{3,j,k}}}{e}^{-({\mathcal{X}}_{1,j,k}+h{c}_3/2)/k_{\rho }}{({\mathcal{X}}_{2,j,k}+{\displaystyle \frac{h}{2}}{q}_3)}^2-g\hfill \end{array}$$

(94)

yields the sigma point time-update equations as follows [9]:

$$\begin{array}{cc}\hfill {\mathcal{X}}_{1,j,k+1}^{-}& ={\mathcal{X}}_{1,j,k}+{\displaystyle \frac{h}{6}}(c_1+2c_2+2c_3+c_4)\hfill \end{array}$$

(95)

$$\begin{array}{cc}\hfill {\mathcal{X}}_{2,j,k+1}^{-}& ={\mathcal{X}}_{2,j,k}+{\displaystyle \frac{h}{6}}(q_1+2q_2+2q_3+q_4)\hfill \end{array}$$

(96)

$$\begin{array}{cc}\hfill {\mathcal{X}}_{3,j,k+1}^{-}& ={\mathcal{X}}_{3,j,k}\hfill \\ \hfill & (j=1,\dots ,7)\hfill \end{array}$$

(97)

Similar to the Euler method, the predicted state estimate and its covariance matrix are obtained from the time-updated sigma points in Eqs. (95)–(97) via Eqs. (9) and (10). The observation update proceeds similarly.

6.2.3. AB-UKF

Combining Eqs. (80)–(82), we redefine the nonlinear dynamics ${\mathit{f}}_c\left(x\right)$ of the continuous-time state equation as

$${\displaystyle \frac{d}{dt}}\left[\begin{array}{c}{x}_1\\ x_2\\ x_3\end{array}\right]=\left[\begin{array}{c}{f}_1\\ f_2\\ f_3\end{array}\right]=\left[\begin{array}{c}{x}_2\\ {\displaystyle \frac{{\rho }_0}{2x_3}}{e}^{-x_1/k_{\rho }}{x}_2^2-g\\ 0\end{array}\right]\triangleq {\mathit{f}}_c\left(x\right).$$

(98)

Then, the sigma point matrix ${\mathcal{X}}_k$ at time k and the matrix ${\mathcal{F}}_c\left({\mathcal{X}}_k\right)$ obtained by substituting ${\mathcal{X}}_k$ into ${\mathit{f}}_c\left(x\right)$ are represented by the following $3\times 7$ matrices:

$$\begin{array}{cc}\hfill {\mathcal{X}}_k& =\left[\begin{array}{ccc}{\mathcal{X}}_{1,1,k}& \dots & {\mathcal{X}}_{1,7,k}\\ {\mathcal{X}}_{2,1,k}& \dots & {\mathcal{X}}_{2,7,k}\\ {\mathcal{X}}_{3,1,k}& \dots & {\mathcal{X}}_{3,7,k}\end{array}\right]\hfill \end{array}$$

(99)

$$\begin{array}{cc}\hfill {\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,7,k}\right)\\ f_2\left({\mathcal{X}}_{2,1,k}\right)& \dots & f_2\left({\mathcal{X}}_{2,7,k}\right)\\ f_3\left({\mathcal{X}}_{3,1,k}\right)& \dots & f_3\left({\mathcal{X}}_{3,7,k}\right)\end{array}\right]\hfill \\ \hfill & =\left[\begin{array}{ccc}{\mathcal{X}}_{2,1,k}& \dots & {\mathcal{X}}_{2,7,k}\\ {\displaystyle \frac{{\rho }_0}{2{\mathcal{X}}_{3,1,k}}}{e}^{-{\mathcal{X}}_{1,1,k}/k_{\rho }}{\mathcal{X}}_{2,1,k}^2-g& \dots & {\displaystyle \frac{{\rho }_0}{2{\mathcal{X}}_{3,7,k}}}{e}^{-{\mathcal{X}}_{1,7,k}/k_{\rho }}{\mathcal{X}}_{2,7,k}^2-g\\ 0& \dots & 0\end{array}\right].\hfill \end{array}$$

(100)

To distinguish this ${\mathcal{F}}_c\left({\mathcal{X}}_k\right)$ specific to the falling body model from the general one in Eq. (26), we redefine the above as ${\mathcal{F}}_c^{fb}\left({\mathcal{X}}_k\right):={\mathcal{F}}_c\left({\mathcal{X}}_k\right)$. Then, the 4th-order AB method yields the sigma point update equation:

$$\begin{array}{cc}\hfill {\mathcal{X}}_{k+1}^{-}& ={\mathcal{X}}_k+{\displaystyle \frac{h}{24}}(55{\mathcal{F}}_c^{fb}\left({\mathcal{X}}_k\right)-59{\mathcal{F}}_c^{fb}\left({\mathcal{X}}_{k-1}\right)\hfill \end{array}$$

(101)

where the superscript $fb$ denotes the falling body model.

6.3. Estimation Conditions

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

$$\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}$$

(102)

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

(103)

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

(104)

The initial state estimate and its covariance matrix are

$$\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}$$

(105)

$$\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}$$

(106)

as used in [9].

6.4. Simulation Results

6.4.1. State Estimation Accuracy

Figure 3 (a) shows the estimation results for the ballistic coefficient $\beta$, and (b) shows the time evolution of the state estimation error calculated using the root mean squared error (RMSE) 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},$$

(107)

for the three methods: Euler-UKF, RK-UKF, and AB-UKF. The results are averages from 100 simulation runs for each method.

Figure 3(a) shows that the parameter $\beta$ converges well for all three methods: Euler-UKF, RK-UKF, and AB-UKF. On the other hand, Figure 3(b) indicates that after the parameter $\beta$ converges as shown in Figure 3(a), the state estimation error is suppressed more effectively in the order of Euler-UKF, RK-UKF, and AB-UKF. Table 1 presents the average RMSE values of the state estimation error from 100 simulation runs for each of the three methods. These computations were performed on a laptop with 16 GB RAM, an AMD Ryzen7 4800H with Radeon Graphics CPU, under Windows 10, using MATLAB 2022a.

As seen in Table 1, the Euler method has the largest RMSE, while the RK and AB methods are smaller than Euler’s. This result is attributed to the difference in discretization order: Euler method is 1st-order accurate, while RK and AB methods are 4th-order accurate.

6.4.2. Computation Time

To more clearly demonstrate the differences between methods, the estimation conditions from Section 6.3 were modified: $h=0.01$ and total sampling points $N=5000$. Table 2 shows the comparison results for the total UKF algorithm computation time (from Eq. (3) to Eq. (19)) and the prediction-only computation time (Eq. (8)).

Table 2 shows that computation time is shortest for the Euler method, followed by the AB method, then the RK method. The prediction time for Euler’s method is particularly short, a result stemming from the difference in discretization order.

6.5. Discussion

The results shown in Section 6.4 can be attributed to the following two potential issues.

1. The falling body model has a small number of state variables.

This is considered the most significant issue. Although the AB method was applied to the falling body model for comparison with prior work, this model has only three state variables, and $x_3$ represents a parameter estimation problem. Consequently, as evident from the sigma point time-update equations, the discretization formulas were actually applied only to state variables $x_1$ and $x_2$, making it a model where differences between the RK and AB methods are less likely to manifest.

2. The UT occupies a small portion of the overall UKF algorithm.

This issue varies with each model, but for this falling body model, the following can be stated. As mentioned, the falling body model had relatively few state variables and simple discretization equations, making it a problem where computational load differences are less pronounced. Therefore, within the overall UKF estimation algorithm, the time spent on UT for this model constituted a small proportion, resulting in less noticeable differences in computation time due to the discretization method.

Considering the above two points, 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

The UAV vehicle conditions are based on the Osprey-type drone with 2-DOF tiltable coaxial rotors by Itakura et al. [26]. 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, $\xi ={\left[xyz\right]}^T$, follows the translational motion equation expressed by Eq. (61) or Eq. (62).

On the other hand, the vehicle’s angular acceleration ${\dot{\omega }}_B={\left[\dot{p}\dot{q}\dot{r}\right]}^T$ is given by Eq. (57) or Eq. (58). 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, Eq. (58) 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]$$

(108)

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 Eqs. (62) and (108) 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}$$

(109)

Here, $A_1\left(x\right),A_2\left(x\right),\dots ,A_9\left(x\right)$ in Eq. (109) 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.$$

(110)

7.2. Estimation Algorithms

Section 7.2.1 to Section 7.2.3 present the time-update equations for sigma points when applying the Euler method, RK method, and AB method, respectively, to the UT within the UAV model, similar to the falling body model.

7.2.1. Euler-Type UKF

Applying the Euler method to Eq. (109) yields the following discrete-time state equation:

$$\begin{array}{cc}\hfill & {\left[\begin{array}{c}{x}_{1,k+1}{x}_{2,k+1}\dots x_{11,k+1}{x}_{12,k+1}\end{array}\right]}^T\hfill \\ \hfill & =\left[\begin{array}{c}{x}_1+h{x}_2\\ x_2+h\left(A_1\left(x\right)T_x+A_2\left(x\right)T_y+A_3\left(x\right)T_z\right)\\ x_3+h{x}_4\\ x_4+h\left(A_4\left(x\right)T_x+A_5\left(x\right)T_y+A_6\left(x\right)T_z\right)\\ x_5+h{x}_6\\ x_6+h\left(A_7\left(x\right)T_x+A_8\left(x\right)T_y+A_9\left(x\right)T_z+g\right)\\ x_7+h{x}_8\\ x_8+h\left({\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)\right)\\ x_9+h{x}_{10}\\ x_{10}+h\left({\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)\right)\\ x_{11}+h{x}_{12}\\ x_{12}+h\left({\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)\right)\end{array}\right]\hfill \end{array}$$

(111)

Therefore, by substituting ${\mathcal{X}}_k$ into Eq. (111), the time-update equation for the sigma points is derived as:

$$\begin{array}{cc}\hfill {\mathcal{X}}_{1,j,k+1}^{-}& ={\mathcal{X}}_{1,j,k}+h{\mathcal{X}}_{2,j,k}\hfill \end{array}$$

(112)

$$\begin{array}{cc}\hfill {\mathcal{X}}_{2,j,k+1}^{-}& ={\mathcal{X}}_{2,j,k}+h\left(A_1\left({\mathcal{X}}_{(9,11),j,k}\right)T_x+A_2\left({\mathcal{X}}_{(7,9,11),j,k}\right)T_y+A_3\left({\mathcal{X}}_{(7,9,11),j,k}\right)T_z\right)\hfill \end{array}$$

(113)

$$\begin{array}{cc}\hfill {\mathcal{X}}_{3,j,k+1}^{-}& ={\mathcal{X}}_{3,j,k}+h{\mathcal{X}}_{4,j,k}\hfill \end{array}$$

(114)

$$\begin{array}{cc}\hfill {\mathcal{X}}_{4,j,k+1}^{-}& ={\mathcal{X}}_{4,j,k}+h\left(A_4\left({\mathcal{X}}_{(9,11),j,k}\right)T_x+A_5\left({\mathcal{X}}_{(7,9,11),j,k}\right)T_y+A_6\left({\mathcal{X}}_{(7,9,11),j,k}\right)T_z\right)\hfill \end{array}$$

(115)

$$\begin{array}{cc}\hfill {\mathcal{X}}_{5,j,k+1}^{-}& ={\mathcal{X}}_{5,j,k}+h{\mathcal{X}}_{6,j,k}\hfill \end{array}$$

(116)

$$\begin{array}{cc}\hfill {\mathcal{X}}_{6,j,k+1}^{-}& ={\mathcal{X}}_{6,j,k}+h\left(A_7\left({\mathcal{X}}_{9,j,k}\right)T_x+A_8\left({\mathcal{X}}_{(7,9),j,k}\right)T_y+A_9\left({\mathcal{X}}_{(7,9),j,k}\right)T_z+g\right)\hfill \end{array}$$

(117)

$$\begin{array}{cc}\hfill {\mathcal{X}}_{7,j,k+1}^{-}& ={\mathcal{X}}_{7,j,k}+h{\mathcal{X}}_{8,j,k}\hfill \end{array}$$

(118)

$$\begin{array}{cc}\hfill {\mathcal{X}}_{8,j,k+1}^{-}& ={\mathcal{X}}_{8,j,k}+h\left({\displaystyle \frac{1}{I_{Bxx}}}({\tau }_x^B+{\tau }_x^c)+{\mathcal{X}}_{10,j,k}{\mathcal{X}}_{12,j,k}\left({\displaystyle \frac{I_{Byy}-I_{Bzz}}{I_{Bxx}}}\right)\right)\hfill \end{array}$$

(119)

$$\begin{array}{cc}\hfill {\mathcal{X}}_{9,j,k+1}^{-}& ={\mathcal{X}}_{9,j,k}+h{\mathcal{X}}_{10,j,k}\hfill \end{array}$$

(120)

$$\begin{array}{cc}\hfill {\mathcal{X}}_{10,j,k+1}^{-}& ={\mathcal{X}}_{10,j,k}+h\left({\displaystyle \frac{1}{I_{Byy}}}({\tau }_y^B+{\tau }_y^c)+{\mathcal{X}}_{8,j,k}{\mathcal{X}}_{12,j,k}\left({\displaystyle \frac{I_{Bzz}-I_{Bxx}}{I_{Byy}}}\right)\right)\hfill \end{array}$$

(121)

$$\begin{array}{cc}\hfill {\mathcal{X}}_{11,j,k+1}^{-}& ={\mathcal{X}}_{11,j,k}+h{\mathcal{X}}_{12,j,k}\hfill \end{array}$$

(122)

$$\begin{array}{cc}\hfill {\mathcal{X}}_{12,j,k+1}^{-}& ={\mathcal{X}}_{12,j,k}+h\left({\displaystyle \frac{1}{I_{Bzz}}}({\tau }_z^B+{\tau }_z^c)+{\mathcal{X}}_{8,j,k}{\mathcal{X}}_{10,j,k}\left({\displaystyle \frac{I_{Bxx}-I_{Byy}}{I_{Bzz}}}\right)\right)\hfill \end{array}$$

(123)

7.2.2. RK-Type UKF

First, regarding Eq. (109), define ${\mathit{f}}_c={[f_1\left(x\right)f_2\left(x\right)\dots f_{12}\left(x\right)]}^T$. Then, define the derivatives $a_1\left(x\right)$–$a_4\left(x\right)$,…,$l_1\left(x\right)$–$l_4\left(x\right)$ for each state variable as follows. Note that some equations are omitted due to their considerable number.

$$\begin{array}{cc}\hfill a_1\left(x\right)& =f_1\left(x_2\right)\hfill \end{array}$$

(124)

$$\begin{array}{cc}\hfill a_2\left(x\right)& =f_1(x_2+{\displaystyle \frac{b_1}{2}})\hfill \end{array}$$

(125)

$$\begin{array}{cc}\hfill a_3\left(x\right)& =f_1(x_2+{\displaystyle \frac{b_2}{2}})\hfill \end{array}$$

(126)

$$\begin{array}{cc}\hfill a_4\left(x\right)& =f_1(x_2+b_3)\hfill \\ \hfill ⋮\end{array}$$

(127)

$$\begin{array}{cc}\hfill l_1\left(x\right)& =f_{12}(x_8,x_{10})\hfill \end{array}$$

(128)

$$\begin{array}{cc}\hfill l_2\left(x\right)& =f_{12}(x_8+{\displaystyle \frac{h_1}{2}},x_{10}+{\displaystyle \frac{j_1}{2}})\hfill \end{array}$$

(129)

$$\begin{array}{cc}\hfill l_3\left(x\right)& =f_{12}(x_8+{\displaystyle \frac{h_2}{2}},x_{10}+{\displaystyle \frac{j_2}{2}})\hfill \end{array}$$

(130)

$$\begin{array}{cc}\hfill l_4\left(x\right)& =f_{12}(x_8+h_3,x_{10}+j_3)\hfill \end{array}$$

(131)

Thus, applying the RK method to Eq. (109) yields the discrete-time state equation using $a_1\left(x\right)$–$a_4\left(x\right)$, …,$l_1\left(x\right)$–$l_4\left(x\right)$:

$$\begin{array}{cc}\hfill & {\left[\begin{array}{c}{x}_{1,k+1}{x}_{2,k+1}\dots x_{11,k+1}{x}_{12,k+1}\end{array}\right]}^T\hfill \\ \hfill & =\left[\begin{array}{c}{x}_{1,k}+{\displaystyle \frac{h}{6}}\left(a_1\left(x\right)+2a_2\left(x\right)+2a_3\left(x\right)+a_4\left(x\right)\right)\\ x_{2,k}+{\displaystyle \frac{h}{6}}\left(b_1\left(x\right)+2b_2\left(x\right)+2b_3\left(x\right)+b_4\left(x\right)\right)\\ x_{3,k}+{\displaystyle \frac{h}{6}}\left(c_1\left(x\right)+2c_2\left(x\right)+2c_3\left(x\right)+c_4\left(x\right)\right)\\ x_{4,k}+{\displaystyle \frac{h}{6}}\left(d_1\left(x\right)+2d_2\left(x\right)+2d_3\left(x\right)+d_4\left(x\right)\right)\\ x_{5,k}+{\displaystyle \frac{h}{6}}\left(e_1\left(x\right)+2e_2\left(x\right)+2e_3\left(x\right)+e_4\left(x\right)\right)\\ x_{6,k}+{\displaystyle \frac{h}{6}}\left(f_1\left(x\right)+2f_2\left(x\right)+2f_3\left(x\right)+f_4\left(x\right)\right)\\ x_{7,k}+{\displaystyle \frac{h}{6}}\left(g_1\left(x\right)+2g_2\left(x\right)+2g_3\left(x\right)+g_4\left(x\right)\right)\\ x_{8,k}+{\displaystyle \frac{h}{6}}\left(h_1\left(x\right)+2h_2\left(x\right)+2h_3\left(x\right)+h_4\left(x\right)\right)\\ x_{9,k}+{\displaystyle \frac{h}{6}}\left(i_1\left(x\right)+2i_2\left(x\right)+2i_3\left(x\right)+i_4\left(x\right)\right)\\ x_{10,k}+{\displaystyle \frac{h}{6}}\left(j_1\left(x\right)+2j_2\left(x\right)+2j_3\left(x\right)+j_4\left(x\right)\right)\\ x_{11,k}+{\displaystyle \frac{h}{6}}\left(k_1\left(x\right)+2k_2\left(x\right)+2k_3\left(x\right)+k_4\left(x\right)\right)\\ x_{12,k}+{\displaystyle \frac{h}{6}}\left(l_1\left(x\right)+2l_2\left(x\right)+2l_3\left(x\right)+l_4\left(x\right)\right)\end{array}\right]\hfill \end{array}$$

(132)

Therefore, by substituting ${\mathcal{X}}_k$ into Eq. (111), the time-update equation for the sigma points is derived as:

$$\begin{array}{cc}\hfill {\mathcal{X}}_{1,j,k+1}^{-}& ={\mathcal{X}}_{1,j,k}+{\displaystyle \frac{h}{6}}\left(a_1\left({\mathcal{X}}_{2,j,k}\right)+2a_2\left({\mathcal{X}}_{2,j,k}\right)+2a_3\left({\mathcal{X}}_{2,j,k}\right)+a_4\left({\mathcal{X}}_{2,j,k}\right)\right)\hfill \end{array}$$

(133)

$$\begin{array}{cc}\hfill {\mathcal{X}}_{2,j,k+1}^{-}& ={\mathcal{X}}_{2,j,k}+{\displaystyle \frac{h}{6}}\left(b_1\left({\mathcal{X}}_{(7,9,11),j,k}\right)+2b_2\left({\mathcal{X}}_{(7,9,11),j,k}\right)+2b_3\left({\mathcal{X}}_{(7,9,11),j,k}\right)+b_4\left({\mathcal{X}}_{(7,9,11),j,k}\right)\right)\hfill \end{array}$$

(134)

$$\begin{array}{cc}\hfill {\mathcal{X}}_{3,j,k+1}^{-}& ={\mathcal{X}}_{3,j,k}+{\displaystyle \frac{h}{6}}\left(c_1\left({\mathcal{X}}_{4,j,k}\right)+2c_2\left({\mathcal{X}}_{4,j,k}\right)+2c_3\left({\mathcal{X}}_{4,j,k}\right)+c_4\left({\mathcal{X}}_{2,j,k}\right)\right)\hfill \end{array}$$

(135)

$$\begin{array}{cc}\hfill {\mathcal{X}}_{4,j,k+1}^{-}& ={\mathcal{X}}_{4,j,k}+{\displaystyle \frac{h}{6}}\left(d_1\left({\mathcal{X}}_{(7,9,11),j,k}\right)+2d_2\left({\mathcal{X}}_{(7,9,11),j,k}\right)+2d_3\left({\mathcal{X}}_{(7,9,11),j,k}\right)+d_4\left({\mathcal{X}}_{(7,9,11),j,k}\right)\right)\hfill \end{array}$$

(136)

$$\begin{array}{cc}\hfill {\mathcal{X}}_{5,j,k+1}^{-}& ={\mathcal{X}}_{5,j,k}+{\displaystyle \frac{h}{6}}\left(e_1\left({\mathcal{X}}_{6,j,k}\right)+2e_2\left({\mathcal{X}}_{6,j,k}\right)+2e_3\left({\mathcal{X}}_{6,j,k}\right)+e_4\left({\mathcal{X}}_{6,j,k}\right)\right)\hfill \end{array}$$

(137)

$$\begin{array}{cc}\hfill {\mathcal{X}}_{6,j,k+1}^{-}& ={\mathcal{X}}_{6,j,k}+{\displaystyle \frac{h}{6}}\left(f_1\left({\mathcal{X}}_{(7,9,11),j,k}\right)+2f_2\left({\mathcal{X}}_{(7,9,11),j,k}\right)+2f_3\left({\mathcal{X}}_{(7,9,11),j,k}\right)+f_4\left({\mathcal{X}}_{(7,9,11),j,k}\right)\right)\hfill \end{array}$$

(138)

$$\begin{array}{cc}\hfill {\mathcal{X}}_{7,j,k+1}^{-}& ={\mathcal{X}}_{7,j,k}+{\displaystyle \frac{h}{6}}\left(g_1\left({\mathcal{X}}_{8,j,k}\right)+2g_2\left({\mathcal{X}}_{8,j,k}\right)+2g_3\left({\mathcal{X}}_{8,j,k}\right)+g_4\left({\mathcal{X}}_{8,j,k}\right)\right)\hfill \end{array}$$

(139)

$$\begin{array}{cc}\hfill {\mathcal{X}}_{8,j,k+1}^{-}& ={\mathcal{X}}_{8,j,k}+{\displaystyle \frac{h}{6}}\left(l_1\left({\mathcal{X}}_{(10,12),j,k}\right)+2l_2\left({\mathcal{X}}_{(10,12),j,k}\right)+2l_3\left({\mathcal{X}}_{(10,12),j,k}\right)+l_4\left({\mathcal{X}}_{(10,12),j,k}\right)\right)\hfill \end{array}$$

(140)

$$\begin{array}{cc}\hfill {\mathcal{X}}_{9,j,k+1}^{-}& ={\mathcal{X}}_{9,j,k}+{\displaystyle \frac{h}{6}}\left(i_1\left({\mathcal{X}}_{10,j,k}\right)+2i_2\left({\mathcal{X}}_{10,j,k}\right)+2i_3\left({\mathcal{X}}_{10,j,k}\right)+i_4\left({\mathcal{X}}_{10,j,k}\right)\right)\hfill \end{array}$$

(141)

$$\begin{array}{cc}\hfill {\mathcal{X}}_{10,j,k+1}^{-}& ={\mathcal{X}}_{10,j,k}+{\displaystyle \frac{h}{6}}\left(j_1\left({\mathcal{X}}_{(8,12),j,k}\right)+2j_2\left({\mathcal{X}}_{(8,12),j,k}\right)+2j_3\left({\mathcal{X}}_{(8,12),j,k}\right)+j_4\left({\mathcal{X}}_{(8,12),j,k}\right)\right)\hfill \end{array}$$

(142)

$$\begin{array}{cc}\hfill {\mathcal{X}}_{11,j,k+1}^{-}& ={\mathcal{X}}_{11,j,k}+{\displaystyle \frac{h}{6}}\left(k_1\left({\mathcal{X}}_{12,j,k}\right)+2k_2\left({\mathcal{X}}_{12,j,k}\right)+2k_3\left({\mathcal{X}}_{12,j,k}\right)+k_4\left({\mathcal{X}}_{12,j,k}\right)\right)\hfill \end{array}$$

(143)

$$\begin{array}{cc}\hfill {\mathcal{X}}_{12,j,k+1}^{-}& ={\mathcal{X}}_{12,j,k}+{\displaystyle \frac{h}{6}}\left(l_1\left({\mathcal{X}}_{(8,10),j,k}\right)+2l_2\left({\mathcal{X}}_{(8,10),j,k}\right)+2l_3\left({\mathcal{X}}_{(8,10),j,k}\right)+l_4\left({\mathcal{X}}_{(8,10),j,k}\right)\right)\hfill \end{array}$$

(144)

The derivatives are as follows (similarly, some are omitted):

$$\begin{array}{cc}\hfill a_1\left({\mathcal{X}}_{2,j,k}\right)& =f_1\left({\mathcal{X}}_{2,j,k}\right)\hfill \end{array}$$

(145)

$$\begin{array}{cc}\hfill a_2\left({\mathcal{X}}_{2,j,k}\right)& =f_1({\mathcal{X}}_{2,j,k}+{\displaystyle \frac{b_1}{2}})\hfill \end{array}$$

(146)

$$\begin{array}{cc}\hfill a_3\left({\mathcal{X}}_{2,j,k}\right)& =f_1({\mathcal{X}}_{2,j,k}+{\displaystyle \frac{b_2}{2}})\hfill \end{array}$$

(147)

$$\begin{array}{cc}\hfill a_4\left({\mathcal{X}}_{2,j,k}\right)& =f_1({\mathcal{X}}_{2,j,k}+b_3)\hfill \\ \hfill ⋮\end{array}$$

(148)

$$\begin{array}{cc}\hfill l_1\left({\mathcal{X}}_{(8,10),j,k}\right)& =f_{12}({\mathcal{X}}_{8,j,k},{\mathcal{X}}_{10,j,k},)\hfill \end{array}$$

(149)

$$\begin{array}{cc}\hfill l_2\left({\mathcal{X}}_{(8,10),j,k}\right)& =f_{12}({\mathcal{X}}_{8,j,k},+{\displaystyle \frac{h_1}{2}},{\mathcal{X}}_{10,j,k}+{\displaystyle \frac{j_1}{2}}),\hfill \end{array}$$

(150)

$$\begin{array}{cc}\hfill l_3\left({\mathcal{X}}_{(8,10),j,k}\right)& =f_{12}({\mathcal{X}}_{8,j,k},+{\displaystyle \frac{h_2}{2}},{\mathcal{X}}_{10,j,k}+{\displaystyle \frac{j_2}{2}}),\hfill \end{array}$$

(151)

$$\begin{array}{cc}\hfill l_4\left({\mathcal{X}}_{(8,10),j,k}\right)& =f_{12}({\mathcal{X}}_{8,j,k},+h_3,{\mathcal{X}}_{10,j,k}+j_3)\hfill \end{array}$$

(152)

7.2.3. AB-Type UKF

First, the sigma point matrix ${\mathcal{X}}_k$ at time k and the matrix ${\mathcal{F}}_c\left({\mathcal{X}}_k\right)$ obtained by substituting ${\mathcal{X}}_k$ into Eq. (109) are represented by the following $12\times 25$ matrix.

$$\begin{array}{cc}\hfill {\mathcal{X}}_k& =\left[\begin{array}{ccc}{\mathcal{X}}_{1,1,k}& \dots & {\mathcal{X}}_{1,25,k}\\ ⋮& \ddots & ⋮\\ {\mathcal{X}}_{12,1,k}& \dots & {\mathcal{X}}_{12,25,k}\end{array}\right]\hfill \\ \hfill \end{array}$$

(153)

$$\begin{array}{cc}\hfill {\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,25,k}\right)\\ ⋮& \ddots & ⋮\\ f_{12}\left({\mathcal{X}}_{12,1,k}\right)& \dots & f_{12}\left({\mathcal{X}}_{12,25,k}\right)\end{array}\right]\hfill \end{array}$$

(154)

Here, similar to Eq. (100), to distinguish this from the general ${\mathcal{F}}_c\left({\mathcal{X}}_k\right)$ in Eq. (26), we redefine the above as ${\mathcal{F}}_c^U\left({\mathcal{X}}_k\right):={\mathcal{F}}_c\left({\mathcal{X}}_k\right)$. Then, the sigma point update equations using the 2nd to 6th-order AB methods for the UAV model are expressed as follows.

• Sigma point update equation using the 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^U\left({\mathcal{X}}_k\right)-{\mathcal{F}}_c^U\left({\mathcal{X}}_{k-1}\right)\right)\end{array}$$

  (155)
• Sigma point update equation using the 3rd-order Adams-Bashforth method:

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

  (156)

• Sigma point update equation using the 4th-order Adams-Bashforth method:

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

  (157)
• Sigma point update equation using the 5th-order Adams-Bashforth method:

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

  (158)

• Sigma point update equation using the 6th-order Adams-Bashforth method:

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

  (159)

Here, the superscript U denotes the UAV model.

7.3. Description of Estimation Conditions

This section describes the UAV’s physical parameters and the covariance of system and observation noise used in the estimation simulation, based on the UAV model and each estimation algorithm presented in Section 7.1 and Section 7.2.

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. First, Table 3 summarizes the definitions and values of each parameter.

All initial states are set to $\mathit{x}\left(0\right)={[0...0]}^T$, and the initial estimation information is set to $\widehat{\mathit{x}}\left(0\right|0)={[0...0]}^T$ and $P\left(0\right|0)=\mathrm{diag}(1,...,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 RK and 4th-order AB 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}$$

(160)

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

(161)

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}$$

(162)

Here, $5/2$ in the above equation 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$ [${\mathrm{rad}}^2$] for attitude angles $\varphi ,\theta ,\psi$ and $R_2$ [${\mathrm{rad}}^2{\mathrm{/s}}^2$] 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}}$].

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

(163)

Squaring the value obtained in Eq. (163) yields:

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

(164)

Here, the dimension [Hz] can also be expressed as [1/s]; thus, the dimension [${\mathrm{rad}}^2$/Hz] can also be expressed as [rad${}^2·$ s]. Therefore, the observation noise variance $R_1$ [${\mathrm{rad}}^2$] for attitude angles $\varphi ,\theta ,\psi$ is obtained by dividing Eq. (164) 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}$$

(165)

Then, the observation noise variance $R_2$ [${\mathrm{rad}}^2{\mathrm{/s}}^2$] for attitude angular velocities $\dot{\varphi },\dot{\theta },\dot{\psi }$ is obtained by dividing Eq. (165) 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}$$

(166)

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}$$

(167)

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

(168)

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}$$

(169)

and 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}\left(222R_1{R}_2{R}_1{R}_2{R}_1{R}_2\right).\end{array}$$

(170)

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 [27].

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

$$\begin{array}{cc}\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]}^T\hfill \\ \hfill & =\left[\begin{array}{c}sin({\displaystyle \frac{\pi }{10}}·(t-10))\\ cos\left({\displaystyle \frac{\pi }{4}}\right)\left(-1+cos({\displaystyle \frac{\pi }{10}}·(t-10))\right)\\ -1+sin\left({\displaystyle \frac{\pi }{4}}\right)\left(-1+cos({\displaystyle \frac{\pi }{10}}·(t-10))\right)\\ {\displaystyle \frac{\pi }{4}}·sin({\displaystyle \frac{\pi }{40}}·(t-10))\\ -{\displaystyle \frac{\pi }{4}}·sin({\displaystyle \frac{\pi }{40}}·(t-10))\\ 0\end{array}\right]\hfill \end{array}$$

(171)

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

$${\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({\displaystyle \frac{\pi }{10}}·(t-10))\\ -{\displaystyle \frac{\pi }{10}}cos\left({\displaystyle \frac{\pi }{4}}\right)sin({\displaystyle \frac{\pi }{10}}·(t-10))\\ -{\displaystyle \frac{\pi }{10}}sin\left({\displaystyle \frac{\pi }{4}}\right)sin({\displaystyle \frac{\pi }{10}}·(t-10))\\ {\displaystyle \frac{{\pi }^2}{160}}·cos({\displaystyle \frac{\pi }{40}}·(t-10))\\ -{\displaystyle \frac{{\pi }^2}{160}}·cos({\displaystyle \frac{\pi }{40}}·(t-10))\\ 0\end{array}\right]$$

(172)

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

$${\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({\displaystyle \frac{\pi }{10}}·(t-10))\\ -{\displaystyle \frac{{\pi }^2}{100}}cos\left({\displaystyle \frac{\pi }{4}}\right)cos({\displaystyle \frac{\pi }{10}}·(t-10))\\ -{\displaystyle \frac{{\pi }^2}{100}}sin\left({\displaystyle \frac{\pi }{4}}\right)cos({\displaystyle \frac{\pi }{10}}·(t-10))\\ -{\displaystyle \frac{{\pi }^3}{6400}}·sin({\displaystyle \frac{\pi }{40}}·(t-10))\\ {\displaystyle \frac{{\pi }^3}{6400}}·sin({\displaystyle \frac{\pi }{40}}·(t-10))\\ 0\end{array}\right]$$

(173)

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, while 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, RK, and 4th-order AB” 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. Here, N is the total number of sampling points, determined from the simulation time of 70 s and the discretization (i.e., sampling) period h [s] as $N=70/h$. For example, if $h=0.01$ [s], then $N=7000$.

$$\begin{array}{cc}\hfill MS{E}_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}$$

(174)

$$\begin{array}{cc}\hfill RMS{E}_x\left(k\right)& =\sqrt{MS{E}_x\left(k\right)}\hfill \end{array}$$

(175)

Here, $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. Equations (174) and (175) 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 $RMS{E}_x\left(N\right),RMS{E}_y\left(N\right),\dots ,RMS{E}_{\dot{\psi }}\left(N\right)$ for the 12 state variables at the final discrete time $k=N$ is denoted as $RMSE\left(N\right)$, and this value is used for comparison.

Here, we discuss the theoretical meaning of $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}$$

(176)

the trace of this error covariance matrix is denoted as $\mathrm{tr}\left(P\left(t_k\right|t_k)\right)$. The aforementioned $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 Eq. (8))
• Measurement Time II: Computation time for the UKF algorithm (computation from Eq. (3) to Eq. (19))
• 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.

7.6. Results and Discussion of Estimation Accuracy

7.6.1. Comparison of Three Methods: Euler, RK, and AB (4th Order)

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.

Next, estimation accuracy is compared under different discretization periods: $h=0.01,0.02,\dots ,0.1$. Figure 7 shows the change in $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 $RMSE\left(N\right)$ value for the RK method is the smallest. Furthermore, for the 4th-order AB method, estimation values could not be obtained (diverged) for h values greater than 0.08. Details regarding this are discussed later.

The specific values for each point in Figure 7 are shown in Table 4. The “N/A” in Table 4 indicates that estimation values could not be obtained due to divergence.

7.6.2. 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 2nd to 6th-order AB methods for $h=0.01$. From Figure 8, under the condition $h=0.01$, all AB methods from 2nd to 6th 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 $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 6th-order AB method diverged for h greater than 0.03, the 5th-order for h greater than 0.05, and the 4th-order for h greater than 0.08, preventing estimation value acquisition.

The specific values for each point in Figure 9 are shown in Table 5.

Additionally, for the 2nd and 3rd-order AB methods, further verification confirmed that they similarly diverge for values greater than $h=0.25$ and $h=0.15$, respectively.

7.7. Results and Discussion of Computational Efficiency

7.7.1. Comparison of Three Methods: Euler, RK, and AB (4th Order)

Table 6 shows the computation times for Time I, Time II, and Time III for the three methods (Euler, RK, and 4th-order AB) with step size $h=0.01$.

From Table 6, the reduction rate in computation time for the 4th-order AB method is 12.276% compared to Euler and 41.119% compared to RK for Time I; 7.063% compared to Euler and 29.876% compared to RK for Time II; and 5.072% compared to Euler and 20.870% compared to RK 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. 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 4th-order AB method. The result is given in Table 7. 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. Conclusion

This paper proposed a UKF that newly incorporates the AB method instead of the RK method into the sigma point time-update equation, maintaining estimation accuracy comparable to the RK method while enabling estimation with more efficient computation time. This is an alternative to the state estimation method using UKF with the RK method adapted to the sigma point time-update equation, as proposed by Takeno et al. [9]. First, we reviewed the UKF, a type of nonlinear filter, and presented its algorithm. Next, we explained that both the RK and AB methods can be applied to the UT and presented the sigma point time-update equations based on these two methods plus the Euler method. For the actual verification of estimation performance, we focused on the falling body model used by Takeno et al. as a preliminary experiment and the UAV model as the main subject. We detailed the derivation of sigma point time-update equations when applying UKF with three discretization methods (Euler, RK, AB) to each model. Numerical simulation results demonstrated that for both models, the proposed method maintains estimation accuracy comparable to the RK method while enabling more efficient computation. Notably, the improvement in computational efficiency was more pronounced for the UAV model with more complex state equations.

Future work includes implementing the proposed algorithm on a microcontroller in a real vehicle environment, starting with the UAV model used in this state estimation simulation, to verify the actual degree of improvement in estimation accuracy and computational efficiency. Also, as mentioned earlier, while the standard UKF prepares $(2n+1)$ sigma points to compute estimates, methods using fewer sigma points have already been proposed. For example, by using the Spherical Simplex Unscented Transformation [28,29,31,32] or the Simplex Unscented Transformation [30], the number of sigma points can be reduced to $n+2$ or $n+1$, constituting a CD-UKF that can reduce computational load. Furthermore, applying the proposed method to the CD Derivative-Free EKF [4,33,35], which uses n sample points, is also an interesting challenge. Combining these methods with the AB method is expected to lead to the development of UKF or EKF methods in continuous-discrete environments that offer improved computational efficiency with reasonable processing speed while maintaining a certain level of computational accuracy.