# Combinatory Attitude Determination Method for High Rotational Speed Rigid-Body Aircraft

Authors: Liangliang An; Liangming Wang; Ning Liu; Jian Fu
Journal: Mathematical Problems in Engineering
 (2020)

---

## Abstract

In this paper, we present a novel multisensor combinatory attitude determination method that enables high-accuracy measurement of the attitude of a high rotational speed rigid-body aircraft. We analyze the external moments of the aircraft during flight and develop the method using theoretical deductions based on the motion equations of a rigid body rotating around the centroid. The proposed method fuses the data measured from GPS, gyrometer, and magnetometer and uses the improved unscented Kalman filter (UKF) algorithm to perform filtering. First, appropriate assumptions and simplifying approximations are made for around-centroid motion equations of a rigid body according to the motion characteristics of the high rotational speed aircraft. Using these assumptions and approximations, the constraint equations between the Euler attitude angles and flight-path angle, trajectory deflection angle are derived to serve as the state equation. Second, the roll angle with error is calculated using the geomagnetic field model and the geomagnetic intensity measured by a three-axis magnetometer and then fused with the angular velocity information obtained from the gyroscope for constructing the measurement equations. Finally, the state equations are discretized using the Runge–Kutta method during the UKF prediction stage, improving the prediction accuracy. Simulation results show that the proposed method can effectively determine the attitude information of the high rotational speed aircraft, achieving high level of reliability and accuracy thanks to the combination of information from GPS, gyroscope, and magnetometer.


---

## Body

## 1. Introduction

Acquisition of high-accuracy flight attitude information of a high rotational speed aircraft is of great significance for analyzing the flight dynamics of the carrier and providing support for the navigation and guidance system. However, accurate measurement of the attitude has been a major challenge for the last several years due to high rotational speeds and highly dynamic characteristics. With the development of microelectro-mechanical sensors (MEMS), microsensor systems such as solar sensor, inertial measurement unit (IMU), and magnetometer could be applied in a wide range of applications [1–4], significantly advancing the research on attitude measurement. However, measurement systems consisting of a single type of sensor have limitations. For example, solar sensors require good weather conditions [5], angular velocity gyroscopes tend to drift, where the drift is proportional to the angular velocity [6], the IMUs are plagued by error accumulation [7], and the magnetometers are highly susceptible to interference from external magnetic fields [8]. Therefore, for attitude measurement of the high rotational speed aircraft, different types of sensors can be combined to take full advantage of the features of all the sensors, overcoming the limitations of each type of sensor and consequently improving measurement accuracy.Researchers have carried out a significant amount of work in the area of multisensor combinatory attitude measurement. Gebre-Egziabher et al. [9] designed an inexpensive multisensor attitude estimation system, developed a sensor fusion algorithm based on Euler angles and quaternions, and proposed methods for scheduling gain and estimating pole placement. Mao et al. [10] proposed a method for accurate attitude determination based on data fusion of a three-axis gyroscope, a three-antenna GPS, and a star sensor. Tetanize and Shirazi [11] combined low-cost MEMS and a nonlinear attitude estimation algorithm to design an inexpensive and accurate system support for navigation and attitude determination.The aforementioned combinatory attitude measurement methods mainly focus on nonrotating or low rotational speed platforms, such as satellites, vehicles, and airplanes, and cannot be applied to high rotational speed flight carriers. Zhang et al. [12] designed a test with geomagnetic sensor circuit module to reveal the coning motion law of high speed spin-stabilized projectile. A research group at the Nanjing University of Science and Technology applied the combined GPS/SINS attitude measurement method to the high rotational speed rocket platform. The approach solved the attitude of the rocket by using segmentation filtering to correct the roll angle, which is somewhat inefficient [13]. Li et al. [14] proposed a scheme for constructing a combinatory attitude determination system using a magnetometer and an inertial sensor, but the scheme ignored the influence of magnetic declination in the calculation process. An et al. [15] proposed a novel method for estimating the pitch and yaw angles of the projectile through a special combination of magnetometers, but did not estimate its roll angle due to the extremely high rotational speed. All the abovementioned combinatory attitude measurement methods focus on the post-processing of measurement data, which indicates that insufficient effort has been dedicated to study the pattern of attitude variations.Both EKF and UKF are common and mature algorithms for the filtering algorithms used in the combined attitude measurement system. However, EKF ignores or approximates the high-order terms of the Taylor expansion and involves calculation of the Jacobian matrix that is cumbersome. Compared with the EKF, the unscented Kalman filter (UKF) exhibits good robustness in the presence of nonlinearity and uncertainty; therefore, it is better at dealing with complex models with high nonlinearity [16–19]. In the UKF algorithm, when the continuous system is discretized, the discretization method and discrete step size directly affect the filtering accuracy. When the step size is large, the discrete models processed by the conventional methods such as the Euler method significantly differ from the continuous models. On the contrary, reducing the discrete step size increases the computational complexity. When the fourth-order classical Runge–Kutta method [20, 21] is used as the discretization method, the reliance on discrete step size is reduced greatly. Consequently, the discrete models become closer to the theoretical continuous models, and the filtering accuracy is improved.In this paper, we developed a novel attitude measurement method by studying high rotational speed rocket. First, appropriate assumptions and simplifying approximations were made for around-centroid motion equations [22] of a rigid body with six degrees of freedom based on an analysis of the external moments and dynamics of the high rotational speed flight platform. The lateral angle relationship contained in the external moments was extracted and the new motion equations containing the constraint relationship between the lateral attitudes, i.e., pitch and yaw, and flight-path angle, and trajectory deflection angle were derived. Second, a complete roll angle calculation formula was obtained based on the geomagnetic field model and the data was measured using the magnetometer. According to the angular motion characteristics of the high rotational speed rocket and the error characteristics of the gyroscope and magnetometer, the measured gyroscope data and the roll angle were combined to obtain the angular velocity conversion relationship between different coordinate systems. Third, with the derived motion equations serving as state equations and the angular velocity conversion relationship serving as the measurement equation, the UKF algorithm employing the fourth-order Runge–Kutta discretization method was used to estimate the attitude of the high rotational speed rocket. Finally, the effectiveness and reliability of the proposed method were verified through simulations.


## 2. Coordinate Systems

To establish the differential equations of projectile dynamics, we use the approach described in [22] to introduce several basic coordinate systems: the ground coordinate system O−XYZ, the body coordinate system O−X1Y1Z1, the ballistic coordinate system O−X2Y2Z2, the projectile axis coordinate system O−ξηζ, and the second projectile axis coordinate system O−ξη2ζ2, abbreviated as coordinate systems N, B, V, A, and A2.Figure1 shows the angular relationships between these coordinate systems. In the figure, the angles φa and φ2 are the pitch and yaw Euler angles, respectively, the angle θa is the angle between the velocity vector and the horizontal plane, the angle ψ2 is the angle between the velocity vector and the vertical plane, respectively, i.e., flight-path angle and trajectory deflection angle, and δ is the total attack angle of the rocket. Figure 2 further illustrates the pitch component δ1 and the yaw component δ2 of the total attack angle.Figure 1
The angular relationships between these coordinate systems.Figure 2
Coordinate systemV turns to coordinate system A2.Both coordinate systemsA and A2 are nonrolling coordinate systems that do not roll with the rocket. The axis Oξ of each coordinate system is the vertical axis of the rocket and the only difference between the coordinate planes Oηζ and Oη2ζ2 is a turning angle β. The coordinate system B was attached to the carrier and moves along with it. It differs from the coordinate system A by a rotational angle γ, i.e., the roll angle [22].


