A Novel High-Precision Trajectory Generator with Complex Motion Simulation for Enhanced Inertial Navigation Algorithm Testing

Abstract

With advancements in inertial navigation measurement units (IMUs), the focus of inertial navigation accuracy has shifted from hardware limitations to algorithm performance. To effectively test algorithms using high-precision IMUs, trajectory generators are essential; however, existing generators often lack diverse motion patterns, making them inadequate for evaluating algorithms under complex and challenging conditions, particularly for unmanned aerial vehicle (UAV) applications. To address this, we designed a high-precision trajectory generator that enhances traditional models by incorporating coning motion, paddle motion, and coning motion with angular velocity precession to simulate high-dynamic environments. Additionally, a one-sample-plus-one-previous-per-update algorithm was developed to improve the accuracy of the IMU output inversion by generating precise gyroscope and accelerometer data for processing within inertial navigation systems. The results demonstrate that both the one-sample-plus-one-previous-per-update and three-sample algorithms significantly improve the navigation accuracy under high-dynamic motion compared to single-sample algorithms. This trajectory generator effectively validates the accuracy of inertial navigation algorithms in complex conditions, particularly for UAVs, and provides a solid foundation for testing higher-precision algorithms.The proposed methodology directly supports the development of advanced actuator control systems in UAVs by enabling rigorous validation of navigation algorithms under realistic high-dynamic scenarios, a critical requirement for next-generation autonomous platforms.

1. Introduction

During the motion of a carrier, the navigation system is the core tool for obtaining key information such as the position, attitude, and velocity. Among these, inertial navigation has become an essential component of various carrier navigation systems due to its independence, non-reliance on external information, and resistance to electromagnetic interference. For UAVs, inertial navigation plays a crucial role in enabling accurate and continuous positioning and motion tracking in environments where external signals such as GPS may be unavailable or unreliable. UAVs, operating in diverse and often complex environments such as high altitudes, urban canyons, or remote areas, face unique challenges due to the potential loss of satellite signals, rapid motion dynamics, and the need for precise control and stability. Inertial navigation, particularly when combined with high-precision sensors, provides a reliable solution for UAVs to navigate autonomously and accurately, ensuring mission success in tasks such as aerial mapping, surveillance, disaster response, and environmental monitoring [1,2,3,4]. With the development of next-generation inertial measurement units (IMUs), especially the application of atomic accelerometers and atomic gyroscopes, the hardware measurement accuracy of inertial navigation systems has significantly improved [5,6]. However, the improvement in hardware accuracy has raised higher demands for navigation algorithms. According to the literature [7], the algorithm error in inertial navigation must be controlled within 5% of the hardware error, but the main source of error in current inertial navigation systems has shifted from hardware limitations to algorithmic errors.

In recent years, several researchers have proposed higher-precision inertial navigation algorithms. For example, reference [8] presents an improved high-precision attitude-compensation algorithm based on Taylor series expansion, which addresses noncommutativity errors and simplifies angular increment terms, demonstrating higher accuracy in both pure coning and high-dynamic environments. Reference [9] proposes an improved high-order numerical algorithm for attitude compensation in strapdown inertial navigation systems, which reduces the noncommutative error and improves the attitude update accuracy, particularly in coning motion and high-dynamic conditions. Reference [10] introduces a new rotation vector algorithm that compensates for non-commutativity errors and enhances attitude computation accuracy without increasing the gyro output frequency, outperforming both classical and optimal algorithms in high and low dynamic environments. These advancements highlight the critical need for robust trajectory generators to validate high-precision algorithms, especially in high-dynamic scenarios relevant to UAVs and other autonomous systems. The development of such tools not only supports the advancement of navigation algorithms but also contributes to the broader field of actuator-driven control systems, where precise motion simulation and validation are essential for ensuring system reliability and performance in real-world applications. In conclusion, current research on high-precision inertial navigation systems focuses on improving algorithm performance in high-dynamic environments. In practical UAV applications, factors such as rapid maneuvers, high-speed flight, attitude changes, and airflow disturbances frequently occur [11], placing higher demands on inertial navigation algorithms.

However, due to the fact that high-precision IMU components are still under development and have not yet been commercially scaled, practical testing in the early stages of high-precision inertial navigation system research is difficult to carry out. To address this issue, it is necessary to design and use a trajectory generator to simulate the gyroscope and accelerometer outputs under different motion conditions, generating test data to provide the necessary support for the verification of inertial navigation algorithms [12,13]. This method provides an important way to research high-precision inertial navigation algorithms and effectively compensates for the shortcomings of practical experiments.

