A New Fast Initial Alignment for the Helicopter

Young Min Yoo, School of Mechanical & Aerospace Engineering Seoul National University Kwang Jin Kim, Doosan DST Co., Ltd. Chan Gook Park, School of Mechanical & Aerospace Engineering Seoul National University

BIOGRAPHY Young Min Yoo is a Ph.D student in the School of Mechanical and Aerospace Engineering of Seoul National University, Seoul, Korea. He received his B.S, degrees in Information and Control Engineering from Kwangwoon University in 2005. And he received his M.S. degree in the School of Mechanical and Aerospace Engineering of Seoul National University in 2007. His current research topics include the Strapdown Inertial Navigation Systems, MEMS-based IMU Applications, Positioning for Ubiquitous Sensor Networks. Homepage: http://nesl.snu.ac.kr E-mail: ym0903@snu.ac.kr Kwang Jin Kim is chief research engineer in the Defence Research Center, Doosan Defence System & Technology Co., Ltd. He received his Ph.D. degree in School of Mechanical and Aerospace Engineering from Seoul National University in 2008. From 1997 to 2004, he was a researcher with the Agency for Defense Development, Daejun, Korea. His research interests include the inertial navigation system, GPS and nonlinear filtering. Homepage: http://www.doosandst.com E-mail: kwangjin1.kim@doosan.com Chan Gook Park is a processor in the School of Mechanical and Aerospace Engineering at Seoul National University. He received his Ph.D. degree in Control and Instrumentation Engineering from Seoul National University in 1993. In 1998, he worked with Prof. Jason L. Speyer about formation flight at UCLA as a visiting scholar. He is currently in charge of IEEE AES Korean Society. His current research topics include the development of Inertial Navigation Systems, GPS/INS integration, MEMSbased IMU applications, Positioning for Ubiquitous Sensor Network and advanced filtering techniques. Homepage: http://nesl.snu.ac.kr E-mail: chanpark@snu.ac.kr

ABSTRACT This paper shows a new fast initial self alignment algorithm could be applicable to military helicopters with INS(Inertial Navigation System). There are a few critical problems in the navigation system for military helicopters. First, unlike the civil aircraft, the military helicopter could not maintain static state for several tens minutes on the ground in case of urgent need. Second, the sensor noise due to rotor vibration is very severe so that it takes more time to calculate the initial attitude and has more large alignment error. Finally, the soldiers loading and wind buffet effects during the alignment can suddenly change the initial attitude of the helicopter. To provide solution of these critical problems, the navigation grade strapdown IMU(Inertial Measurement Unit) with noise caused by vibration of a rotor is modelled and IIR filter is designed to reject these rotor and undesirable sensor noise. Finally, this paper proposes a new fast initial self alignment algorithm which reduces the alignment convergence time and enhances the alignment accuracy. 1. INTRODUCTION The process of initial self alignment for SDINS(StrapDown Inertial Navigation System) consists of two stages, that is, the coarse alignment and fine alignment. The purpose of coarse alignment is to provide a fairly good initial coordinate transformation matrix from body frame to navigation frame for the fine alignment. In general, the coarse alignment stage would use the analytic alignment scheme, which utilizes the measurement of the gravity and earth rotation vectors to directly compute the transformation matrix[1]. This is also called as open-loop alignment. On the other hands, the purpose of the fine alignment stage is to refine the initial estimate of the transformation matrix

