research article cone algorithm of spinning vehicles under ...tional coning correction algorithms...

Research ArticleCone Algorithm of Spinning Vehicles underDynamic Coning Environment

Shuang-biao Zhang,1,2 Xing-cheng Li,1,2 and Zhong Su3

1School of Aerospace Engineering, Beijing Institute of Technology, Beijing 100081, China2Key Laboratory of Dynamics and Control of Flight Vehicle, Ministry of Education, Beijing 100081, China3Beijing Key Laboratory of High Dynamic Navigation Technology, Beijing Information Science & Technology University,Beijing 100101, China

Correspondence should be addressed to Xing-cheng Li; [email protected]

Received 20 August 2015; Revised 30 October 2015; Accepted 10 November 2015

Academic Editor: Hikmat Asadov

Copyright © 2015 Shuang-biao Zhang et al. This is an open access article distributed under the Creative Commons AttributionLicense, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properlycited.

Due to the fact that attitude error of vehicles has an intense trend of divergence when vehicles undergo worsening coningenvironment, in this paper, themodel of dynamic coning environment is derived firstly.Then, through investigation of the effect onEuler attitude algorithm for the equivalency of traditional attitude algorithm, it is found that attitude error is actually the roll angleerror including drifting error and oscillating error, which is induced directly by dynamic coning environment and further affectsthe pitch angle and yaw angle through transferring. Based on definition of the cone frame and cone attitude, a cone algorithmis proposed by rotation relationship to calculate cone attitude, and the relationship between cone attitude and Euler attitude ofspinning vehicle is established. Through numerical simulations with different conditions of dynamic coning environment, it isshown that the induced error of Euler attitude fluctuates by the variation of precession and nutation, especially by that of nutation,and the oscillating frequency of roll angle error is twice that of pitch angle error and yaw angle error. In addition, the rotation angleis more competent to describe the spinning process of vehicles under coning environment than Euler angle gamma, and the realpitch angle and yaw angle are calculated finally.

1. Introduction

Attitude algorithm is a key part of navigation technology andguarantees directly control system accuracy and navigationaccuracy of aircrafts, ships, and vehicles. Through decadesof persistent efforts by researchers, to expand applicabilityof attitude algorithm, an outstanding two-stage structureof attitude algorithm is refined and outlined [1], whichconsists of attitude matrix update cycle by Jordan [2] androtation vector update cycle by Bortz [3]. Particularly, thecrucial coning correction of noncommutativity, calculatedfrom gyro data in a separate algorithm, is the critical processexecuted in rotation vector update cycle to improve theattitude accuracy, and it has attracted more attentions fromdemanding researchers in navigation technology. This isbecause the coning error induced by coning environment canaffect navigation accuracy and control precision of vehicles

[4, 5].Miller used a quaternion algorithmwith three intervalsfrom gyro to calculate attitude and approximate coningcorrection error for the coning correction coefficients designunder a pure coning condition [6]. Lee et al. improved thequaternion algorithm with four intervals and compared drifterror with other fewer-interval algorithms [7]. Based on theMiller achievement, Ignagni proposed a two-speed structurefor coning correction and introduced nine algorithms torealize optimization of coning correction coefficients [8].Jiang and Lin proposed an improved strapdown coningalgorithm and disclosed the essential relationship betweenrotation vector and quaternion [9]. Li et al. divided minorconing correction into a number of subminor intervals anddelivered a generalized coning compensation algorithm thatis independent of the number of incremental angles [10]. Tangand Chen proposed a coning correction structure containingcross-product of angular rates, cross-product of angular

Hindawi Publishing CorporationInternational Journal of Aerospace EngineeringVolume 2015, Article ID 904913, 11 pageshttp://dx.doi.org/10.1155/2015/904913

2 International Journal of Aerospace Engineering

increments, and cross-product of angular rate and increment,and it can analyze effect on attitude by basing on time Taylorseries and frequency Taylor series [11]. Meanwhile, the gyrofrequency response is a primary cause for pseudo-coningerror that is another form of coning error, and the tradi-tional coning correction algorithms cannot work to high-order accuracy. Mark and Tazartes proposed a tuning high-order coning algorithm to match the frequency responsecharacteristics of the gyro and avoided overcompensationfor pseudo-coning correction [12]. Kim and Lee added self-vibration of gyro to the causes of pseudo-coning. Theyshowed that vibration had a form generated by different fre-quency inputted to two orthogonal axes, and coning motionwas just a specific case [13]. Savage thought that stochasticdynamic environment sensed by high precision gyro couldalso cause pseudo-coning error, so he used explicit frequencyshaping by minimum least-squares estimation for stochasticdata to design coning correction coefficients [14]. Song et al.combined with Miller’s and Savage’s methods and delivereda supplementary coning error equation to achieve superiormaneuver accuracy [15]. Considering measurement error ofgyro, Fu et al. developed a two-time scale model by singularperturbation technique to realize coning and pseudo-coningcorrection [16]. Wang et al. used a new coning motion modelthat contains pure constant coning environment and spin-ning motion of vehicles to examine the ignored triple-cross-product term of noncommutativity error in attitude rotationvector and test the performance of previous algorithms [17].Patera disclosed that no attitude error was propagated byusing a slewing frame under oscillating coning environment[18] and extended the slewing frame to improve attitude prop-agation attitude for cases of time varying coning motion [19].Unfortunately, themethod based on the slewing frame cannotprovide the common attitude of spinning vehicles undercomplex coning environment because the relationship of theslewing frame and the common attitude of vehicles is unclear.

Synthesizing and analyzing research achievements above,we find that the recognized coning error and the pseudo-coning error are generated directly while calculating rotationvector increment from gyro in rotation vector update cycle,and the optimization of attitude algorithm is mainly achievedby using the total model and the simplified model of theclassical coning environment to restrain drifting componentof coning error. However, the oscillating error of attitudeexists when the optimization of the two-stage structureof attitude algorithm is used for vehicles under classicalconing environment. Although attitude error is not largeenough for nonspinning vehicles to attract more attentions,it has an intense trend of divergence over time when coningenvironment is becoming worse. However, up to now, thereis no literature to exploit this issue, and the in-depth study ofattitude algorithm of vehicles under high dynamic environ-ment is hardly carried on. Therefore, it is necessary for us toinvestigate the real attitude of vehicles anddisclose the hiddenrelationship between vehicles’ attitude and dynamic coningenvironment.