The trajectory generator primarily predefines the motion states of the carrier, such as stationary, constant velocity, accelerated motion, turning, and other typical motion states, which are then combined and stitched together to form the final trajectory [14,15,16]. Based on the characteristics of different motion states, the ideal attitude, velocity, and position (AVP) are calculated, and from the AVP parameters, the angular velocity and specific force information of the IMU output are derived. After adding IMU errors, the input for the inertial navigation system is obtained for inertial navigation computation. Currently, there has been considerable research on trajectory generators, which can be classified based on the vehicle types into three categories: land vehicles, ships, and aircraft. Compared to other vehicles, aircraft have the most complex maneuvering modes. As the most commonly used trajectory generator, aircraft trajectory generators can be simplified to obtain the ideal trajectories for land vehicles and ships. Therefore, most trajectory generator research focuses on aircraft [17]. For example, in references [18,19], aircraft are used as the vehicle type, and several basic movements are combined to generate simulation trajectories, achieving the basic functionality of the trajectory generator. However, the inversion of gyroscope and accelerometer outputs is considered too simply, which reduces the precision of the IMU output. References [20,21] consider the actual movement states of vehicles and ships, taking into account environmental influences on the trajectory generator. These studies provide high accuracy and better reflect real-world conditions, but they have fewer motion modes compared to aircraft. Reference [22] added conical motion and paddling motion to the aforementioned movements, However, these motions only served to expand the range of motion types and did not improve the accuracy of the inversion algorithms. Additionally, no further analysis was conducted on the significance of high-dynamic motion for trajectory generators. For strapdown inertial navigation systems, the algorithm’s accuracy greatly affects the precision of the navigation solution in high-dynamic environments, especially for attitude estimation algorithms [23]. In summary, the current research into and design of trajectory generators face several issues, such as limited motion types, predominantly focusing on low-dynamic motion and lacking high-dynamic motion. When high-dynamic motion is included, the corresponding inversion algorithm accuracy remains insufficient, and there has been no verification or analysis of inertial navigation algorithms. These problems require further study and resolution.

To address this issue, this paper builds upon the traditional motion foundation of trajectory generators and introduces conical motion and paddling motion, as referenced in [22]. Additionally, it proposes the use of conical motion with angular velocity precession as a high-dynamic motion mode. To improve the precision of the trajectory generator, this paper further references [15], where the inversion algorithm incorporates the one-sample-plus-one-previous-per-update algorithm to account for conical errors and paddling errors, obtaining angular velocity and specific force outputs. This method replaces the traditional Euler angle approach and specific force equations used to directly calculate IMU outputs.

The structure of this paper is organized as follows (a brief flowchart is shown in Figure 1): Section 2 presents the different motion modes of the designed trajectory generator and their corresponding AVP outputs. Section 3 analyzes and derives the IMU inversion algorithm for obtaining IMU outputs under different motion modes. In Section 4, combined motion trajectories are generated, and various inertial navigation algorithms are used to solve the trajectories output by the trajectory generator, validating its effectiveness. Section 5 discusses the application scenarios and significance of this design, and envisions its future applications. Finally, Section 6 summarizes the work presented and outlines future research directions.

2. Trajectory Control Equation

To facilitate the description, a simple introduction to the coordinate system used in this design is provided. The body coordinate system is rigidly connected to the UAVs, with the origin located at the center of the UAVs. The x-axis points to the right side of the UAVs, the y-axis follows the UAVs’s longitudinal axis and points forward, and the z-axis points upwards in accordance with the right-hand rule. The navigation coordinate system used is the East-North-Up (ENU) geographic coordinate system.

For ease of description, the coordinate systems used in this design are briefly introduced as follows:

• Inertial Coordinate System (i-frame): The origin is at the Earth’s center of mass. The z-axis points toward the Earth’s rotation axis, the x-axis lies in the equatorial plane and points toward the vernal equinox, and the y-axis completes the right-handed coordinate system with the other two axes. This coordinate system does not rotate with the Earth’s rotation.
• Earth Coordinate System (e-frame): The origin is also at the Earth’s center of mass. The x-axis extends through the intersection of the prime meridian (0° longitude) and the equator, the z-axis extends through the North Pole (coincident with the Earth’s rotation axis), and the y-axis forms a right-handed coordinate system with the other two axes.
• Body Coordinate System (b-frame): This system is fixed to the vehicle, with the origin located at the center of the vehicle. The x-axis points to the right of the vehicle, the y-axis follows the longitudinal axis and points forward, and the z-axis points upward according to the right-hand rule. A schematic diagram of the body coordinate system is shown in Figure 2.
• Navigation Coordinate System: The North-East-Up (ENU) geographical coordinate system is chosen for the navigation frame.

The designed trajectory motion in this paper includes basic motion types such as stationary, uniform motion, accelerated motion, and turning motion, as well as newly introduced conical motion, paddle motion, and conical motion with angular velocity precession. The following sections introduce each of these motion types along with their corresponding acceleration and angular velocity control equations.

2.1. Stationary/Uniform Motion

During this motion, the UAV is either stationary or moves with uniform velocity, with the attitude angles and acceleration remaining constant. That is, the pitch angle, yaw angle, and roll angle are all zero, and the acceleration is also zero. The stationary or uniform motion of the UAVs depends on the velocity from the previous time segment. If the velocity in the previous segment is zero, the current segment will be stationary. If the velocity in the previous segment is non-zero, the current segment’s velocity will be the same as the previous segment’s velocity.

The control equations for the UAVs are shown in Equation (1):

$$\begin{array}{cc}\hfill & \omega ={\left[\begin{array}{ccc}\dot{\theta }& \dot{\gamma }& \dot{\psi }\end{array}\right]}^T=0\hfill \\ \hfill & a={\left[\begin{array}{ccc}{a}_x^b& a_y^b& a_z^b\end{array}\right]}^T=0,\hfill \end{array}$$