## 3. Combinatory Attitude Determination Method

### 3.1. Establishing Dynamic Constraint Equations

The movement of the rocket in air consists of two parts: the centroid motion and the around-centroid motion. The former is mainly characterized by the position and velocity of the projectile and is governed by the law of centroid movement. The latter is characterized by the attitude of the rocket and is governed by the theorem of angular momentum. To study the attitude variation pattern of a rigid body, it is necessary to analyze the pattern of around-centroid motion and conduct an in-depth analysis of the external moments acting on the rigid body.Assume that the research object is an ideal axisymmetric model and that both earth’s surface curvature and earth’s rotation are ignored. Subsequently, a new set of around-centroid motion equations can be derived from the trajectory equations of the rigid body with six degrees of freedom given in reference [22] as follows:(1)ω˙ξ=1CMξ,ω˙η=1AMη−CAωξωζ+ωη2tanφ2,ω˙ζ=1AMζ+CAωξωη−ωηωζtanφ2,φ˙a=ωζcosφ2,φ˙2=−ωη,γ˙=ωξ−ωζtanφ2,where Mξ, Mη, and Mζ are the components of the external moments in the coordinate system O−ξηζ; A and C are the rotational inertia coefficients; ωη, ωξ, and ωζ are the projection components of the angular velocity in the coordinate system O−ξηζ; and φa, φ2 and γ are the Euler attitude angles.During flight, the external moments acting on the rocket mainly include static moment, equatorial damping moment, extreme damping moment, and tail guiding moment. When the external moments are projected onto the coordinate systemA, the extreme damping moment and the tail guiding moment only have projection components along axis Oξ and the static moment and the equatorial damping moment only have projection components along axes Oη and Oζ. Thus, the moment components on the three axes of the coordinate system A are as follows [22]:(2)Mξ=Mxzξ+Mxwξ=−ρvr2Sldmxz′ωξ+ρvr22Slmxw′δf,Mη=Mzη+Mzzη=ρSl2vrmz′vrζ−ρvr2Sldmzz′ωη,Mζ=Mzζ+Mzzζ=−ρSl2vrmz′vrη−ρvr2Sldmzz′ωζ,where Mxzξ and Mxwξ are the projected components of the extreme damping moment and the tail guiding moment along axis Oξ; Mzη and Mzζ are the projection components of the static moment; Mzzη and Mzzζ are the projection components of the equatorial damping moment; ρ is the air density; S is the cross-sectional area; l is the length of the rocket; d is the diameter of the rocket; δf is the slanting angle of the tail; mz′, mzz′, mxz′, and mxw′ represent the derivative of the static moment coefficient, the derivative of the equatorial damping moment coefficient, the derivative of the extreme damping moment coefficient, and the derivative of the tail guiding moment, respectively; vr is the speed relative to the wind, in the absence of wind, vr is replaced by v; and vrη and vrζ are the components of the speed vr along axes Oη and Oζ of coordinate system A, respectively.Let the components of velocityvr in system A2 be denoted as vrη2 and vrζ2, and the relationship between vrη and vrζ and vrη2 and vrζ2 is as follows:(3)vrη=vrη2cosβ+vrζ2sinβ,vrζ=−vrη2sinβ+vrζ2cosβ.As shown in Figure2, the rotation relationship between the coordinate system B and A2 leads to vrη2=−vrδ1 and vrζ2=−vrδ2. Consequently, (3) can be further written as(4)vrη=−vrδ1cosβ−vrδ2sinβ,vrζ=vrδ1sinβ−vrδ2cosβ.For a normally flying projectile, as the attack angle and the ballistic deflection are small, the following relationship holds:(5)β≈0,δ1≈φa−θa,δ2≈φ2−ψ2.Consequently, (5) can be further written as(6)vrη=−vrδ1=−vrφa−θa,vrζ=−vrδ2=−vrφ2−ψ2.If the influence of wind is ignored, the static moment componentsMzη and Mzζ in (2) can be written as(7)Mzη=−ρSl2v2mz′φ2−ψ2,Mzζ=ρSl2v2mz′φa−θa.DefiningAm=ρSl/2Amz′, (7) can be rewritten as(8)Mzη=−AAmv2φ2−ψ2,Mzζ=AAmv2φa−θa.In the same way, definingCm=−ρSld/2Amzz′, the components Mzzη and Mzzζ of equatorial damping moment in (2) can be written as(9)Mzzη=ACmvωη,Mzzζ=ACmvωζ.DefiningBm=−ρSld/2Cmxz′, component Mxzξ of extreme damping moment in (2) can be written as(10)Mxzξ=CBmvωξ.DefiningDm=ρSl/2Cmxw′δf, component Mxwξ of tail guiding moment in (2) can be written as(11)Mxwξ=CDmv2.Substituting (8)–(11) into (2), the equations can be obtained as follows:(12)Mξ=CBmvωξ+CDmv2,Mη=−AAmv2φ2−ψ2+ACmvωη,Mζ=AAmv2φa−θa+ACmvωζ.The dynamics constraint equations between two Euler angles and flight-path angle and trajectory deflection angle are obtained as follows:(13)ω˙ξ=Bmvωξ+Dmv2,ω˙η=−Amv2φ2−ψ2+Cmvωη−CAωξωζ+ωη2tanφ2,ω˙ξ=Amv2φa−θa+Cmvωζ+CAωξωη−ωηωζtanφ2,φ˙a=ωζcosφ2,φ˙2=−ωη,γ˙=ωξ−ωζtanφ2,A˙m=0,C˙m=0,B˙m=0,D˙m=0,where the rocket velocity v, the flight-path angle θa, and the trajectory deflection angle ψ2 can be calculated from the GPS data. The calculation formula can be derived from around-centroid motion equations of the rigid body as follows:(14)θa=arctanvyvx,ψ2=arctanvzvxcosθa,where vx, vy, and vz are components of velocity v in the coordinate system N, which can be calculated from GPS data.


### 3.2. Solving Roll Angle with Three-Axis Magnetometer

Among the three angular velocity components of the high rotational speed aircraft, the roll angular velocity is much larger than the angular velocities in the other two directions, resulting in a very large gyrodrift error in the roll direction. Consequently, the gyroscope’s measurement is inaccurate even after calibration, and the calculated roll angle can have a very large error. By contrast, the magnetometer can prevent the accumulation of the calculation error and will not generate measurement distortion under the condition of a large angular velocity, which makes it very suitable for measuring the angular velocity and roll angle of the high rotational speed carrier.


#### 3.2.1. Longitudinal Rotation Angle of Rocket

As shown in Figure3, when the rocket is flying in the air, the coordinate system B rotates along with it in the Earth’s magnetic field. The projection components of the geomagnetic intensity vector M⟶ on the coordinate system B are Mbx, Mby, and Mbz, respectively, wherein, Mby and Mbz constitute the lateral projection component MH. Thus, the longitudinal rotation angle ϕ of the carrier is as follows:(15)ϕ=arctanMbzMby.Figure 3
The component of the geomagnetic vector on the rocket. (a) Overall view (b) Lateral view.
(a)
(b)WhenMby=0, a calculation blind spot occurs in equation (15). As can be seen from Figure 3(b), axis OY1 rotates to an angle perpendicular to the direction of the Earth’s magnetic field and the roll angle should be either π/2 or 3π/2, depending on the sign of Mbz.


#### 3.2.2. Calculation of the Roll Angle