calculated by the coarse alignment algorithm. To do this, some fine alignment schemes were developed. Two conventional methods are as follows : One is the Kalman filtering with zero velocity measurement update and the other is the selfcorrective alignment scheme. The former is called ZUPT and the latter is called gyrocompassing. The ZUPT has poor observability and spent much time to converge into analytical attitude error. The gyrocompassing has trade-off between alignment accuracy and alignment convergence time. So it can reduce some alignment time but it cannot track the changed attitude even can diverge when helicopters initial attitude is suddenly changed. Thus these methods are not suitable for military helicopters which require a fast alignment. A new initial alignment algorithm proposed in this paper calculates the attitude angles with openloop scheme and estimates the accelerometer and gyro biases with the closed-loop scheme at once. The estimated bias errors are used to correct the sensor errors that are utilized to calculate the attitude angles in the open-loop scheme. The computer simulation results illustrate the efficiency of this new alignment algorithm. 2. SENSOR MODELING This paper presents a model of the navigation grade strapdown IMU(Inertial Measurement Unit) sensor data and the noise model due to vibration of a rotor and other sensor outputs model due to the attitude change. The simulation results of proposed algorithm in this paper based on the model of the RLG(Ring Laser Gyroscope) as navigation grade strapdown IMU sensor data. The RLG is an active Sagnac interferometer in which internally generated laser beams are travelling in opposite directions around a triangular or square contour defined by highly reflecting mirrors. These mirrors are rigidly attached to the sensor block. When the sensor is rotated around an axis perpendicular to the laser beam plane, the Sagnac effect causes a frequency shift between the two laser beams, which is strictly proportional to the rate of rotation. Superimposing both laser beams through a beam splitter at the output of one of the mirrors generates an interferogram and makes this frequency shift accessible[3]. The advantage of using the RLG is that there are no moving parts compared to the conventional spinning gyroscope, this means there is no friction, additionally, the entire unit is compact, light weight. However, there is the disadvantage like a dead-band where, even though the rotation rate is none-zero, the fringe frequency is fixed at zero, i.e. the fringe pattern is stationary. This phenomenon is called

lock-in. This occurs when the rotation rate becomes very small. This problem can be overcome in a number of ways, each essentially adding bias to the frequency difference between the beams. The typical technique which can be used to overcome lock in phenomenon is mechanical dithering. The RLG is subjected to a constant rotation so that the frequency difference between the CW and CCW beams is never so small to allow lock-in to occur[4]. However, this dithering provokes vibration to the IMU, and random walk noise error of the RLG increases greatly. Thus we established the output signals of three axes RLG model not only the true angular rate but also additional angular rate due to dithering. Also the gyro fixed bias error and random walk noise error were included in the RLG model. The output signals of three axes RLGs are modelled mathematically by:
x (t) = xt (t) +xm1 sin(2 fxt +gx1) +xm2 sin(2 f yt +gx2 )
+xm2 sin(2 fzt +gx3 ) +xf +xr (t)

y (t) = yt (t) +ym1 sin(2 f yt +gy1) +ym2 sin(2 fxt +gy2 ) (1)
+ym2 sin(2 fzt +gy3 ) +yf +yr (t)

z (t) = zt (t) +zm1 sin(2 fzt +gz1) +zm2 sin(2 fxt +gz2 )
+zm2 sin(2 f yt +gz3 ) +zf +zr (t)

where
x (t ), y (t ), z (t ) : x, y, z axis RLG output signal

(angle increment) : xt (t ), yt (t ), zt (t ) true angle increment of x, y, z axis

xf (t ), yf (t ), zf (t ) : fixed gyro bias error of x, y, z

axis : xr (t ), yr (t ), zr (t ) random walk noise error of x, y, z axis

xm1, xm2 , ym1, ym2 , zm1, zm2 : amplitude of dithering

noise gxi , gyi , gzi : phase difference of noise due to dithering frequency
f x , f y , f z : frequency of dithering on x, y, z axis

Similarly, the output signals of three axes accelerometers are modelled mathematically by:
vx (t ) = vxt (t ) + vxm1 sin(2 f yt + ax1 ) + vxm2 sin(2 f zt + ax2 )
+ vxf + vxr (t )

vy (t ) = vyt (t ) + vym1 sin(2 f xt + ay1 ) + vym2 sin(2 f zt + ay 2 ) (2)

+ vyf + vyr (t )

vz (t ) = vzt (t ) + vzm1 sin(2 f xt + az1 ) + vzm2 sin(2 f yt + az 2 )

+ vzf + vzr (t )

where
vx (t ), vy (t ), vz (t ) : x, y, z axis accelerometer output

signals (velocity increment)

vxt (t ), vyt (t ), vzt (t ) : true velocity increment of x, y,

z axis vxf (t ), vyf (t ), vzf (t ) : fixed accelerometer bias error of x, y, z axis

vxr (t ), vyr (t ), vzr (t ) : random walk noise error of x,

y, z axis vxm1, vxm2 , vym1 , vym2 , vzm1, vzm2 : amplitude of dithering

axi , ayi , azi : phase difference of noise due to

dithering frequency
f x , f y , f z : frequency of dithering on x, y, z axis

noise