(1)

where the variables $\omega$, a, $\theta$, $\gamma$, and $\psi$ represent the carrier’s attitude angular velocity, acceleration, pitch angle, roll angle, and yaw angle, respectively. $a_x^b$, $a_y^b$, and $a_z^b$ represent the acceleration of the UAVs along the three axes.

2.2. Accelerated Motion

In this process, the UAV undergoes accelerated or decelerated motion, while the attitude angles remain unchanged. Since the UAV is undergoing varying-speed motion, the acceleration along the direction of motion is non-zero. If the direction of acceleration is the same as the direction of motion, the UAV undergoes acceleration; if the direction of acceleration is opposite to the direction of motion, it is deceleration.

The control equations for the UAV in this case are given in Equation (2):

$$\begin{array}{cc}\hfill & \omega ={\left[\begin{array}{ccc}\dot{\theta }& \dot{\gamma }& \dot{\psi }\end{array}\right]}^T=0,\hfill \\ \hfill & a_x^b=a_{\mathrm{z}}^b=0,a_y^b=c,\hfill \end{array}$$

(2)

where c represents the acceleration value, with the sign (positive or negative) determining whether the UAV undergoes acceleration or deceleration.

2.3. Turning Motion

The turning motion process can take two forms depending on the type of carrier. A simpler form, commonly used in vehicle turning, only changes the yaw angle, while the pitch and roll angles remain unchanged, with a zero rate of change. In the case of the UAV as the carrier, the roll angle is typically adjusted by changing the orientation of the vehicle’s thrusters. By modifying the roll angle, hydrodynamic forces are used to alter the yaw angle. After completing the maneuver, the thrusters are adjusted to return the UAV to a neutral position.

To clarify the application scenario, this paper makes the following assumptions. In this study, we focus on low-speed turning maneuvers, where the roll angle is assumed to remain within 30°. This assumption is based on the typical operational limits of UAVs during low-speed navigation, where excessive roll angles are neither necessary nor desirable. Under this condition, the aerodynamic and hydrodynamic forces acting on the UAV can be approximated as linear functions of the roll angle, simplifying the dynamic model. This approximation is valid because, for small roll angles, the nonlinear effects (e.g., cross-coupling between roll and yaw dynamics) are negligible.

Thus, the trajectory generator designed in this paper divides this process into the following three stages:

• Roll angle change phase:
  In this phase, it is assumed that the roll angle angular rate is ${\dot{\gamma }}_0$, with only the roll angle changing and all other angular velocities being zero. The component of gravity along the x-axis causes acceleration, and the roll angle at time t is calculated as $\gamma =\dot{\gamma }·t.$ The control equations for the UAVs during this phase are shown in Equation (3):

  $$\begin{array}{cc}\hfill & \dot{\theta }=\dot{\psi }=0, \dot{\gamma }={\dot{\gamma }}_0, \hfill \\ \hfill & a_{\mathrm{y}}^b=a_{\mathrm{z}}^b=0, a_{\mathrm{x}}^b=\mathrm{g}·tan\left(\gamma \right).\hfill \end{array}$$

  (3)
• Turning phase:
  In this phase, the roll angle obtained in the previous phase remains unchanged, and only the yaw angle changes. The control equations for the UAVs during this phase are given in Equation (4):

  $$\begin{array}{cc}\hfill & \dot{\theta }=\dot{\gamma }=0, \dot{\psi }=\mathrm{g}·tan\left(\gamma \right)/V_y, \hfill \\ \hfill & a_{\mathrm{y}}^b=a_{\mathrm{z}}^b=0, a_{\mathrm{x}}^b=\mathrm{g}·tan\left(\gamma \right),\hfill \end{array}$$

  (4)

  where $V_y$ represents the UAV’s velocity in the y-axis direction.
• Roll angle recovery phase:
  This phase is the reverse of reverse of the Roll angle change phase. The roll angle angular rate is inverse, and the rest of the parameters remain unchanged. The control equations for the UAVs during this phase are shown in Equation (5):

  $$\begin{array}{cc}\hfill & \dot{\theta }=\dot{\psi }=0, \dot{\gamma }=-{\dot{\gamma }}_0\hfill \\ \hfill & a_{\mathrm{y}}^b=a_{\mathrm{z}}^b=0, a_{\mathrm{x}}^b=\mathrm{g}·tan\left(\gamma \right).\hfill \end{array}$$

  (5)

2.4. Pitch Motion

In this process, the UAV either ascends or descends. Taking ascent as an example, the UAV moves along the vertical axis, with the pitch angle increasing at a constant rate. During this phase, the heading angle remains constant, and the roll angle is minimal or zero to ensure stable vertical motion. Acceleration primarily occurs along the vertical axis (z-axis) as the UAV gains altitude. Once the pitch angle reaches the target value, and the UAV achieves the desired climb angle, it transitions to constant-speed motion. Just before reaching the target altitude, the UAVs adjusts its pitch to level off, stabilizing for horizontal flight. Therefore, the trajectory generator designed in this study divides this process into the following three stages:

• Pitch angle change stage:
  In this stage, the pitch angle changes at a constant rate, with the angular velocity being positive for upward climb and negative for downward descent. The heading angle and roll angle remain unchanged, and the z-axis acceleration is related to the y-axis velocity and pitch rate. The UAV’s control equations are given in Equation (6):

  $$\begin{array}{cc}\hfill & \dot{\gamma }=\dot{\psi }=0, \dot{\theta }={\dot{\theta }}_0\hfill \\ \hfill & a_{\mathrm{x}}^b=a_{\mathrm{y}}^b=0, a_{\mathrm{z}}^b=\dot{\theta }·V_y.\hfill \end{array}$$

  (6)
• Climb or descent stage:
  This is the stage where the UAV is climbing or descending. At this point, the pitch angle, roll angle, and heading angle remain unchanged, and the UAV moves upward or downward at a constant speed. The UAV’s control equations are given in Equation (7):

  $$\begin{array}{cc}\hfill & \omega ={\left[\begin{array}{ccc}\dot{\theta }& \dot{\gamma }& \dot{\psi }\end{array}\right]}^T=0\hfill \\ \hfill & a={\left[\begin{array}{ccc}{a}_x^b& a_y^b& a_z^b\end{array}\right]}^T=0.\hfill \end{array}$$

  (7)
• Leveling section:
  The leveling stage of the UAV is the reverse of the pitch angle change stage. The pitch rate is reversed, and the rest of the conditions remain unchanged. The UAV’s control equations are shown in Equation (8):

  $$\begin{array}{cc}\hfill & \dot{\gamma }=\dot{\psi }=0,\dot{\theta }=-{\dot{\theta }}_0\hfill \\ \hfill & a_{\mathrm{x}}^b=a_{\mathrm{y}}^b=0,a_{\mathrm{z}}^b=\dot{\theta }·V_y.\hfill \end{array}$$

  (8)

2.5. Coning Motion

Coning motion involves angular oscillations about two axes with the same frequency but different phases. For strapdown inertial navigation systems, coning motion represents the most challenging operational condition, as it induces severe drift in the mathematical platform. Consequently, coning motion is typically used to evaluate attitude update algorithms. If an algorithm can minimize drift under coning motion conditions, it can ensure minimal drift under other conditions as well. Coning motion can be approximated as angular oscillations with the same frequency but different phases about attitude angles. When the cone angle is $\alpha$ and the motion angular velocity is $\omega$, the UAV’s control equations are as given in Equation (9):

$$\begin{array}{cc}\hfill & \dot{\theta }=\alpha \omega cos\left(\omega t\right),\dot{\gamma }=\alpha \omega sin\left(\omega t\right),\dot{\psi }=0\hfill \\ \hfill & a={\left[\begin{array}{ccc}{a}_x^b& a_y^b& a_z^b\end{array}\right]}^T=0.\hfill \end{array}$$

(9)

2.6. Paddling Motion

Paddling motion involves angular and linear oscillations of the same frequency and phase. Similar to coning motion, paddling motion is often used as a test condition to evaluate velocity update algorithms. When the carrier undergoes angular oscillations about the pitch axis with an amplitude of $\alpha$ and motion angular velocity $\omega$, and linear oscillations along the roll axis with an amplitude of p and the same frequency, the UAV’s control equations are as given in Equation (10):

$$\begin{array}{cc}\hfill & \dot{\theta }=\alpha \omega cos\left(\omega t\right),\dot{\gamma }=0,\dot{\psi }=0\hfill \\ \hfill & a_{\mathrm{x}}^b=a_{\mathrm{z}}^b=0,a_{\mathrm{y}}^b=-p{\omega }^{2}sin\left(\omega t\right).\hfill \end{array}$$

(10)

2.7. Coning Motion with Constant Angular-Rate Precession

As mentioned in [24], when considering the triad terms in the Bortz equation, a coning motion with constant angular-rate precession is more effective than a traditional coning motion for validating the accuracy of inertial navigation algorithms. This motion adds a constant angular rate to the traditional coning motion. When the cone angle is $\alpha$, the motion angular velocity is $\omega$, and the constant angular rate is c, the UAV’s control equations are as shown in Equation (11):

$$\begin{array}{cc}\hfill & \dot{\theta }=\alpha \omega cos\left(\omega t\right)+c\omega ,\dot{\gamma }=\alpha \omega sin\left(\omega t\right),\dot{\psi }=0\hfill \\ \hfill & a={\left[\begin{array}{ccc}{a}_x^b& a_y^b& a_z^b\end{array}\right]}^T=0.\hfill \end{array}$$

(11)

The attitude angles output by the trajectory generator under this condition are shown in Figure 3.

3. IMU Inversion Algorithm Design

This section first introduces the IMU output algorithm in the traditional trajectory generator, followed by a detailed description of the IMU output algorithm used in the trajectory generator proposed in this paper. The two algorithms are then compared.

3.1. Traditional IMU Inversion Algorithm

3.1.1. Gyroscope Output

The gyroscope output provides the angular velocity of the carrier relative to the inertial space in the carrier coordinate system [25]. In the trajectory generator’s IMU output, to obtain the gyroscope output, the attitude angles must be reverse-engineered and converted into the angular velocity output by the gyroscope.

The transformation relationship between the navigation coordinate system and the carrier coordinate system is given by Equation (12):