The roll angleγ is the angle between axis OY1 of the coordinate system B and the vertical plane containing the longitudinal axis of the body, i.e., the angle that the coordinate system B has rotated relative to the coordinate system A. The rotation angle ϕ calculated by equation (15) is the longitudinal rotation angle of the body, which is mistaken as a roll angle in many papers. The rotation angle ϕ is calculated from the projection component of the geomagnetic intensity. Since the geomagnetic field does not coincide with the geographic field, giving rise to magnetic declination, there is a phase difference γ0 between ϕ and γ, as shown in Figure 4.Figure 4
Projection of the geomagnetic componentMH in the lateral direction of coordinate system A and B.The calculation formula of the roll angle can be obtained from Figure4 as follows:(16)γ=γ0−ϕ.In Figure4, the projected components of the geomagnetic vector M⟶ along axes Oη and Oζ of the coordinate system A are Mη and Mζ, respectively. Therefore, the phase difference γ0 is given as follows:(17)γ0=arctanMζMη.Then, the complete calculation formula for the roll angle can be obtained by combining (15)–(17):(18)γ=arctanMζMη−arctanMbzMby,where Mby and Mbz are the magnetometer output signals and Mη and Mζ can be obtained using the geomagnetic field model and the relevant coordinate system transfer matrix. The geomagnetic field vector is generally described by a geographic coordinate system OXNYEZD, referred to as the coordinate system E. Assuming that the components of the geomagnetic intensity vector in the north-east-down (NED) are MN, ME, and MD, respectively, they can be calculated using the geomagnetic field model. Subsequently, Mη and Mζ can be calculated through two rounds of coordinate system conversion: transferring from the coordinate system E to N, followed by transferring from the coordinate system N to A.Assume that the fire direction, i.e., the angle between the shooting direction and the geographical north isαN. The matrix for transferring from the coordinate system E to N is(19)CEN=cosαNsinαN000−1−sinαNcosαN0.The matrix for transferring from the systemN to A is(20)CNA=cosφ2cosφacosφ2sinφasinφ2−sinφacosφa0−sinφ2cosφa−sinφ2sinφacosφ2.The transfer matrix from the coordinate systemE to A can be obtained by combining (19) and (20):(21)CEA=CNACEN=cosφ2cosφacosαN−sinφ2sinαNcosφ2cosφasinαN+sinφ2cosαN−cosφ2sinφa−sinφacosαN−sinφasinαNcosφa−sinφ2cosφacosαN−cosφ2sinαN−sinφ2cosφasinαN+cosφ2cosαNsinφ2sinφa.Then, the projection componentMξ, Mη, and Mζ of the geomagnetic vector M⟶ in the coordinate system A is(22)MξMηMζ=CEAMNMEMD,wherein, Mη and Mζ required for (18) are(23)Mη=−sinφacosαNMN−sinφasinαNME+cosφaMD,Mζ=−sinφ2cosφacosαN−cosφ2sinαNMN⋯,−sinφ2cosφasinαN−cosφ2cosαNME+sinφ2sinφaMD.Since the attack angle of the rocket during steady flight is small, the attitude anglesφa and φ2 in the abovementioned equation can be replaced by the flight-path angle θa and trajectory deflection angle ψ2, respectively.


#### 3.2.3. Angular Velocity Conversion Equations

The three-axis gyroscope measures the three angular velocities of the carrier, i.e., the componentsωbx, ωby, and ωbz along the three axes of the coordinate system B, whereas the results of calculation using (13) are the projection components ωξ, ωη, and ωζ of the angular velocity on the coordinate system A. Therefore, a conversion between the gyroscope measurements and the calculated results is needed. Since the coordinate system A differs from the coordinate system B only by a roll angle γ, the required transfer matrix is as follows:(24)CBA=1000cosγ−sinγ0sinγcosγ.Thus, the conversion equations between the two sets of angular velocities are given as(25)ωbx=ωξ,ωby=ωηcosγ+ωζsinγ,ωbz=−ωηsinγ+ωζcosγ.


## 3.1. Establishing Dynamic Constraint Equations

The movement of the rocket in air consists of two parts: the centroid motion and the around-centroid motion. The former is mainly characterized by the position and velocity of the projectile and is governed by the law of centroid movement. The latter is characterized by the attitude of the rocket and is governed by the theorem of angular momentum. To study the attitude variation pattern of a rigid body, it is necessary to analyze the pattern of around-centroid motion and conduct an in-depth analysis of the external moments acting on the rigid body.Assume that the research object is an ideal axisymmetric model and that both earth’s surface curvature and earth’s rotation are ignored. Subsequently, a new set of around-centroid motion equations can be derived from the trajectory equations of the rigid body with six degrees of freedom given in reference [22] as follows:(1)ω˙ξ=1CMξ,ω˙η=1AMη−CAωξωζ+ωη2tanφ2,ω˙ζ=1AMζ+CAωξωη−ωηωζtanφ2,φ˙a=ωζcosφ2,φ˙2=−ωη,γ˙=ωξ−ωζtanφ2,where Mξ, Mη, and Mζ are the components of the external moments in the coordinate system O−ξηζ; A and C are the rotational inertia coefficients; ωη, ωξ, and ωζ are the projection components of the angular velocity in the coordinate system O−ξηζ; and φa, φ2 and γ are the Euler attitude angles.During flight, the external moments acting on the rocket mainly include static moment, equatorial damping moment, extreme damping moment, and tail guiding moment. When the external moments are projected onto the coordinate systemA, the extreme damping moment and the tail guiding moment only have projection components along axis Oξ and the static moment and the equatorial damping moment only have projection components along axes Oη and Oζ. Thus, the moment components on the three axes of the coordinate system A are as follows [22]:(2)Mξ=Mxzξ+Mxwξ=−ρvr2Sldmxz′ωξ+ρvr22Slmxw′δf,Mη=Mzη+Mzzη=ρSl2vrmz′vrζ−ρvr2Sldmzz′ωη,Mζ=Mzζ+Mzzζ=−ρSl2vrmz′vrη−ρvr2Sldmzz′ωζ,where Mxzξ and Mxwξ are the projected components of the extreme damping moment and the tail guiding moment along axis Oξ; Mzη and Mzζ are the projection components of the static moment; Mzzη and Mzzζ are the projection components of the equatorial damping moment; ρ is the air density; S is the cross-sectional area; l is the length of the rocket; d is the diameter of the rocket; δf is the slanting angle of the tail; mz′, mzz′, mxz′, and mxw′ represent the derivative of the static moment coefficient, the derivative of the equatorial damping moment coefficient, the derivative of the extreme damping moment coefficient, and the derivative of the tail guiding moment, respectively; vr is the speed relative to the wind, in the absence of wind, vr is replaced by v; and vrη and vrζ are the components of the speed vr along axes Oη and Oζ of coordinate system A, respectively.Let the components of velocityvr in system A2 be denoted as vrη2 and vrζ2, and the relationship between vrη and vrζ and vrη2 and vrζ2 is as follows:(3)vrη=vrη2cosβ+vrζ2sinβ,vrζ=−vrη2sinβ+vrζ2cosβ.As shown in Figure2, the rotation relationship between the coordinate system B and A2 leads to vrη2=−vrδ1 and vrζ2=−vrδ2. Consequently, (3) can be further written as(4)vrη=−vrδ1cosβ−vrδ2sinβ,vrζ=vrδ1sinβ−vrδ2cosβ.For a normally flying projectile, as the attack angle and the ballistic deflection are small, the following relationship holds:(5)β≈0,δ1≈φa−θa,δ2≈φ2−ψ2.Consequently, (5) can be further written as(6)vrη=−vrδ1=−vrφa−θa,vrζ=−vrδ2=−vrφ2−ψ2.If the influence of wind is ignored, the static moment componentsMzη and Mzζ in (2) can be written as(7)Mzη=−ρSl2v2mz′φ2−ψ2,Mzζ=ρSl2v2mz′φa−θa.DefiningAm=ρSl/2Amz′, (7) can be rewritten as(8)Mzη=−AAmv2φ2−ψ2,Mzζ=AAmv2φa−θa.In the same way, definingCm=−ρSld/2Amzz′, the components Mzzη and Mzzζ of equatorial damping moment in (2) can be written as(9)Mzzη=ACmvωη,Mzzζ=ACmvωζ.DefiningBm=−ρSld/2Cmxz′, component Mxzξ of extreme damping moment in (2) can be written as(10)Mxzξ=CBmvωξ.DefiningDm=ρSl/2Cmxw′δf, component Mxwξ of tail guiding moment in (2) can be written as(11)Mxwξ=CDmv2.Substituting (8)–(11) into (2), the equations can be obtained as follows:(12)Mξ=CBmvωξ+CDmv2,Mη=−AAmv2φ2−ψ2+ACmvωη,Mζ=AAmv2φa−θa+ACmvωζ.The dynamics constraint equations between two Euler angles and flight-path angle and trajectory deflection angle are obtained as follows:(13)ω˙ξ=Bmvωξ+Dmv2,ω˙η=−Amv2φ2−ψ2+Cmvωη−CAωξωζ+ωη2tanφ2,ω˙ξ=Amv2φa−θa+Cmvωζ+CAωξωη−ωηωζtanφ2,φ˙a=ωζcosφ2,φ˙2=−ωη,γ˙=ωξ−ωζtanφ2,A˙m=0,C˙m=0,B˙m=0,D˙m=0,where the rocket velocity v, the flight-path angle θa, and the trajectory deflection angle ψ2 can be calculated from the GPS data. The calculation formula can be derived from around-centroid motion equations of the rigid body as follows:(14)θa=arctanvyvx,ψ2=arctanvzvxcosθa,where vx, vy, and vz are components of velocity v in the coordinate system N, which can be calculated from GPS data.