Figure 1. UH-60 vibration characteristic depends on main rotor First, in IIR LPFs case, use the first order IIR LPF and the input/output equation is as following.
y(n) = ay(n 1) + (1 a) x(n)

The vibration environment of a helicopter is well characterized in MIL-STD-810F issued by the United State Armys development test command, to specify various environmental tests to simulate conditions that the tested item will encounter in the field. Helicopter model to perform simulation in this paper is similar to UH-60, and environment characteristic of helicopter rotor noise is known such as Figure 1. In the figure 1, we only consider the noise invoked from the fixed frequency f1 = 4.3 [Hz], f 2 = 17.2 [Hz], f3 = 34.4 [Hz], and f1 = 51.6 [Hz] which have the same amplitude of maximum dithering frequency noise. To simulate a real operation procedure of helicopter, we assume that the speed of rotor reaches linearly to the maximum speed for early 1 minute from zero speed. 3. DIGITAL FILTER DESIGN FIR(Finite Impulse Response) filter has advantages that implementation is simple and fixed point calculation is stable, but have shortcoming that need much memory and time delay occurs. To supplement these shortcomings, this paper presents IIR(Infinite Impulse Response) filter that can form null in frequency area that wish to remove at the same time having LPF (Low Pass Filter) characteristic. That is, the filter consists of serial connection of IIR notch filter that remove specification frequencies component like rotor frequency noises and IIR LPF that remove high frequency band noise like dithering and random walk noise.

(3)

In a qualitative point of view, output y (n) is weighted sum of own past value y (n 1) and present input value x(n) . Therefore, enlarge importance of past value, past values are accumulated and the filter acts by strong LPF, because the effect of new input is reflected to be less. In other words, if weight a is nearer to 1, cut-off frequency becomes low. Figure 2 shows the result of IIR LPF and show that this filter has good noise rejection effect as much as -60[dB] in high frequency band. The amplitude of sine wave noises due to rotor vibration decreased tens dBs in low frequency band, but we can see that the amplitude is big yet than white noise decreases. Therefore, this paper proposes the algorithm that the signal passed IIR LPF input to the IIR notch filter bank again to remove sine wave noises.

(a) before IIR LPF

4. INITIAL ALIGNMENT FOR SDINS The requirement of initial alignment is related to the necessity for the transformation of the sensor output into a best estimate of the attitude, velocity and position of a vehicle with respect to the reference navigation frame. A two-stage alignment scheme appears promissing in this regard, i.e., the coarse alignment and the fine alignment. Since SDINS is entirely self-contained, it can align itself by using the measurement of local garavity and earth rate. (b) after IIR LPF Figure 2. Comparison of power spectrum of RLG before and after IIR low pass filtering Transfer function of IIR notch filter to remove specific frequency component is as following.
H ( z) = 1+ r 1 + 2az 1 + z 2 2 1 + a(1 + r ) z 1 + rz 2

4.1 Coarse Alignment The coarse alignment utilizes directly the body mounted sensors with accelerometers and gyros for attitude estimation. The accelerometer outputs are used for solving the levelling problem while the gyro outputs are required for azimuth estimation. The accelerometers and gyros will measure components of the specific force needed to overcome gravity and components of earth rate, denoted by the vector quantities f b and b respectively. fb = (5) fx f y fz

(4)

where a = cos and = f r / f T 2 , f r is frequency component that wish to remove, fT is sampling frequency. This IIR filter has specification that poles radius is bigger, that is, pole approaches in zeros position, the width of frequency band that wish to remove narrows. We composed IIR notch filter bank of 54 notch filters in series to remove 1, 2, , 50[Hz] frequency components, and 4 notch filter to remove frequency components that occur by rotor noise is added. Figure 3 shows the result of IIR notch filtering of the RLG data after IIR LPF. The remaining sine wave frequency components were removed in low frequency band comparing with figure 2.(b). At the same time, we can see that white noise amplitude decreases.

b = x y z

(6)

The roll and pitch angles are calculated by f g sin cos 1 y = tan 1 = tan g cos cos fz
= tan 1
g sin g cos 1 tan = 2 2 f y + fz fx

(7)

(8)

where and represent roll and pitch angle, respectively. Eqs. (7) and (8) show that the roll and pitch angles are decided by using only accelerometer outputs. The yaw angle is calculated by
= tan 1
z sin y cos x cos + y sin sin + z cos sin