$$\begin{array}{cc}\hfill & C_n^b=C_2^b{C}_1^2{C}_n^1=\left[\begin{array}{ccc}cos\gamma & 0& -sin\gamma \\ 0& 1& 0\\ sin\gamma & 0& cos\gamma \end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& cos\theta & sin\theta \\ 0& -sin\theta & cos\theta \end{array}\right]\left[\begin{array}{ccc}cos\psi & -sin\psi & 0\\ sin\psi & cos\psi & 0\\ 0& 0& 1\end{array}\right]\hfill \\ \hfill & =\left[\begin{array}{ccc}cos\gamma cos\psi +sin\gamma sin\psi sin\theta & -cos\gamma sin\psi +sin\gamma cos\psi sin\theta & -sin\gamma cos\theta \\ sin\psi cos\theta & cos\psi cos\theta & sin\theta \\ sin\gamma cos\psi -cos\gamma sin\psi sin\theta & -sin\gamma sin\psi -cos\gamma cos\psi sin\theta & cos\gamma cos\theta \end{array}\right].\hfill \end{array}$$

(12)

In the process of 3-axis coordinate system rotation, the angular velocity of the carrier coordinate system relative to the geographic coordinate system can be projected onto the carrier coordinate system through the Euler angle differential equations, as in Equation (13):

$$\begin{array}{cc}\hfill & {\omega }_{nb}^b=\left[\begin{array}{c}{\omega }_{nbx}^b\\ {\omega }_{nby}^b\\ {\omega }_{nbz}^b\end{array}\right]=C_1^b\left[\begin{array}{c}\dot{\theta }\\ 0\\ -\dot{\psi }\end{array}\right]+\left[\begin{array}{c}0\\ \dot{\gamma }\\ 0\end{array}\right]=\left[\begin{array}{ccc}cos\gamma & sin\theta sin\gamma & -cos\theta sin\gamma \\ 0& cos\theta & sin\theta \\ sin\gamma & -sin\theta cos\gamma & cos\theta cos\gamma \end{array}\right]\left[\begin{array}{c}\dot{\theta }\\ 0\\ -\dot{\psi }\end{array}\right]+\left[\begin{array}{c}0\\ \dot{\gamma }\\ 0\end{array}\right]\hfill \\ \hfill & =\left[\begin{array}{c}\dot{\theta }cos\gamma +\dot{\psi }cos\theta sin\gamma \\ -\dot{\psi }sin\theta +\dot{\gamma }\\ \dot{\theta }sin\gamma -\dot{\psi }cos\theta cos\gamma \end{array}\right]=\left[\begin{array}{ccc}cos\theta sin\gamma & cos\gamma & 0\\ -sin\theta & 0& 1\\ -cos\theta cos\gamma & sin\gamma & 0\end{array}\right]\left[\begin{array}{c}\dot{\psi }\\ \dot{\theta }\\ \dot{\gamma }\end{array}\right].\hfill \end{array}$$

(13)

The Earth’s rotational angular velocity relative to the inertial space and the navigation frame’s rotational angular velocity relative to the Earth are given in Equations (14) and (15):

$${\omega }_{ie}^n=C_e^n{\omega }_{ie}^e=\left[\begin{array}{c}0\\ {\omega }_{ie}cosL\\ {\omega }_{ie}sinL\end{array}\right],$$

(14)

$${\omega }_{en}^n={\left[\begin{array}{ccc}{\displaystyle \frac{-V_y^n}{R}}& {\displaystyle \frac{-V_{\mathrm{x}}^n}{R}}& {\displaystyle \frac{V_{\mathrm{x}}^{n}tanL}{R}}\end{array}\right]}^T,$$

(15)

where L represents the local latitude.

From Equations (14) and (15), the angular velocity of the navigation frame (n) relative to the inertial frame (i) can be obtained as Equation (16):

$${\omega }_{in}^n={\omega }_{ie}^n+{\omega }_{en}^n.$$

(16)

The ideal output of the gyroscope ${\omega }_{ib}^b$ is shown in Equation (17):

$${\omega }_{ib}^b=C_n^b{\omega }_{in}^n+{\omega }_{nb}^b.$$

(17)

3.1.2. Accelerometer Output

The accelerometer output represents specific force information in the body frame (b), which is derived from the rate of change of velocity. Specifically, the accelerometer measures the specific force, which is the total acceleration experienced by the body minus the gravitational acceleration. Based on the strapdown inertial navigation force in Equation (18):

$$\begin{array}{c}\hfill f_{ib}^n=\left[\begin{array}{c}{f}_{ibx}^n\\ f_{iby}^n\\ f_{ibz}^n\end{array}\right]=\left[\begin{array}{c}{\dot{V}}_x^n\\ {\dot{V}}_y^n\\ {\dot{V}}_z^n\end{array}\right]-\left[\begin{array}{ccc}0& 2{\omega }_{iez}^n+{\omega }_{enz}^n& -2{\omega }_{iey}^n-{\omega }_{eny}^n\\ -2{\omega }_{iez}^n-{\omega }_{enz}^n& 0& 2{\omega }_{iex}^n+{\omega }_{enx}^n\\ 2{\omega }_{iey}^n+{\omega }_{eny}^n& -2{\omega }_{iex}^n-{\omega }_{enx}^n& 0\end{array}\right]C_b^n\left[\begin{array}{c}{V}_x^b\\ V_y^b\\ V_z^b\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ g\end{array}\right],\end{array}$$