## 3.2. Solving Roll Angle with Three-Axis Magnetometer

Among the three angular velocity components of the high rotational speed aircraft, the roll angular velocity is much larger than the angular velocities in the other two directions, resulting in a very large gyrodrift error in the roll direction. Consequently, the gyroscope’s measurement is inaccurate even after calibration, and the calculated roll angle can have a very large error. By contrast, the magnetometer can prevent the accumulation of the calculation error and will not generate measurement distortion under the condition of a large angular velocity, which makes it very suitable for measuring the angular velocity and roll angle of the high rotational speed carrier.


### 3.2.1. Longitudinal Rotation Angle of Rocket

As shown in Figure3, when the rocket is flying in the air, the coordinate system B rotates along with it in the Earth’s magnetic field. The projection components of the geomagnetic intensity vector M⟶ on the coordinate system B are Mbx, Mby, and Mbz, respectively, wherein, Mby and Mbz constitute the lateral projection component MH. Thus, the longitudinal rotation angle ϕ of the carrier is as follows:(15)ϕ=arctanMbzMby.Figure 3
The component of the geomagnetic vector on the rocket. (a) Overall view (b) Lateral view.
(a)
(b)WhenMby=0, a calculation blind spot occurs in equation (15). As can be seen from Figure 3(b), axis OY1 rotates to an angle perpendicular to the direction of the Earth’s magnetic field and the roll angle should be either π/2 or 3π/2, depending on the sign of Mbz.


### 3.2.2. Calculation of the Roll Angle

The roll angleγ is the angle between axis OY1 of the coordinate system B and the vertical plane containing the longitudinal axis of the body, i.e., the angle that the coordinate system B has rotated relative to the coordinate system A. The rotation angle ϕ calculated by equation (15) is the longitudinal rotation angle of the body, which is mistaken as a roll angle in many papers. The rotation angle ϕ is calculated from the projection component of the geomagnetic intensity. Since the geomagnetic field does not coincide with the geographic field, giving rise to magnetic declination, there is a phase difference γ0 between ϕ and γ, as shown in Figure 4.Figure 4
Projection of the geomagnetic componentMH in the lateral direction of coordinate system A and B.The calculation formula of the roll angle can be obtained from Figure4 as follows:(16)γ=γ0−ϕ.In Figure4, the projected components of the geomagnetic vector M⟶ along axes Oη and Oζ of the coordinate system A are Mη and Mζ, respectively. Therefore, the phase difference γ0 is given as follows:(17)γ0=arctanMζMη.Then, the complete calculation formula for the roll angle can be obtained by combining (15)–(17):(18)γ=arctanMζMη−arctanMbzMby,where Mby and Mbz are the magnetometer output signals and Mη and Mζ can be obtained using the geomagnetic field model and the relevant coordinate system transfer matrix. The geomagnetic field vector is generally described by a geographic coordinate system OXNYEZD, referred to as the coordinate system E. Assuming that the components of the geomagnetic intensity vector in the north-east-down (NED) are MN, ME, and MD, respectively, they can be calculated using the geomagnetic field model. Subsequently, Mη and Mζ can be calculated through two rounds of coordinate system conversion: transferring from the coordinate system E to N, followed by transferring from the coordinate system N to A.Assume that the fire direction, i.e., the angle between the shooting direction and the geographical north isαN. The matrix for transferring from the coordinate system E to N is(19)CEN=cosαNsinαN000−1−sinαNcosαN0.The matrix for transferring from the systemN to A is(20)CNA=cosφ2cosφacosφ2sinφasinφ2−sinφacosφa0−sinφ2cosφa−sinφ2sinφacosφ2.The transfer matrix from the coordinate systemE to A can be obtained by combining (19) and (20):(21)CEA=CNACEN=cosφ2cosφacosαN−sinφ2sinαNcosφ2cosφasinαN+sinφ2cosαN−cosφ2sinφa−sinφacosαN−sinφasinαNcosφa−sinφ2cosφacosαN−cosφ2sinαN−sinφ2cosφasinαN+cosφ2cosαNsinφ2sinφa.Then, the projection componentMξ, Mη, and Mζ of the geomagnetic vector M⟶ in the coordinate system A is(22)MξMηMζ=CEAMNMEMD,wherein, Mη and Mζ required for (18) are(23)Mη=−sinφacosαNMN−sinφasinαNME+cosφaMD,Mζ=−sinφ2cosφacosαN−cosφ2sinαNMN⋯,−sinφ2cosφasinαN−cosφ2cosαNME+sinφ2sinφaMD.Since the attack angle of the rocket during steady flight is small, the attitude anglesφa and φ2 in the abovementioned equation can be replaced by the flight-path angle θa and trajectory deflection angle ψ2, respectively.


### 3.2.3. Angular Velocity Conversion Equations

The three-axis gyroscope measures the three angular velocities of the carrier, i.e., the componentsωbx, ωby, and ωbz along the three axes of the coordinate system B, whereas the results of calculation using (13) are the projection components ωξ, ωη, and ωζ of the angular velocity on the coordinate system A. Therefore, a conversion between the gyroscope measurements and the calculated results is needed. Since the coordinate system A differs from the coordinate system B only by a roll angle γ, the required transfer matrix is as follows:(24)CBA=1000cosγ−sinγ0sinγcosγ.Thus, the conversion equations between the two sets of angular velocities are given as(25)ωbx=ωξ,ωby=ωηcosγ+ωζsinγ,ωbz=−ωηsinγ+ωζcosγ.


## 3.2.1. Longitudinal Rotation Angle of Rocket