The scheme of this paper is as follows. In Section 2, wederive themodel of dynamic coning environment by rotationvector. In Section 3, we provide the traditional two-stage

structure of attitude algorithm and investigate the effectof dynamic coning environment on attitude algorithm. InSection 4, based on the definition of the cone frame andcone attitude, we propose a cone algorithm for cone attitudeby using the rotation relationship and build the relationshipbetween cone attitude and Euler attitude. In Section 5, thesimulations are made to explain the effect of dynamic coningenvironment on tradition attitude algorithm and verify thevalidity of the cone algorithm. In Section 6, we conclude ourresearch in this paper and provide our future work.

2. Dynamic Coning Environment

Before studying the effect of dynamic coning environmenton attitude algorithm, the derivation of modelling dynamicconing environment is made in this section.

Generally, the classical coning environment is defined asa condition where two orthogonal axes in a vehicle simul-taneously experience sinusoidal oscillations that aremutuallyphase-shifted by 90 degrees [14].The third axis, which is orth-ogonal with the other two oscillating axes, will precessaround an axis and generate a conical surface in inertialspace. Under the classical coning environment, cone half-angle and oscillating frequency are all constant, and theprecession frequency of the third axis is equal to oscillatingfrequency. Obviously, this process has constant cone half-angle and constant precession frequency, so the classical con-ing environment is static. In contrast, the dynamic coningenvironment is a complex process with varying cone half-angle, so rotation vector can be defined as

𝜙 =[

[

[

0

Λ𝑡 cosΩ𝑡Λ𝑡 sinΩ𝑡

]

]

]

, (1)

where Λ is the changing rate of cone half-angle and Ωis the precession frequency. Assuming that Λ and Ω areconstant during small time interval, the derivation of (1) canbe obtained:

̇𝜙 =[

[

[

0

Λ cosΩ𝑡 − ΩΛ𝑡 sinΩ𝑡Λ sinΩ𝑡 + ΩΛ𝑡 cosΩ𝑡

]

]

]

. (2)

Referring to [3], the angular velocity can be described as

𝜔 = ̇𝜙 −1 − cos

𝜙

𝜙

2𝜙 × ̇𝜙 +

1

𝜙

2(1 −

sin

𝜙

𝜙

)𝜙

× (𝜙 × ̇𝜙) .

(3)

Substituting (1) and (2) into (3), the angular velocity ofdynamic coning environment in vehicles can be obtained:

𝜔 =[

[

[

(cosΛ𝑡 − 1)ΩΛ cosΩ𝑡 − Ω sinΛ𝑡 sinΩ𝑡Λ sinΩ𝑡 + Ω sinΛ𝑡 cosΩ𝑡

]

]

]

. (4)

International Journal of Aerospace Engineering 3

From (4), we see that if the changing rate of cone half-angle is zero but the cone half-angle is nonzero, (4) can besimplified as

𝜔 =[

[

[

(cos𝛼 − 1)Ω−Ω sin𝛼 sinΩ𝑡Ω sin𝛼 cosΩ𝑡

]

]

]

, (5)

where 𝛼 is the constant cone half-angle. It is clear that (5) isthe model of the classical coning environment, and it is just aspecial case that has no nutation rate and no rotation [3, 6, 9].

3. Effect of Dynamic Coning Environment onAttitude Algorithm

Since the common optimization algorithm is developedunder static coning environment, it is reasonable to con-sider how attitude algorithm is affected by dynamic coningenvironment. In this section, we will investigate this ques-tion.

3.1. Definition of Relative Frames. To describe the movementof vehicles with respect to the reference frame, the commonframes are introduced firstly.

The Body Frame 𝑂𝑥𝑏𝑦𝑏𝑧𝑏. The origin is located at the center

ofmass of a vehicle, 𝑥𝑏axis coincides with longitudinal axis of

a vehicle, 𝑦𝑏axis is pointing up and perpendicular to 𝑥

𝑏axis

in symmetry plane of a vehicle, and 𝑧𝑏axis is obtained by the

right-hand rule.

The Earth Frame 𝑂𝑥𝑒𝑦𝑒𝑧𝑒. The origin is located at the point

of launch site, 𝑥𝑒axis is pointing to a target, 𝑦

𝑒axis is pointing

up and perpendicular to 𝑥𝑒axis in vertical plane, and 𝑧

𝑒axis

is obtained by the right-hand rule.The rotation order of 𝑂𝑥

𝑒𝑦𝑒𝑧𝑒and 𝑂𝑥

𝑏𝑦𝑏𝑧𝑏is as follows:

𝑂𝑥𝑒𝑦𝑒𝑧𝑒

�̇�

→

𝑦𝑒

𝑂𝑥

𝑦𝑒𝑧

̇𝜗

→

𝑧

𝑂𝑥𝑏𝑦

𝑧

̇𝛾

→

𝑥

𝑏

𝑂𝑥𝑏𝑦𝑏𝑧𝑏. (6)

As is shown in Figure 1, the relationship of 𝑂𝑥𝑏𝑦𝑏𝑧𝑏and

𝑂𝑥𝑒𝑦𝑒𝑧𝑒can be described by a transformation matrix:

C𝑒𝑏=

[

[

[

cos 𝜗 cos𝜓 − sin 𝜗 cos𝜓 cos 𝛾 + sin𝜓 sin 𝛾 sin 𝜗 cos𝜓 sin 𝛾 + sin𝜓 cos 𝛾sin 𝜗 cos 𝜗 cos 𝛾 − cos 𝜗 sin 𝛾

− cos 𝜗 sin𝜓 sin 𝜗 sin𝜓 cos 𝛾 + cos𝜓 sin 𝛾 − sin 𝜗 sin𝜓 sin 𝛾 + cos𝜓 cos 𝛾

]

]

]

, (7)

where C𝑒𝑏represents transformation from the body frame to

the earth frame, 𝑒 stands for the earth frame, 𝑏 stands for thebody frame and Euler attitude angles 𝜗, 𝜓, and 𝛾 are the pitchangle, yaw angle, and roll angle, respectively. The range of 𝜗is [−90∘ 90∘], the range of 𝜓 is [−90∘ 90∘], and the range of 𝛾is [0∘ 360∘].