(18)

where ${\dot{V}}_x^n,{\dot{V}}_y^n,{\dot{V}}_z^n$ are the velocity increments in the inertial navigation frame, g is the gravitational acceleration, varying with altitude according to Equation (19):

$$g=g_0{\displaystyle \frac{R_e^2}{{\left(R_e+h\right)}^2}}.$$

(19)

The gravity acceleration on the surface of the equatorial values is 9.78049 m per second squared. The valve of $R_e$ is 6,378,137 m.

The ideal output of the gyroscope $f_{ib}^b$ is given in Equation (20):

$$f_{ib}^b=C_n^b{f}_{ib}^n.$$

(20)

3.2. One Sample Plus One Previous per Update IMU Inversion Algorithm

3.2.1. Gyroscope Output

The actual output of the gyroscope is the angular increment, not the angular velocity. In this paper, following the approach in [20], the equivalent rotation vector is computed using the one-sample-plus-one-previous-per-update algorithm, which is used to reverse calculate the angular increment output of the gyroscope.

The formula for calculating the equivalent rotation vector is shown in Equation (21):

$${\varphi }_m=\Delta {\theta }_m+1/12·\Delta {\theta }_{m-1}\times \Delta {\theta }_m.$$

(21)

Thus, the formula for calculating the gyroscope angular increment based on the equivalent rotation vector is given in Equation (22):

$$\Delta {\theta }_{\mathrm{k}}={\left(I_{3\times 3}+1/12·\Delta {\theta }_{\mathrm{k}-1}\times \right)}^{-1}{\varphi }_k.$$

(22)

After selecting an appropriate algorithm to compute the equivalent rotation vector using the Bortz equation, the ideal gyroscope angular increment output can be obtained from the formula above.

However, in the actual gyroscope output, there will be constant bias errors and random errors. With these errors included, the gyroscope output becomes Equation (23):

$$\Delta {\theta }_{\mathrm{actual}}=\Delta \theta +\Delta {\theta }_{\mathrm{bias}}+\Delta {\theta }_{random},$$

(23)

where $\Delta {\theta }_{\mathrm{bias}}$ is the constant bias error, and $\Delta {\theta }_{random}$ is the random error, which is implemented in the trajectory generator using a first-order Gaussian distribution.

3.2.2. Accelerometer Output

The actual output of the accelerometer is the velocity increment rather than the specific force. According to [26], the velocity increment caused by the specific force is given in Equation (24):

$$\Delta {\mathrm{v}}_{sfm}=\Delta {\mathrm{v}}_m+\Delta {\mathrm{v}}_{rotm}+\Delta {\mathrm{v}}_{sculm},$$

(24)

where $\Delta {\mathrm{v}}_m$ is the velocity increment of the accelerometer during the time interval [$t_{m-1}$, $t_m$], $\Delta {\mathrm{v}}_{rotm}$ is the rotation effect compensation term, and $\Delta {\mathrm{v}}_{sculm}$ is the sculling effect compensation term.

When using the one-sample-plus-one-previous-per-update algorithm to compensate for the accelerometer velocity increment, the following relations are shown in Equations (25) and (26):

$$\Delta {\mathrm{v}}_{rotm}=1/12·\Delta {\theta }_m\times \Delta {\mathrm{v}}_m,$$

(25)

$$\Delta {\mathrm{v}}_{sculm}=1/12·\left(\Delta {\theta }_{m-1}\times \Delta {\mathrm{v}}_m+\Delta {\mathrm{v}}_{m-1}\times \Delta {\theta }_m\right).$$

(26)

Thus, based on Equations (24)–(26), the accelerometer output velocity increment can be expressed as Equation (27):

$$\begin{array}{cc}\hfill & \Delta {\mathrm{v}}_m={\left[I_{3\times 3}+\left(1/2·\Delta {\theta }_m+1/12·\Delta {\theta }_{m-1}\right)\times \right]}^{-1}\hfill \\ \hfill & \left(\Delta {\mathrm{v}}_{sfm}-1/12·\Delta {\mathrm{v}}_{m-1}\times \Delta {\theta }_m\right).\hfill \end{array}$$

(27)

In the actual accelerometer output, a constant bias error and random error will be present. With these errors included, the accelerometer output is given in Equation (28):

$$\Delta v_{\mathrm{actual}}=\Delta v+\Delta v_{\mathrm{bias}}+\Delta v_{random},$$

(28)

where $\Delta v_{\mathrm{bias}}$ is the constant bias error, and $\Delta v_{random}$ is the random error, implemented in the trajectory generator using a first-order Gaussian distribution.

3.3. Inversion Algorithm Simulation

To verify the effectiveness of the inertial navigation inversion algorithm used in this paper, four experiments were designed, with the set trajectories and motion information as shown in

Table 1. Accelerated motion parameters.

State	Time (s)	Acceleration (m/s2)	Angular Velocity (rad/s)
Accelerated Motion	100	(0, 0.1, 0)	(0, 0, 0)
Uniform Motion	50	(0, 0, 0)	(0, 0, 0)

,

Table 2. Coning motion parameters.