As shown in Figure3, when the rocket is flying in the air, the coordinate system B rotates along with it in the Earth’s magnetic field. The projection components of the geomagnetic intensity vector M⟶ on the coordinate system B are Mbx, Mby, and Mbz, respectively, wherein, Mby and Mbz constitute the lateral projection component MH. Thus, the longitudinal rotation angle ϕ of the carrier is as follows:(15)ϕ=arctanMbzMby.Figure 3
The component of the geomagnetic vector on the rocket. (a) Overall view (b) Lateral view.
(a)
(b)WhenMby=0, a calculation blind spot occurs in equation (15). As can be seen from Figure 3(b), axis OY1 rotates to an angle perpendicular to the direction of the Earth’s magnetic field and the roll angle should be either π/2 or 3π/2, depending on the sign of Mbz.


## 3.2.2. Calculation of the Roll Angle

The roll angleγ is the angle between axis OY1 of the coordinate system B and the vertical plane containing the longitudinal axis of the body, i.e., the angle that the coordinate system B has rotated relative to the coordinate system A. The rotation angle ϕ calculated by equation (15) is the longitudinal rotation angle of the body, which is mistaken as a roll angle in many papers. The rotation angle ϕ is calculated from the projection component of the geomagnetic intensity. Since the geomagnetic field does not coincide with the geographic field, giving rise to magnetic declination, there is a phase difference γ0 between ϕ and γ, as shown in Figure 4.Figure 4
Projection of the geomagnetic componentMH in the lateral direction of coordinate system A and B.The calculation formula of the roll angle can be obtained from Figure4 as follows:(16)γ=γ0−ϕ.In Figure4, the projected components of the geomagnetic vector M⟶ along axes Oη and Oζ of the coordinate system A are Mη and Mζ, respectively. Therefore, the phase difference γ0 is given as follows:(17)γ0=arctanMζMη.Then, the complete calculation formula for the roll angle can be obtained by combining (15)–(17):(18)γ=arctanMζMη−arctanMbzMby,where Mby and Mbz are the magnetometer output signals and Mη and Mζ can be obtained using the geomagnetic field model and the relevant coordinate system transfer matrix. The geomagnetic field vector is generally described by a geographic coordinate system OXNYEZD, referred to as the coordinate system E. Assuming that the components of the geomagnetic intensity vector in the north-east-down (NED) are MN, ME, and MD, respectively, they can be calculated using the geomagnetic field model. Subsequently, Mη and Mζ can be calculated through two rounds of coordinate system conversion: transferring from the coordinate system E to N, followed by transferring from the coordinate system N to A.Assume that the fire direction, i.e., the angle between the shooting direction and the geographical north isαN. The matrix for transferring from the coordinate system E to N is(19)CEN=cosαNsinαN000−1−sinαNcosαN0.The matrix for transferring from the systemN to A is(20)CNA=cosφ2cosφacosφ2sinφasinφ2−sinφacosφa0−sinφ2cosφa−sinφ2sinφacosφ2.The transfer matrix from the coordinate systemE to A can be obtained by combining (19) and (20):(21)CEA=CNACEN=cosφ2cosφacosαN−sinφ2sinαNcosφ2cosφasinαN+sinφ2cosαN−cosφ2sinφa−sinφacosαN−sinφasinαNcosφa−sinφ2cosφacosαN−cosφ2sinαN−sinφ2cosφasinαN+cosφ2cosαNsinφ2sinφa.Then, the projection componentMξ, Mη, and Mζ of the geomagnetic vector M⟶ in the coordinate system A is(22)MξMηMζ=CEAMNMEMD,wherein, Mη and Mζ required for (18) are(23)Mη=−sinφacosαNMN−sinφasinαNME+cosφaMD,Mζ=−sinφ2cosφacosαN−cosφ2sinαNMN⋯,−sinφ2cosφasinαN−cosφ2cosαNME+sinφ2sinφaMD.Since the attack angle of the rocket during steady flight is small, the attitude anglesφa and φ2 in the abovementioned equation can be replaced by the flight-path angle θa and trajectory deflection angle ψ2, respectively.


## 3.2.3. Angular Velocity Conversion Equations

The three-axis gyroscope measures the three angular velocities of the carrier, i.e., the componentsωbx, ωby, and ωbz along the three axes of the coordinate system B, whereas the results of calculation using (13) are the projection components ωξ, ωη, and ωζ of the angular velocity on the coordinate system A. Therefore, a conversion between the gyroscope measurements and the calculated results is needed. Since the coordinate system A differs from the coordinate system B only by a roll angle γ, the required transfer matrix is as follows:(24)CBA=1000cosγ−sinγ0sinγcosγ.Thus, the conversion equations between the two sets of angular velocities are given as(25)ωbx=ωξ,ωby=ωηcosγ+ωζsinγ,ωbz=−ωηsinγ+ωζcosγ.


## 4. UKF Design for Combinatory Attitude Identification

The UKF mainly consists of two phases: the prediction phase and the correction phase. In the prediction phase, a set of state prediction Sigma points should be generated. Since the state equation is a continuous model, discretization needs to be carried out that can directly affect the accuracy of the filtering results. The Runge–Kutta method is often used in ballistic calculations as it outperforms other methods in terms of discretization accuracy under the same step size. In this paper, the fourth-order classical Runge–Kutta method is used for state estimation in the prediction stage. Therefore, the filtering algorithm used in this paper is called the RK4-UKF algorithm.Assume that the state equation of a continuous nonlinear system is(26)X˙k=fXk−1,k−1+Wk−1.The measurement equation is(27)Yk=hXk,k+Vk.The workflow of the RK4-UKF algorithm is as follows:① Calculation of the sigma point set:(28)Xk−10=x^k−1,Xk−1i=x^k−1+n+λPx,i=1,2,…,n,x^k−1−n+λPx,i=n+1,…,2n.② Prediction phase:(29)k1=fXk−1i,k2=fXk−1i+h2k1,k3=fXk−1i+h2k2,k4=fXk−1i+hk3,Xk/k−1i=Xk−1i+h6k1+2k2+2k3+k4,x^k/k−1=∑i=02nWimXk/k−1i,Pk/k−1=∑i=02nWicXk/k−1i−x^k/k−1Xk/k−1i−x^k/k−1T+Qk.③ Correction phase:(30)Yk/k−1i=hXk/k−1i,y^k/k−1=∑i=02nWimYk/k−1i,PYYk/k−1=∑i=02nWicYk/k−1i−y^k/k−1Yk/k−1i−y^k/k−1T+Rk,PXYk/k−1=∑i=02nWicXk/k−1i−x^k/k−1Yk/k−1i−y^k/k−1T,Kk=PXYk/k−1PYYk/k−1−1,X^k=X^k/k−1+KkYk−y^k/k−1,Pk=Pk/k−1−KYYk/k−1KkT.


### 4.1. State Equation

Given the continuous nonlinear state equations in (13), the state variables are written as(31)x=x1x2x3x4x5x6x7x8x9x10=ωξωηωζφaφ2γAmCmBmDm.Then, (17) can be written as(32)x˙=fx=x9x1v+x10v2−x7v2x5−ψ2+x8x2v−CAx1x3+x22tanx5x7v2x4−θa+x8x3v+CAx1x2−x2x3tanx5x3cosx5−x2x1−x3tanx50000+W.As the nonlinear equations given in (13) only approximately describe the around-centroid motion of the high rotational speed rocket, there will be certain errors. Therefore, Gaussian white noise W∼N0,Q is introduced to model these errors.


### 4.2. Measurement Equation