(9)

From (7), (8) and (9), it is known that the initial attitudes are roughly calculated with an open-loop scheme[5]. 4.2 New Fine Alignment Algorithm In order to overcome the flaw of fine alignment with the ZUPT and gyrocompassing method, the new alignment algorithm that uses simultaneously both open-loop and closed-loop scheme is proposed in this section. The new alignment algorithm calculates the attitude angles with the open-loop scheme and estimates the accelerometer and gyro biases with the closed-loop scheme[6].

Figure 3. Power spectrum of RLG after IIR filtering

The open-loop algorithm was given by (7), (8), and (9). For the closed-loop scheme, the sensor biases for the system equation of the Kalman filter are modelled as random constants & =v f (10) & = vg where v f and v g represent random white noises. In the new alignment scheme, the accelerometer and gyro outputs are directly used to the filter measurements since the filter states of (10) are only sensor errors. From the accelerometer outputs (3), the accelerometer measurements are represented as
% g sin % z f f x x % + g sin % cos % z fy = f y % + g cos % cos % f z z fz

(15). The following attitude error equations are used to get rid of these error terms[5]. sin y cos (16) = z g cos cos + sin ( y sin + z cos ) (17) = x
g

h9 x + h10 y + h11 z N

(18)

where h9 = c s , h10 = s s s + c c h11 = c c s s c . For the Kalman filter, inserting (16), (17) and (18) into (12) and (15) yields the following measurement matrix H .
H H = 11 H 21 033 H 22 s s c s 2 c2 s c c2 c s c s c c2 , c2 c2

(11)

(19)

The equation (11) could be calculated by utilizing the perturbation method as follows:
z f x g cos x z f y = y + g cos cos g sin sin g sin cos g cos sin z fz z

where
s2 H11 = s s c c s c

(12)

The gyro outputs are given by

N c c D s x = (s s c c s ) + s c (13) D y N z N (c s c + s s ) + D c c where s = sin , c = cos , s = sin , c = cos ,

c h 1 g c h 4 H 21 = g c h 7 g
h h 2 9 N h h H 21 = 5 9 N h8 h9 N

s s h1 g s s h4 c h3 g g c s s h7 c h6 g g c
h2 h10 N h5 h10 N h8 h10 N h2 h11 N h5 h11 . N h8 h11 N

c s h1 g c s h4 s h3 , + g g c c s h7 s h6 + g g c

s = sin , and c = cos .

The gyro measurements are represented as

z % c % %x N c % D s x % % % % % % y N (s s c % c s % ) D s c zy = % s % c % s % c % % N (c % + s % ) D c zz z

(14)

The equation (14) could be calculated by using the perturbation method as follows:
z h1 + h2 + x x z y = h3 + h4 + h5 + y h + h7 + h8 + z zz 6

4.3 New Fast Initial Alignment Algorithm A new fast intial alignment algorithm is summarized in Figure 4. 5. SIMULATIONS To evaluate the proposed a new fast intial alignment algorithm, simulations are performed by using the sensor model described in section 2. Where, accelerometer bias is 100[ug] and the RLG bias is 0.1 [deg/hr] and white noise of accelerometer is 10[ug], and white noise of RLG is 0.01 [deg/hr]. In the simulation, coarse alignment is performed for early 10[sec], and fine alignment is performed for 590[sec] after coarse alignment process. Figure 5 and 6 show that the simulation results of a new fast initial alignment algorithm. From the figure 5, the roll and pitch angles are converged to a

(15)

where h1 = N s c + D c , h2 = N c s , h3 = ( N c s c + D c c + N s s ) , h4 = N s c s + D s s , h5 = N s s s + N c c , h6 = N s c c + D s c N c s , h7 = N c c c + D c s , h8 = N c s s N s c . The measurement equations must be constituted with system states that are accelerometer biases and gyro biases. However, the attitude error terms are contained in the measurement equations, (12) and

Angle [ Deg]

desirable value with very fast rate because the proposed algorithm estimates the x and y accelerometer biases independently with the attitude error components. Also from the figure 6, we can see that the heading angle is converged into a desirable value and has a good performance against the white noise from the 100 times Monte Carlo simulation.
Pre-filtering
For the RLG and accelerometer, perform IIR LPF, IIR notch filtering