State	Time (s)	Acceleration (m/s2)	Angular Velocity (rad/s)
Accelerated Motion	100	(0, 0.1, 0)	(0, 0, 0)
Uniform Motion	50	(0, 0, 0)	(0, 0, 0)
Accelerated Motion	100	(0, −0.1, 0)	(0, 0, 0)
Coning Motion	100	(0, 0, 0)	($a·w·sinwt,a·w·coswt,0$)

,

Table 3. Paddling motion parameters.

State	Time (s)	Acceleration (m/s2)	Angular Velocity (rad/s)
Accelerated Motion	100	(0, 0.1, 0)	(0, 0, 0)
Uniform Motion	50	(0, 0, 0)	(0, 0, 0)
Paddling Motion	100	(−p $·w^2·sinwt$, 0, 0)	($a·w·coswt$, 0, 0)

and

Table 4. Coning motion with constant angular-rate precession parameters.

State	Time (s)	Acceleration (m/s2)	Angular Velocity (rad/s)
Accelerated Motion	100	(0, 0.1, 0)	(0, 0, 0)
Uniform Motion	50	(0, 0, 0)	(0, 0, 0)
Accelerated Motion	100	(0, −0.1, 0)	(0, 0, 0)
Coning Motion with Constant Angular-Rate Precession	50	(0.001, 0, 0)	($a·w·sinwt+a·w·coswt$, 0)

.

In the cone motion, paddle motion, and coning motion with angular velocity precession, the cone angle a was set to $1°$, the motion angular velocity $\omega$ was set to $0.2\pi$, and the amplitude p in the paddle motion was set to $0.001\mathrm{m}$.

The simulation results are shown in the figure below (Figure 4, Figure 5, Figure 6 and Figure 7). Figure (a) represents the attitude of the inertial navigation system, and Figure (b) represents the velocity of the inertial navigation system.

In the figures below, the red, blue, and black plots represent the traditional algorithm, the one-sample algorithm with the previous cycle, the three-sample algorithm, and the ground truth, respectively. From the comparative analysis of the algorithms presented above, the following observations can be made. For low-dynamic motions, such as accelerated motion, the differences in attitude and velocity calculated using the two inversion methods are negligible. During paddling motion, the velocity differences between the two methods remain relatively small, while the attitude discrepancies are more pronounced. The proposed method demonstrates significantly smaller velocity errors compared to the traditional inversion approach. In the case of coning motion, the differences between the two methods become more substantial, particularly in terms of velocity, where the traditional algorithm exhibits significant divergence after inversion and calculation. Under coning motion with angular velocity precession, the proposed algorithm shows a clear advantage in eastward velocity estimation over the traditional method. However, neither method is able to accurately track attitude or northward velocity, highlighting the need for further validation using higher-precision inertial navigation algorithms in future work. These findings effectively demonstrate that the one-sample-plus-one-previous-per-update algorithm method outperforms the traditional approach in most high-dynamic environments.

4. INS Algorithm Simulation

To further demonstrate that the trajectory generator proposed in this paper can validate the accuracy of inertial navigation algorithms, three common inertial navigation algorithms were selected for testing: the single-sample algorithm, the one-sample-plus-one-previous-per-update algorithm, and the three-sample algorithm. These algorithms were tested on turning motion, conical motion, paddling motion, and conical motion with angular velocity precession. The attitude and velocity information obtained from the three algorithms were compared in different motion scenarios. As discussed in [27], the performance of these algorithms varies significantly under different dynamic conditions, with the three-sample algorithm demonstrating superior accuracy in high-dynamic environments. Furthermore, the work in [28] has shown that the one-sample-plus-one-previous-per-update algorithm achieves comparable precision to the two-sample algorithm. Based on these findings, we can conclude that the three-sample algorithm outperforms both the one-sample-plus-one-previous-per-update algorithm and the single-sample algorithm in terms of accuracy. The calculation process for the one-sample-plus-one-previous-per-update algorithm can be referenced from [15], and the calculation process for the three-sample algorithm can be found in [29].

The specific motion parameters are shown in

Table 5. Turning motion parameters.

State	Time (s)	Acceleration (m/s2)	Angular Velocity (rad/s)
Accelerated Motion	100	(0, 0, 1.0)	(0, 0, 0)
Uniform Motion	50	(0, 0, 0)	(0, 0, 0)
Turning Motion	55	(0.5 $·\pi /180·10,0,0$)	(0, 0, 0.5 $·\pi /180$)

,

Table 6. Coning motion parameters.

State	Time (s)	Acceleration (m/s2)	Angular Velocity (rad/s)
Accelerated Motion	100	(0, 0, 1.0)	(0, 0, 0)
Uniform Motion	50	(0, 0, 0)	(0, 0, 0)
Accelerated Motion	100	(0, −0.1, 0)	(0, 0, 0)
Coning Motion	100	(0, 0, 0)	($a·w·sinwt,a·w·coswt,0$)

,

Table 7. Paddling motion parameters.

State	Time (s)	Acceleration (m/s2)	Angular Velocity (rad/s)
Accelerated Motion	100	(0, 0, 1.0)	(0, 0, 0)
Uniform Motion	50	(0, 0, 0)	(0, 0, 0)
Paddling Motion	100	(−$p·w^2·sinwt,0,0$)	($a·w·coswt,0,0$)