3.2. Two-Stage Structure of Attitude Algorithm. Theoptimiza-tion of attitude algorithm is developed by using two-stagestructure of attitude algorithm under the classical coningenvironment, and the two-stage structure algorithm in mod-ern strapdown inertial navigation systems is given by [1]

C𝑖(𝑙)𝑏(𝑙)

= C𝑖(𝑙)𝑏(𝑙−1)

⋅ C𝑏(𝑙−1)𝑏(𝑙)

, (8)

C𝑏(𝑙−1)𝑏(𝑙)

= I + 𝑓1(𝜙𝑙) (𝜙𝑙×) + 𝑓

2(𝜙𝑙) (𝜙𝑙×)

2

, (9)

𝑓1(𝜙𝑙) =

sin

𝜙𝑙

𝜙𝑙

= ∑

𝑘=1

(−1)

𝑘−1

𝜙𝑙

2(𝑘−1)

(2𝑘 − 1)!

, (10)

𝑓2(𝜙𝑙) =

1 − cos

𝜙𝑙

𝜙𝑙

2= ∑

𝑘=1

(−1)

𝑘−1

𝜙𝑙

2(𝑘−1)

(2𝑘)!

, (11)

𝜙𝑙× =

[

[

[

[

0 −𝜙𝑧

𝜙𝑦

𝜙𝑧

0 −𝜙𝑥

−𝜙𝑦

𝜙𝑥

0

]

]

]

]

, (12)

where 𝑖 represents the reference frame, 𝑏 represents the bodyframe, C𝑖(𝑙)

𝑏(𝑙)is a transformation matrix at attitude updating

cycle 𝑙,C𝑏(𝑙−1)𝑏(𝑙)

is a transformationmatrix that transforms vec-tors from the body frame at cycle 𝑙 into the body frame at cycle𝑙 − 1, 𝜙

𝑙is a rotation vector used to update C𝑏(𝑙−1)

𝑏(𝑙), and 𝜙

𝑙× is

cross-product antisymmetric matrix composed of 𝜙𝑙compo-

nents. The updating structure of rotation vector is given by

𝜙𝑙= 𝛼𝑙+ 𝛿𝜙𝑙, (13)

𝛼𝑙= ∫

𝑡

𝑡𝑙−1

𝜔 𝑑𝑡, (14)

𝛿𝜙𝑙≈ ∫

𝑡𝑙

𝑡𝑙−1

(

1

2

𝜙𝑙× 𝜔 +

1

12

𝜙𝑙× (𝜙𝑙× 𝜔)) 𝑑𝑡, (15)

where 𝜔 is measuring data from a triaxial gyro. Equation (15)is referred to as the coning error or coning correction.

According to the common optimizing methods of atti-tude algorithm by using angular increment, some coefficientsare designed to restrain the direct component of𝑥-axial angu-lar increment under static coning environment [8, 14, 15]:

𝛿𝜙𝑙=

𝑁−1

∑

𝑗=1

𝑁

∑

𝑘=𝑗+1

𝑏𝑗𝑘Δ̃𝜙𝑙(𝑗) × Δ

̃𝜙𝑙(𝑘) , (16)

where 𝑏𝑗𝑘

is the correction coefficient depending on theconing correction structure and Δ̃𝜙

𝑙is an angular increment


y

𝛾

yb

ze

z

O

ye

𝛾

𝜗

𝜗

xb

xe𝜓

x

zb

𝜓.

.

.

Figure 1: Rotational relationship of 𝑂𝑥𝑒𝑦𝑒𝑧𝑒and 𝑂𝑥

𝑏𝑦𝑏𝑧𝑏.

sample over a fixed time interval. Due to the sampling num-ber of a triaxial gyro, single-sample algorithm, two-samplealgorithm, three-sample algorithm, and so on have beendeveloped, and the validity of algorithm is described bydrifting error.

One stage is updating the attitude in an attitude updatingcycle, and it is referred to in (8) to (12). The other stageis calculating the rotation vector including an integratedgyro sensed angular rate and a coning correction calcu-lated from gyroscopes data, and it is referred to in (13) to(16).

By analyzing the optimizing process, the so-called coningerror is independent directly to vehicles’ movement dueto (13) to (16), and 𝑓

1(𝜙𝑙) and 𝑓

2(𝜙𝑙) are both scalar by

substituting the modular of 𝜙𝑙. So the coning correction 𝛿𝜙

𝑙

under coning environment cannot change the structure ofC𝑖𝑏

and the effect on Euler attitude inC𝑖𝑏by optimizing. Although

drifting error is dealt with through optimization, oscillatingerror remains.

3.3. Effect of Dynamic Coning Environment on AttitudeAlgorithm. It is convenient to use the two-stage structurealgorithm to design coefficients and restrain drifting error ofattitude under static coning environment, but it is difficult toinvestigate the effect of dynamic coning environment clearlyby using two-stage structure algorithm because it is quitedifficult to derive simple and clear descriptive equationsfor oscillating error and drifting error. Nevertheless, two-stage structure algorithm and the Euler attitude algorithmboth use Euler angles to define the relationship between thebody frame and the earth frame [18], so they are equivalentwithout considering singular problem [20]. For derivationand analysis of the effect on attitude algorithm clearly, wecan use the Euler attitude algorithm to make the investiga-tion.

According to the rotation relationship in Figure 1, therelationship between angular velocity in the body frame andEuler angular velocity can be described as [21]

𝑏𝜔 = �̇� + ̇𝜗 + �̇�, (17)

where 𝑏𝜔 is angular velocity in the body frame and �̇�, ̇𝜗, and�̇� are Euler angular velocities, respectively. Projecting Eulerangular velocity to each axis of the body frame and expanding(17), angular velocity in the body frame can be obtained:

[

[

[

[

[

[

𝑏𝜔𝑒𝑏𝑥

𝑏𝜔𝑒𝑏𝑦

𝑏𝜔𝑒𝑏𝑧

]

]

]

]

]

]

=

[

[

[

[

[

0 sin 𝜗 1

sin 𝛾 cos 𝜗 cos 𝛾 0

cos 𝛾 − cos 𝜗 sin 𝛾 0

]

]

]

]

]

[

[

[

[

[

̇𝜗

�̇�

̇𝛾

]

]

]

]

]

, (18)

