PRECISION QUATERNION BASED ONE STEP STRAPDOWN ATTITUDE ALGORITHM

Introduction. Calculative algorithms of Strapdown inertial navigation systems (SINS) can be divided on navigation algorithms, which transform accelerometers output signals into local coordinates and attitude algorithms, which transform gyroscopes output signals into vehicle angular attitude [1]. Wherein, navigation task solution requires double integration of acceleration and attitude task – integration of kinematic attitude equation, related measured object angular velocity with attitude parameters. Paper considered of autonomous position determination methods based on vehicle angular velocity information without acceleration measurement. Thus, attitude algorithms are considered only. Paper researched the errors of algorithm based on quaternion attitude equation, moreover algorithm error drifts were accepted as a main accuracy characteristic The main part. Algorithm researched by imitation modeling of vehicle’s computer with SINS attitude algorithm. The main task of modeling is defining depends between algorithm drift and sensor’s call step in cases of different frequencies and amplitudes of base angular oscillations. It was researched four one-step algorithms: reverse, based on modified Euler method; Picard method with two successive approximations and the new author’s algorithm which combines formulas of first two algorithms. It was studied depends of algorithm drift and faze shift between two orthogonal axes oscillations. It was shown, the biggest drift values are obtained in case of base conning movement. It was made the modeling researches of algorithm drift amplitudes relatively to sensor sample steps and oscillation frequencies in dimensionless form. It was shown, substantial increase new algorithm accuracy compared to other researched. Conclusions. The algorithm drift accuracy of new algorithm in 2600 times exceeds the revers algorithm. Small modification of one-step algorithm allowed increase accuracy in few orders, almost without computing increase. Received results allows to expend attitude algorithms application area and prognose their accuracy with different base movement.


Introduction
There are known six types of kinematic parameters [2]: − Euler-Krylov rotation angles; − directional cosines between navigation and base frames; − Euler orientation vector; − Gibbs and Rodrigo rotation vectors which are variants of orientation vector; − quaternions of rotation [3].
SINS algorithm researching [31,32] showed: mean errors values increasing, in case of some angular motions, with constant speed, which called algorithm error's drift speed or "algorithm drift". Wherein, algorithm drift, in some cases, could be so high, that in a few minutes its value becomes much bigger than other errors. Since that time, the error which depends of algorithm drift defines the algorithm accuracy. Therefore, algorithm drift errors can be used as main SINS algorithm accuracy characteristic.

Problem statement
Attitude algorithms based on measurement of angular velocity vector ( ) k k t = ω ω or quasicoordinate vector k q in discrete time moment separated by sensor sample step h : The algorithm error researching is easy to make by computer simulation in Matlab which allows to imitate data computing on-board by SINS.
It was studied the error drift of ψ angle, assuming the base movement, when this angle remains unchanged (and equal zero). At the same time ϑ and γ angles makes synchronic oscillations with frequency ω , same amplitude m a and phase shift ε . This ensures the best conditions to algorithm drift appearance. Such type of base moving is customary to call "coning" in special inertial navigation works. The final result of computer simulation is defining the dependence between attitude integration algorithm errors and sensors polling rate with different base angular oscillation amplitudes and frequencies.
According to Eq.1, quaternion Λ elements initially calculates using angular rate values at the beginning of the sample ("straight run"), then quaternion Λ elements defines in revers sequence by using angular rate values at the end of the sample ("revers run"). As a result, previous calculating step quaternion 1 1 ( ) n n t − − Λ = Λ transforms into current calculating step quaternion ( ) n n t Λ = Λ . Let write these formulas compact, using quaternions.

Algorithm RK21 of Runge-Kutta method
One of the most commonly used integration method for mathematical and engineering research is Runge-Kutta algorithm. It could be applied to different accuracy orders or step size.

Algorithm Pic1h2tQuatOm of Picard method
The algorithm of quaternion rate where 1 N is solution of equation at the time moment 1 n n t t t h Eq. 5 can be solved by Picard method as successive approximations.

First approximation
In the zero approximation will be (0) ( ) 1 N t = . Using this to the right part of Eq.3 get solution in first approximation:

Refine solutions
In case of one-step algorithm, there are known two values of angular values ω , measured at the beginning and at the end of polling rate. In this case, angular rate can be approximated by linear function: 0 ( ) 2 τ τ = + ⋅ ω ω ε . At the end of polling rate (and integration as well) h τ = measured value of angular rate will be: Since the required function ( ) τ ω defines by expression ( ) where dimensionless time value presented as h τ ζ = .
Now Eq.4 in first approximation can be written as: At the end of integration sample 1 ζ = , thus final formula of algorithm will be ( )

Second approximation
First of all, we need to calculate double integral: Considering that, quaternion product of two quaternion vectors is i.e. difference between their vector's product and scalar product. Thus, So, the second approximation solution will be ( )

2
(2), 1 1 0 Separating scalar part from vector's, we get ( ) The final formula of the algorithm is ( ) Computer simulation of the algorithms The computer simulation software was created for studying the properties of created algorithms. Software simulates information processing process by vehicle's computer using all received methods.
Some of simulation results presented on Fig. 1 -3. Test moving parameters were accepted as: The main simulation calculated value is error drift of angle ψ defining, which shouldn't move at all.
Studding of the results obtained to the following conclusions: 1) the biggest drift value has RK21 algorithm; it about twice bigger then Revers1h; the most accurate is Pic1h2tQuatOm algorithm, which showed error drift more than 20 times less then Revers1h; 2) all algorithms have second-order accuracy by error drifts; 3) algorithms Revers1h and Pic1h2tQuatOm showed continuously amplitude error incising; Algorithm RK21 showed steady error oscillations by base angle oscillations; Errors oscillation instability can be eliminated by quaternion rating operation. The simulation result with adding of quaternion rating operation showed on Fig. 4 -6.
As we see, quaternion rating operation leads to full elimination of error unsteady oscillation. At the same time, it does not change errors of Runge-Kutta method.

Dependence of error drift and phase shift
Influence of base oscillation phase shift value on algorithms error received by computer simulation is presented on Fig. 7   We can see from Fig. 8, that Revers algorithm is twice accurate than one-step RK21, and Pic1h2tQuatOm algorithm is 20 times more accurate than Revers.

High-accurate algorithm
Studying phase characteristics (Fig. 7) of presented algorithms, it can be noticed, that reverse algorithm and Runge-Kutta method both changes harmonically with anti-phase for all polling rate values. It allows us to create combination of these algorithms to significant decrease of maximum error drift.
The formula of corresponding algorithm is The results of simulation algorithm (7) by function ExpRvrs1h presented on Fig. 9.
After simulations for couples polling rate values and changing the phase shift values from -180 о to 180 о , we found relation between drift of new algorithm and phase shift.
On Fig. 10  More detail results of new algorithm maximum drift measuring presented on Fig. 10, 11.
Dependence of error drifts and polling rates presented on Fig. 11.

Conclusions
Research [31,33] shows, if we could find two algorithms with the same error drift accuracy but op-posite in sign in all faze shift range, it allows to increase accuracy on few orders without significant computation increasing. Computer simulations showed: error drift value of new developed algorithm is 2600 times exceeds similar accuracy of now-used revers algorithm.
Received results allows to expend the using area of strapdown attitude algorithms and to predict their accuracy in different body movement.