and

Table 8. Coning motion with constant angular-rate precession parameters.

State	Time (s)	Acceleration (m/s2)	Angular Velocity (rad/s)
Accelerated Motion	100	(0, 0, 1.0)	(0, 0, 0)
Uniform Motion	50	(0, 0, 0)	(0, 0, 0)
Coning Motion with Constant Angular-Rate Precession	50	(0.001, 0, 0)	($a·w·sinwt+a·w·coswt,0$)

.

In the cone motion, paddle motion, and coning motion with angular velocity precession, the cone angle a was set to $1°$, the motion angular velocity $\omega$ was set to $0.2\pi$, and the amplitude p in the paddle motion was set to $0.001\mathrm{m}$.

In the figures below, the red, blue, green, and black plots represent the one-sample algorithm, the one-sample algorithm with the previous cycle, the three-sample algorithm, and the ground truth, respectively. This color scheme was consistently applied throughout the manuscript to ensure clear and accurate representation of the results. Based on the simulation results, it can be observed that for low dynamic motion, such as the turning motion shown in Figure 8 and Figure 9, the differences between the three algorithms are minimal, indicating that under low dynamic conditions, low-precision inertial navigation algorithms can already meet the requirements of an inertial navigation system. In the simulation results of the coning motion and paddling motion (Figure 10, Figure 11, Figure 12 and Figure 13), the three-sample algorithm clearly outperforms the other two algorithms. However, under the harsh conditions of angular velocity precession in coning motion (Figure 14 and Figure 15), all three algorithms fail to accurately track the attitude. Despite this, the three-sample algorithm still demonstrates superior performance in velocity calculation compared to the other two algorithms. This confirms that the trajectory generator designed in this paper can effectively verify the accuracy of inertial navigation. However, future research should focus on validating the system using higher-precision inertial navigation algorithms to further enhance its performance and reliability, providing a more robust foundation for the development of advanced inertial navigation technologies.

5. Discussion

As the simulation results presented in this paper show, in low-dynamic environments, improving the algorithm accuracy does not significantly enhance the precision of inertial navigation solutions. At this stage, the primary source of the error comes from hardware inaccuracies. The proposed trajectory generator demonstrates significant potential in simulating high-dynamic environments, enabling rigorous validation of inertial navigation algorithms under challenging conditions. However, its current implementation is limited by the assumption of ideal sensor models, which may not fully capture real-world sensor imperfections.

Similarly, the one-sample-plus-one-previous-per-update algorithm shows promise in balancing computational efficiency and navigation accuracy, particularly in scenarios with moderate dynamics. Nevertheless, its performance is constrained by the inherent trade-off between the update frequency and the error accumulation, which becomes more pronounced in extreme high-dynamic environments. These limitations highlight the need for further research into adaptive algorithms that can dynamically adjust to varying operational conditions.

By addressing these challenges, the trajectory generator and algorithm framework presented in this work provide a robust mathematical foundation for testing higher-precision algorithms in the future, while also offering a cost-effective approach to system validation and development.

6. Conclusions

This paper designed a high-precision trajectory generator with complex motion for inertial navigation testing. By integrating basic motions, coning motion, and paddling motion, the trajectory generator incorporates coning motion with angular velocity precession to simulate high-dynamic environments. The proposed one-sample-plus-one-previous-per-update inversion algorithm was employed to obtain IMU outputs, and its performance was compared with traditional inertial navigation algorithms. Through comprehensive simulation analysis and verification, the following conclusions are drawn. The proposed one-sample-plus-one-previous-per-update inversion algorithm demonstrates superior performance in high-dynamic environments. In addition to the extreme harsh conditions proposed in this paper, the oscillation range of the algorithm remains within 10% of the ground truth, and the maximum error is reduced by 90%, while traditional algorithms exhibit velocity divergence under coning and other high-dynamic motions. The trajectory generator effectively validates the accuracy of inertial navigation algorithms under diverse motion scenarios. The incorporation of a coning motion with angular velocity precession enriches the motion modes of the trajectory generator, providing a comprehensive framework for evaluating high-precision inertial navigation algorithms in harsh environments. However, this study has certain limitations: the trajectory generator currently assumes ideal sensor models, which may not fully capture real-world sensor imperfections, and the proposed algorithm’s performance in ultra-high-dynamic scenarios requires further investigation. For practitioners in the field, we recommend carefully considering the trade-offs between computational complexity and navigation accuracy when implementing the proposed methods, as well as validating the algorithms with real-world data to ensure robustness. These findings highlight the practical significance of the proposed methods in developing robust inertial navigation systems for UAVs and other high-dynamic platforms. Future research directions include experimental validation of the proposed trajectory generator and inversion algorithm using physical testbeds, application of the proposed framework to other platforms such as autonomous vehicles and underwater drones, and integration of higher-precision inertial navigation algorithms to further validate and enhance the system’s performance, demonstrating the continuity and importance of this research in advancing the field of inertial navigation. By bridging the gap between theoretical algorithm validation and practical actuator-driven systems, this work contributes to the advancement of high-dynamic control technologies in aerospace and robotics, aligning with the growing demand for precision and reliability in autonomous navigation.