where ̇𝜗, �̇�, and ̇𝛾 are Euler angular speeds, respectively, andthey are all scalar; 𝑏𝜔

𝑒𝑏𝑥, 𝑏𝜔𝑒𝑏𝑦

, and 𝑏𝜔𝑒𝑏𝑧

are components ofangular velocity in the body frame to describe the rotationof a vehicle with respect to the earth frame, and they canbe obtained by a triaxial gyroscope fixed in the body frame.Transforming (18), the differential equation of Euler attitudeis obtained:

[

[

[

[

[

̇𝜗

�̇�

̇𝛾

]

]

]

]

]

=

[

[

[

[

[

[

0 sin 𝛾 cos 𝛾

0

cos 𝛾cos 𝜗

−

sin 𝛾cos 𝜗

1 − tan 𝜗 cos 𝛾 tan 𝜗 sin 𝛾

]

]

]

]

]

]

[

[

[

[

[

[

𝑏𝜔𝑒𝑏𝑥

𝑏𝜔𝑒𝑏𝑦

𝑏𝜔𝑒𝑏𝑧

]

]

]

]

]

]

. (19)

In (19), there is singular problem when the pitch angleis equal to 90 degrees. Under pure coning environment, thepitch angle will never attain to 90 degrees, so the singularproblem can be ignored reasonably.

Substituting (4) into (19), Euler attitude equations can beobtained:

[

[

[

[

[

̇𝜗

�̇�

̇𝛾

]

]

]

]

]

=

[

[

[

[

[

[

[

[

Ω sinΛ𝑡 cos (Ω𝑡 + 𝛾) + Λ sin (Ω𝑡 + 𝛾)

−Ω sinΛ𝑡 sin (Ω𝑡 + 𝛾) + Λ cos (Ω𝑡 + 𝛾)cos 𝜗

−2Ωsin2Λ𝑡2

+ Ω sinΛ𝑡 tan 𝜗 sin (Ω𝑡 + 𝛾) − Λ tan 𝜗 cos (Ω𝑡 + 𝛾)

]

]

]

]

]

]

]

]

. (20)

Under dynamic coning environment, there is no rollingmotivation for nonspinning vehicles, so the real roll angleshould be zero. However, we note that the roll angle locatedon the right of the third element of (20) is not zero atall, although the initial value of the roll angle is chosen aszero. This means that the roll angle error is generated. Sothe third element of (20) actually represents the roll angleerror equation.Therefore, the roll angle error rate induced bydynamic coning environment can be determined as

Δ ̇𝛾 = −2Ω sin2Λ𝑡2

+ 𝐴 sin (Ω𝑡 + Δ𝛾 + 𝜑) sinΩ𝑡 sinΛ𝑡1 − sin2Ω𝑡 sin2Λ𝑡

,

(21)

𝐴 =√

(Ω sinΛ𝑡)2 + (Λ)2, (22)

𝜑 = −arctanΩ sinΛ𝑡Λ

. (23)

In (21), the first term is direct component and impliesdrifting error, which has been attracting many researchers’


interests. The second term is alternating component andimplies oscillating error. Apparently, drifting error and oscil-lating error fluctuate by the cone half-angle and the pre-cession frequency. When the cone half-angle is increasing,drifting error and amplitude of oscillating error are bothincreasing.

From the derivation above by using Euler attitude algo-rithm, we confirm that attitude of vehicles is affected to gen-erate drifting error and oscillating error under dynamic con-ing environment.

From the first and second elements of (20), it is seen thatthe roll angle error affects accuracy of the pitch angle and yawangle. The accurate pitch angle and yaw angle can be deter-mined without roll angle error:

[

[

̇𝜗

�̇�

]

]

=

[

[

[

Ω sinΛ𝑡 cosΩ𝑡 + Λ sinΩ𝑡

−Ω sinΛ𝑡 sinΩ𝑡 + Λ cosΩ𝑡cos 𝜗

]

]

]

. (24)

With the small angle approximation, the pitch angle errorand yaw angle error can be obtained by comparing (20) and(24):

Δ̇

𝜗 = Δ𝛾 (Ω sinΛ𝑡 sinΩ𝑡 + Λ cosΩ𝑡) ,

Δ�̇� =

Δ𝛾 (Ω sinΛ𝑡 cosΩ𝑡 + Λ sinΩ𝑡)cos 𝜗

.

(25)

In fact, the pitch angle error and yaw angle error are allinduced by the transferred roll angle error under dynamicconing environment, so attitude error is actually a sort ofinduced error by dynamic coning environment. This is thereason why so many researchers pay more attentions toattitude error by coning environment, in, specially, the rollangle error.

4. Coning Algorithm of Spinning Vehiclesunder Dynamic Coning Environment

From the analysis above, attitude algorithm is affected bydynamic coning environment badly. To calculate attitudeaccurately, in this section, we propose a coning algorithm ofspinning vehicles based on a cone frame and cone attitudeand build the relationship between cone attitude and Eulerattitude.

4.1. Definition of Cone Frame. Compared with commonangular movement, coning motion of spinning vehicles is

a sort of angular movement of longitudinal axis of vehiclesunder coning environment. Actually, this special movementdirectly shows precession and nutation of longitudinal axis,and self-rotation is accompanying. Since attitude error isinduced by coning motion of periodicity, a reasonablemethod is describing coning motion accurately and calcu-lating attitude without error. Therefore, cone frame and coneangles should be defined specially to describe angular move-ment.

Referring to gyrodynamics [22], a frame is defined byrotation relationship to describe movement of a rotor in agyroscope that has typical nutation and precession. To avoidconfusion, in this paper, we call this frame the cone frame𝑂𝑥𝑐𝑦𝑐𝑧𝑐. In the cone frame, the origin is located in the

center of mass of a vehicle, 𝑥𝑐axis coincides with cone axis,

𝑦𝑐axis is pointing up and perpendicular to 𝑥

𝑐axis in the

vertical surface, and 𝑧𝑐axis is obtained by the right-hand

rule. Meanwhile, cone attitude angles including precessionangle 𝛿

1, nutation angle 𝛿

2, and rotation angle 𝛿

3is defined

relatively. The initial position of 𝛿1is horizontal on the right

of cone, and the range is [0∘ 360∘].The initial position of 𝛿2is

coincident with cone axis, and the range is [0∘ 90∘].The initialposition of 𝛿