The output of the three-axis gyroscope and the roll angle calculated by the magnetometer are taken as the measured values and are recorded asy=ωbxωbyωbzγT. According to (25), the measurement equation is constructed as follows:(33)y=Hx+V=x1x2cosx6+x3sinx6−x2sinx6+x3cosx6x6+V,where, the measurement noise V is Gaussian white noise and V∼N0,R.


## 4.1. State Equation

Given the continuous nonlinear state equations in (13), the state variables are written as(31)x=x1x2x3x4x5x6x7x8x9x10=ωξωηωζφaφ2γAmCmBmDm.Then, (17) can be written as(32)x˙=fx=x9x1v+x10v2−x7v2x5−ψ2+x8x2v−CAx1x3+x22tanx5x7v2x4−θa+x8x3v+CAx1x2−x2x3tanx5x3cosx5−x2x1−x3tanx50000+W.As the nonlinear equations given in (13) only approximately describe the around-centroid motion of the high rotational speed rocket, there will be certain errors. Therefore, Gaussian white noise W∼N0,Q is introduced to model these errors.


## 4.2. Measurement Equation

The output of the three-axis gyroscope and the roll angle calculated by the magnetometer are taken as the measured values and are recorded asy=ωbxωbyωbzγT. According to (25), the measurement equation is constructed as follows:(33)y=Hx+V=x1x2cosx6+x3sinx6−x2sinx6+x3cosx6x6+V,where, the measurement noise V is Gaussian white noise and V∼N0,R.


## 5. Combinatory Attitude Determination System

The combinatory attitude determination system consists of a data acquisition unit, a preprocessing unit, and a filtering unit, and the block diagram of which is shown in Figure5. The data acquisition unit of the system is responsible for data collection, which consists of a GPS, a three-axis gyroscope, and a three-axis magnetometer. The preprocessing unit is responsible for (1) denoising the signals; (2) calculating the velocity, flight-path angle, and trajectory deflection angle using the GPS signal; (3) obtaining the geomagnetic information using the position information and the geomagnetic model; (4) calculating the roll angle using the magnetometer signal. The filtering unit is responsible for processing signals according to the constructed state equations and measurement equations and finally outputting the attitude and other additional information.Figure 5
Flowchart of combinatory attitude determination.


## 6. Simulation and Analysis

Assume an ideally axisymmetric rocket with uniform mass distribution. The shape of the rocket has no aerodynamic eccentricity and there is no wind. The combinatory attitude determination method proposed above is simulated, and the performance of the proposed RK4-UKF is evaluated.Firstly, the data of the entire rocket trajectory is simulated as the data source according to the rocket motion equations [22].Because the combinatory attitude determination method uses magnetometer signal of the lateral axis of the carrier and the geomagnetic direction is approximately north-south, the elevation angleθ0 is set to 42 and the fire direction αN is set to 100, as shown in Figure 6. The launch position can be set to any value on the ground. In the simulation, the latitude and longitude coordinates of the launch position are (N40°, E100°), and the altitude is set to zero. C++ language is used in the simulation program, and the time step size of the simulation is 1 ms.Figure 6
The elevation angle and fire direction.Then, the output signals of various sensors required by the combined attitude determination method are simulated by the ballistic data source. The error-free output signal of the gyroscope is simulated using the transfer matrixCAB of coordinate system A to coordinate system B, i.e., the inverse matrix of (24). The GPS output signal is simulated using the rocket position information, the transfer matrix of related coordinate system [22]. The output signal of the magnetometer is simulated using the rocket position information, the initial launching element, the geomagnetic field model, and the transfer matrix CEB of coordinate system E to coordinate system B, where matrix CEB is obtained as follows:(34)CEB=CABCEA,where CAB and CEA can be obtained by formulas (24) and (21), respectively.The signal simulation process is shown in Figure7. In order to facilitate the visualization of the data, MATLAB is used for data conversion in the simulation process.Figure 7
Flowchart of signals simulation.Finally, the combinatory attitude determination method proposed in paper is simulated after the signals simulation. MATLAB is used in the simulation program. For displaying the details clearly, the figure only shows the simulation data and processing results corresponding to the trajectory segment during the initial 5 s.


### 6.1. Relationship between Attitude Angles and Flight-Path Angle and Trajectory Deflection Angle

When a high rotational speed rocket is in stable flight, its stability fits with the Lyapunov stability if resonance is not considered. The longitudinal axis of the rocket will oscillate periodically around the velocity line after the rocket experiences a disturbance. Pitch angleφa oscillates up and down around the flight-path angle θa and yaw angle φ2 oscillates left and right around the trajectory deflection angle ψ2, with the oscillation amplitude diminishing continuously, as shown in Figure 8. The simulation results also indicate that the attitude angles of the rocket are affected by the initial disturbance at the time of launch. The oscillation amplitude is large at the beginning and then decreases gradually.Figure 8
Relationship between attitude angles and flight-path angle and trajectory deflection angle. (a) Pitch angle versus flight-path angle. (b) Yaw angle versus trajectory deflection angle.
(a)
(b)


### 6.2. Simulation of Roll Angle Calculation

The simulated GPS signal provides position information such as latitude and longitude, and altitude and can be used to calculate the flight velocityv, velocity components vx, vy, and vz, flight-path angle θa, and trajectory deflection angle ψ2. Meanwhile, it is also possible to calculate the geomagnetic information of the whole trajectory using the IGRF12, chiefly the geomagnetic components MN, ME, and MD in the NED directions, and then calculate the projected components Mη and Mζ. The simulated magnetometer signal provides the projection components Mby and Mbz of the geomagnetic intensity vector M⟶ on the rocket body.According to equation (18), the roll angle is obtained and compared with the true value, as shown in Figure 9. Comparing the calculated value and the true value, it can be observed that the calculation accuracy is high, the difference between the two values is on the order of magnitude of 10−3 rad. The main cause of this error is the use of flight-path angle and trajectory deflection angle instead of attitude angles. As seen by observing the calculation error shown in Figure 9(b) and the correlation of the relationship between the attitude angles and the flight-path angle and trajectory deflection angle shown in Figure 8, it is evident that the larger the amplitude of the attitude angles oscillation, the larger the calculation error, with the error decreasing as the amplitude of the oscillation diminishes. Since flight-path angle and trajectory deflection angle differ slightly from the attitude angles, the error is generally small and within an acceptable range.Figure 9
Calculation of roll angle. (a) True value and calculated value of roll angle. (b) Error of calculation.
(a)
(b)


### 6.3. RK4-UKF Simulation