Leveling
x 10 -5. 66 -5. 67 -5. 68 -5. 69 -5.7 -5. 71 -5. 72 -5. 73 -5. 74 -5. 75
-3

Total Alignment Result. . ROLL

100

200

300 Time [ Sec ]

400

500

600

x 10 5. 76

-3

Total Alignment Result. . PITCH

Coarse Alignment
Perform coarse alignment by using open-loop scheme during initial 10~20[sec] with total average sensor data
f g sin cos 1 y = tan g cos cos fz g sin fx 1 = tan 1 = tan f y2 + f z2 g cos z sin y cos sin 1 = tan 1 N = tan cos + sin sin + cos sin y z N cos x
Angle [ Deg]

5. 75 5. 74 5. 73 5. 72 5. 71 5.7 5. 69

= tan 1

5. 68 5. 67

100

200

300 Time [Sec]

400

500

600

Sensor Data Pre-processing

Total or moving average for sensor data

Figure 5. Levelling results of a new fast initial alignment algorithm

Heading
Total Alignment Result. . YAW -0. 043 -0. 044

Fine Alignment
Angle [ Deg]

-0. 045 -0. 046

Attitude calculation by using open-loop alignment scheme

-0. 047 -0. 048

-0. 049

Generation H matrix from prior estimate attitude

-0. 05

-0. 051

100

200

300 Time [Sec]

400

500

600

Generation measurement vector by using prior estimate attitude

Mont ec arlo Res ult of YAW Angle -0.038

-0. 04

-0.042

Kalman filtering

Angle [ Deg]

-0.044

-0.046

Estimated sensor bias compensation to the sensor averaged data

-0.048

-0. 05

100

200

300 Time [Sec]

400

500

600

Figure 4. Block diagram of a new fast alignment

Figure 6. Heading results of a new fast initial alignment algorithm

6. CONCLUSIONS In this paper, a new fast initial alignment applicable to the military helicopter was proposed. To increase the convergence rate of the Kalman filter in the fine alignment stage, the proposed algorithm combined open-loop with closed-loop scheme, that is, the attitude angles with the openloop scheme were calculated and the accelerometer and gyro biases with the closed-loop scheme were estimated. The estimated bias errors were used to correct the sensor errors utilized to calculate the attitude angles in the open-loop scheme. This paper presented a model of the navigation grade strapdown IMU(Inertial Measurement Unit) sensor data, the noise model due to vibration of a rotor and other sensor outputs model. IIR filter to reject these rotor and undesirable sensor noise was also proposed. Finally, this paper verified performance of a new fast initial self alignment algorithm which reduces the alignment convergence time and enhances the alignment accuracy. ACKNOWLEDGEMENTS This work was partially supported by the Ministry of Education, Science and Technology of Republic of Korea under NSL(National Space Lab, Project number S10801000163-08A0100-16310) and Korea Science and Engineering Foundation (KOSEF) grant funded by the Korean government(MOST)(No.01-2006-000-101890). Also, this study is accomplished as a fundamental research project (UD080015FD) of Defence Acquisition Program Administration (DAPA) and Agency for Defense Development (ADD). And we would like to express our thanks to the support of Automatic System Research Institute (ASRI). REFERENCES [1] Britting, K. R, Inertial Navigation Systems Analysis, John Wiley & Sons, Inc. 1971. [2] Oleg Salychev, Applied Inertial Navigation: Problems and Solutions, BMSTU. Press. Moscow. 2004. [3] R. Franco-Anaya, A.J. Carr and K.U. Schreiber, "Qulification of Fibre-Optic Gyroscopes for Civil Engineering Application," Enginerring an Earthquake Resilient New Zealand 2008 Conference, April 11, 2008. [4] A.Lawrence, Modern Inertial Technology Navigation. Guidance and Control, 2nd ed., Springer-Verlag, 1998. [5] Park, C. G, Kim, K. J, Park, H. W and Lee, J. G., "Development of an Initial Coarse Alignment Algorithm for Strapdown Inertial Navigation

System," Journal of Control, Automation, and System Engineering, Vol 4, No. 5. 1998. [6] K. J. Kim, C. G. Park, "A New Initial Alignment Algorithm for Strapdown Inertial Navigation System Using Sensor Output, " IFAC World Congress 2008, Seoul, Korea, July 6-11, 2008.