3is located in symmetry plane of a vehicle, and

the range is [0∘ 360∘].The rotation order of 𝑂𝑥

𝑐𝑦𝑐𝑧𝑐and 𝑂𝑥

𝑏𝑦𝑏𝑧𝑏is as follows:

𝑂𝑥𝑐𝑦𝑐𝑧𝑐

̇𝛿1

→

𝑥𝑐

𝑂𝑥

𝑐𝑦

𝑐𝑧

𝑐

̇𝛿2

→

𝑦

𝑐

𝑂𝑥

𝑏𝑦

𝑏𝑧

𝑏

̇𝛿3

→

𝑥

𝑏


In the rotation order, ̇𝛿1, ̇𝛿2, and ̇𝛿

3are corresponding

angular speeds. However, through in-depth study of therotation order, we find that the rotation angle is obtained byrotating discontinuously along 𝑥-axis twice, and this rota-tional process allows the rotation angle to contain precessionprocess. It means that the rotation angle is equal to thenegative precession angle if a nonspinning vehicle is underconing environment. Therefore, an isolation of the influenceby precession is needed. An indispensable rotation ̇𝛿

1along

𝑥-axis is added before the second rotation along 𝑥-axis, andthe improved rotation order is shown as

𝑂𝑥𝑐𝑦𝑐𝑧𝑐

̇𝛿1

→

𝑥𝑐

𝑂𝑥

𝑐𝑦

𝑐𝑧

𝑐

̇𝛿2

→

𝑦

𝑐

𝑂𝑥

𝑏𝑦

𝑏𝑧

𝑏

̇𝛿3− ̇𝛿1

→

𝑥

𝑏


As is shown in Figure 2, the relationship of 𝑂𝑥𝑏𝑦𝑏𝑧𝑏and

𝑂𝑥𝑐𝑦𝑐𝑧𝑐can be described by a transformation matrix:

C𝑐𝑏

=