Gaussian white noise was added to the gyroscope error-free signal to simulate real gyroscope output signal. According to the angular variation characteristics of the high rotational speed rocket, Gaussian white noise with different variances were added to the angular velocities and served as measured values. As shown in Figure10, noise with distribution dx∼N0.1rad,1rad/s was added to the angular velocity ωbx, noise with distribution dyz∼N0.01rad,0.1rad/s was added to both angular velocities ωby and ωbz, and noise with distribution dγ∼N0,1.5° was added to the real value of the roll angle. Figure 10(a) shows the Gaussian white noise added to the three-axis gyroscope. The maximum error of the angular velocity ωbx is about ±4rad/s, and the maximum error of both angular velocities ωby and ωbz is about ±0.4rad/s. Figure 10(b) shows the Gaussian white noise added to the roll angle, and the maximum error is about ±6∘.Figure 10
Noise of the measured value. (a) Noise of gyroscope output. (b) Noise of roll angle.
(a)
(b)Filtering was performed separately using the RK4-UKF algorithm and the UKF algorithm with the standard Euler method, with the discretization step sizes set to 1 ms and 10 ms. The attitude angles of the rocket were estimated and compared with the true values.Figure11 shows the filtering result under the discretization step size of 1 ms. Figures 11(a) and 11(b) compare pitch, yaw, and roll angles. It can be seen from the figures that the estimated values are very close to the true values for both UKF and RK4-UKF algorithms. Only minor deviations can be observed at certain locations.Figure 11
Comparison of filtered results and true values. (a) Pitch and yaw angles. (b) Roll angle.
(a)
(b)Figure12 further shows the filtering errors for the three attitude angles. Both the standard UKF and RK4-UKF yield relatively high filtering accuracy: the errors for pitch and yaw angles are on the order of magnitude of 10−3 rad, and the error for roll angle is slightly larger, on the order of magnitude of 10−2 rad. Since the filter step size is the same as the step size of the ballistic data simulation, both of which is 1 ms, the approximation model used for filtering exhibits higher precision. The standard UKF and RK4-UKF yield roughly the same filtering precision. It can also be seen from the figures that the error is relatively large in the initial segment of the trajectory, where the oscillation amplitude of the longitudinal axis is relatively large and the error decreases as the oscillation amplitude diminishes. The key causes of the error include ① the combinatory attitude determination method proposed in this paper is derived using the kinematics equations, and assumptions are made for constraining the attitude angles with flight-path angle and trajectory deflection angle. Therefore, the more different the attitude angles are compared to the flight-path angle and trajectory deflection angle, the lower the filtering accuracy; ② as the flight-path angle and trajectory deflection angle are used instead of the attitude angles in the calculation of the roll angle measured value, the resulting calculation error will be passed to the attitude estimation system. In short, the larger the oscillation of the longitudinal axis of the rocket, consequently, the larger the filtering error.Figure 12
Error of estimation. (a) Error of pitch and yaw angle. (b) Error of roll angle.
(a)
(b)The filtering result with the discretization step size set to 10 ms is shown in Figure13. Compared with the filtering result under the discretization step size of 1 ms, the filtering accuracies of pitch, yaw, and roll angles decrease. It can be seen from the figure that the filtering sampling points are sparse, and the filtering result deviates significantly from the true value at some locations.Figure 13
Comparison of estimated values and true values. (a) Pitch angle and yaw angle. (b) Roll angle.
(a)
(b)Figure14 shows the estimation errors. Compared with the filtering results under the discretization step size of 1 ms, both UKF and RK4-UKF yield larger filtering errors, but the filtering accuracy of the latter algorithm is significantly higher than that of the latter algorithm. The pitch and yaw errors of the standard UKF increase to the order of magnitude of 10−2 rad, and the roll angle error increases to the order of magnitude of 10−1 rad. The RK4-UKF algorithm is more robust than the standard UKF.Figure 14
Comparison of filtered results between RK4-UKF and UKF. (a) Error of pitch and yaw angle. (b) Error of roll angle.
(a)
(b)Finally, the computing time of UKF and RK4-UKF is compared. Perform 1000 calculations and record the computing time. When the step size is 1 ms, the average computing time of UKF is about 3.6143 s and the average computing time of RK4-UKF is about 5.0344 s. When the step size is 10 ms, the average computing time of UKF is about 0.4272 s and the average computing time of RK4-UKF is about 0.5347 s. Compared with UKF, RK4-UKF takes slightly longer time to calculate.


### 6.4. Simulation Analysis

As shown by the simulations and processing results, the combinatory attitude determination method proposed in the paper could estimate the attitude angles of stable flying rockets with high precision. Moreover, the RK4-UKF algorithm is more robust than the standard UKF algorithm and can determine attitude parameters with relatively high accuracy when the discretization step size is large. The following points can be summarized based on the simulation process and analysis:① Several assumptions were made for deriving the motion model of the high rotational speed aircraft: the rocket was ideally axisymmetric, the shape of the rocket had no aerodynamic eccentricity, and the rocket mass was distributed uniformly. Moreover, during the derivation process, the influence of wind and the relatively small Magnus moment were ignored, and the approximations ofδ1=φa−θaandδ2=φ2−ψ2 were made for the attack angle components. Consequently, the Gaussian white noise W in the state equations (32) is uncertain.② The attitude information is often used for navigation and guidance of aircraft, which is usually based on the geographic coordinate system, for example, the GPS. Therefore, during the process of measuring and calculating the roll angle with a magnetometer, it is necessary to consider the magnetic declination, i.e., the angle between the geomagnetic north and the geographic north. Therefore, when the geomagnetic data are used to calculate the roll angle, a phase difference must be introduced if the geographic coordinate system is used as the benchmark.③ Under the same sampling step size, the fourth-order Runge–Kutta discretization method yields lower discretization errors, and the state equations after discretization are close to the continuous model. Compared with the UKF algorithm employing the Euler discretization method, the RK4-UKF algorithm is more robust and less dependent on the discretization step size, but takes slightly longer time.


## 6.1. Relationship between Attitude Angles and Flight-Path Angle and Trajectory Deflection Angle

When a high rotational speed rocket is in stable flight, its stability fits with the Lyapunov stability if resonance is not considered. The longitudinal axis of the rocket will oscillate periodically around the velocity line after the rocket experiences a disturbance. Pitch angleφa oscillates up and down around the flight-path angle θa and yaw angle φ2 oscillates left and right around the trajectory deflection angle ψ2, with the oscillation amplitude diminishing continuously, as shown in Figure 8. The simulation results also indicate that the attitude angles of the rocket are affected by the initial disturbance at the time of launch. The oscillation amplitude is large at the beginning and then decreases gradually.Figure 8
Relationship between attitude angles and flight-path angle and trajectory deflection angle. (a) Pitch angle versus flight-path angle. (b) Yaw angle versus trajectory deflection angle.
(a)
(b)


## 6.2. Simulation of Roll Angle Calculation

The simulated GPS signal provides position information such as latitude and longitude, and altitude and can be used to calculate the flight velocityv, velocity components vx, vy, and vz, flight-path angle θa, and trajectory deflection angle ψ2. Meanwhile, it is also possible to calculate the geomagnetic information of the whole trajectory using the IGRF12, chiefly the geomagnetic components MN, ME, and MD in the NED directions, and then calculate the projected components Mη and Mζ. The simulated magnetometer signal provides the projection components Mby and Mbz of the geomagnetic intensity vector M⟶ on the rocket body.According to equation (18), the roll angle is obtained and compared with the true value, as shown in Figure 9. Comparing the calculated value and the true value, it can be observed that the calculation accuracy is high, the difference between the two values is on the order of magnitude of 10−3 rad. The main cause of this error is the use of flight-path angle and trajectory deflection angle instead of attitude angles. As seen by observing the calculation error shown in Figure 9(b) and the correlation of the relationship between the attitude angles and the flight-path angle and trajectory deflection angle shown in Figure 8, it is evident that the larger the amplitude of the attitude angles oscillation, the larger the calculation error, with the error decreasing as the amplitude of the oscillation diminishes. Since flight-path angle and trajectory deflection angle differ slightly from the attitude angles, the error is generally small and within an acceptable range.Figure 9
Calculation of roll angle. (a) True value and calculated value of roll angle. (b) Error of calculation.
(a)
(b)


## 6.3. RK4-UKF Simulation