[

[

[

[

[

[

[

cos 𝛿2

− sin (𝛿1− 𝛿3) sin 𝛿

2cos (𝛿

1− 𝛿3) sin 𝛿

2

sin 𝛿2sin 𝛿1

cos (𝛿1− 𝛿3) cos 𝛿

1+ sin (𝛿

1− 𝛿3) cos 𝛿

2sin 𝛿1sin (𝛿1− 𝛿3) cos 𝛿

1− cos (𝛿


2sin 𝛿1

− sin 𝛿2cos 𝛿1cos (𝛿

1− 𝛿3) sin 𝛿

1− sin (𝛿


2cos 𝛿1sin (𝛿1− 𝛿3) sin 𝛿

1+ cos (𝛿


2cos 𝛿1

]

]

]

]

]

]

]

,

(28)


O

yc(yb)

yc

yb

𝛿1

𝛿1

𝛿

�̇�1

zc

zc zbzb

xb(xb)

xc(xc)

𝛿2

𝛿2

𝛿3 − 𝛿1

𝛿3 − 𝛿1

3 − 1.

𝛿.

�̇�2

Figure 2: Rotational relationship of 𝑂𝑥𝑐𝑦𝑐𝑧𝑐and 𝑂𝑥

𝑏𝑦𝑏𝑧𝑏.

where C𝑐𝑏represents transformation from the body frame to

the cone frame.

4.2. Cone Algorithm Based on Rotation Relationship. Accord-ing to the rotation relationship in Figure 2, the angularmotion model can be derived as

[

[

[

[

[

[

𝑏𝜔𝑐𝑏𝑥

𝑏𝜔𝑐𝑏𝑦

𝑏𝜔𝑐𝑏𝑧

]

]

]

]

]

]

=

[

[

[

[

[

cos 𝛿2− 1 0 1

− sin (𝛿1− 𝛿3) sin 𝛿

2cos (𝛿

1− 𝛿3) 0

cos (𝛿1− 𝛿3) sin 𝛿

2sin (𝛿1− 𝛿3) 0

]

]

]

]

]

[

[

[

[

[

[

̇𝛿1

̇𝛿2

̇𝛿3

]

]

]

]

]

]

,

(29)

where 𝑏𝜔𝑐𝑏𝑥

, 𝑏𝜔𝑐𝑏𝑦

, and 𝑏𝜔𝑐𝑏𝑧

are measuring data of a triaxialgyro in the body frame to describe the motion of a vehiclewith respect to the cone frame. It is clear that (29) can describethe angular movement of spinning vehicles under dynamicconing environment, so it is modeling for this condition.Assuming the initial values of 𝛿

1and 𝛿

3are both zero, the

dynamic coning model can be obtained by substituting 𝛿1=

̇𝛿1𝑡 and 𝛿

3=

̇𝛿3𝑡 into (29):

[

[

[

[

[

[




]

]

]

]

]

]

=

[

[

[

[

[

[

̇𝛿1cos 𝛿2− (

̇𝛿1−

̇𝛿3)

−̇

𝛿1sin {( ̇𝛿

1−

̇𝛿3) 𝑡} sin 𝛿

2+

̇𝛿2cos {( ̇𝛿

1−

̇𝛿3) 𝑡}

̇𝛿1cos {( ̇𝛿

1−

̇𝛿3) 𝑡} sin 𝛿

2+

̇𝛿2sin {( ̇𝛿

1−

̇𝛿3) 𝑡}

]

]

]

]

]

]

.

(30)

From (30), we see that if ̇𝛿3is zero, it can be simplified as

[

[

[

[

[

[




]

]

]

]

]

]

=

[

[

[

[

[

[

̇𝛿1cos 𝛿2−

̇𝛿1

−̇

𝛿1sin ̇𝛿1𝑡 sin 𝛿

2+

̇𝛿2cos ̇𝛿1𝑡

̇𝛿1cos ̇𝛿1𝑡 sin 𝛿

2+

̇𝛿2sin ̇𝛿1𝑡

]

]

]

]

]

]

. (31)

Equation (31) is the same as (4), and this means that ̇𝛿1=

Ω and ̇𝛿2

= Λ. In addition, (30) in fact provides measuringdata from a triaxial gyro in the body frame for vehicles underdynamic coning environment, and, especially, (30) includesrotation information. When vehicles make nutation androtation without precession, the measuring data representspitching by providing ̇𝛿

2projected on 𝑦-axis and 𝑧-axis of

the gyro. When vehicles only make rotation without coningenvironment, the measuring data provides ̇𝛿

3. It means that

(4) can decouple the rotation of the vehicle from pitching andyawing that are represented equivalently by the precessionand nutation.Thus, we are sure that (29) or (30) can describethe whole coning motion for vehicles.

By transforming (29), cone attitude equation for 𝛿1, 𝛿2,

and 𝛿3can be derived as

[

[

[

[

[

[

̇𝛿1

̇𝛿2

̇𝛿3

]

]

]

]

]

]

=

[

[

[

[

[

[

[

[

0 −

sin (𝛿1− 𝛿3)

sin 𝛿2

cos (𝛿1− 𝛿3)

sin 𝛿2

0 cos (𝛿1− 𝛿3) sin (𝛿

1− 𝛿3)

1 − sin (𝛿1− 𝛿3) tan𝛿2

2

cos (𝛿1− 𝛿3) tan𝛿2

2

]

]

]

]

]

]

]

]

[

[

[

[

[

[




]

]

]

]

]

]

.

(32)

We noted that (32) has singular problem when 𝛿2is equal

to 0 or 𝜋/2. But, in fact, this model is specially derived forconing environment, and it is unnecessary to use it when𝛿2is equal to 0. Moreover, vehicles are swinging in a plane

when 𝛿2is equal to 𝜋/2, and the attitude does notmake sense.

Therefore, the singular problem can be neglected reasonably.Because the dynamic coning model is derived by using

the cone frame, cone attitude is not affected by coningenvironment. In other words, there is no error in the thirdaxis although the other two axes are oscillating periodically:

ΔA = 0, (33)

where ΔA is cone attitude error [Δ𝛿1, Δ𝛿2, Δ𝛿3]

𝑇.The coning environment can be described in the earth

frame or the cone frame, so the relationship of Euler attitudeand cone attitude is built according to the geometricalrelationship in Figure 3:

sin2𝜗 = sin2𝛿1sin2𝛿2,

sin2𝛿2= cos2𝜗 sin2𝜓 + sin2𝜗.

(34)


𝜗

xb

𝜓𝛿1

𝛿2

ye,c

xe,cO

ze,c

Figure 3: Geometrical relationship of Euler attitude and coneattitude.

Table 1: Symbol of 𝜗.

𝛿1

𝜗

[0∘ 180∘] +[180∘ 360∘] −

Table 2: Symbol of 𝜓.

𝛿1

𝜓

[90∘ 270∘] +[0∘ 90∘] ∪ [270∘ 360∘] −

Therefore, the pitch angle and yaw angle can be calculatedby

𝜗 = ± arcsin

sin 𝛿1sin 𝛿2

,

𝜓 = ± arcsin√ sin2𝛿2− sin2𝜗

cos2𝜗.

(35)

According to Figure 3, the maximum of 𝜗 is equal to 𝛿2

when 𝛿1is (2𝑖 + 1) 𝜋/2 (𝑖 = 0, 1, 2, 3, . . .). The maximum of 𝜓

is equal to 𝛿2when 𝛿

1is 𝑖𝜋. This means that 𝜗 and 𝜓 are both

less than 𝛿2, so the square root of (9) is not complex-valued.

The symbol of 𝜗 and 𝜓 can be determined referring to Tables1 and 2. It is apparent that the pitch angle and yaw angle aredependent on 𝛿

1and 𝛿2. Therefore, the accuracy of the pitch

angle and yaw angle is dependent on the precession angle andnutation angle as well.

5. Simulation and Analysis

To disclose the effect of coning environment on traditionalattitude algorithm and verify the validity of the cone attitudealgorithm, numerical simulations are designed on purpose inthis section.

We choose a nonspinning vehicle for simulations becauseit is convenient to check whether the roll angle or the rotationangle is zero. We assume that the initial Euler attitude ofthe nonspinning vehicle is 𝜗 = 0∘, 𝜓 = 1∘, and 𝛾 = 0∘,the total simulation time is 600 s, and the update time is0.0001 s.The optimized coefficients of the two-stage structure

Table 3: Varying Ω while Λ = 0.011∘/s.

Simulation 1 2 3 4Ω (∘/s) 360 720 1080 1440

Table 4: Varying Λ while Ω = 360∘/s.

Simulation 1 2 3 4Λ (∘/s) 0.011 0.023 0.034 0.046

of attitude algorithm are chosen as 𝑏1= 0.45, 𝑏

2= 0.675 [8],

which is enough for investigating.The conditions of dynamicconing environment are shown in Tables 3 and 4. Under eachcondition, we will investigate how attitude error is affected.

Attitude error by Table 3 is represented in Figure 4. FromFigure 4, we see that as the precession frequency of coningenvironment is enlarging, attitude error is oscillating anddiverging, and the amplitude of both pitch angle error andyaw angle error becomes larger. Euler attitude error is driftingslightly, and this is because the cone half-angle is too small tomake attitude error drift obviously. In addition, the oscillatingfrequency of pitch angle error and yaw angle error is twicethat of roll angle error. In Figure 5, the roll angle error isenlarged up to 4∘ by small Λ during 600 s, which means theincreasing process of amplitude of attitude error is acceleratedrapidly by nutation.We confirm that the changing rate of conehalf-angle affects the roll angle badly more than the preces-sion frequency. It is quite necessary for researchers to attachimportance to the effect of dynamic coning environment onattitude algorithms, especially variant nutation condition forvehicles.

Combining Tables 3 and 4, we choose the precessionangular speed 1440∘/s and the changing rate of cone half-angle 0.046∘/s as the wicked condition of dynamic coningenvironment, and triaxial angular velocity for this conditionis shown in Figure 6.The𝑥-axial component of angular veloc-ity is nonzero due to the precession angular speed. The mag-nitude of each axial component is increasing as the nutationangle is enlarging. The cone attitude is calculated by the conealgorithm. As is shown in Figure 7, we see that the precessionangle and nutation angle are changing due to the precessionfrequency and the changing rate of cone half-angle. Par-ticularly, we note that, during 600 s under dynamic coningenvironment, the magnitude of rotation angle of the vehicleis below 10𝑒 − 5. In fact this error results from the calculatingerror of computer, and ideally the rotation angle has no error.This is because the rotation order of the cone frame shows theprecession, nutation, and rotation of vehicles under arbitraryconing motion, and the angular motion model is establishedaccording to the rotation order. When resolving the angularequation, cone attitude can be obtained without error.There-fore, we confirm that dynamic coning environment can bedescribed by the cone frame and the cone attitude of spinningvehicles can be calculated by the cone algorithm. In addition,the rotation angle is more competent to describe the spinningprocess of vehicles under coning environment than Eulerangle gamma.The pitch angle and yaw angle of the vehicle arecalculated due to the geometrical relationship of cone attitude


0 100 200 300 400 500 600

−0.010

0.01

−0.02

0

0.02

−0.5

0

0.5

t (s)

0 100 200 300 400 500 600t (s)

0 100 200 300 400 500 600t (s)

= 360

= 720

= 1080

= 1440

= 360

= 720

= 1080

= 1440

1 = 360

1 = 720

= 1080

= 1440

Δ𝜗(∘)

Δ𝜓(∘)

Δ𝛾(∘)

𝛿.

𝛿. 1

1

𝛿.

𝛿.

1

1

𝛿.

𝛿. 1

1

𝛿.

𝛿.

1

1

𝛿.

𝛿. 1

1

𝛿.

𝛿.

(a) The whole process of attitude error

440 441 442 443 444 445

−0.010

0.01

−0.010

0.01

−0.2

0

0.2

t (s)

440 441 442 443 444 445t (s)

440 441 442 443 444 445t (s)

= 360

= 720

= 1080

= 1440

= 360

= 720

= 1080

= 1440

= 360

= 720

= 1080

= 1440

Δ𝜗(∘)

Δ𝜓(∘)

Δ𝛾(∘)

1𝛿.

1𝛿. 1

𝛿.

1𝛿.

1𝛿.

1𝛿. 1

𝛿.

1𝛿.

1𝛿.

1𝛿. 1

𝛿.

1𝛿.

(b) Amplification of attitude error

Figure 4: Attitude error by varying Ω.

−0.010

0.01

−0.010

0.01

−5

0

5

= 0.011

= 0.023

= 0.034

= 0.046

0 100 200 300 400 500 600t (s)

= 0.011

= 0.023

= 0.034

= 0.046

0 100 200 300 400 500 600t (s)

2 = 0.011

= 0.023

= 0.034

= 0.046

0 100 200 300 400 500 600t (s)

Δ𝜗(∘)

Δ𝜓(∘)

Δ𝛾(∘)

𝛿.

2𝛿. 2𝛿

.

2𝛿.

2𝛿.

2𝛿. 2𝛿

.

2𝛿.

2𝛿.

2𝛿. 2𝛿

.

2𝛿.

(a) The whole process of attitude error

445 446 447 448 449 450

−0.01

0

0.01

−0.01

0

0.01

−202

t (s)

= 0.011

= 0.023

= 0.034

= 0.046

445 446 447 448 449 450t (s)

= 0.011

= 0.023

= 0.034

= 0.046

445 446 447 448 449 450t (s)

= 0.011

= 0.023

= 0.034

= 0.046

Δ𝜗(∘)

Δ𝜓(∘)

Δ𝛾(∘)

2𝛿.

2𝛿. 2

𝛿.

2𝛿.

2𝛿.

2𝛿. 2𝛿

.

2𝛿.

2𝛿.

2𝛿. 2𝛿

.

2𝛿.

(b) Amplification of attitude error

Figure 5: Attitude error by varying Λ.


0 100 200 300 400 500 600

0 100 200 300 400 500 600

0 100 200 300 400 500 600

−200

−100

0

−1000

0

1000

−1000

0

1000

t (s)

t (s)

t (s)

b𝜔ebx(∘ ·s−

1)

b𝜔eby(∘ ·s−

1)

b𝜔ebz(∘ ·s−

1)

Figure 6: Triaxial angular velocity.

400 405 410 415 420

0 100 200 300 400 500 600

0 100 200 300 400 500 600

0

200

400

0

20

40

−5

0

5

t (s)

t (s)

t (s)

×10−5

𝛿1(∘)

𝛿2(∘)

𝛿3(∘)

Figure 7: Cone attitude angles 𝛿1, 𝛿2, and 𝛿

3.

and are shown in Figure 8, which can be regarded as the realpitch angle and yaw angle of vehicles.

6. Conclusion

Attitude error of vehicles has an intense trend of divergencewhen vehicles undergo worsening coning environment. Inthis paper, we firstly derive the model of dynamic coningenvironment. Then through investigation of the effect onEuler attitude algorithm for the equivalency of traditionalattitude algorithm, we find that attitude error is actually theroll angle error including drifting error and oscillating error,which is induced directly by dynamic coning environmentand further affects the pitch angle and yaw angle throughtransferring. Based on the definition of the cone frame andcone attitude, we propose a cone algorithm by rotationrelationship to calculate cone attitude and establish the rela-tionship between cone attitude and Euler attitude of spinningvehicle. Finally, numerical simulations are made with variantprecession frequency and changing rate of cone half-angle toverify the effect of dynamic coning environment on attitude

algorithm and validity of cone algorithm. The results showthat the induced error of Euler attitude fluctuates by the varia-tion of precession and nutation, especially by that of nutation,and the oscillating frequency of roll angle error is twice that ofpitch angle error and yaw angle error. In addition, the rotationangle is more competent to describe the spinning process ofvehicles under coning environment than Euler angle gamma,and the real pitch angle and yaw angle are calculated finally.

This paper is beneficial for calculating real attitude ofspinning bodies, understanding mechanism of attitude errorby coning environment, and developing attitude algorithm,and it also provides a new view on the effect of coningmotion.Our future work will focus on the optimization of attitudealgorithm under dynamic coning environment and real-timeattitude algorithm of spinning vehicles under high dynamicmovement.

Conflict of Interests

The authors declare that there is no conflict of interestsregarding the publication of this paper.


0 100 200 300 400 500 600−30

−20

−10

0

10

20

30

𝜓𝜗

t (s)

𝜗,𝜓

(∘)

(a) The whole process of pitch angle and yaw angle

405 405.5 406 406.5 407−20

−15

−10

−5

0

5

10

15

20

𝜓𝜗

t (s)

𝜗,𝜓

(∘)

(b) Amplification of pitch angle and yaw angle

Figure 8: Pitch angle and yaw angle of the vehicle.

Acknowledgment

This study is financially supported by the National NaturalScience Foundation of China (nos. 61471046, 61473039, and11202023).

References

[1] P. G. Savage, “Strapdown inertial navigation integration algo-rithm design part 1: attitude algorithms,” Journal of Guidance,Control, and Dynamics, vol. 21, no. 1, pp. 19–28, 1998.

[2] J. W. Jordan, “An accurate strapdown direction cosine algo-rithm,” NASA Technical Note NASA TN D-5384, NASA, 1969.

[3] J. E. Bortz, “A new mathematical formulation for strapdowninertial navigation,” IEEE Transactions on Aerospace and Elec-tronic Systems, vol. 7, no. 1, pp. 61–66, 1971.

[4] C. W. Kang, N. I. Cho, and C. G. Park, “Approach to directconing/sculling error compensation based on the sinusoidalmodelling of IMU signal,” IETRadar, Sonar andNavigation, vol.7, no. 5, pp. 527–534, 2013.

[5] M. U. Salman and B. Chang, “Active coning compensation forcontrol of spinning flying vehicles,” in Proceedings of the IEEEInternational Conference on Control Applications Part of IEEEMulti-Conference on Systems and Control (CCA ’10), pp. 1832–1837, IEEE, Yokohama, Japan, September 2010.

[6] R. B. Miller, “A new strapdown attitude algorithm,” Journal ofGuidance, Control, and Dynamics, vol. 6, no. 4, pp. 287–291,1983.

[7] J. G. Lee, Y. J. Yoon, J. G.Mark, andD. A. Tazartes, “Extension ofstrapdown attitude algorithm for high-frequency base motion,”Journal of Guidance, Control, and Dynamics, vol. 13, no. 4, pp.738–743, 1990.

[8] M. B. Ignagni, “Optimal strapdown attitude integration algo-rithms,” Journal of Guidance, Control, and Dynamics, vol. 13, no.2, pp. 363–369, 1990.

[9] Y. F. Jiang and Y. P. Lin, “Improved strapdown coning algo-rithms,” IEEE Transactions on Aerospace and Electronic Systems,vol. 28, no. 2, pp. 484–490, 1992.

[10] Z.-T. Li, T.-J. Wu, and L.-H. Ma, “A coning compensation algo-rithm for SINS in high dynamic motion,” Control Engineeringand Applied Informatics, vol. 13, no. 3, pp. 32–40, 2011.

[11] C.-Y. Tang and X.-Y. Chen, “A generalized coning correctionstructure for attitude algorithms,” Mathematical Problems inEngineering, vol. 2014, Article ID 614378, 15 pages, 2014.

[12] J. G. Mark and D. A. Tazartes, “Tuning of coning algorithms togyro data frequency response characteristics,” Journal of Guid-ance, Control, and Dynamics, vol. 24, no. 4, pp. 641–647, 2001.

[13] K. Kim and T. G. Lee, “Analysis of the two-frequency coningmotion with SDINS,” in Proceedings of the AIAA Guidance,Navigation, and Control Conference and Exhibit, Montreal,Canada, August 2001.

[14] P. G. Savage, “Explicit frequency-shaped coning algorithms forpseudoconing environments,” Journal of Guidance, Control, andDynamics, vol. 34, no. 3, pp. 774–782, 2011.

[15] M. Song, W.-Q. Wu, and X.-F. Pan, “Approach to recoveringmaneuver accuracy in classical coning algorithms,” Journal ofGuidance, Control, and Dynamics, vol. 36, no. 6, pp. 1872–1880,2013.

[16] L. Fu, L. L. Wang, and J. H. Hu, “Coning algorithm based onsingular perturbation,”Aircraft Engineering andAerospace Tech-nology, vol. 85, no. 3, Article ID 17086924, pp. 178–185, 2013.

[17] M.-S. Wang, W.-Q. Wu, J.-L. Wang, and X. Pan, “High-order attitude compensation in coning and rotation coexistingenvironment,” IEEE Transactions on Aerospace and ElectronicSystems, vol. 51, no. 2, pp. 1178–1190, 2015.

[18] R. P. Patera, “Attitude propagation for a slewing angular ratevector,” Journal of Guidance, Control, and Dynamics, vol. 33, no.6, pp. 1847–1855, 2010.

[19] R. P. Patera, “Attitude propagation for a slewing angular rate vec-tor with time varying slew rate,” in Proceedings of the AAS/AIAAAstrodynamics Specialist Conference (ASTRODYNAMICS ’11),pp. 2529–2546, Girdwood, Alaska, USA, August 2011.


[20] A. Janota, V. Šimák, D. Nemec, and J. Hrbček, “Improving theprecision and speed of Euler angles computation from low-costrotation sensor data,” Sensors, vol. 15, no. 3, pp. 7016–7039, 2015.

[21] X.-F. Qian, R.-X. Lin, and Y.-N. Zhao, Missile Flight Dynamics,Beijing Institute of Technology Press, Beijing, China, 2000.

[22] Y.-Z. Liu, Gyrodynamics, China Science Publishing & Media,Beijing, China, 2009.

International Journal of

AerospaceEngineeringHindawi Publishing Corporationhttp://www.hindawi.com Volume 2014

RoboticsJournal of

Hindawi Publishing Corporationhttp://www.hindawi.com Volume 2014


Active and Passive Electronic Components

Control Scienceand Engineering

Journal of



RotatingMachinery


Hindawi Publishing Corporation http://www.hindawi.com

Journal ofEngineeringVolume 2014

Submit your manuscripts athttp://www.hindawi.com

VLSI Design



Shock and Vibration


Civil EngineeringAdvances in

Acoustics and VibrationAdvances in



Electrical and Computer Engineering

Journal of

Advances inOptoElectronics

Hindawi Publishing Corporation http://www.hindawi.com

Volume 2014

The Scientific World JournalHindawi Publishing Corporation http://www.hindawi.com Volume 2014

SensorsJournal of


Modelling & Simulation in EngineeringHindawi Publishing Corporation http://www.hindawi.com Volume 2014


Chemical EngineeringInternational Journal of Antennas and

Propagation




Navigation and Observation



DistributedSensor Networks


research article cone algorithm of spinning vehicles under ...tional coning correction algorithms...

Documents