Gaussian white noise was added to the gyroscope error-free signal to simulate real gyroscope output signal. According to the angular variation characteristics of the high rotational speed rocket, Gaussian white noise with different variances were added to the angular velocities and served as measured values. As shown in Figure10, noise with distribution dx∼N0.1rad,1rad/s was added to the angular velocity ωbx, noise with distribution dyz∼N0.01rad,0.1rad/s was added to both angular velocities ωby and ωbz, and noise with distribution dγ∼N0,1.5° was added to the real value of the roll angle. Figure 10(a) shows the Gaussian white noise added to the three-axis gyroscope. The maximum error of the angular velocity ωbx is about ±4rad/s, and the maximum error of both angular velocities ωby and ωbz is about ±0.4rad/s. Figure 10(b) shows the Gaussian white noise added to the roll angle, and the maximum error is about ±6∘.Figure 10
Noise of the measured value. (a) Noise of gyroscope output. (b) Noise of roll angle.
(a)
(b)Filtering was performed separately using the RK4-UKF algorithm and the UKF algorithm with the standard Euler method, with the discretization step sizes set to 1 ms and 10 ms. The attitude angles of the rocket were estimated and compared with the true values.Figure11 shows the filtering result under the discretization step size of 1 ms. Figures 11(a) and 11(b) compare pitch, yaw, and roll angles. It can be seen from the figures that the estimated values are very close to the true values for both UKF and RK4-UKF algorithms. Only minor deviations can be observed at certain locations.Figure 11
Comparison of filtered results and true values. (a) Pitch and yaw angles. (b) Roll angle.
(a)
(b)Figure12 further shows the filtering errors for the three attitude angles. Both the standard UKF and RK4-UKF yield relatively high filtering accuracy: the errors for pitch and yaw angles are on the order of magnitude of 10−3 rad, and the error for roll angle is slightly larger, on the order of magnitude of 10−2 rad. Since the filter step size is the same as the step size of the ballistic data simulation, both of which is 1 ms, the approximation model used for filtering exhibits higher precision. The standard UKF and RK4-UKF yield roughly the same filtering precision. It can also be seen from the figures that the error is relatively large in the initial segment of the trajectory, where the oscillation amplitude of the longitudinal axis is relatively large and the error decreases as the oscillation amplitude diminishes. The key causes of the error include ① the combinatory attitude determination method proposed in this paper is derived using the kinematics equations, and assumptions are made for constraining the attitude angles with flight-path angle and trajectory deflection angle. Therefore, the more different the attitude angles are compared to the flight-path angle and trajectory deflection angle, the lower the filtering accuracy; ② as the flight-path angle and trajectory deflection angle are used instead of the attitude angles in the calculation of the roll angle measured value, the resulting calculation error will be passed to the attitude estimation system. In short, the larger the oscillation of the longitudinal axis of the rocket, consequently, the larger the filtering error.Figure 12
Error of estimation. (a) Error of pitch and yaw angle. (b) Error of roll angle.
(a)
(b)The filtering result with the discretization step size set to 10 ms is shown in Figure13. Compared with the filtering result under the discretization step size of 1 ms, the filtering accuracies of pitch, yaw, and roll angles decrease. It can be seen from the figure that the filtering sampling points are sparse, and the filtering result deviates significantly from the true value at some locations.Figure 13
Comparison of estimated values and true values. (a) Pitch angle and yaw angle. (b) Roll angle.
(a)
(b)Figure14 shows the estimation errors. Compared with the filtering results under the discretization step size of 1 ms, both UKF and RK4-UKF yield larger filtering errors, but the filtering accuracy of the latter algorithm is significantly higher than that of the latter algorithm. The pitch and yaw errors of the standard UKF increase to the order of magnitude of 10−2 rad, and the roll angle error increases to the order of magnitude of 10−1 rad. The RK4-UKF algorithm is more robust than the standard UKF.Figure 14
Comparison of filtered results between RK4-UKF and UKF. (a) Error of pitch and yaw angle. (b) Error of roll angle.
(a)
(b)Finally, the computing time of UKF and RK4-UKF is compared. Perform 1000 calculations and record the computing time. When the step size is 1 ms, the average computing time of UKF is about 3.6143 s and the average computing time of RK4-UKF is about 5.0344 s. When the step size is 10 ms, the average computing time of UKF is about 0.4272 s and the average computing time of RK4-UKF is about 0.5347 s. Compared with UKF, RK4-UKF takes slightly longer time to calculate.


## 6.4. Simulation Analysis

As shown by the simulations and processing results, the combinatory attitude determination method proposed in the paper could estimate the attitude angles of stable flying rockets with high precision. Moreover, the RK4-UKF algorithm is more robust than the standard UKF algorithm and can determine attitude parameters with relatively high accuracy when the discretization step size is large. The following points can be summarized based on the simulation process and analysis:① Several assumptions were made for deriving the motion model of the high rotational speed aircraft: the rocket was ideally axisymmetric, the shape of the rocket had no aerodynamic eccentricity, and the rocket mass was distributed uniformly. Moreover, during the derivation process, the influence of wind and the relatively small Magnus moment were ignored, and the approximations ofδ1=φa−θaandδ2=φ2−ψ2 were made for the attack angle components. Consequently, the Gaussian white noise W in the state equations (32) is uncertain.② The attitude information is often used for navigation and guidance of aircraft, which is usually based on the geographic coordinate system, for example, the GPS. Therefore, during the process of measuring and calculating the roll angle with a magnetometer, it is necessary to consider the magnetic declination, i.e., the angle between the geomagnetic north and the geographic north. Therefore, when the geomagnetic data are used to calculate the roll angle, a phase difference must be introduced if the geographic coordinate system is used as the benchmark.③ Under the same sampling step size, the fourth-order Runge–Kutta discretization method yields lower discretization errors, and the state equations after discretization are close to the continuous model. Compared with the UKF algorithm employing the Euler discretization method, the RK4-UKF algorithm is more robust and less dependent on the discretization step size, but takes slightly longer time.


## 7. Conclusions

The attitude measurement system of a rigid-body aircraft has higher requirements on the sensors due to high rotational speed and highly dynamic characteristics. Combining measurement system of multiple sensors can increase the accuracy of attitude estimation. In this study, we analyzed the external moments and angular motion characteristics of the aircraft during stable flight and made a few appropriate assumptions and approximations for around-centroid motion equations of the rigid body. Subsequently, we obtained a set of dynamic constraint equations involving the lateral attitude angles and the flight-path angle and trajectory deflection angle through deduction. The dynamic constraint equations were used as the driving equations for attitude determination. The advantages of different sensors were combined by fusing the data from multiple sensors, and accurate flight attitudes were obtained using the RK4-UKF algorithm. Finally, the feasibility and effectiveness of the proposed method were verified through a simulation in which different discretization step sizes were used. The problems encountered in the simulation process were analyzed and discussed, and the following points are worth noting:① During the process of deriving the dynamic constraint equations, the following approximations can be made only when the aircraft is in stable flight state: the attack angle componentδ1 in the pitch direction is the difference between the pitch angle φa and the flight-path angle θa, and the attack angle component δ2 in the yaw direction is the difference between the yaw angle φ2 and the trajectory deflection angle ψ2. For flight carriers that do not have the three major stability properties, the proposed method is less suitable.② The research object studied in this paper is a high rotational speed rocket. Therefore, the motion equations and external moments described in this paper are only relevant to high rotational speed rockets. For other types of aircrafts, the motion equations and external moments will be different. Nonetheless, the combinatory attitude determination method proposed in this paper is applicable to all high rotational speed uncontrolled axisymmetric flight carriers such as rotating tail projectiles.Based on the motion equations and dynamics of the rigid body, the proposed method combines the advantages of multiple types of sensors and is easy to implement. In line with the current trend of intelligent aircraft, the proposed method can play an important role in navigation and guidance of high rotational speed flight carriers and has the potential of being widely used in high-precision flight control.



---

*Source: 7130142-2020-04-21.xml*