Текст

Oleg Salychev Applied Inertial Navigation: Problems and Solutions 11 Ж

Applied Inertial Navigation Problems and Solutions Oleg S. Salychev Laboratory of Inertial Geodetic Systems at the Bauman Moscow State Technical University Published by the BMSTU Press Moscow, Russia 2004

Dr. Oleg S. Salychev The Bauman Moscow State Technical University Moscow, Russia The new book by professor Oleg S.Salychev, head of the Laboratory of Inertial Geodetic Systems (LIGS), is dedicated to the latest achievements of the author’s leaded team in the field of inertial navigation system deve- lopment and INS/GPS integration techniques. Great scientific and research experience of the LIGS is implemented in practical developments, which are described in details in the present book. Meeting actual challenges in de- sign of a compact low-cost inertial device, the author introduces the readers into the specifics of the MEMS/GPS navigation system functioning. The book is intended to the engineers and postgraduate students, practically involved in the inertial instrumentation development and INS/GPS inte- gration techniques. BMSTU Press, 5, 2-nd Baumanskaya str., Moscow, 105005, Russia. This work is a subject of copyright. All rights are reserved, whether the whole part of the material is concerned, specifically these of translation, reprinting, re-use of illustrations, broadcasting, reproduction by the photocopying machines or similar means, and storage in data banks. ISBN 5-7038-2395-1 ©Oleg Salychev, 2004 Printed in Russia

Contents Introduction 7 1. Coordinate Frames 9 1.1. Inertial Frame...................................... 9 1.2. Earth-Fixed Frame.................................. 10 1.3. Local-Level Frame ................................. 12 1.4. Wander Frame ...................................... 14 1.5. Body Frame......................................... 16 1.6. Navigation Frame................................... 17 1.7. Platform Frame..................................... 17 1.8. Sensor Frame....................................... 18 2. Coordinate Transformation 20 2.1. Direction Cosine Matrix............................ 20 2.2. Puasson Equation................................... 27 2.3. Rotation Vector and Quaternions.................... 30 3. Principles of Inertial Navigation 42 3.1. General Navigation Equation ....................... 42 3.2. Classification of Inertial Navigation Systems...... 51 3.2.1. Local-Level INS............................. 51 3.2.2. Strapdown Inertial Navigation System........ 54 3.2.3. Principle of INS Alignment ................. 55 4. Applied Navigation Algorithm 61 4.1. Strapdown System Navigation Algorithm.............. 61

4 4.1.1. Attitude Calculation......................... 75 4.2. Applied Alignment Procedures........................ 75 4.2.1. Stored Azimuth Alignment..................... 82 4.2.2. Alternative Fine Alignment Procedure......... 82 4.3. Description of SINS Functional Scheme............... 85 4.4. Alignment Accuracy Analysis......................... 87 4.5. Multi-Step Alignment Procedure ..................... 91 5. INS Error Model 94 5.1. Strapdown and Local-Level Error Model...............102 5.2. Schuler Loop .......................................109 5.3. Influence of Azimuth Misalignment...................Ill 5.4. Relation between Ф#, Ф/v, Фир and Pitch, Roll, Heading 114 5.5. Error Model of Vertical Channel.....................117 6. INS Calibration Procedures 121 7. Applied Estimation Theory 133 7.1. State Space Representation of Linear Systems........133 7.2. Introduction to Traditional Estimation Methods .... 138 7.2.1. Kalman Filter................................138 7.2.2. Kalman Filter with Control Signal ...........155 7.2.3. Kalman Filter with Coloured Measurement and Input Noise..................................156 7.3. Kalman Filter Adjustment............................158 7.3.1. Influence of Initial Condition on Accuracy of Kalman Filter.......................................158 7.3.2. Influence of Noise Covariances...............159 7.4. Kalman-Schmidt Approach.............................165 7.5. Estimation Modes....................................174 7.6. Adaptive Filtering..................................178 8. INS testing 183 8.1. INS Lab Testing Procedures..........................183 8.1.1. Day-to-Day Accelerometer and Gyro Bias Stability ............................183

5 8.1.2. Lab Checking of Calibration Errors..........185 8.1.3. Calibration of Gyro Drift Bias..............186 8.2. Bias and Azimuth Misalignment Influence............188 8.3. INS Field Testing..................................190 9. INS/GPS Integration 191 9.1. INS/GPS Integration Methods........................191 9.1.1. Cascade Scheme of INS/GPS Integration .... 192 9.1.2. Loosely Coupled Approach of INS/GPS Integration .......................................194 9.1.3. Tightly Coupled Approach of INS/GPS Integration .......................................197 9.2. Observability of INS Errors Using GPS Measurements . 198 9.3. Calibration of Non-Stationary Component of INS Error 202 9.4. Kinematic Azimuth Alignment........................208 9.5. Special Vehicle Motion Behaviour...................210 10. Low-Cost INS/GPS Integration 214 10.1. Motion Pak™/GPS Integrated System.................214 10.2. INS Error Damping.................................218 10.3. Analysis of Error Damping Behaviour...............222 10.4. Estimation of Low-Cost INS Errors.................224 10.5. Roll, Pitch Correction Loop Implementation........228 10.6. Heading Correction Loop Implementation ...........229 10.7. Implementation of Prediction Mode.................231 10.8. Position and Velocity Correction Loop.............232 10.9. Test Description and Result Analysis..............232 10.9.1. Land Vehicle Tests.........................232 10.9.2. Airborne Tests ............................235 10.10V ertical Channel of Low-Cost Integrated System . . . . 240 10.llU ltra Compact Integrated Navigation System.......244 11. Medium-Level Accuracy IMU/GPS Integration 249 11.1. Honeywell HG1700/GPS Integrated System............249 11.2. Estimation Block Implementation...................252 11.3. Output INS Error Correction.......................258

6 11.4. Prediction Mode Implementation.............258 11.5. HG1700/DGPS Test and Results...............259 11.6. Vertical Channel of Medium Accuracy IMU ...263 11.7. Low-Cost and Medium Accuracy Systems Comparison 264 11.8. Land Navigation Using INS/GPS/Odometer Information266 A. Mathematics of GPS 270 B. Relationship Between Attitude Errors of INS 273 C. Measurement Model in Second Calibration Procedure276 D. Coning Calculation 284 E. Possibility of Autonomous Mode Improvement 293 F. Companav II 295

Introduction Five years have passed since previous book ’’Inertial Systems in Navigation and Geophysics” was published. Five years is an epoch for modern technology development. The common direction of high tech- nology evolution these days is to meat individual needs and to make affordable the usage of high technologies in every day life. Many inex- pensive products, such as mobile phones, pocket PCs, personal GPS receivers, have become available in the market for common people. So these tools appear to be standard outdoor kit of XXI century. The innovations taking place in the inertial technology and INS/GPS integration field have a large variety of implementations nowadays. The stand-alone usage of GPS provides accurate naviga- tion information, but is limited to open area applications and moreover is unable to cover all the areas of tasks related to navigation and espe- cially attitude determination. The usage of an inertial system, which operates autonomously, solves many of these problems, however large INS errors make challengeable its stand-alone operation. From the above discussion, the integration of GPS and INS technologies is ap- propriate since each system compensates for the other’s shortcomings. A high performance INS cannot be used in many civil applications due to its high price, therefore the latest investigations are focused on inte- gration GPS with low-cost inertial systems. Micro-Electro-Mechanical (MEM) technology has brought to the market the inexpensive sensors, which can be used to create a low-cost navigation system. A number of companies worldwide produces motion sensors, which provide a user with 3-axis acceleration and angular rate raw information, but with- out applying special adaptive techniques they cannot be utilized for

8 Introduction the design of a ready-to-use navigation system. The Laboratory of Inertial Geodetic Systems (LIGS) has accumulated great experience in the inertial navigation system development and INS/GPS integra- tion field. These knowledge and experience are now implemented into the full-performance integrated inertial system based on MEM sen- sors. Such a system fits the total navigation solution in a pocket size, and is affordable for an average user. The developed technology, which is discussed in this book, is ap- plicable for the wide range of inertial systems. We think, we can claim to have a new line of integrated inertial products opened. The actual book presents the new technologies of the inertial sensor data pro- cessing and the new approaches in the field of INS/GPS integration. It introduces the fundamentals of inertial technology and follows up to the algorithmic implementations. It is important to outline, that all described methods and algorithms are implemented in the soft- ware and hardware. The book content is dedicated to the people, who were practically involved in that project: researchers, engineers and scientists. I would like to express my gratitude to the staff of the Lab- oratory of Inertial Geodetic Systems and my colleagues in TeKnol Ltd. company, namely Leonid Gushturov, Ilya Shakhov, Dr. Vladimir Voronov, Dr. Vadim Lukianov for the long time productive joint work and also to Mr. Valery M. Pissarev for help in the book publication and effective project management. I also want to thank my daughter Anastasia for text editing and inspiration in this book working. Oleg Salychev November, 2003 We would appreciate your comments of this book contents, e-mail : contact@ligs.ru FAX : (7-095)504-4500 (INTECHNOL 2540) Phone : (7-095)263-6891 WWW : http://www.ligs.ru

Chapter 1 Coordinate Frames In surveying and navigation positioning, the final output required by a client usually includes the coordinates of a point namely latitude, longitude and height, and their accuracies. The measurements sensed by an Inertial Navigation System (INS) are three orthogonal compo- nents of the body rotation rates and three accelerations in a coordi- nate frame, which is not directly related to any geodetic curvilinear coordinate frame. These measurements have to be analytically in- tegrated and transformed through several coordinate frames, which yields changes in the ellipsoidal coordinates. It is, therefore, impor- tant that all coordinate frames involved in the transformation of the measurements, and results of integrations are well defined before any discussion of an inertial navigation system is presented. The defini- tion of various coordinate frames associated with an inertial navigation system is given in this section. 1.1. Inertial Frame According to the Newtonian definition, an inertial frame is a frame, which does not rotate or accelerate. Such a frame is easy to define in theory, but it is almost impossible to realize in practice. The best approximation of the truly inertial frame would be one, that is inertial with respect to the distant stars. One approximation of such is a frame

10 1. Coordinate Frames Figure 1.1. Inertial Frame used in surveying applications, which is a right ascension system. The right ascension system as given in a catalogue precesses and nutates at the rate of less than 3.6 • 10~7 arc sec/s, which is well bellow the noise level of sensors in present inertial survey systems. Thus, for all practical inertial surveying purposes, the right ascension system can be treated as the inertial coordinate frame. The definition of the inertial frame is the following: origin - at the center of the Earth X/-axis - towards mean vernal equinox at To Yaxis - complete a right handed system Z/-axis - towards the north celestial pole at epoch t0. A graphical representation of the inertial frame is shown in Fi- gure 1.1. Note that this is an abstract definition of the inertial frame for computation purposes. Measurements in the inertial frame, ob- tained from gyros have much poorer accuracy. 1.2 Earth-Fixed Frame The Earth-fixed frame is a frame, in which the output coordinates of the inertial survey system are given. This frame is not inertial. It is re- volving around the sun and rotating at a rate of 7.292115 • 10-5 rad/sec

1.3. Local-Level Frame 11 Figure 1.2. Earth-Fixed Frame The definition of the Earth-fixed frame is the following: origin - at the mass centre of the Earth Xe-axis - pointing towards the Greenwich meridian, in the equa- torial plane Ye-axis - 90° east of Greenwich meridian, in the equatorial plane Ze-axis - axis of rotation of the reference ellipsoid. The coordinates in the Earth-fixed frame can be transformed to the inertial frame by a negative rotation about the Z-axis by the amount of the Greenwich Mean Sidereal Time (GMST). The reference ellipsoid used in this research is the WGS 84 system. The semi-major and semi-minor axes are: а = 6378137.0 m and b = 6356752.3 m respectively. The direction of the axes of the Earth-fixed frame are illustrated in Figure 1.2.

12 1. Coordinate Frames Figure 1.3. Local-Level Frame 1.3. Local-Level Frame The Local-level frame is a coordinate frame, which is known by sur- veyors as a local geodetic frame, if a change in the direction of the x and у axes is made. Velocity of the inertial survey system is usually defined as components along the axes of the local-level frame. The definition of the local-level frame is the following (see Figure 1.3): origin - at topocentre X-axis - ellipsoidal east (also denoted as E-axis) Y-axis - ellipsoidal north (also denoted as X-axis) Z-axis - upward direction along the ellipsoidal normal (also de- noted as Ep-axis). The transformation matrix between the local-level frame and the Earth-fixed frame has a form [1, 2]: ' Xе ‘ Ye Z€ — sin A — sin p cos A cos A 0 — sin <p sin A cosip cos <p cos A cos p sin A sin<p N L up where p - latitude

1.3. Local-Level Frame 13 A - longitude. The angular velocity of the local-level frame for the spherical Earth model can be represented as VN О/E = R 4- h VE . Tr = = R + h ,CZCOSV ^Up = Vjg = ——- tan <p 4- U sin p (1.1) R + h where U - Earth’s rate R - radius of the Earth Vjv, Ve - relative velocities of the local-level frame with respect to the Earth h - altitude above the spherical Earth model. Or for the ellipsoidal Earth model VN = R^ 4- h VE WN = = R. + h lf/coS^ Vb ^Up ~ = — tan ip 4- U sm Rx + h where h - altitude above the reference ellipsoid Rv, R\ are the radii of the curvature of the reference ellipsoid in north-south and east-west directions respectively, that is Д.(1-е2) (1 — e2sin2 <^>)3/2 Re (1 — e2sin2 «p)1/2

14 1. Coordinate Frames Here, Re is the equatorial radius of the Earth and e is an eccen- tricity given by e2 = 1 — b2/а2 (a is a semi-major axis of the reference ellipsoid and b is a semi-minor axis). 1.4. Wander Frame Figure 1.4. Wander Frame The local-level frame is convenient for expressing directions but it is not the best coordinate frame to perform integration of data from the inertial survey system. The Y axis of the local-level is always pointing towards the North. At very high latitudes, a large rotation about the Z axis is necessary to maintain the orientation of the local- level frame whenever it is moved towards the East, even by a small movement. This problem may be avoided by performing all the com- putations in a coordinate frame that does not point to the North. Such a coordinate frame is called a Wander Frame. The wander frame is the same as the local-level frame in all aspects except that its Y axis is not slaved to the North direction. Therefore, it is allowed to wander off the North axis at a rate chosen by a user. The angle between the Y and North axes is called the wander or azimuth angle. The correspondence between the local-level and wander frames, as well as the positive direction of wander angle e is shown in Fi- gure 1.4. According to the introduction of wander angle e, the equa-

1.4. Wander Frame 15 tion for ё is the following: VE ё = — —------- tan Rx + h The definition of the wander frame is the following: origin - at topocentre X\v-axis - rotated in the level plane by angle £ from the East towards the North. Angle e is called the wander or azimuth angle. It is chosen to be equal to the meridian convergence from a point of alignment Yw-axis - orthogonal to the X-axes in the level plane Zw-axis - upwards along the ellipsoidal normal. The transformation matrix between the wander frame and the local-level frame can be described as N = Yw cos e 4- Xw sin e E = —Yw sin e 4- Xw cos e The transformation matrix between the Earth-fixed frame and the wander frame has a form [1]: Xw 5ц 512 5i3 ’ Xе ' Yw = &21 &22 5гз Ye Zw 5з1 5з2 5зз Ze су 5ц = — sin cos A sin e — sin Л cos e 512 = sin sin A sin e -I- cos A cos e 513 = cos </> sin E b2i = — sin cos A cos e 4- sin A sin e b22 = — sin <p sin A cos e — cos A sin e b23 = cos tp cos E 531 — cos <p cos A b32 = cos <p sin A b33 = sin <p

16 1. Coordinate Frames The absolute angular velocity of the wander frame can be de- scribed as u>xW = cos e 4- u)N sin e u)yw ~ sin e 4- ujn cos E wzW = U sin <p where - absolute angular velocities of the local-level frame described by equations (1.1) U - Earth rotation rate ip - latitude. 1.5. Body Frame Figure 1.5. Body Frame Representation The body frame is an orthogonal frame, in which the measure- ments of a strapdown inertial navigation system are made. Its axes coincide with the output axes of the sensor block, which is called an Inertial Measurement Unit (IMU). Thus, the IMU raw data contains the components of the rotation rate and the accelerations experienced by a sensor unit along its body axes. The local-level frame can be rotated to the body-frame by three consecutive right-handed rota- tions about its three axes. The first rotation is made about its Z-axis

1.6. Navigation Frame 17 and the angular change is called heading of an INS (in Russian system heading has an opposite direction). The second rotation is made about the rotated X-axis by an angle, which is called pitch of an INS. The third rotation about the rotated У-axis completes the total rotation between the two frames. The amount of this rotation is defined as roll of an INS. The three angles: roll, pitch and heading are commonly referred to the Euler angles. The definition of the body frame of an inertial survey system is the following (see Figure 1.5): origin - at the center of a strapdown inertial survey system Хь-axis - towards the right side of an INS Yt,-axis - towards the front of an INS Zb-axis - upwards and perpendicular to the XY plane. An INS box is installed in a vehicle such a way, that the Yb axis coincides with the vehicle longitudinal axis, the Zb axis coincides with the vertical vehicle body and the Xb axis completes a right handed system. The transformation matrix between the local-level frame and the body frame is described by equation (2.1). 1.6. Navigation Frame Х^У/у,^ axes of the navigation frame can be optionally coincided with the local-level frame or with the wander frame. 1.7. Platform Frame The platform frame was created mainly for the derivation of the error equations. The platform frame is an image of the navigation frame, which is established on an on-board computer using the real output from sensors. For the ideal sensors, the platform frame coincides with the navigation frame. However, in reality the platform frame has a small deviation from the navigation frame due to the sensor errors. 2 - 9437

18 1. Coordinate Frames The above angular deviations are called platform attitude errors. The definition of the platform frame is listed below: origin - at the centre of the inertial navigation system Xp-axis - slightly misaligned due to attitude errors with the X axis of the navigation frame Yp-axis - slightly misaligned with the Y axis of the navigation frame and orthogonal to the X axis mentioned above Zp-axis - complete a orthogonal right-handed system. The transformation matrix between the navigation and platform frames has a form where Фж, Фу, Фг - INS attitude errors. 1.8. Sensor Frame The sensor frame coincides with sensitivity axes of accelerometers and gyros. Actually, the above frames are close but do not coincide with the body frame because of installation errors of sensors in an IMU. During the factory calibration the above errors can be estimated and compensated (see Chapter 6). As a result, in the INS algorithm the coincidence between sensor frame and body frame is actually assumed. The transformation matrix between the sensitivity axes of ac- celerometers and the body frame can be described as Xb~ Yb Zb 1 C*yx OLZX &xz Olyz 1 xa Ya Za 1 where otxy, axz, ayx, ayz, azx, azy - installation errors of the accelerometer axes with respect to the body frame.

I 8. Sensor Frame 19 The transformation matrix between the gyro sensitivity axes and the body frame has a similar form ' xb Yb Zb 1 Pyx Bzx Pxy 1 fizy (3XZ Pyz 1 Xg Yg Zg where ftxy, fiXZi fiyXi /3yz, (3ZXi fizy - installation errors of gyro axes with respect to the body frame. The above errors are estimated and compensated due to the factory calibration (see Chapter 6). 2*

Chapter 2 Coordinate Transformation 2.1. Direction Cosine Matrix In order to transform an arbitrary vector from one coordinate frame to another one, the transformation matrix between the above frames can be introduced as where Cj - transformation matrix between the I frame and b frame. Frequently, the transformation matrix is called a direction cosine matrix. The most commonly employed method for generation a coordi- nate transformation matrix is based on the computation of the direc- tion cosines in between each pair of axes of two frames respectively. One of the most used methods of the direction cosine matrix determi- nation is based on the three ordered right-handed rotations [2, 3, 4]. The angles of rotation are called Euler angles. The above an- gles are three independent quantities, which are capable to define the position of one coordinate frame with respect to another.

2.1. Direction Cosine Matrix 21 Let’s introduce the following Euler angles: Фх, Фу, Ф2. Using a series of three ordered right-handed rotations on the Euler angles, the coincidence between two arbitrary frames can be achieved. Let’s coincide XYZ coordinate frame with ХьУъ^ъ coordinate frame. The first rotation of XYZ system is made about the Z axis by angle Ф2. The rotation about Z axis of Ф2 results in a new set of axes X1Y1Z1 as shown in Figure 2.1. This rotation and the subsequent rotations are made in a positive, that is, anti-lock wise, direction, when looking down the axis of rotation towards the origin. A position vector in the new system can be expressed in terms of the original coordinates as Xi = X cos Ф2 4- Y sin Ф2 Yi = —X sin Ф2 4- Y cos $z Z, = Z or in matrix form cos $z sin Фг 0 — sin $z cos $z 0 0 0 1 Figure 2.1. Rotation by $z Angle

22 2. Coordinate Transformation 2.1. Direction Cosine Matrix 23 The second rotation is made by rotating XiYiZi system about the Xi axis by angle Ф1} see Figure 2.2. As a result new coordinate frame X2Y2Z2 is introduced. The transformation between the X2Y2Z2 and XiYiZi systems can be described as or and X2 = Хг Y2 = Yi cos Фд. 4- Zi sin Фх Z2 = — Ki sin Фх + Zi cos Фж ’ Xb' Yb J ’ COS Фу 0 sin Фу 0 0 — sin Фу 0 COS Фу Z2 — C3 0 0 cos Фд. sin Фд. — sin Фя cos Фд. Figure 2.2. Rotation by Фд. Angle Z2 Figure 2.3. Rotation by Фу Angle The total transformation matrix can be defined using multipli- cation of C3, C2l Ci matrices, then Finally, third rotation about the Y2 axis by angle Фу moves X2Y2Z2 system to XbYbZb, see Figure 2.3. The corresponding trans- formation has a form C = C3C2Ci Xb = X2 cos Фу — Z2 sin Фу Yb = Y2 Zb = X2 sin Фу + Z2 cos Фу where C = Cll C12 C13 C21 C22 C23 C31 C32 C33

24 2. Coordinate Transformation and Сц = cos Фу cos Ф2 — sin Фх sin Фу sin Ф2 Ci2 = cos Фу sin Ф2 + sin Фх sin Фу cos Ф2 Ci3 = - cos Фх sin Фу c2i = — cos Фх sin Ф2 C22 = cos Фх COS Ф2 с2з = sin Фх c3i = sin Фу cos Ф2 4- sin Фх cos Фу sin Ф2 Сз2 = sin Фу sin Ф2 - sin Фх cos Фу cos Ф2 C33 = cos Фх cos Фу (2.1) It should be pointed out, however, that the Euler angles are not uniquely defined, since there is an infinite set of choices. Unfortunately, there is no standardized definition of the Euler angles. For a particular choice of the Euler angles, the rotation order selected and defined must be used consistently. That is, if the order of the rotations is interchanged, different direction cosine matrix C representation is defined. Let’s consider the properties of direction cosine matrix C. The above matrix in between two arbitrary chosen coordinate frames has the following properties. 1. Determinant of matrix C equals to identity det C — 1 2. C-1 = CT Using the above properties one can easily transform a vector from one coordinate system to another one. Introduce, for example, the transformation matrix between the body frame and the local-level frame as

2.1. Direction Cosine Matrix 25 where CfL - direction cosine matrix of the transformation from b frame to LL frame. For the backward transformation the above equation can be rewritten as = (W A matrix, which has the above property in the matrix algebra, is called an orthogonal matrix. Hence, direction cosine matrix C is an orthogonal matrix. In order to define orientation of a vehicle body with respect to the local-level frame, the Euler angles, namely, pitch, roll and heading (19, 7, H) are usually introduced. Assume, that one has the infor- mation about direction cosine matrix C|L between the local-level and body frames. The elements of the above matrix can be represented via ?9, 7, H angles. Substituting Фх, Фу, Фг angles by pitch, roll and heading in equation (2.1), such representation can be defined. In Russian inertial systems the positive direction of heading an- gle H is opposite with respect to Фг angle, then H = — Фг [1]. Using last remark, the transformation matrix between the local- level frame and the body frame can be represented as CbLL = C11 C21 C31 c12 c13 C22 Сгз C32 c33 (2.2)

26 2. Coordinate Transformation where Cu = cos 7 cos H 4- sin $ sin 7 sin H C12 = — cos 7 sin H 4- sin ‘O sin 7 cos H C13 = — cos & sin 7 c2i = cos $ sin H C22 = cos cos H C23 = sin c3i = sin 7 cos H — sin $ cos 7 sin H C32 = — sin 7 sin H — sin cos 7 cos H C33 = cos $ cos 7 and $, 7, H - pitch, roll, heading of the vehicle body frame. Let us consider the case of small deviations between xyz and xbybzb frames. It means that Euler angles Фх, Фу, Фх can be con- sidered as small angles. In this case transformation matrix C can be rewritten (совФ, « 1; вшФ, « Ф,) as 1 . Ф» фг -ф, 1 Фх -ф« 1 1 0 0 Г о фг -ф„ = / 4-C = 0 1 0 4- 1 ф н О ф н 0 0 1 Ф» -Фх 0 (2.3) It can be shown, that for the small rotations, angles Фх, Фу, Фх can be represented as a vector, and for them the vector addition rule is valid. It means, that if three ordered sequential small rotations are made from an original frame, then for the linear approximation of the total transformation matrix between the initial and final systems, the following formula is valid: C = G C2C3 = I 4- Сг 4- C2 4- C3 where (14- Ci) - transformation matrix of a small rotation.

2.2. Puasson Equation 2.2. Puasson Equation 27 Let us introduce an important rule for the differentiation of any vector with respect to the inertial space. This rule can be described by the following formula: dR\ _ dR dt I/ dt + umx R m (2-4) - derivative (total) of vector R with respect to the inertial frame dR\ dt Im - derivative (local) of vector R with respect to m - arbitrary frame bjm - absolute angular velocity of m frame. Mathematically, the derivative of some vector with respect to the certain frame can be achieved by the following operation. At first, the projections of some vector on the certain coordinate frame have to be calculated. The next step is the differentiation of the above projections. As a result, the above projections describe the derivative of the considered vector with respect to the certain coordinate frame. In literature [3, 4] the equation (2.4) is called Coriolis formula. Equation (2.4) can be rewritten in a matrix form as R™ — Rm + ^m^ (2.5) where 0 ^2 0 U)y -Ux 0 and R™ - total derivative of vector R in projection on m frame.

28 2. Coordinate Transformation In order to define the differential equation for the transformation matrix between non-inertial frame m to inertial frame I, the following relationship can be used: Ri — By differentiating of both sides of the above equation, one can define Ri ~ = = + An) Let us assume that the following formula is valid: or C‘m = С‘тй>т (2.6) then R{ ~ Cmtfim + &mRm) (2-7) Obtained equation (2.7) is valid because of Coriolis for- mula (2.5), thus equation (2.6) is valid as well. Expression (2.6) is known as Puasson equation. The above equa- tion describes behaviour of the transformation matrix between an ar- bitrary non-inertial frame and the inertial frame. Usually in inertial navigation systems, the transformation ma- trix between two non-inertial systems is needed. The behaviour of the direction cosine matrix between m and n arbitrary non-inertial systems can be described in a form of equa- tion (2.6), as = (2-8) where wm_n - matrix, which elements are relative angular velocities between m and n coordinate frames. Equation (2.8) is not usually applied for attitude determination, because for its implementation relative, but not absolute, angular ve- locities are needed, while inertial sensors provide the absolute angular

2.2. Puasson Equation 29 velocity of the body. But the above equation will be used for the coordinate determination in the navigation algorithm, see Chapter 4. Another representation of equation (2.8) can be defined using the following relationship: СП {-ins'll (2.9) and equations for C^, C\ in form (2.6) = crc‘m <in = c?c' (2.10) By differentiating of both sides of equation (2.9), one can deter- mine C" = C?C‘ + c?c£ m 1 m 1 i and substituting equations (2.10) C' n ргт/ s-'m/’il m — -b On Uj °m Cn Wm &n or Here = C”u,m - (2.11) and cj™, cj” - absolute angular velocities of m and n frames.

30 2. Coordinate Transformation 2.3. Rotation Vector and Quaternions Instead of the three ordered rotations for coincidence of two coordinate frames, it is possible to use one rotation around a single, fixed axis. Let us introduce rotation vector Ф, which defines such operation. Rotation vector Ф is directed along the axis of rotation and has a magnitude equal to the rotation angle in radians. The equation of the rotation vector can be defined as Ф = |Ф|г = cos a cos/3 cos 7 (2.12) where |Ф| - magnitude of the rotation vector a, 0, 7 - angles between the axis of rotation and a coordinate frame. It can be defined [12, 13] that the rotation vector satisfies the following differential equation: i _ 1-e _ 1 Л Ф sin Ф ,= . Ф = ш + -Фхш + I 1 - -7—!--IФ x (Ф x w) 2 Ф 2\ 2(1 - cos Ф )/ v 7 where w - angular velocity between frames. For small Ф angle it can be rewritten in a form Ф = бй4-^Фха> + -^-(Ф x (Ф x w)) Z LZi (2-13) Usually, for convenience of the transformation operations, Hamilton quaternion algebra is used, which implements the rotation vector idea. Hamilton’s quaternion is defined as a hyper-complex number of the form [2]: Q = <7o + Qii + + Язк

2.3. Rotation Vector and Quaternions 31 where go, Qi> Q2, дз are real numbers and set {1, i, j, k} forms a basis for a quaternion vector space. 9o = gi = Я2 = дз = Conceptually, gb q2, дз define a vector in space and g0 is the amount of rotation about that vector. In other words, the quaternion elements can be described as Д cos — 2 • M sin — cos а 2 u sin — cos p . Д sm-cos7 where p is the amount of rotation a, (3, у - angles between the axis of rotation and a coordinate system. Obviously, the quaternion elements can be represented through the parameters of rotation vector Ф. Using expression (2.12), the quaternion elements can be described as Qo gi Q2 дз |ф| C°S 2 . |ф| sin 2 . |ф| sm 2 . |ф| sin 2 |Ф| Фу |Ф| Ф2 |ф| (2.14) ф In order to use the quaternion technique, the following algebraic properties of a quaternion have to be introduced. Norm of quaternion N(Q) = gj 4- gj 4- gf 4- gf = 1

32 2. Coordinate Transformation Addition The sum of two quaternions [Q] and [5] is as fol- lows: [Q] 4- [S] = (g0 + qii 4- 92 j 4- Чзк) + (s0 + sii + s2j + s3k) = — (<7o 4- ®o) 4- (qi 4- $i)i 4- (q2 4- s2)j + (q3 4- s3)k Subtraction Quaternion subtraction is merely addition of a negative quaternion -(<?] = (-i)lQ] or [Q] - [S] = Q 4- (-l)S = ((to - So, 91 ~ Si, q2 - s2,9з - s3) Multiplication The product of two quaternions [Q] and [S] is [Q][S] - (go 4- qii 4- q2j 4- g3^)(s0 4- sp 4- s2j 4- s3k) = = GfoSo — 9isi — q2s2 — g3s3) 4- 4“ (9osi 4- 9i$o 4- g2s3 — q3s2)i 4- 4" (9oS2 — 9is3 4- q2sG 4- 9sSi)J 4- 4- (90S3 4- q\S2 — q2Si 4- q3s0)k In general, quaternion multiplication is not commutative, that is, [Q][S] 0 [S][Q]- Multiplication by a scalar Л yields A[Q] = Ago 4- Agii 4- Xq2j 4- Xq3k Quaternion multiplication is defined by using the distributive law on the elements as in ordinary algebra, except that the order of unit vectors must be preserved. The bases г, j, к are generalizations of л/—1, which satisfy the relationships i2=j2 = k2 = _1

2.3. Rotation Vector and Quaternions 33 and by cyclic symmetry U = k jk - -kj = i ki = —ik — j Conjugate The conjugate [Q]* of a quaternion is defined as follows: [Q]* = Qo - <hi - - Язк Moreover, norm (or length) of quaternion N(Q) is a scalar de- fined as the product of the quaternion and its conjugate. Thus, N(Q) = [<?][<?]• = [<?]*[«] = = 9о+91+9г + 9з (215) Using the isomorphism one can get [Q][Q]* = det(Q). Conventionally, det(Q) is called norm, denoted by N(Q), so that [Q][Q]* = N(Q). Similarly, [Q]*[Q] = N(Q), or [Q][Q]* = N(Q) = [Q]*[Q]. The norm of the product of two quater- nions is equal to the product of these norms. Hence, N(QS) = N(Q)N(S) and consequently, |[Q][S]| — |[Q]||[S]|. By the mathe- matical induction, the product of n quaternion factor is N(QiQ2 •Qn) = N(Q1)N(Q2)... JV(Qn) Inverse If [Q] is not zero, then inverse quaternion [Q]-1 is defined as [QHQ]’1 = IQ]’*[Q] = i Using the norm concept, one can obtain [Q]"1 = [Q]71V(Q) where N(Q) / 0. Thus, 3 9437

34 2. Coordinate Transformation Identities 1. Zero quaternion [Qo] is a quaternion with zero scalar and zero vector [0] = 0 4- 0г 4- 0j + Ok 2. Unit quaternion is defined as any quaternion whose norm is 1. Thus [1] = 1 4- 0г 4- 0j 4- Ofc Since there is a single redundancy in the four-parameter descrip- tion of coordinate rotations, as opposed to six for the direction cosines, the quaternion parameters satisfy a single constraint equation. The constraint equation on a unit quaternion is <Zo + Qi + + Яз — [Q][Q]* — 1 Computationally, (qq 4- qJ 4- q2 + <Й)1/2 can be used as a normalizing factor for each parameter. This is analogous to the periodic orthogonalization procedure used in conjunction with the direction cosine propagations. Equality The equality of two quaternions [Q] and [S] is de- fined when their scalars are equal and their vectors are equal. Thus, [Q] = [S'] if and only if 9o = so, Qi = «1, q2 = s2, 9з = $з, or [Q], = [S]», г = 0,1,2,3 From the preceding discussion, a unit quaternion can now be defined in the similar transformation, in which vector R in the inertial coordinate frame is transformed into the body frame as MG] = 1 (2.16) Now consider {г, j, fc} as the orthogonal basis for real-vector space R3. Then, any vector x = xii + x2j+ x$k in R3 can be expressed

2.3. Rotation Vector and Quaternions 35 in the quaternion form with zero scalar term. Similarly, from the previous definition of a quaternion, any quaternion [Q] = Qo + 9i« + Q23 + Чзк can be expressed as a sum of a scalar and a vector ([Q] = qo + q). Figure 2.4 depicts the spatial orientation of a quaternion. Figure 2.4. Spatial Representation of Quaternion Next, how the quaternions are used in coordinate transforma- tions will be discussed. Consider the two coordinate systems illustrated in Figure 2.5. Furthermore, it will be assumed that the coordinate system (X, У, Z) is fixed in space, while the coordinate system (X', У', Z') is moving in some arbitrary manner, however, both coordinate systems have the same origin. Using Euler’s theorem, the (X', У', Z') coordinate system is rotated by the angle д about some fixed axis, which makes angle a, /3 and 7 with the (X, У, Z) axes, respectively. Note that this axis of rotation makes the same angles a, /3, у with the (X', Y', Z') axes, also. From the four-parameter system (д, a, /3, 7) another transformation matrix will be derived that aids in the development of the differential equations of the four quaternion parameters. The transformation of vector R from one coordinate frame - b to another - i, can be described through quaternion Q as follows: Ri; = Q • Rb • <Z = = (<7o + qii + q2j + qsty(rbxi + rbj + rbk)(q0 - qj - q2j - q3k)

36 2. Coordinate Transformation Figure 2.5. Rotation Axis of ’’Vector” about which System XYZ is Rotated in order to Coincide with System X’V’Z’ Using the above expression and multiplication rules of i, j, fc, the above quaternion transformation can be determined in a matrix form as where 9o + 9i “ 9г ~ 9? 2(9192 + 9о9з) 2(919з — 9о9г) 2(9192 ~ 9з9о) 2(gi<fo + 9092) 9o - <A + 92 - 9з 2(4293 - 9o9i) 2(9293+ 9b9i) . (217) Formula (2.17) gives the correspondence between the direction cosine matrix and the quaternion. In other words, if the quaternion elements 91, Q2, <7з are determined, then the direction cosine matrix components can be defined by expression (2.17).

2.3. Rotation Vector and Quaternions 37 The quaternion can also be expressed as a 4 x 4 matrix. Thus Qo Qi 9г 9з q = “91 9o -9з 92 —92 9з 9o ~91 ~9з “92 91 9o where, as before, ft, ft, ft, 93 are quaternion components. A primary advantage of using the quaternion technique lies in the fact, that only four unknowns are necessary for calculation of new transformation matrix, while the direction cosine method requires nine. It can be shown [2], that the quaternion analog of Puasson equa- tion has a form (2.18) The recurrent solution of the above equation can be determined as Qk+l = Qk + yQkWT or Q*+1 = Qk(l + = Qk&Q (2.19) & where T - sampling period AQ = 14- - quaternion of a small rotation (updating quaternion). Let’s deduce the quaternion expression of a small rotation de-

38 2. Coordinate Transformation fined beyond. By the definition, one get Qo 9i 92 9з = cos |ф| 2 |Ф| Ф» = sin 2 |Ф| |Ф| Ф» = sin 2 |Ф| = sin |Ф| Фх 2 |Ф| Using only first component of expression (2.13) which is valid for small angle Ф, the quaternion of a small rotation can be determined as 9o = 1 9i — ^4 1 92 = ^yT 9з = ^zT Let’s compare the Puasson equation solutions in the direction cosine matrix representation with ones in the quaternion form for small rotations (see equation (2.19)). Puasson equation between some non- inertial coordinate frame - b and the inertial coordinate frame - I has a form (see (2.6)): Cl = Cfa where - absolute angular velocity of frame b. The recurrent solution of Puasson equation can be defined as Cb(tk+i) — Qfafc) = Cl(tk)tib(tk)T or Cl (ifc+1) = Cl(tk)(I + 0>b(tk)T) = Cl(tk) ACl (2.20)

2.3. Rotation Vector and Quaternions 39 where &Cb = I + wb(tk)T can be considered as a direction cosine matrix of a small rotation (see equation (2.3)). Comparison of equa- tions (2.19) and (2.20) shows the similar form of Puasson equation in the direction cosine matrix and quaternion representation. It is important to note, that equation (2.19) is valid for the transformation from non-inertial frame - b to the inertial frame - I. In case, when the transformation between I and b frames is needed, the above formula can be rewritten as Qk+i — ^Q*Qk where AQ* - conjugate of quaternion Qk. The similar expression can be defined in the Puasson equation form. Indeed, the transformation matrix between I and b frames can be written as cb, = (C')T and (C')T = (С'йь)т = -wb(* or Cb! = -UbCb! The solution in a recurrent form can be defined as C?(t*+I) = (I - ^T)C?(tt) = AC? • Cb,(tk) (2.21) where AC, = (7 — wbT) is a direction cosine matrix of a small rotation on angles ojbT and can be considered as an analog of quaternion conjugate - AQ*. The comparison of the transformation algebra in the direction cosine matrix and quaternion forms is shown in Table 2.1. The reason of the quaternion application instead of the direction cosine method lies in the following quaternion algebra advantages.

40 2. Coordinate Transformation At first, only four quaternion unknowns are necessary for calculation of the new transformation matrix, while the direction cosine method requires nine. Moreover (which is more important), Puasson equation in the quaternion form provides an orthogonal direction matrix while the direct cosine matrix approach requires the special normalization procedure. As a result, the calculation error in the quaternion solution is smaller than in the direct cosine approach implementation.

2.3. Rotation Vector and Quaternions 41 Table 2.1. Comparison Table of Transformation Algebra in Direction Cosine Matrix and Quaternion Forms Direction cosine matrix Quaternion Transformation of vector R from b to I frame rx rx ry — Cl Ty R1 = QRbQ* = x (rbi 4- rbj 4- rbk) x x (go - qii - <hj - дз&) Puasson equation Cl = cfo Q — Recurrent solution of Puasson equation Cl(tk+l) = C‘(tk)M[ ЬС1 = (1 + йьТ) Cj(tt+1) = AC?C?(t*) AC| = (I-й>ьТ) Qk+1—Qk^Q inertial ( body Qk+^Q'Qk body inertial

Chapter 3 Principles of Inertial Navigation 3.1. General Navigation Equation The main idea of the inertial navigation is based on the acceleration integrations. A device, which can measure vehicle acceleration is called accelerometer. First integration of the vehicle acceleration provides velocity. Second integration gives vehicle position increments with respect to an initial point. For the determination of the naviga- tion parameters (velocity, position, attitude) in a certain navigation frame, the acceleration projections on that frame have to be provided. In order to coincide sensitive axes of accelerometers with a certain navigation coordinate frame, different types of gyroscopes are used. There are two approaches for the navigation frame simulations in the inertial system technique. First one deals with the physical implementation of the navigation frame using a three-axes gyro- stabilized platform with three orthogonally placed accelerometers. Such type of a system is called platform or gimbaled INS. The second one, called strapdown INS (SINS), provides the analytical image of the navigation frame in on-board computer, using measurements from accelerometers and rate gyros installed directly on a vehicle body.

3.1. General Navigation Equation 43 In order to define the navigation equations for the position and velocity determination, accelerometer measurements have to be intro- duced. Accelerometer measures specific force /, which can be described as f ~ О 9m (3.1) dr (It /J I Л- d О — - absolute acceleration (acceleration with respect to the inertial frame) 9m ~ gravitational acceleration due to the mass attraction, considered positive towards the center of the Earth. In other words, the specific force is proportional to the inertial (absolute) acceleration of a system due to all forces except gravity. Let us define the equation for absolute acceleration a. Using Coriolis formula (2.4) (see Chapter 2), one can determine d [dr dt dt = -y- V + Uxr dt (3-2) where V - vehic _ dr Ht i + U x r = V 4- U x r E e velocity with respect to the Earth-fixed frame U - Earth angular velocity. J i Equation (3.2) can be rewritten in a form dV a = —r~ dt i - dr 1- и X — at I (3.3) and - dr U x — dt = U x (V + U x i f) = U x V + U x (U x r) (3.4) dV _dV dt j dt + X V N (3-5)

44 3. Principles of Inertial Navigation dV ~3t frame CjN - absolute angular velocity of the navigation frame. - derivative of vector V with respect to the navigation Substituting equation (3.4) (3.5) into expression (3.3), one can define dV _____ a = — + wNxV + UxV + Ux(Uxf) (3.6) at N Using the above expression, the equation for the specific force can be rewritten as / = — +wjvxV + t7xV + C7x(Nxr)-5m at N or jr dV _ — у — _ / = — + uNxV + UxV-g dt N (3.7) (3-8) where g = gm — U x (U x r) - apparent gravity. Apparent gravity is a vector difference between the gravitational acceleration and centripetal acceleration due to the Earth rotation. Equation (3.8) is a general navigation equation. In order to define velocity and position of a vehicle, one can integrate this equation. The integration can be implemented with re- spect to the navigation frame because, as it was mentioned above, an INS provides (physically or mathematically) the coincidence of ac- celerometer axes with this frame. Consequently, for the position and velocity determination the first component in equation (3.8) is use- ful only, whereas other ones must be compensated. The second and third components of the above equation, called Coriolis correction, can be removed by analytical computation. The fourth component, which includes the apparent gravity vector, can be compensated by the phys- ical or analytical mechanization of the horizontal plane an INS. In this case, for the horizontal channels, the projections of apparent gravity will be zero.

3.1. General Navigation Equation 45 Let’s apply the local-level frame as a navigation frame and define the projections of equation (3.8) on the local-level axes. In order to do that, the following vector multiplication rule must be introduced: ' i j к ' m x n — det{ mx my mz } = Tlx = (mynz — mzny)i - (mxnz - mznx)j 4- (mxny — mynx)k (3.9) Using the above rule, the projection of the specific force on the local-level frame can be described as fE = + WjvKp — ^up^N + UtfVup — UupVN ~ ~ ШЕ^ир + WupVe — UeVup + UupVe CLb fup = “+ weVn — ^'nVe 4- UeVn ~ UnVe 4- д (3.10) at Let’s define the projections of absolute angular velocity cu on the local-level frame for the spherical Earth model (see equation (1.1)) VN U/£ — R 4~ h VE (Vjv = R + h + Uc°SV ^up = V»? z . ——- tan (p 4- V sin ip (3-11) R 4“ h The Earth rate projections are UE = 0; UN = U cos<p; Uup = Usiny? Substituting the angular velocity expressions into equa- tion (3.10), it is possible to define fE = ~ tan <p - 2U sin <pVN 4- Vup( 4- 2U cos tp) at R + n, rt + fi Sn = RTXtan¥’ + 2l7sin‘*’VE + V“pyTT at it ~r a rt ~r fi f" = ^-l^-^h-VE2u™*+9 Ш2)

46 3. Principles of Inertial Navigation In real applications the ellipsoidal Earth model is used. In this case, the projections of the local-level angular velocity on its axes can be rewritten, as VN WE Rv + h Ve uN = ——— + U cos 99 R\ + h VE wUp ~ Ъ-------Гtan sin </9 (3.13) лд + h where h - altitude above the reference ellipsoid Ry, R\ are the radii of curvature of the reference ellipsoid in north-south and east-west directions, respectively. That is = 74(1 - e2) v (1 — e2 sin2 9?)3/2 ^A = 77-------2 A2 -U/2 (3-14) (1 — e2 siir 9?)1'2 Here, Re is the equatorial radius of the Earth and e is the ec- centricity given by e2 = 1 — b2/a2 (a is the semi-major axis of the reference ellipsoid and b is the semi-minor axis). For the ellipsoidal model equation (3.10) has a form « dVp FpF/v Vp fE = -777- rtan^-2[/sin^VN +Vup(—— 4-217 cos <p) at 4- ri /Сд 4- ti , dVN V2 VN fu = -j- + „ tan <p + 20 sin <pVE + V ———- at K\ + h R^ + h , dVuv VS VZ fup ~ dt R^ + h" Rx + h~ Vb2Ucosv + 9 (3.15) Let us consider instead of the local-level frame the wander frame. The relationship between the local-level frame and the wander frame as well as a positive direction of azimuth (wander) angle e are demon- strated in Figure 3.1.

3.1. General Navigation Equation 47 Figure 3.1. Relationship between Local-Level Frame and Wander Frame According to the introduced relationship, one can get N = Yw cos e + Xw sin e E = —Yw sine + Xw cose (3.16) where e - azimuth (wander) angle, which can be determined as VE In order to simplify the notation, new symbols can be introduced as E Rv + h о = Ve N Rx + h (317) ЛД + n and lje — = + U cos wUp — 4- U sin <p. Here Qjv, Qup can be considered as the projections of relative angular velocity of the navigation (local-level) frame with respect to the Earth-fixed frame.

48 3. Principles of Inertial Navigation Using the introduced wander frame axes (see equation (3.16)), one can write the relative angular velocity of the wander frame cis = Q# cos e +Qjv sine flyw — —Qe sine+ Q;v cose (3.18) or VN . VE — —------ cos e 4- —--- sin e Rip 4- h R\ 4- h VN . , VE ~------ sin e + —---- cos e Rip 4- h R\ 4~ h (3.19) The above expression can be rewritten through the relative wan- der frame velocity as 1 — —/(1 — 3 cos2 e cos2 — sin2ecos2<p) 4- Re [2/ sin e cos e cos2 (/?] I'xlV - x f-t r» ‘ 2 2 2 2 \ — 1 — —--------- 3 sm e cos 92 — cos e cos ip) 4- Y Re [2/ sin e cos e cos2 </>] J.Le VxW Re •*ce + Re (3.20) = = where f is flattening (/ = 1 — b/a, a is the semi-major axis and b is the semi-minor axis of the reference ellipsoid) Re - equatorial radius of the Earth V®vv, Vyw ~ relative linear velocity of the wander frame with respect to Earth.

3.1. General Navigation Equation 49 In Russian INSs the above formulas have a form — = 1 1 VyW , VcW 2 2 —----1----e COS ip Sin E COS E а VVW 2 2 ——e cos <p sm £ cos e а n Sin ip 2 2-2 1 — e------1- e cos <p sin e- 2 а •) sin ip 2 2 2 h 1 — e------1- e cos ip cos e- 2 a Vxw Rx 1 a 1 a (3.21) ;«2 where e - eccentricity of the reference ellipsoid h - altitude а - semi-major axis of the reference ellipsoid. Using the introduced wander frame definition and rotation, the following relations between the absolute and relative velocities can be derived: WXW — + UxW UyW = ^yW +Uyw ^zW = UzW (3.22) and UxW — U cos ip sine; Uyw — U cos ip cose; UzW — U sin ip. Let’s remind (see section 1.4.), that the wander frame absolute angular velocity is wzW = U sin ip and = 0. Using that, one can write general navigation equation (3.8) for the wander frame as fxW — , 2(7^ H/W + (Ц/w + ^Uyw)Vzw fyW = ^^+2UtWVxW-^xw + 2UlW)VzW (3.23) at fzW = -i- Vywi^xW + 2Uaciv) — UxW(Qvly 4- 2t7yw) + 9 at 4 — 9437

50 3. Principles of Inertial Navigation After the determination of wander frame relative velocities Vxw, Vyw, it is possible to recalculate them into the local-level frame as follows: V/v — Vyw cos £ + Vxw sin £ VE — —VyW sin 8 + Vxw COS 8 (3.24) The coordinate values (latitude, longitude) can be calculated as Vn R<p 4- h Ve (Rx 4- h) cos 9? (3.25) Using the parameters of the transformation matrix between the local-level frame and the body frame (see expression (2.2)), one can determine the attitude angles: pitch - tf, roll - 7, heading - H as follows: * = arctan (g) , ,9 e g; -g] /Ci3\ 7 = — arctan l — I , 7 € [тг; — тг] \Ь33/ (C \ ~~ I , H € [тг; —тг] С22/ Co = + С32з (3.26) If instead of the local-level frame the wander frame is used as a navigation frame, then attitude angles can be calculated by same equation (3.26). But in this case, instead of the heading angle yaw angle V’ is calculated. Let us introduce definition of yaw angle ф. The heading angle is an angle between the longitudinal axis of the vehicle body (y-axis of the body frame) in projection on the horizontal plane and north direction. Yaw angle ф, in general, is an angle between the longitudinal axis of a vehicle in projection on the horizontal plane and the y-axis

3.2. Classification of Inertial Navigation Systems 51 Figure 3.2. Definition of Heading and Yaw of the wander frame. Consequently if the local-level system is used as a navigation frame, then H = гр. If the wander frame is used, then H — гр — e (3.27) where гр - yaw angle; e - azimuth angle. The corresponding picture is shown in Figure 3.2. In this chapter the usually applied in Russia definitions of yaw, and heading positive directions, is used as shown in Fi- azimuth gure 3.2. 3.2. Classification of Inertial Navigation Systems 3.2.1. Local-Level INS A local-level (platform) system contains a three-axis gyro-stabilized platform with three orthogonally placed accelerometers. The main property of uncontrolled gyrostabilizer is the ability to maintain con- stant orientation relative to fixed stars. It means that the uncontrolled 3-dimensional gyroplatform physically implements the inertial frame 4*

52 3. Principles of Inertial Navigation and accelerometer axes, in principle, coincide with this frame. Ac- celerometer indications in projections on a certain navigation frame are actually needed for the implementation of equation (3.8). In order to provide that, the platform frame (we assume, that the accelerom- eter axes coincide with the platform axes) must simulate a certain navigation frame, for example, the local-level. For the above purpose the gyroplatform has to be controlled by the same angular velocity as the local-level frame. In order to do that, the control signal propor- tional to the local-level absolute angular velocity must be introduced into gyro torques. The above angular velocity is calculated in the on-board computer, using accelerometer measurements as an input. As a result, the platform axes precess with the same angular velocity as the local-level frame. Consequently, if at the beginning the plat- form axes are aligned with respect to local-level frame, then when the system starts to move, the platform axes will coincide with the local-level frame due to the feedback from the accelerometers output through the computer to the gyro torques. Such type of a system is called undisturbed INS, since the platform orientation, in principle, does not depend on vehicle motion due to the platform imitation of the local-level frame. The functional scheme of the local-level INS is shown in Fi- gure 3.3. Here, using the output of accelerometers and Coriolis correc- tions, the angular velocity of the local-level frame can be calculated as Wfi = <^N — WUp = Vn Rip 4- h VE r-.. +Ucos<p fix 4- h VE ------- tan (p 4- U sin <z> Rx 4- h (3.28) and introduced as a control signal for the gyro torques. At the same time, using acceleration projections on the local-level frame velocity Viv, Ve and coordinates <p, X can be calculated. Attitude information

3.2. Classification of Inertial Navigation Systems 53 ~v-(r^+2Ucos<p)+ +(Usin(p+colip)VN VE(0) Gyroplatform Accelerometer E Gyro E Accelerometer Up Gyro Up GyroN Accelerometer N On VN 1 -(Usintp+coJVg VN(0) 1 Rx+h Figure 3.3. Local-Level System

54 3. Principles of Inertial Navigation (pitch, roll, yaw) can be defined by direct measuring of the angles between the INS system block and the platform. The similar scheme can be designed for the wander frame orientation of the platform. For this purpose equations (3.23) have to be used. 3.2.2. Strapdown Inertial Navigation System A strapdown navigation system contains three accelerometers and three rate gyroscopes, which measure the projections of specific force and the projections of absolute angular velocity on their sensitive axes. The sensors are fixed in the box and such configuration is called an inertial measurement unit (IMU). In this case the IMU axes coincide with the body axes and the projections of specific force and angular velocity on the body frame are available. In order to recalculate the above projections into the navigation frame, the direction cosine ma- trix between the body and navigation frames is needed. This matrix can be defined from the Puasson equation, see (2.11), which has a form С? = - (3.29) where - transformation matrix from the body frame to the navigation frame o)b ~ matrix with projections of absolute angular velocity on the body frame &n ~ matrix with projections of absolute angular velocity on the navigation frame. Here, u)b and have a following form: 0 — wj * у &b = шьг 0 0 -wf ы" Wjv = ы? 0 -w" 0 (3.30)

3.2. Classification of Inertial Navigation Systems 55 In order to solve the above equation, the information of is needed. The absolute angular velocity of the body frame can be measured directly as an output of rate-gyros, whereas the absolute angular velocity of the navigation frame has to be calculated. Above calculation is based on the application of accelerometer measurements. The functional scheme of a strapdown inertial navigation system is shown in Figure 3.4. Here, the local-level frame is used as a navigation frame. Let’s interpret the above scheme. Assume, that the initial value of C* matrix is known. Using this information, the projections of specific force can be transformed from the body frame to the local- level frame and absolute angular velocity of the local-level frame can be calculated. Using gyro measurements and the calculated above local-level angular velocity, it is possible to solve Puasson equation (3.29). As a result, transformation matrix will be available for next steps. The position and velocity calculation scheme is absolutely the same as in the local-level system. Attitude information can be calculated directly from the elements of matrix. In philosophical meaning, the local-level system and strapdown system has the identical oper- ational scheme. Indeed, in strapdown configuration, the block with Puasson equation calculation can be considered as an analytical im- age of a gyroplatform and a feedback from uj? calculation block to the Puasson equation is physically implemented by a computer to the gyro torque loop in the local-level system. 3.2.3. Principle of INS Alignment In order to start the navigation computations, the initial coincidence between the accelerometer sensitive axes and the local-level axes has to be provided. Such procedure is called INS alignment. For a strap- down system the alignment purpose is to estimate the initial value of the direction cosine matrix between the body and local-level (naviga- tion) frames C^L. The elements of the above matrix (see Chapter 2) can be rearranged through three attitude angles ($ - pitch, 7 - roll, H - heading). It means that for the SINS alignment the magnitudes

56 3. Principles of Inertial Navigation Body frame Navigation frame Figure 3.4. Strapdown System

3.2. Classification of Inertial Navigation Systems 57 of ?9(0), 7(0), H(0) have to be provided. The alignment contains two stages. The first stage is horizontal alignment, while the second one is azimuth alignment. Let’s consider the first stage. Assume, that hori- zontal misalignment angles $(0), 7(0) are relatively small (3—5 deg). Such assumption is reasonable due to the INS box installation on the horizontal plane of a vehicle body. An accelerometer measures the specific force (see equation (3.1)): f = а - дт On the unmoving foundation with respect to the Earth, the above formula can be rewritten as f = ~9 where д — дт — U x (U x f) - apparent gravity. The above formula in projections on the local-level frame has a form fE fN fup 0 0 9 Accelerometer sensitive axes coincide with the body frame and consequently accelerometer measurements can be written as fxb fyb fzb 0 (3.31) 9 where - transformation matrix between the local-level frame and the body frame, represented by formula (2.2). Substitu- ting formula (2.2) in equation (3.31) and using representation of small angles - $(0), 7(0) as cos 7(0) = cos$(0) — 1 sin 7(0) = 7(0) sin i?(0) = t9(0)

58 3. Principles of Inertial Navigation one can get fxb = -fT7(O) fvb = 9<Kty (3.32) The above formulas can be directly obtained from Figure 3.5, which describes the initial orientation of the body frame with respect to the local-level frame under the definition of specific force (/ = — g). Indeed, if at the beginning the body frame have the small deviations from the local-level frame then the projections of apparent gravity will appear and the accelerometers can measure above deviations. plane Ль=-5У(0) (accelerometer measurement) b / \L' Horizontal gv-* plane Figure 3.5. Horizontal Alignment Principle /yb~ff3(0) (accelerometer measurement)

3.2. Classification of Inertial Navigation Systems 59 The real accelerometer measurements contain errors as well, that is zXb = -gy(ty + Bxb zyb = ptf(O) + Byb (3.33) where Bxb, Byb - accelerometer biases. Hence, using the accelerometer output as a measurement, one can estimate horizontal misalignment angles 7(0), $(0). Equa- tion (3.33) yields, that the accuracy of the horizontal alignment is restricted by the level of the accelerometer biases, i.e. 7(0) = tf(0) = (3.34) 9 where 7(0), $(0) - errors of horizontal alignment. The purpose of azimuth alignment is to estimate the initial value of heading angle. Figure 3.6. Azimuth Alignment Principle The principle of azimuth alignment is illustrated in Figure 3.6. In case when misalignment angle Я(0) between the body frame and

60 3. Principles of Inertial Navigation local-level frame does exist, rate gyros measure the projection of the Earth rotation rate on their axes as = —{/cosy? sin H(0) Шу — U cos y> cos 77(0) (3.35) and u)b H(0) = — arctan (3.36) % Consequently, using the output of хъ and уь rate gyros, the estimate of azimuth misalignment angle (initial heading angle) H(0) can be provided. The usual azimuth alignment includes two steps (coarse align- ment and fine alignment) because of the possibly large magnitude of H(0). After the coarse alignment (first step) the approximate value of H(0) is provided. The target of the fine alignment (second step) is to estimate remained after the first step heading angle For the implementation of the second step the same gyro mea- surements are used but under the condition of small remained angle Keeping in mind, that real gyro measurements include the drift rate bias, equation (3.35) can be rewritten for small 67/(0) angle as z(wi) = cos ^<5Я(0) + w* (3.37) where - gyro measurements ~ ёУго drift rate bias. Consequently, the accuracy of the azimuth alignment is re- stricted by the level of gyro drift rate bias, i.e. ,.dr (3.38) U cos where - misalignment error. Indeed, using the gyro indications it is impossible to separate azimuth misalignment angle 6H(0) from the drift rate bias.

Chapter 4 Applied Navigation Algorithm 4.1. Strapdown System Navigation Algorithm The total algorithm can be divided into two parts. The first part deals with the information processing of accelerometer indications. The se- cond one is connected with the gyro output measurement prepara- tion. Using the estimates of accelerometer biases, scale factors and installation errors, obtained by the factory calibration procedure (see Chapter 6), the compensation of the above errors are implemented to raw accelerometer indications. After the above errors compensation one can calculate velocity increments using the following formula дш xb,ybtzb rtk+hNl axb,yb,zb^t (4.1) where x^, уъ, гъ - body frame aXj(>yb,Zb - accelerometer output hNl - sampling period. The similar procedure is used for raw gyro output data. Here, gyro biases, scale factors and installation errors are compensated as

62 4. Applied Navigation Algorithm well. Angle increments can be calculated by the equation: rtk+h/ii ''tk (4-2) where uXbiVbiZb - gyro output. Usually, the output frequency of raw data (accelerometer and gyro output) is sufficiently high (from 400 to 800 Hz). For most appli- cations, such a high frequency of navigation solution is not required. Traditionally, the frequency of INS output signals is about 40 — 50 Hz. In order to reduce the frequency of raw data, the preliminary integra- tion of accelerometer and gyro indications is used. Let’s consider the preliminary integration of accelerations. The absolute object acceleration can be written in a form (see Coriolis formula): dVI dV _ - ~ x V dt I/ dt ь (4.3) —— - ’’total” derivative of the absolute velocity vector dt 11 with respect to the inertial frame dV\ —— - ’’local” derivative of the absolute velocity vector dt I ъ with respect to the body frame Wb - absolute angular velocity of the body frame. The projections of specific force on the body frame are available as an output of accelerometers. Consequently, their integration can be considered as integration with respect to the body frame. Hence equation (4.3) has to be rewritten with respect to local derivative of velocity dV dV\ - ~тг ~ "лГ - x V dt ъ dt I/ The integration of the above equation within time interval hyv3 = gives ftk+hNS лСг ftk+hN3 / ^Td<=/ ~^dt+l

4.1. Strapdown System Navigation Algorithm 63 / ^dt = Jtk dt j 4-/^3 ЛХ/ rtk+hN3 / -^dt + / (uXbV4 - ulbVXb)dt hk at Jtk Jtk dt j rtk+bf/з jy rtk+^Na 1 dt dt + I (^уь^хь ^xb^yb^dt tk Ilk The recurrent solution of these equations can be defined as = 1 4" JVyb)fc_iQ2l>,k “ 1ауь,к 4" Af^a:b,fc Wybjfc — ^УьЛ—1 4” ^Z|>,k- ^хь,Л—l^zb,fc 4“ Wzb.k = Wzb,k—1 4" Wxb,k~ l^yb,fc ^УьЛ~l®xb,fc 4" ^^zb,k f^zb,fc = Wzb,k—1 4" Wa;bЛ^УьЛ 4" AlVZbjfc ^УьЛ = ^уьЛ-1 4" ^zbtkaxbtk ~ ^Xb,kazb,k 4" AWyb>fc Wxb,k ~ Wxb,k—1 4“ ^yb,fc^zb,fc ^zb,fc®i/b,fc 4“ AlV^fc (4-4) with initial conditions WXb = WVb = WZb = 0. The above procedure contains certain steps (here, for instance, m steps). The integration in such manner in literature is called the sculling compensation. After the above procedure velocity increments can be recalcu- lated into the navigation frame, as ’ aw; ‘ Г ^Xb 1 = c^ aw2 » О X where - transformation matrix from the body frame xb, Уъ> 4 to the navigation frame x, y, z. The determination of the above transformation matrix is a goal for the attitude algorithm calculation. The next step of the navigation algorithm is based on the de- termination of the direction cosine matrix between the body and nav- igation frames. As it was shown beyond, for this procedure Puasson

64 4. Applied Navigation Algorithm equation can be used. But for real applications, the quaternion tech- nique is usually applied. For the considered navigation algorithm such procedure is implemented in two steps. First step deals with the determination of the quaternion be- tween the body and navigation frames under the condition that the navigation frame has not changed its position since the last sample. It means that the navigation frame can be considered as the inertial frame during one sample. The quaternion equation for the transfor- mation from non-inertial (body) frame to the inertial frame has a form (see section 2.3): Qn+i = Qi ДА (4.5) where Qf - final quaternion Qp - preliminary quaternion ДА — Ao + ДА11 + ДА27 4- ДА3А; - quaternion of a small rotation (update quaternion) which can be represented using the rotation vector as ДА0 = ДФ = c°s^- ДА! = ДФжь . ДФ - 7 , Sin —— ДФ 2 ДА2 = ДФуь . ДФ = sin —- ДФ 2 ДА3 - ДФг6 . ДФ = - —sin ДФ 2 The second step can be considered as the correction of the quaternion according to the change of the navigation frame with re- spect to the inertial space within last sample. Such correction can be considered as a transformation from the inertial to navigation frames. Using the quaternion equation for such procedure (see section 2.3), one can define Qn+l = ^m*Qn+i (4.6)

4.1. Strapdown System Navigation Algorithm 65 where Am* = Amo — Атгг — Am2j — Am3fc - quaternion of a small rotation between the navigation frame and the inertial frame. It can be also represented through the rotation vector between the above frames. The rotation vector in this case can be described by the first component of equation (2.13) due to the fact, that the transformation between the navigation and inertial frames can be considered as a slow motion. Consequently, Am can be described as Am = cos —-----------sm —-—г-------- sin —-—j------sin------к 2 и 2 w 2 и 2 where wx, wz - projections of the absolute angular velocity of the navigation frame x, t/, z on its axes hyv3 - sampling interval. The considered procedure has a recurrent form and the output from equation ( 4.6) is an input to equation (4.5) for the next sample, see the following diagram: <&. = Qf Q^+i = Aw* Q?+1 Splitting up the quaternion calculations on two steps has a cer- tain reason. Indeed equation (4.5), which describes the transformation between the body frame and the inertial frame can be called as fast motion. The angles between the above frames may have large mag- nitudes, whereas the transformation between the inertial frame and the navigation frame can be called as slow motion, because of the slow orientation change of the navigation frame with respect to the inertial space. The implementation of the quaternion calculations in 5 - 9437

66 4. Applied Navigation Algorithm one formula leads to addition of two different magnitudes of the an- gular velocities. One of them by three or four order is greater than another, which causes calculation errors. Moreover such splitting of the calculation scheme provides clear physical interpretation of the platform simulation in the on-board computer. Indeed, the first equa- tion (4.5) provides the simulation of the space stable (uncontrolled) platform. The second equation reflects the control signal to coincide it with the navigation frame and quaternion Am* can be considered as a calculated image of the gyro torques. Using the quaternion rule of multiplication, equa- tions (4.5), (4.6) can be rewritten in matrix form as КI AAq —AAi —ДА2 —ДА3 ' 9o ' 9f AAr AAq AA3 -aa2 9i ДА2 — A A3 AAq AAi ?2 . 9f . Л+1 ДА3 ДА2 —AAi AAq - Чз - (4.7) and ’ 9o ‘ 9i 9o” 9f -9f 9o -92 9з 1 1 1 AmJ AmJ 92 -9f 9o 9f AmJ . 9з - fc+1 <12 ~9i 9o . fc+1 . Дтз . (4.8) In order to implement quaternion equation (4.5), the increments of rotation vector ДФ have to be calculated. Equation (2.12) describes the rotation vector behaviour, as Ф = а> + |фха? + ^-(Ф x (Ф x w)) and for small Ф angle Ф = w + -Ф x ш Integrating the above equation within sampling interval hNi, one can get _ rtn+hp/3 £ rtn+hff3 ~ ДФ = I wdt + - / (Ф x w)dt 2 Jt_ (4.9)

4.1. Strapdown System Navigation Algorithm 67 The second component, referred as ’’coning correction” or ’’non- commutability rate correction” [5, 6], reflects the fact, that in general vector 4 does not coincide with vector a). The above correction is computed by sampling the gyro outputs at a substantially higher rate than the basic attitude matrix update frequency. By doing that, accuracy of the attitude reference solution is preserved from relatively high-frequency angular rate inputs, such as might arise in vibrational environments. Different algorithms repre- senting the coning correction is based on the numerical approximation of equation (4.9) [5, 6]. In present publication the four-step algorithm is used (deduction see in Appendix D). In a matrix form the above algorithm can be represented by 4 £ <*xb(j) 4 £ <*vb(j) + j=i £ ttzb(j) Myb №zb ДФ where sb <*yb(2) ®zb(2) ^xb(3) Qyfr(3) + ttzfr(3) <W3) 0 ttzbO) . -ttybO) ’ otxb(j) a(j) = Qyb(j) _ <*zb(j) _ ttzb(3) -ttzbO) 0 ’ »xi>(4) 1 г + . <*zb(4) J ’ «хь(4) I <M(4) } (4.1C _ ®zb(4) J (*yb(j) -<*xb(j) ; j = 1,2,3,4 0 - angle increments. |(A + P2 j

68 4. Applied Navigation Algorithm Consequently, four-step algorithm can be used for the rotation vector calculation. In order to synchronize the angle and velocity calculations with the same time step = mhjvi, each a(j) contains sum of m/4 neighboring measurements with hm sampling. Using the determined value of the rotation vector one can obtain the quaternion expression as follows: ДА = ДАо 4- ДА1? + ДА2^ 4“ ДА3А; where ДАо - ,ДФА = cos(-H ДА1 = ДФхь . ,ДФ, = ~л sin(-—) ДФ v 2 7 ДА2 = ДФу6 . ,ДФХ ДФ v 2 7 । ДА3 = ДФгЬ . ,ДФ« = sin(—-) (4.11) ДФ v 2 7 v 7 The series expansion gives the following formula for the quater- nion components: ДАо = ДАг = да2 = ДА3 = ДФ2 Дф4 1 8 1 384 гДФхь гДФуЬ гДФгЬ 1 ДФ2 ДФ4 г = 2 48 3840 ^4'12^ According to the quaternion property, a quaternion norm equals: 9o + 9i + 9г + ?з = 1 Above equality can be violated due to calculation errors. In order to remove this effect, the following normalization procedure can be applied. If the condition Д — 1 — (<7o 4- 9? 4- 92 4- <73) > Ao

4.1. Strapdown System Navigation Algorithm 69 is satisfied, then qn+i = Ji - A ~ + y) (413) otherwise Qn+i = Qn+i Taking into account the relationship between the quaternion and transformation matrix parameters (see equation (2.17)), one can describe Си = Й + Ci2 = 2(qiq2 — <7о<7з) С1з = 2(<71<?з + Qoft) c2i = 2(qiq2 + 9о9з) -2 , -2 -2 -2 c22 = q0 + q2 - <h - <7з C23 = 2(q2Q3 ~ <7o9i) c3i = 2(<7i</3 — qoq2) C32 = 2(q2?3 + tfoQi) C33 = 9o+?3 “Si “92 (4.14) where cu, Ci2j ci3, c2i, c22, с2з, c31, c32, c33 - elements of the transformation matrix between the body and navigation frames. Let’s introduce the navigation frame as the wander frame (see section 1.4). The axes x and у of the above frame are situated in horizontal plane, while the axis z has an azimuth angle with respect to the north direction described by Vetanip

70 4. Applied Navigation Algorithm The transformation matrix between the Earth-fixed frame Xje, Ye, Ze and the navigation frame x, y, z can be described by &11 &12 &13 ^21 &22 &23 &31 &32 &33 XE Ye Ze The elements of this transformation matrix can be represented via latitude 99, longitude Л and azimuth (wander) e angle by the ex- pressions (see section 1.4.): Ьц = — sin p cos Л sin e — sin A cos e bi2 = sin p sin A sin e + cos A cos e bn = cos p sin E bzi = — sin 99 cos A cos e + sin A sin e 622 — — sin p sin A cos e — cos A sin e Ь2з = cos p cos E 631 = cos p cos A Z>32 = cos p sin A Ьзз = sin p (415) The projections of vehicle velocity with respect to the Earth on the navigation frame have a form (see Chapter 3): /•t Vx = AWX* + / (Vv2C4 - Vz(tty + 2Uy))dt J to Vy = AW; - [ (VX2UZ - VZ(QX + 2Ux))dt (4.16) Jto where AW*, AW; - sum of velocity increments calculated be- yond Ux, Uy, Uz- projections of the Earth rotation on the navigation frame £lx, $ly ~ projections of relative angular velocity of the naviga- tion frame with respect to the Earth, which can be described by (see

4.1. Strapdown System Navigation Algorithm 71 Chapter 3) V V « Qa; = ---~e &13&23 йу а fly — 4—^е2Ь1зЬ23 Rx а К = ^1-еу + Л--7) 2. = l(1_e2^+e2^_2) (4.17) о а а where e - eccentricity of the reference ellipsoid а - semi-major axe of the reference ellipsoid H - object altitude. The projections of absolute angular velocity of the navigation frame on its axes have a form (see Chapter 3): CJjp = + Ux ^y = ^y Uy = Uz (4.18) where Ux = Ub13, Uy = Ub23, Uz — Ub33 U - angular velocity of the Earth rotation. In order to define coordinates in the applied navigation algo- rithm, direct integration of velocity is not used. The reason of that lies in the integration of a large magnitude of - j CQg~ *п case °f high latitudes. Usually, for the coordinate calculation Puasson equa- tion is used. Let’s consider transformation matrix between the navigation frame and Earth-fixed frame. Puasson equation for the above matrix can be described as (see Chapter 2) B& — BE&wn_e

72 4. Applied Navigation Algorithm where E HxT d and 0 0 0 0 -Q 0 A recurrent solution of the above Puasson equation has a form: ВД = b12(N - 1) - (lvb32(N - l)hN3 &22(N) = b22(N — 1) 4- — l)hjv3 632(A) = &32(N — 1) + (Qy6i2(N — 1) — fla;622(N — 1))^V3 613 (N) = b13(N-l)-Qyb33(N-l)hN3 623(a) = M^-i) + <U33(iv~ W3 633(A) = 63з(А-1)4-(Пу613(А-1)-ПАз(А-1))^з 631(A) = 612(A)623(A) - 622(A)613(N) (4.19) After determination of the transformation matrix elements, one can calculate geographic navigation parameters as follows: <p — arctan [0 4- ±90°] bo A = arctan [0-r-±180°] (4.20) 631 where bo = \Д?3 + Ь23 £ = arctan ~ [0 4- 360°] 623 East and north velocity components can be calculated by Vn — Vy cos e 4- Vx sin e VE = — sin £ 4- Vx cose (4.21) Consequently the INS output information is updated with step Hn3. The calculation scheme of the navigation algorithm is shown in Figures 4.1, 4.2.

4.1. Strapdown System Navigation Algorithm 73 IMUGxb Gyb Gzb Azb Ayb Axb COxb^ (Oyb^ r (Dzb ^xbv ^ybv ^zb Ию Compensation of gyro errors according to model (6.1) Compensation of acceleration errors according to model (6.1) Calculation of angle increments tk+hNI = j* ^xb’>b’zb^t Calculation of velocity increments Ct xb,yb,zb — Ct xb,yb,zb Coning correction (equation 4.10) дфДдфДдфД alignment Я9,Я? я? Quaternion calculation -1 A) — АФ £аЛо — cos 2 ДХ, = 1 АФ 2 ^=^sinAT A^=^sin^ A Ffxb,yb,zb — J tk Figure 4.1. SINS Navigation Algorithm (Begin)

74 4. Applied Navigation Algorithm Figure 4.2. SINS Navigation Algorithm (End of Figure 4.1)

4.2. Applied Alignment Procedures 75 4.1.1. Attitude Calculation If the parameters of the transformation matrix between the body and navigation frames have been calculated, one can determine attitude angles: pitch - roll - 7, yaw - as follows (see Chapter 3): co = ycii + cia 0 = arctan (c32/c0), i? G £ £ 7 = - arctan(сз1/с33), 7 е [тг;-7г] ip = arctan(C12/C22), 7 € [тг; —тг] (4.22) where - elements of matrix . The above formulas can be provided using the transpose of ma- trix Cx described by equation (2.2). Heading can be calculated as follows (see Chapter 3): H = -0 — H G [тг, — тг] where ip - yaw, e - azimuth (wander) angle. 4.2. Applied Alignment Procedures In order to start the INS calculation in the navigation mode, the initial parameters of transformation matrix are needed. Determination of the above parameters is called INS alignment (see Chapter 3). The alignment of a strapdown system is similar to a local-level system. The only difference is that in a strapdown system the platform frame is calculated, while in a local-level system it is physically implemented. In the alignment mode the following sequential procedures are imple- mented: 1) coarse horizontal alignment 2) coarse azimuth alignment 3) fine alignment.

76 4. Applied Navigation Algorithm The goal of SINS alignment is to estimate the initial parame- ters of the direction cosine matrix between the body frame and the navigation frame. Two possible solutions of the alignment problem can be introduced. The first one deals with the direct estimation of the initial parameters of the direction cosine matrix between the body frame and the navigation frame. In this case the values of $(0), 7(0), H(0) angles have to be defined. In order to implement such procedure, the navigation frame is not simulated in an on-board computer. This procedure is called open-loop alignment. The second one is based on the simulation of the navigation frame in a computer using the origi- nal navigation algorithm. Note, that instead of the navigation frame, the INS implements platform frame, which is misaligned with respect to the ’’true” navigation frame due to unknown initial angles and sen- sor errors. In this case the parameters of the direction cosine matrix between the platform and navigation frames are searched. Using the certain control law, the coincidence of the platform frame with the navigation frame has to be achieved. Consequently, this procedure uses the same navigation calculations as in the navi- gation mode (see section 4.1), but for the coincidence of the platform frame with the navigation frame, the additional control signal is intro- duced. Such procedure is called close-loop alignment. In real systems the close-loop alignment usually is applied. Let us consider the close-loop alignment scheme as an applied procedure, whereas for the open-loop procedure necessary remarks will be done bellow. The transformation matrix between the platform and navigation frames (in alignment mode it coincides with the local-level frame) can be described as (see equation (2.1) under the assumption that ФХ,ФЙ are small angles) СО8Ф2 — sin $z Фу cos Фг + Фх sin Фг sin Фх cos Фг Фу sin Ф2 — Фж cos Фг -ф„ Фх (4.23)

4.2. Applied Alignment Procedures 77 where Фх, Фу - horizontal misalignment angles between the local-level and platform frames, assumed to be sufficiently small (1 — 3 deg). Such assumption is reasonable due to the box instal- lation on the ’’horizontal plane” of a vehicle body Ф2 - azimuth misalignment angle between the local-level and platform frames which can be large. The alignment goal is to reduce misalignment angles Фх, Фу, Ф2 to zero. When Фх = Фу = Ф2 = 0, the quaternion parameters converge to true values, it means, that the initial matrix between the body frame and the local-level frame is determined and 0(о) = адо), 7(0) = 7<r(0), н(о) = нег(о) where $tr(0), 7tr(0), V’tr(O) - true initial values of pitch, roll, heading. Puasson equation for the direction cosine matrix between the navigation and platform frames has a form (see Chapter 2) On — — wpCtf where - absolute angular velocity of the navigation frame - absolute angular velocity of the platform frame. The above equation can be rewritten as C?C£ = C%uNCp - uP

78 4. Applied Navigation Algorithm Substituting the parameters and Cp from expression (4.23) into left side of the above equation, one can get (under the condition that Фг ~ 0) the following scalar equations for the horizontal channels Ф® + шх cos &z + шу sin &z — ш2Фу = ш£ Фу - шх sin Ф2 + Шу cos Фг + о>2Фж = ш£ (4.24) The above equation is called general alignment equation. Let introduce some remarks to the values of platform absolute angular velocities , ш^, ш2. The platform frame simulates the navigation frame using the navigation algorithm calculations. It means, that the platform frame rotates with the same angular velocity as the naviga- tion frame. In order to provide that, angular velocities of the navi- gation frame are calculated and introduced as a control law into the Am quaternion. If the calculated platform is not controlled (the first block of quaternion calculations is used only), then the platform axes will simulate the space-stable (inertial) system and the projections of , ш^, ш? are equal to zero. In the considered case, the calculated platform is controlled by introduction of the navigation frame angular velocity. Moreover, in alignment mode, the additional control signals шсх, ш^ are provided. Consequently, the expression for ш?, ш£, шх here can be rep- resented as + ^+ш* = + where on an unmoving basis ш* = 0, Шу =U cos <p, шх ~U sin <p (шх, ш*, ш*т - components of gyro drift rate.

4.2. Applied Alignment Procedures 79 Let us introduce the following low for the additional angular velocity wj wcx = —kb6Vy = kbSVx (4.25) and 6VX = AWx — k6Vx 6Vy = AWy — k6Vy (4.26) where ДЖХ, &Wy - projections of acceleration on the platform frame, which on an unmoving basis can be described as AWX = -дФу ли; = дФх (4.27) Substituting expressions (4.25)—(4.27) into equation (4.24) and using the Laplace transform of the alignment equation, one can define $x(s)s2 + &x(s)sk + кьдФх(з) = a(s) Фу(в)в2 + Фу(з)зк + кьдФу(з) = /?($) where a(s), /3(s) - Laplace transform of equation (4.24) cross coupling and initial conditions. In time domain the above equations has a form Фх + 2£do$x + и$Фх = a*(t) Фу + 2£ш0Фу+ш2Фу = /?*(«) (4.28) where к = 2£wo> kbg = Wq. Equations (4.28) describe the second order oscillator, where the initial horizontal misalignment errors attenuate with time. The be- haviour of these angles is illustrated in Figure 4.3.

80 4. Applied Navigation Algorithm Figure 4.3. Horizontal Misalignment Angle Attenuation After termination of the horizontal alignment, the following con- ditions are satisfied Фх = Фу = 0. As a result equations (4.24) can be rewritten as follows: шхсов(Ф2) + ojy sin Ф2 - а^Фу = -UJX sin Ф2 + tdy cos Фг + ujz$x = Vy + U cos(<p) (4.29) where wx — 0, wy = U cos (</?). In above equations bjyr were neglected. Azimuth misalignment angle Фг can be estimated under the assumption that wz$y,wz$x are neglected such as or - Ф — arctan - ^ + I7cos(y>) Ф2 = arctan (4.30) Py + U cos(<p) т т where /3X = £ f c^dt; = ^ / Uydt о о

4.2. Applied Alignment Procedures 81 T - alignment time. Expression (4.30) is written in an integral form in order to smooth measurement noise. In oder to correct the platform frame by the obtained estimate of the azimuth misalignment, one can introduce an additional control signal to Am quaternion within only one sample as As a result the calculated platform frame turns to the navigation frame on Ф2 angle. After the termination of the coarse azimuth alignment and in- troduction of the quaternion correction, the fine azimuth alignment starts. The fine alignment can be described by equation (4.29) under the condition, that Фг is a small angle (совФг ~ 1, 8шФг ~ ФД i.e. шуФг = wx After the termination of the fine azimuth alignment, one can specify azimuth angle by the formula 5Ф2 = U cos<p T where px — - f Cfxdt о T - time of the fine alignment. At the end of the alignment procedure, the obtained coarse az- imuth orientation is specified by the estimate of 5Фг. Such procedure can be implemented by the final Am quaternion correction, using a control signal within one sample h/уз as 6Ф, 6 — 9437

82 4. Applied Navigation Algorithm A functional alignment scheme is shown in Figure 4.4. Note, that in case when the navigation algorithm is not used (platform frame is not simulated), general alignment equation (4.24) has to be rewrit- ten for the direction cosine matrix between the navigation and body frames and the expression for the azimuth misalignment estimation, defined from equation (4.24) will coincide with the formula, obtained in Chapter 3. 4.2.1. Stored Azimuth Alignment In order to organize the stored azimuth alignment mode the informa- tion about the initial azimuth or heading have to be introduced into the system. In this case the accuracy of azimuth determination is defined by the accuracy of the external heading information applied. The above external information has to be introduced into quaternion Am within one sample as where Ф2 - introduced external heading. After that, the considered above close-loop horizontal alignment starts. During this procedure the horizontal misalignment angles are attenuated. 4.2.2. Alternative Fine Alignment Procedure In the considered above alignment procedure, all stages for the close- loop scheme are described by introduction of control angular velocities шу. It means, that during the whole alignment procedure, quater- nion Am contains absolute angular velocities wx, ljz and control angular velocities uij. Let us consider the case, when the coarse alignment procedure remain unchanged, whereas the fine alignment deals with removing of the control angular velocities, i.e. = 0.

4.2. Applied Alignment Procedures 83 Figure 4.4. SINS Alignment Functional Scheme 6*

84 4. Applied Navigation Algorithm In this case the model for the misalignment angles in the fine alignment has a form: Фж + Фгии - с^Фу = Фу + Фха>г - шжФг = и* Фг + Фушх - шуФх = where Фх, Фу, Ф2 - misalignment angles cjyr, о^г - gyro drift rates wx = 0; ujy = U cos (p; ujz = U sin (p. The measurement model can be described as follows: Wx = zx = —дФу 4- 6WX + Vx Wy = zy^x + 8Wy + Vy where zx, zy - acceleration measurements in projections on the navigation frame <5WX, 6Wy - accelerometer errors Vx, Vy - vibration errors due to external environment. The problem statement is to estimate misalignment angles using current measurements. However, the east-component of drift rate is not observable (see Chapter 3). Let’s consider estimation of the misalignment angles only using Kalman filter. The equations of the three order Kalman filter has a form (see Chapter 7): Xk Pk/k-l Kk Pk Фхк-\ + Kk(zk - НФхк^) *pk-'*T Pk/k_lHT(HPk/k_lHT + Я]-1 (I-KkH)Pk^ where x = Фх - state vector

4.3. Description of SINS Functional Scheme 85 Xk - estimate of state vectors;* 1 uzT -cjzT 1 ujyT —wxT -Шут UXT 1 - transition matrix M[8WX + Vx]2 0 0 M\8Wy 4- Vy]2 - covariance matrix of measurement noise 0 —g 0 g о о - observation matrix. H = The preferable alignment method has to be selected by a user. It depends on the applied IMU parameters as well as on environments. 4.3. Description of SINS Functional Scheme The theoretical analysis of the SINS navigation and alignment algorithms has been considered beyond. Functional schemes of the above algorithms are shown in Figures 4.1, 4.2, 4.4. The first mode for SINS is an alignment, which can be started just after the introduction of initial point coordinates (see Figure 4.4). The initial condition for the quaternion initialization is: 1 0 It is assumed, that the platform frame coincides with the nav- igation frame because of the unknown misalignment angle values. In reality, some deviations between the above frames are present and the alignment problem is to reduce them to zero. In order to solve this problem, the control angular velocity can be added to the absolute

86 4. Applied Navigation Algorithm angular velocity and introduced into quaternion Дтп. Such opera- tion provides damping process for the horizontal misalignment angles. This procedure is called coarse horizontal alignment. After the ter- mination of the above procedure the horizontal misalignment angles have been attenuated to small values, and the quaternion parameters for horizontal orientation converge to ’’true” attitude angles. Coarse azimuth alignment starts after the termination of the previous stage. During this procedure, control angular velocities are averaged and rough azimuth misalignment angle is estimated. After termination of the coarse azimuth alignment, one can correct quater- nion q by the obtained azimuth misalignment angle. The next stage (fine alignment) is implemented similarly to last one but with the previously obtained quaternion. At the end of the alignment procedure, previously obtained coarse azimuth estimate is specified by fine correction <$Ф2 and introduced into quaternion Am. It is important to note that during all alignment stages the naviga- tion algorithm works in traditional manner, except the calculation of quaternion Am* which can be described as follows: AmJ = . (j) 4- ujc . = c°s( 2 ) AmJ = . zw4-o/\ = -sin(—-—) w4-wc 4 2 7 AmJ = v sm( \ uj 4- ojc 2 Дшз = wz . zw4-o/c. = u, Sm( 2 ) UJ z Navigation mode starts after the termination of the alignment procedure. Functional scheme of the navigation mode is shown in Fig- ures 4.1, 4.2. According to the above scheme, the parameters of ac- celerometer and gyro error models are compensated in raw accelerom- eter and gyro data. The next step of the navigation algorithm deals with coning (non-commutativity) and sculling compensations. After that, the attitude determination is implemented in the quaternion form. As a result, the direction cosine matrix between the body and

4.4. Alignment Accuracy Analysis . 87 navigation frames is obtained. Using the above matrix, it is possible to recalculate accelerometer indications from the body frame to the navigation frame. Roll, pitch, yaw and heading are calculated from the obtained direction cosine matrix parameters. Current coordinates can be determined using the Puasson equation solution. Puasson equa- tion describes the behaviour of the transformation matrix between the Earth-fixed frame and the navigation frame. The solution of such equation is implemented in recurrent form. It is important to empha- size that the navigation computer simulates the platform frame by the quaternion solution and all further navigation calculations are similar to a gimbaled system. 4.4. Alignment Accuracy Analysis In order to analyze the alignment accuracy, the error model of the above procedure has to be determined. This model is given according to the general alignment equation (see equation 4.24) under the con- dition that Фх, Фр, Ф2 are small angles. Фх 4- шуФг - шгФу = ш? -шх Фу - шхФг + шгФх = Шу - Шу Фх -Ь шхФу - ШуФх = wf - ш2 (4-31) where шх = 0, шу = U cos <р, шг — U sin <р - local-level angular velocity on an unmoving basis Фх, Фу, Фг - errors remained after the alignment , Шу , ш? - angular velocity of the platform frame. Angular velocity of the platform frame can be described as

88 4. Applied Navigation Algorithm Here шуг, - gyro drift rate in projections on the local-level frame. Substituting the above relationship into equation (4.31) one can get Фж + шуФг - шгФу = + w* Фу - ШхФг + шгФх = и* + ШуГ Фг + шхФу-шуФх - (4.32) Control angular velocities wj according to the alignment procedure (see equations (4.25), (4.26)) are formed as = -kbdVy 6Vy = SWy-k8Vy шсу = kb5Vx 6VX = &Wx-k6Vx Here AWZ, ДИу - projections of specific force on the local-level frame, which on an unmoving basis can be described as AH"Z = -рФу + Bx аи; = дФх + Ву where Bx, By - accelerometer biases. Substituting the control angular velocity expression into the first two equations (4.32), one can get the solution (Bx= const, By— const,

89 4.4. Alignment Accuracy Analysis w*r= const, o^r= const) MO = kb9 Ml fc6<> (4.33) where оц, а2 - initial phases. The steady state value of the above solution can be obtained as Ф«* X ф«* У ВУ 1 к ,,dr д кьд х . к ,dr д кьд у The second components in the expressions for steady state horizontal alignment errors Ф**, Ф£* can be neglected in comparison with the first ones. As a result Ф** X ф«* У By 9 Bx 9 Hence, the accuracy of horizontal alignment is restricted by the level of accelerometer bias. The above deduction corresponds to the analysis of the alignment principle considered in section 3.2.3.

90 4. Applied Navigation Algorithm The first equation of (4.32) at the steady state (Фх = 0) can be rewritten as - о?хфу = + w* and neglecting ЧгФу one can get U cos <p U cos <p Here, the first component describes the estimation equation for azimuth misalignment angle, whereas the second one is the error. Hence, the accuracy of azimuth misalignment is restricted by the level of gyro drift rate bias (see section 3.2.3). Finally, certain remarks for the selection of damping parameters к and kb have to be done. The above parameters depend on natural frequency of undamped oscillation as kbg = and к = 2£lu0 (see equation (4.28)). On one hand high magnitudes of kb and к provide the fast at- tenuation of the misalignment errors, which is important within coarse alignment mode. On the other hand, in fine alignment mode, when the errors are already at the steady state, the selection of high magnitude of kb and к leads to the extension of the system bandwidth. According to equations (4.32) the gyro drift rates are input dis- turbances for the alignment procedure. If the system has a wide band- width, the high frequency component of drift rates (random walk) passes through the system. It leads to the degradation of alignment accuracy. Consequently, the reasonable strategy of fc, kb selection should be the following. Within coarse alignment the magnitudes of fc, kb have to be relatively high in order to reduce the alignment time. Within fine alignment the magnitudes of fc, kb should be reduced for the effective high frequency noise smoothing. Of course, the con- crete selection of к and kb depends on the alignment task statement as well as on the parameters of gyro sensors.

4.5. Multi-Step Alignment Procedure 91 4.5. Multi-Step Alignment Procedure From the alignment error analysis, it’s clear that the azimuth align- ment accuracy is restricted to the level of gyro drift rate bias, i.e. , .dr Ф,(0) = U cos<p where ФДО) - azimuth misalignment error. Even for a high performance gyro (wdr = 0.01 deg/hr) the az- imuth misalignment error has magnitude of 3.6 arc min for moderate latitude (99 = 50 deg). In order to improve the accuracy of azimuth alignment, a multi-stage procedure can be suggested. N Y ь position) Figure 4.5. IMU Body Position Assume, that an INS box is installed into a vehicle as shown in Figure 4.5. According to equations (4.29) and (4.32) for the fine alignment (совФ^ ~1,8тФгя 1) W, = (-0 + U COS <p + u* (/)

92 4. Applied Navigation Algorithm On an unmoved basis for the local-level system (u>x = we = 0; = ujn = Ucosy?, w* — cvj, at the first position one can get Ф.И . U cos y? U cos y? <*£(/) = (434) The relationship between the projections of drift rate bias on the local-level frame and the body frame has a form CO ~ ^b C0S HQ + ^yb sin Ho - ^yb cos Hq - sin HQ . (4.35) where Hq - initial value of heading ujyrb - gyro drift rate bias in the projections on the body frame (own gyro drift rate bias). Assume, that the vehicle (or the IMU box) turns by 90 deg in the positive direction. At the new position (2nd position), the fine alignment can be repeated. The equations for the second azimuth alignment are W . gw+•&!! U cos y? U cos y? «£(п) = (4.36) where the gyro drift bias for the second vehicle position can be arranged as w^(/I) = w*bcos(Ho+900) + w*sin(H0 + 900) = = -w$,sin cos H„ (II) = w*cos(H0 + 90°)-w*sin(Ho + 900) = = -w^sinH0 - ш^совЯо (4.37) In equation (4.37), it is assumed that own gyro drift rate biases have no significant change during the vehicle turn. Hence,

4.5. Multi-Step Alignment Procedure 93 using four measurements (4.34), (4.36) at two body positions, it is possible to find four unknowns - ФД/), as Ф^(0) , c, , (w‘cos#0-o£sinH0) = z(ucx)n + —------------------ U cos 9? = z(wj)I sin Ho + z(w‘ )n cos Ho = -z(Wy)i cos Ho + z(u}y)n sin Ho where z(.ux)l z(ux)ll г(шу)п = ^(1) U cos 9? = ucx(") U cos 99 = = ЩИ) Hence, using two positions of an IMU with the double azimuth alignment, the accuracy of the azimuth estimation can be improved under the condition that ’’body gyro biases” at both positions have similar magnitudes. In principle, the considered process can contain the multi-stage alignment procedure with more than two vehicle turns.

Chapter 5 INS Error Model Let us introduce small angles Ф^, Фдг, Фир between the platform and local-level frames. The above angles can be considered as an indicator of the system attitude errors. It is easy to explain using the fact, that the ideal physical or calculated platform axes must coincide with the local-level frame or navigation frame. The transformation matrix between the local-level and platform frames can be described by the following formula (see equation (2.3)) (5.1) Puasson equation for the direction cosine matrix between the navigation and platform frames has a form (see Chapter 2 and sec- tion 4.2) ('ll — (Jll&ll ~ where - skew-symmetric matrix of the local-level frame ab- solute angular velocity tjp - skew-symmetric matrix of the platform frame absolute angular velocity. The above equation can be rewritten C^lC^ =

95 Substituting into the left side of the above equation the param- eters of CPLL and CpL from expression (5.1), one can get the following linear approximation = cpLL b)E bJN + Wup p _ W«P LL Фе Фдг - Ф«р . (5.2) The difference between the platform and local-level frames an- gular velocities is caused by the platform gyro drift wj, w* and calculation errors of the above velocities, Aw^, Alj/v, Awup: bJpj — = + bJtf - шир = Дш-Ч> + Ш * (5-3) Generally, the platform gyro drift rates contain the total gyro error parameters according to the error model (see equation (6.1)). The errors in calculation of angular velocity can be obtained from the variations of their expressions in the projection on the local- level frame (see equation (3.11)) by neglecting the altitude influence clS AuJE = SVn R Aujn = — U sin <p6<p (5.4) R Aivup = tan <p 4- (U cos <p + 8ес2(^))<5^ R R where VE, Vjv» WE, Wn - projections of vehicle velocity of the local-level frame and their errors <p, - latitude and its error U - angular rate of the Earth.

96 5. INS Error Model Substituting expressions (5.3), (5.4) into the equation (5.2), one can obtain $E + $up^N ~ ^up^N —--------- Л d ~ ^E^up = -БГ + ~ U SIH (p5<p it • SVe Hr ^E 2 + $nUe - un$e = tan p + 4- (U cos <p 4- — sec (p)6p it it (5.5) Accelerometer measurements in the platform frame can be writ- ten via specific force in the local-level frame and accelerometer errors as aE 0-E CLEl^E ' BE ' on on 4- O-NP-N 4- Bjv ®up p ^up LL Q'UpPup p Bup p or Да# = a£. — a^L = амФир — аирФн 4- леРе 4- Be = aPN — = aup$E — o-E^up 4- O/vpw 4- Bn ^O-up ^up ^up — O’E^N O'N^E ”Ь Q>upPup H- Bup where p.£, pn, pup - accelerometer scale factors BE, Bn, Bup - accelerometer biases. Here it is assumed that the projections of accelerometer errors on the local-level frame and on the platform frame don’t differ because of small variations between the above frames. In present consideration, only sacle factor and bias components are taken into account by neglecting the influence of other components of accelerometer errors (see equation (6.1)). Considering the errors in calculation of the Coriolis corrections,

97 it is possible to determine Vf 6Ve = o-N^up ~ aup$N + о-еР-е 4 Be 4- (217 sin <p + — tan p)6Vn + R 5Ve Vf 4 VNU cos <p6<p 4- • tan^Vw 4- (Ucosip 4- — sec2 R svN = = ~ aE&up + o-nPn 4 Bn — (2U sin p 4- tan p)6Ve — R - VEU cos рбр — • tan pVE - (U cos p 4 sec2 p)VE&p R R (5-6) The position of the local-level frame can be calculated using equations (3.25), which for the spherical Earth model and neglecting altitude has a form VN V = -R i _ Y? A — у-» " Rcosp By variation of the parameters in the above expressions, one can get the error model for coordinates: 6p - 6VN R 6X = Ve . = о + D tanv?(fy? (5.7) R cos <p R cos p Eventually, the total error model of an inertial system is de- scribed by equations (5.5)—(5.7). The sensor errors contain the accelerometer and gyro biases as well as random walk of the accelerometer and gyro error model. It can be shown, that for the relatively short time of autonomous 7 - 9437

98 5. INS Error Model work (1—1.5 hr) the INS error equations can be simplified as SVe — CbN^up ~ + O'EP'E + BE = aup$E ~~ aE$up + ajvMw + Bn 6VN dr Фе + Фир1*^ — ФNlлJup —---ъ—Ь ше it i - л 6VE dr + ФЕ^ир ~ Фир^Е — ----1- it • , , 5Ve , dr Фир + Ф^^JE — tan <p 4- ufup In practical applications, for short-term analysis it is expedient to use the following simplification of the INS error model 6VE • dr BE 6VN Фе • dr ^E Bn &N 0<N^up ~ CLup^N + (LeHe + Be We t dr 0 0 0 ctup^E ~ а^Фир + cln^n + Bn svn+^ ---5~+шв 0 0 0 The inertial system error model can be decoupled in two parts. The first part called Schuler component, does not depend on object motion parameters, while another part, called non-stationary com- ponent, does depend on object accelerations. The solution of such decoupled model consists of the sum of the two component solutions. The mentioned decomposition can be described by 6V =

99 o«p ~ 9 SV£h = -g*$ + BE ф# = ц?-+<4 «# = 0 Be = 0 svAh = a^B +bn $5? = -^-+^ wg- = 0 Bjv = 0 (5.8) + а^Фир + аЕцЕ xvnst ФХ?‘ = **£- 6V™* = дФ’Ё’* — авФир + ajv/z^ AVn,f фп„ = _W$_ (5-9) The solution of Schuler error components can be described as <5V^h = — $jv(O)/?i/sini/t 4- <5Ve(0) cos pt — — /2(1 — cos Pt) 4- — sin vtBE SVffh = ФБ(0) Rp sin pt 4- <5V/v(0) cos pt 4- 4- tJ^r/2(1 — cos pt) 4- — sin i/tBtf (5.10) where p = The solution of first component 6V8h contains sensor errors os- cillation with the large time period of 84 min, which is called Schuler period. Obviously, this component has a rather narrow bandwidth in low frequency range. Then, the response to input disturbances has a form of slowly varying oscillations. Second component solution 6Vnst can approximately be described by the simple formula (under the condition of Фир — const, p, = const) W£6t $upVN + iiEVE 6V]}et « -ФирУв + p,NVN (5.11) The above expression indicates that accelerometer scale factors and azimuth misalignment error Фир are modulated in the 7*

100 5. INS Error Model error model solution by accelerations of object motion. In real appli- cations, one can observe that the INS output error contains the slowly varying Schuler component as well as high frequency non-stationary component. Figure 5.1 presents the INS velocity error behavior in the real aircraft testing. It well illustrates the above deduction. 11) under the assumption that tan dtp The position error can be determined using the integration of equations (5.10) and (5. is negligible: f = — $e(0)(cos vt — 1) + tfVWO) Д- sin vt + Jo R vR (* - - sin vt) - BN-^~ (cos vt - 1) - v Vя R ФирАЕ-~ -I- hnAN— rc к (512)

101 sx Ja Rcostp = Фдг(0)---(cos i/i — 1) + 6Ve(0) *-sin id — cost? vRcosip - — (t - - sin id) - Be-^-----(cos id - 1) + cost? v idRcosip + ФирД7У—-— + реДЕ-—^— (5.13) Rcostp Rcos(p ' where Д7У, ДЕ - increments of distance from an initial point in the north and east directions described as AN = Дт?(/^ + /г) ДЕ = ДА(/?л + h) cos <p where Дт?, ДА - increments in latitude and longitude from an initial point. The analysis of equations (5.12)—(5.13) shows, that the INS po- sition errors grow with time due to the gyro drift rates (Schuler part) and increase with respect to the distance increments due to azimuth misalignment Фир and accelerometer scale factors (nonstation- ary part). Usually the error time growth has much higher magnitude than the distance rise. Consequently, the magnitude of gyro drift rate can play role an indicator of the INS quality. The considered solution of the INS errors in the form of equa- tions (5.10)—(5.13) is valid within the time duration of 1—1.5 hr. For long-term operation of an INS (10 hrs and more), the INS error so- lution should contain the oscillations of the Earth rotation rate fre- quency as well [2, 3, 4]. It is important to note that the above INS error model has been obtained in the projections on the local-level frame and is correct for strapdown systems as well. But the input disturbances, such as drift rates, accelerometer scale factors and biases in the error model of a strapdown system are not own sensor errors, but their projections on the local-level frame. The relationship between the own gyro and

102 5. INS Error Model accelerometer errors and their projections on the local-level frame can be described by the following expression w* \C^ 0 0 UJdr ' ца — 0 ciL 0 ца В LL 0 0 В в where - body frame to local-level frame transformation matrix, see Chapters 3-4 wLL; Bll ~ projections of gyro drifts, accelerometer scale factors and biases on the local-level frame шв, (да)в, В в - projections of gyro drifts, accelerometer scale factors and biases on the body frame. Obviously, that in the strap down error model the own sensor errors are modulated by the attitude change of object motion. The comparison between the strapdown and platform INS error models with respect to the gyro drift rate bias will be considered in details in section 5.1. 5.1. Comparison between Strapdown and Local-Level Error Model The main idea of this comparison is based on the possibility of strap- down system interpretation as a local-level INS with analytical image of the gyro platform. The above possibility can be illustrated by the representation of the strapdown system calculation through the local- level system image as shown in Figure 5.2. Here, the scheme of the strapdown system calculation can be divided into two branches. The left branch can be considered as an analytical image of the gyro platform in the local-level system. The right branch is the image of on-board computer in the local-level sys- tem. According to such representation of the strap down system its error model in projections on the navigation frame is the same as in the local-level system. But the own errors of gyros and accelerometers

5.1. Strapdown and Local-Level Error Model 103 a y н Figure 5.2. Calculation Scheme of Strapdown Navigation System

104 5. INS Error Model have to be recalculated from the body frame to the navigation frame through matrix as 1 Г or — 0 Да L 0 , .dr Bb . Jb (5-14) 0 0 0 0 If the gyroplatform image in a strapdown system is splitted on the IMU and the calculation block, the above error model analogy between the strapdown and local-level systems is broken. Indeed, in a strapdown system own positive gyro drift rate of an IMU leads to negative attitude errors of $e,n,uP> whereas in the local-level system positive attitude errors $e,n,uP are caused by positive gyro drift rates , 0$, (see equation (5.8)). Let’s show, that the above deduction is correct. For simplicity of the consideration, one can assume that the inertial frame is selected as a navigation frame. In this case, the following Puasson equations (see section 2.2) can be introduced: C^=C^b (5.15) Cf = + (5-16) where - cosine direction matrix between the body and nav- igation frames C?1 - cosine direction matrix between the body and calculated platform frames. Remind, that the calculated platform frame is the navigation frame which is generated in on-board computer using real sensor indi- cations, which contain errors. It leads to the difference in the Puasson equation representation. Let’s introduce the error in cosine matrix calculation els ЛС = cf - of = cf - cficf = (5-17)

5.1. Strapdown and Local-Level Error Model 105 where [Ф] = 10 0 0 10 0 0 1 1 -Фг Фу Фг 1 -Фх -Фу Фх 1 о -Фх ф L и Фг ~Фу 0 -Фх 0 Lpl The derivative of matrix AC has a form (5.18) ДС = [Ф]С?‘ + [Ф]С?‘ and using equation (5.16) ДС = [Ф]С* + [Ф]С^о>ь + [Ф)С? й? (5.19) Neglecting the last component in the right side of equa- tion (5.19), one can get ДС = [Ф]С? + [Ф]С^ь (5.20) The expression for AC can be also obtained from the difference between equations (5.16) and (5.15). ДС = (C? - С?)йь + Cfitf (5.21) ДС Substituting equation (5.17) into expression (5.21), one can get ДС = [Ф]с?'йь + C^r (5.22) Equating the right sides of equations (5.20) and (5.22), the fol- lowing expression can be determined Йе?"+дос? +дм or [Ф] =

106 5. INS Error Model The last expression can be rewritten in a vector form as Фт ‘ Ф» , .dr , .dr ШуЬ . ^zb _ (5.23) Obtained result shows that in a strapdown system, the negative gyro drift rates, passed through the system from an IMU to the gyro platform image, lead to positive attitude errors Ф^, Ф^, Фир. The above model is used in many publications for the strapdown system error model [12, 13]. In principle, it is correct, but if a strapdown system is considered as an analytical image of the local-level system then positive wj, cuj lead to positive attitude errors Ф^, Ф^, Ф In other words, the sign of gyro drift rates in equations (5.8) depends on the method of their compensation. In case, when a user wants to compensate gyro drift rates in raw data, the above errors have a negative sign in error model (5.8). If the correction is orga- nized by the compensation of drift rate in the gyro torque image (Am quaternion), the sign for drift rates in the error model remains positive as in the local-level system. In the future consideration of this book, the error model for the strapdown system with the positive sign of gyro drift rates as in the local-level system will be used, but their compensation is optionally implemented by the scheme shown in Figure 5.3. Here, the positive sign of gyro drift rates is used (according to model (5.5) or (5.8)). But for the compensation in rawr data their sign has to be inverted (it means adding estimates of a)^r to raw gyro indi- cations). For their compensation by the introduction of to the gyro torque the sign must be positive (estimates have to be subtracted). Mathematically the above judgment of the drift rate sign can be illustrated by the following deduction. Let’s consider the effect of the gyro drift rate influence only. For a strapdown system, the source of drift rate is the own gyro drift rate in the body frame. As a result, the platform frame has small deviations with respect to the navigation frame due to the above gyro drift rates. It means that the direction

5.1. Strapdown and Local-Level Error Model 107 Figure 5.3. Scheme of Gyro Drift Rate Compensation

108 5. INS Error Model cosine matrix between the body frame and the platform frame can be described as СГ = СГ(йь + й£)-йР1С? (5.24) Here a)pi differs from due to the calculation errors only. Assume that the strapdown system is considered as an analytical image of the local-level system. In this case, the above equation for Cbl can be rearranged through the drift rate of the platform system. It means that the body drift rates are reduced to the platform drift rates and equation (5.24) can be rewritten as + (5.25) Equating the right sides of equations (5.24) and (5.25) one can get -L /*, rPl — Г'Р1,\ Г^Р1 , .drsiPl '-'b + t-'b 4 — LUpiCb — Cb b)b — U)piCb —Wp^h or syPl-dr _ dr ft Pl Gb ШЬ ~ ~ШР1СЬ and CPI?' dr fb _ ' dr b Pl — ~^Р1 In a vector form the last equation can be rewritten , .dr ^pl x и , L wpl up J a) dr bx bJ bJ dr % bz _

5.2. Schuler Loop 109 Taking into account, that the platform frame has small devia- tions from the navigation frame, the first order error model can be rearranged . •Ar wdr up J ' .dr " , ,dr _ ^bz _ = -cf The obtained equation confirms the deduction determined be- yond, that the navigation frame gyro drift has to the body frame gyro drift sign. 5.2. Schuler Loop The single channel Schuler component of the INS error can be de- scribed by the diagram shown in Figure 5.4. The above diagram has clear physical meaning. The analytical image of the gyro platform (or the physical gyro platform) has devi- ation from the true local-level frame Фдг due to sensor errors. As a result, the accelerometer measures the projection of apparent gravity on its sensitivity axis with its own bias BE. The integration of the above signal gives the INS error in velocity. The second integration leads to the INS position error 6E. In order to provide the coincidence between the platform and navigation frames, the absolute angular ve- locity of the navigation frame has to be introduced as a control signal to gyro torques (or for a strap-down system - Am quaternion). The above signal is calculated through linear velocity VE. As a result, the errors in linear velocity disturb the gyro platform, which causes error angle Фдг between its attitude and the true navigation frame. This feedback is called ’’Schuler loop”. As shown before, the Schuler frequency of INS error change is v — The reason lies in a feedback from the accelerometer, which measures the projection of apparent gravity gt to the calculation of the control angular velocity, which is defined by linear velocity divided by the Earth radius R.

110 5. INS Error Model

5.3. Influence of Azimuth Misalignment 111 As a result, the INS velocity error 6Ve behaviour can be de- scribed as oscillation with a constant magnitude, which does not grow with time because of the feedback from the ’’first integrator”. The second integrator, which generates position errors 6E does not have a feedback and these errors grow over time. The considered type of an INS (Schuler turned INS) is usually called as an undisturbed inertial system. It means, that in primitive analysis, when only the Schuler error component is considered, the error behaviour does not depend upon vehicle motion parameters. The physical interpretation of this effect is a pendulum with an arm of the Earth radius, so that the platform maintains the horizontal plane in any type of motion, see Figure 5.5. Figure 5.5. Interpretation of Gyro Platform as Pendulum 5.3. Influence of Azimuth Misalignment on Output INS Errors Two aspects of the azimuth misalignment influence on the output INS errors can be considered.

112 5. INS Error Model 1. According to the navigation algorithm, accelerations are re- calculated from the body frame to the navigation frame. In real imple- mentations, instead of the navigation frame, the ’’calculated platform frame” (xpt yp, zp) is created in a on-board computer. The platform frame is slightly misaligned with the navigation frame due to sensor errors. In the azimuth direction, xp and yp axes are turned with re- spect to N and E axes by small azimuth error angle Фир. As a result, the underlined components appear in the INS error model: 8Ve = ~9$n 4- a>N$up + Q-ePe + Be SVN — g$E — a-E^up 4- aNp,N 4- Bjv (5.26) The above components are called non-stationary, because they de- pend on vehicle accelerations and a#. Using external information azimuth misalignment Фир can be estimated in case of substantial vehicle acceleration change. 2. According to the principle of inertial navigation, the calcu- lated or physical gyro platform is controlled by angular velocity of the navigation frame. In case, when Фир is present, the recalculation w in the platform frame instead of the navigation frame leads to the underlined error appearance in the model, see equation (5.5): Ф# Фдг &up$N------~ WN$up + Ыд JTL V- reduced drift rate ш^г _ ж dr ~&up$E 4—5—Ь сЧвФир 4" (5.27) reduced drift rate For a vehicle with moderate dynamics, the above errors can be included in reduced gyro drift rates. If external information is used, the above errors together with own drift rates can be estimated and compensated.

5.3. Influence of Azimuth Misalignment 113 In addition, the influence of vertical gyro drift bias o^p *n case of vehicle motion with constant speed has to be explored. Assume that the vehicle speed and acceleration behavior is shown in Figure 5.6. Figure 5.6. Velocity and Acceleration Behaviour The short term solution of azimuth error has the following form *up(t) « Фир(0) + Assuming that (ti — to) is small enough, the first type of the Фир influence, mentioned above, can be described as (East channel) pt rh ft 6VE ~ / aN$updt^ / an$up(0)dt+ / а^(Фир(0) 4- w*)dtw JtQ Jto Jtl « ^Фир(О) As it seen from the above equations, this influence is negligibe for the considered case (steady motion). It’s easy to explain by the fact that if vehicle accelerations equal to zero, there is no impact of the first type of the Фпр influence, while the second type remains. In case of Фдг, Фе estimation, the influence of can be compensated together with the influence of the horizontal gyro drift rate biases. Finally, the possibility of the Фир kinematic estimation using external velocity or position information has to be discussed. From the present analysis, the possibility of the above angle estimation depends on vehicle maneuvers. Using the second type of Фир influence (see equations (5.27)), the ability of the precise azimuth error estimation is problematic. In considered case ш^Фир, ^Фър are slightly separable from own drift rates wfi. Whereas the first type 8 - 9437

114 5. INS Error Model of the Фир influence (equations (5.26)) provides the reasonably precise estimation of the azimuth error in case of a substantial acceleration change of a vehicle. In this case the underlined components in equa- tions (5.26)) have ’’large or variable influence” in comparison with the other error sources. 5.4. Relation between Фе, Ф.у, Фир and Pitch, Roll and Heading Errors According to the INS error model (5.5) the angles between the calcu- lated platform and navigation frames (Фе, Фдг, Ф«р) are called altitude errors. It can be explained by the fact that the actual platform axes have angular deviations from the navigation axes due to sensor errors, which disturb the navigation solution. However, the actual attitude errors, namely the errors in calculations of pitch, roll and heading (5$, fry, Mf), cannot be easily rearranged via Фе, Ф?у Ф«р angles for arbi- trary position of the body frame with respect to the navigation frame (see Appendix B). Let’s determine the relationship among the above errors for the special initial orientation of the body frame. Assume, that using INS indications in navigation mode, the body axes are aligned with respect to the local-level (navigation) axes. It means that the attitude indications of SINS equal to zero, i.e. $cal — Уса! — Heal — 0 and 0 0 1 0 0 1 0 The true body frame orientation with respect to the local-level frame due to sensor errors can be described as CLL ______ s'lLLs'-ip! ____riLL b — '-'pl '-'ь — Upl (5.28)

5.4. Relation between ФЕ, Фдг, Ф„р and Pitch, Roll, Heading 115 where C^L - direction cosine matrix between the calculated plat- form and the local-level frame described by equation (5.1), i.e. 1 Ф«р Ф# /"iLL Фир 1 —Фе —Флг Ф.Е 1 Using the relationship between the elements of matrix C^L and pitch, roll, heading angles (see equation (4.22)), one can get co = dftrue) — 7 (true) = H(true) — Усз1+4з arctan(c32/co) = ФЕ - arctan(c3i/c33) = Ф^ arctan (ci2/c22) = -Фир (5.29) where - elements of matrix C^L. The calculated values of у, H can be arranged as a sum of true values of the above angles plus errors, i.e. deal = 0 = titrue + 8d 7cal ~ 0 = 7true 4" ^7 Heal — 0 = Htrue + 6H (5.30) Substituting expressions (5.29) into equation (5.30), one can get 6d = —ФЕ — — фдг 6H = Фир (5.31) The above relationship can be interpreted by geometrical illus- tration shown in Figure 5.7. 8*

116 5. INS Error Model Zb Figure 5.7. Relation Between d'd and Фв Assume that the INS body is turned in the pitch direction by angle The INS implements the platform frame instead of the navi- gation frame. It means that the output INS indication of pitch is $C(j, as shown in Figure 5.7. The INS measurements of pitch include errors as well, i.e. $cal — $true “t” (5.32) where di? - error of pitch angle dtrue ~ true value of pitch angle. In order to satisfy expression (5.32), the positive direction of 6$ must be from the y-platform axis to the TV-axis as shown in Figure 5.7, whereas the positive direction of Фв angle by definition is from the W-axis to the ^/-platform axis. Consequently, and Фв have opposite signs.

5.5. Error Model of Vertical Channel 117 5.5. Error Model of Vertical Channel According to the general navigation equation in the projections on the local-level frame, one can get (see equation (3.15)) dV f»P = -1г + Д“ор + 9 CiC V2 V2 = -R^k-R^h-2VEUC^ where fup - specific force for the vertical channel Vup - vertical velocity with respect to the Earth g - apparent gravity. The formula for the vertical channel velocity and position can be arranged from equation (5.33) as The variation of equation (5.33) provides the error model for the vertical channel Жр = <5 fup ~ 5Aacup - 5g Neglecting the error in calculation of Да' , one can get 5Vup = &fuP ~ 5g (5 34) The apparent gravity is calculated by the following formula 9 = 9<>(r4t)2 <5'35) zto ' ™ where gG - equatorial gravity h - altitude with respect to the sea level Rq - equatorial radius.

118 5. INS Error Model The variation of equation (5.35) gives o R% 6h _ 2g8h _ 2g6h _ Q Sg = ~29o(Ro + h)2 (Ro + h) = ~(Ro + h} ~ R~ ~~ V (5.36) where v — yf^ ~ Schuler frequency. Substituting expression (5.36) into equation (5.34), the error model for the vertical channel can be rearranged as 8h = = 2v28h + 8fup Assuming that 8fup is caused only by accelerometer error Bup, the final expression for the vertical channel error model is 8h - 2v28h = Bup (5.37) The solution of equation (5.37) under the condition of Bup — const is 6h(t) = 8h(fi) ^_sh\/2t/t + h(0)ch\/2i/t + ^y(chv^i/t — 1) (5.38) where shV2vt = |[exp(V2t/t) — exp(—V2vt)] chv^^t = i[exp(\/2z4) + exp(—\/2i4)] From the solution of (5.38) it is obvious, that the vertical channel of an INS is unstable It can be explained by the fact, that the vertical channel has a positive feedback to both integrators due to the errors in the apparent gravity calculation. While for the horizontal channels, the first integrator has a negative feedback due to the control signal from an accelerometer to a gyro torque It is a reason, that the vertical channel in a stand-alone naviga- tion mode cannot be used. Usually, for the vertical channel stability,

5.5. Error Model of Vertical Channel 119 an external navigation device is applied (for instance, baro-altimeter or GPS). The error damping for the vertical channel, when external in- formation is used, can be described by the following algorithm: V = dWup - Aacup - g — c26h h = Vv-crfh (5.39) where V2 у2 Aa-=-^-^-2t7cos^ dWup - specific force of vertical axis g - apparent gravity 6h - difference between the INS and external navigation device altitude indications Vup - vertical velocity with respect to the Earth h - altitude with respect to the sea level. Equation (5.39) is illustrated by a diagram shown in Figure 5.8. According to the above diagram the error model can be repre- sented as SVup = Bup - c26h + 6h = 8Vup-Cl8h (5.40) or 8h 4- Ci6h + (c2 — 2v2)8h = Bup The above equation describes the second order oscillator, so al- titude error attenuates over time. The ideology of Ci, c2 selection is similar to the approach considered in section 4.4. The selection of large magnitudes of Ci, c2 provides small static errors. However, in this case the system has a wide bandwidth that leads to poor smoothability of high frequency noise. In contrary, in case of small magnitudes of Ci, c2 static errors are large, but the system bandwidth is narrow in the

120 5. INS Error Model Figure 5.8. Error Diagram of INS Vertical Channel Damping low frequency range, that provides strong noise smoothing. In real applications the compromise between the two considered options has to be implemented.

Chapter 6 INS Calibration Procedures In order to implement the navigation and alignment algorithms, cali- bration of sensor errors has to be done previously (see Chapter 4). Let’s introduce the sensor error model 8ciify Scibz — d- СХ-хх^Ъх d- ^xy^by d- ^xz^bz OLy “I- Oiyx^bx d” ayyaby d” ^yz^bz — Otz “I- OLzx^'bx d~ ^zy^by d" O^zzCLbz 8tiJbx = fix d” fixx^bx d" fixy^by d" /^xz^bz d" d" (fixyxClbx d" ftxyyQ’by d” ^xyz^bz)^by d" d" (J^xzxO’bx d" /3XZyCLby d" ^xzz^'bz'j^bz = /3y d" fiyxtdbx d~ 0yy^by d" flyz^bz d- d- (^ухх^Ъх d- ftyxy^by d- fiyxzQ'bz'jbJbx d* d~ (J3yzx&bx d- Pyzy^by d- ^yzz^bz'j^bz 8tUbz = $z d- ^zx^bx d" fizyt^bz d- ftzz^bz d- d- (J^zxxGbx d" l^zxy^by d- (^zxz^bz^^bx d" d" {J3zyxabx d“ fizyyaby d" ^zyz^bz}^by (6-1) Here 8аы,8ыы, (i = x,y,z) - accelerometer and gyro errors in projections on body frame on - accelerometer biases ац - accelerometer scale factors

122 6. INS Calibration Procedures aij - accelerometer installation errors (i / j) Obi - specific force projections (3i - gyro biases ft, ~ gyro scale factors Pij ~ gyro installation errors (г j) Pijk - gyro drift depending on acceleration (flexure errors) u)bi ~ absolute angular velocity in projections on the body frame. In the future discussion the gyro flexure errors are not consid- ered. Model parameters at and ft are assumed to be constant un- known values. The goal of the calibration is to estimate the above parameters. Such procedure can be implemented by installation of an inertial measurement unit (IMU) into the turning table, which axes are oriented precisely with respect to the local-level frame. By rotat- ing the IMU axes with respect to the local-level frame by the different angles, the measurement model for calibration can be created. The extraction of a,, Pi parameters is possible due to the different pro- jections of Earth rotation rate U and apparent gravity vector g on the body frame in the different IMU positions. Two calibration pro- cedures can be implemented for the a, and ft estimation. The first procedure uses raw indications of accelerometers and gyros. The sec- ond one deals with velocity indications of a SINS in the navigation mode. Let us consider the first type of calibration procedure. The initial position of the IMU axes is the following (see Figure 6.1): Xb = E,Yb = N- Zb = Up Projections of the Earth rotation rate on the body axes are = 0; Wy = U cos ubz = U sin tp Projections of specific force on the body frame are аьх = 0; Пу = 0; abz = g

123 Figure 6.1. Initial Position of IMU The accelerometer and gyro measurements in this case according to error model (6.1) can be written as z(ax) = ax + axzg z(ay) = ay d- otyzg z(az) = az + azzg + g z(ljx) = &. + 0xyU cos <p + 0XZU sin <p z(u)y) = (3y + fiyyU cosip + (3yzU sirup + U cosip z(ljz) = flz + fizyU cos ip + fizzU sin ip + U sin ip Let’s rotate the IMU axes with respect to the local-level frame by 7 = 90°. In a new position the body axes will be Xb = -Up- Yb = N- Zb = E Projections of specific force and absolute angular velocity (Earth rate) on the body frame are ax — —g\ au = 0; az = o JL I у 1 Ar ljx = — U sin ip-, wby = U cos ip\ ujbz = 0

124 6. INS Calibration Procedures The accelerometer and gyro measurements in this case can be described as 2(0®) — otx otxxg g z(av) == ay ~ ayx9 --- «z &гхд z(ujx) = (3X- (3XXU sin 9? + (3xyU cos <p - U sin <p z((Jy) = Py~ PyzU Sin + (3yyU cos <p + U cos p z(ujz) - fiz- $ZXU Sin <p + PzyU COS (p Using the above method to create a measurement model, it is possible to define the accelerometer and gyro indications for the dif- ferent rotation angles of an IMU. Designing the measurement model for the different IMU rotations and using the least squares method, one can estimate parameters o?,, fli. Let’s consider an example of the accelerometer scale factor and bias estimation. Accelerometer X lsf measurement Rotation by 7 = 90° z1 (abx) = ax - axxg - g 2nd measurement Rotation by 7 = 270° z2(abx) = ax 4- axxg 4- g Using the above measurements, the estimates of A’-accelerometer scale factor and bias can be estimated as & _ 21(“‘) + z2(a‘) z^a^-z^a^-^g x 2 ’ ““-------------------- J

125 Accelerometer Y 1st measurement Rotation by $ = 90° ^(aby) = ay + ayyg + g 2nd measurement Rotation by -d = 270° z2(4) = ay - ang - g The Y-accelerometer scale factor and bias estimates are . _ 2l(a‘) + *2(<^) . _ z'(fiby) - z2(ab)-2g ay 2 ’ ayy~ 2g Accelerometer Z 1st measurement Initial position ^(az) = ocz^-azzg + g 2nd measurement Rotation by 7 = 180° z2(a*) = az - otzzg - g and z2(a>) + ?(<4) z\ab)-z\ab)-2g az = ------------- azz = ——----------------- The considered calibration approach has at least two main draw- backs. The first one deals with a small magnitude of the Earth rotation rate which leads to the difficulties of the extraction of gyro error model parameter Д. The second one is connected with the requirements of the IMU block precise orientation with respect to the local-level frame,

126 6. INS Calibration Procedures otherwise the orientation errors influence the calibration accuracy. In order to remove the above disadvantages the second calibration pro- cedure has to be used. This calibration approach uses indication of SINS output ve- locities as measurements in the navigation mode. In this case, the IMU block has to be turned by different angles also, but during whole calibration procedure a user deals with the velocity output in the nav- igation mode. The orientation accuracy of the IMU block with respect to the local-level frame is not critical in this case due to the fact, that the SINS velocity indications are available after the termination of the system alignment and the misalignment errors take place as unknowns, which has to be estimated together with other parameters. In order to consider this calibration procedure the simplified SINS error model has to be formed. As it is shown in Chapter 5, the single channel INS error model has a form East channel = ~~9^n + SaE Фдг = North channel = g$E + ^o>n Фе == — + 6o)e where 8aN, 8шЕ, Sljn - accelerometer and gyro errors in the projections on the local-level frame. For the simplicity of the INS error model, an INS works in the navigation mode (after each rotation) not more than 2—5 min Neglecting hVE R ’ SVn R components in the error model, one can

127 get rt SVe — —g$N(ty + SaE — gl dajNdt J to <5Vyv = 5^e(0) 4“ fictN + 9 I Slje^ J to (6-2) where Ф/ДО), ФЕ(О) - horizontal misalignment errors, which can be defined using equation (6.2) and error model (6.1) as $n(0) = -(as + OwP) g Фе(0) = --(ау4-а^р) (6.3) The direction cosine matrix between the body frame and the local-level frame can be written via pitch, roll, heading (see equa- tion (2.2)) as Сц C12 С13 c“ = C21 C22 C23 C31 C32 C33 (6.4) where Сц = cos 7 cos H + sin d sin 7 sin H C12 = cos d sin H C13 = sin 7 cos H — sin d cos 7 sin H c2i = — cos 7 sin H + sind sin 7 cos H c22 = cos d cos H C23 = — sin 7 sin H — sin d cos 7 cos H C31 = — cos d sin 7 c32 = sin d C33 = cos d cos 7 and 7, -H - pitch, roll, heading of the vehicle body frame.

128 6. INS Calibration Procedures The second calibration procedure contains a certain sequence of the IMU rotations by different angles in the navigation mode. After each one-two turns, a SINS has to be switched off in order to guar- antee short enough time for the navigation mode (otherwise the error model (6.2) is not valid). Let us consider an example of the IMU rotation by $ angle. The navigation error model during the above rotation can be created in the following manner. The absolute angular velocity of the body frame during the rotation has a form Wb = (6-5) where $ - angular velocity of rotation. In the above equation the projections of the Earth rate are ne- glected in comparison with the magnitude of $. The direction cosine matrix between the body frame and the local-level frame can be rewrit- ten assuming that angles H, у are relatively small due to the initial orientation of the body frame with respect to the local-level frame. It is not necessary to align the body axes with high accuracy as in the first calibration scheme, but the approximate initial orientation (1—3 deg) of the body frame with respect to the local-level frame has to be done. Under the above assumptions, the transformation matrix be- tween the body and the local-level frames has a form: 1 0 0 0 costf — sin# 0 sintf COST? Using gyro error model (6.1) and body angular velocity equa- tion (6.5), one can define M 6ijbz = /3Z + /3ZX# (6.6)

129 and recalculate gyro errors from the body frame to the local-level frame as 8uje Su)^. 6(jJN Scdlfp = C^L 4 = <5aib cos fl — 8(jjbz sin d 61Л sin d 4- Swb cos d L. </ x (6.7) Substituting equation (6.6) into equation (6.7), the projections of gyro errors on the local-level frame can be defined as — flx -|- ftxx$ 6u)N = Py cos $ 4- (3yxti cosd — sin d - (3zxd sin $ Integrating the above equations, one can determine 6u)Edt = /3xt 4- ftxxti (6.8) [ 6uNdt = sin д + /3yx sin & 4- ^-(cos $ - 1) 4- /32X(cos $ - 1) (6.9) Jo 1? The similar procedure can be used for the accelerometer errors. Indeed, the projections of specific force on the body frame are ’ 0 ' 0 1 0 0 a?, 0 costf — sintf 0 sintf COST? 0 t/sintf gcosd a L 9 J The projections of accelerometer error on the body frame can be described as 6ab = ax 4- ctXy9 sin $ + axzg cos d 6ab = ay 4- ayyg sin $ 4- ayzg cos d Sab = az 4- azyg sin d 4- azzg cos $ (6.10) The projections of acceleration errors on the local-level frame have a form 6aE = Sab 6aN = 8ab cos fl — 5ab sin fl у * (6.11) 9 - 9437

130 6. INS Calibration Procedures Substituting equations (6.10) into the equations (6.11), one can define 6aE = ax 4- axyg sin ti 4- axzg cos ti 6aN = ay cos ti 4- sin $ cos 19 + ayzg cos2 ti — -azsin?9 — azy g sin2 ti — azzg cos ti sin 19 (6.12) Using equations (6.2), (6.3), (6.8), (6.9), (6.12), the velocity measurement model can be designed as 6VE = - axzg+ axyg sin ti + otxzg cos ti - g^-sinti - v -g/3yx sinti - g^(costi - 1) - g/3zx(costi - 1) (6.13) ti 6VN = —ay — ayzg 4- ay cos ti 4- ayyg sin ti cos ti 4- +ayzg cos2 ti — az sin ti — azyg sin2 ti — —azzg cos ti sin ti 4- gfixt 4- g(3xxti (6-14) For ti = 90° the above equations can be rewritten as == &xzg “Ь ^xyQ gfiyx 4" g-r I- gflzx ti ti 7Г — o^y oiyzg az oczyg 4~ gflxxt ~b g^xx^z and for ti = 180° 2Z?z (% = -^-g 4- 2g(3zx - 2axzg ti 6VN = -2ay 4- g/3xt 4- д/Зххтг The equations of velocity measurements for other two rotation angles, 7, H as a example of the gyro installation error calibration are considered in Appendix C.

131 It is important to note, that the defined equations describe the error behaviour within the rotation process only. The total error ve- locity model contains SV* = 8V1 + 8V11 (6.15) 8V1 - velocity errors within the rotation process, described by equations (6.13), (6.14) 8V11 - velocity errors just after rotation within the process of the measurement accumulation (2—3 min). The second component of equation (6.15) is negligible in the comparison with the first one, then for the calibration procedure equa- tions (6.13), (6.14) are used. The second calibration procedure contains the following stages: 1. A system box is installed on the turning (rotational) table approximately oriented with respect to the local-level frame. 2. First, the SINS operates in the alignment mode. After its termination the system switches to the navigation mode. 3. The system box is sequentially turned by different (one or two) angles and the SINS output velocities (during 2—3 min) are stored in a file. 4. The system is switched off and returned to the initial position. 5. The procedure has to be repeated for different rotation an- gles in order to accumulate enough measurements for the calibration parameter identification. 6. The velocity measurement model is created for all rotation positions: to use it the stored velocity indications should be smoothed previously. 7. Using the smoothed velocity measurements and measurement model, obtained from the previous stage, the gyro and accelerometer error model parameters are estimated. For the estimation procedure, the traditional least squares method or Kalman filter can be used. The main problem of calibration is to select the appropriate rotation angles in order to provide the observability of all gyro and accelerometer error model parameters. In real applications the pure 9»

132 6. INS Calibration Procedures first or second calibration procedures are not usually used. A mixed scheme is the eligible solution for the calibration problem. Tradition- ally, total calibration process contains two stages. The first stage, called a preliminary calibration, is based in general on the application of the first calibration approach. As a result, the estimates of unknown calibration parameters in first approximation are determined. The second stage, known as a post calibration procedure, uses the second approach in order to specify the initial estimates of a,, [Зг. In this chapter the calibration of gyro flexure errors was not considered in order to simplify the material exposition. The above parameters can be defined using the similar approach, but for their observability the rotation angles have to be multiple of 45 deg. The final remark deals with the fact, that the gyro and ac- celerometer model can not be considered as a ’’true model”. Equa- tions (6.1) reflect only some physical interpretation of the real error processes. The application of SINS’s velocities as measurements in the navigation mode provides, in averaged sense, right fitting of error model to the reality.

Chapter 7 Applied Estimation Theory 7.1. State Space Representation of Lin- ear Systems The state space representation converts a n-order system of differen- tial equations into a n-coupled first-order differential equations. Such representation is often desirable in many applications, because it al- lows the usage of vector-matrix techniques. In the state space form any linear system can be described as x(t) = A(t)x(t) + B(t)w(t) (7.1) where x(t) is (n x 1) state vector; A(t) is (n x n) system matrix; w(t) is (r x 1) input (system) noise, assumed to be white with zero mean; B(t) is (n x r) input matrix. The part of state vector components or its linear combinations can be physically measured according to the model z(t) = H(t)x(t) 4- v(t) (7.2) where z(t) is (m x 1) measurement vector; H(t) is (m x n) measurement matrix; v(t) is (m x 1) measurement noise, assumed to be white with zero mean.

134 7. Applied Estimation Theory Let’s consider the following example of the state space represen- tation of the system. Assume that the system model is described by the following differential equations x + ay + bx = Wi y = w2 The number of the state vector components equals to the order of the system. Let’s introduce the state vector components as Xi = x x2 = x x3 = x x4 = у x5 = у and X1 = x2 i2 = x3 x3 = —ax4 — bxi + Wi x4 = X5 = w2 0 O' 0 0 1 0 0 0 0 1 The homogeneous matrix differential equation corresponding to equation (7.1) is x(t) = A(t)x(t) (7.3) The solution of the above equation has a form x(t) = <£(t,to)z(o) where Ф(£,£о) is transition matrix.

7.1. State Space Representation of Linear Systems 135 The transition matrix allows calculation of the state vector at some time t, when in the absence of forcing (or disturbing) functions the complete knowledge about the state vector at t0 is given. The transition matrix has the following properties: 1. Ф(М) = / 2. Ф(£,т) = ф-1(т,£) 3. Ф(«,А)Ф(А,т) = Ф(«,т) (7.4) 4 The solution of equation (7.3) can be obtained as x(t) = exp [A(t - t0)] x(t0) (7.5) and Ф(Мо) = exp [Л(£ - t0)] is an exponential of a matrix A, which can be found by means of the power-series expansion as A2t2 A3t3 exp At = I + At + -I- + ... (7.6) The total solution of equation (7.1) is x(t) = Ф(Мо)ж(*о) + / Ф(Л (7.7) J t0 Often a system of interest is naturally defined by continuous- time differential equations, but the system implementation is more convenient in discrete time. In discrete time, the equation can be rearranged as z*+i = Ф*г+1,лж* + Gk+i,kWk (7.8) where Фьыл = Ф(**+ьМ and Gk+i,kWk= f $(tk+i,r)B(r)w(T)dT (7.9) Jtk

136 7. Applied Estimation Theory The first order approximation of Ф^+1ь, Gk matrices can be obtained by the application of the state vector derivative as i(t ) = u gfa+1) ~ xfa) v ' T->0 T For a short time sampling, T = tk+i — tk, one can get *£fc T (7.Ю) where xk+l = x(tk+1), xk = x(tk). Writing equation (7.1) for some time epoch t = tk gives x(tk) = A(tk)x(tk) 4- B(tk)w(tk) Substituting in last equation expression (7.10), one can get Gk+yk ^fc+i — (7 4- A(tk)T) xk 4- B(tk)Twk ®k+l,k (7-11) Here, transition matrix Ф^+1,ь is described by the first two mem- bers of series expansion (7.6). The discrete form of the measurement model can be directly obtained from equation (7.2) as zk = Hkxk + vk (7.12) Note, that in equation (7.11) vector wk assumed to be white noise. The appearance of wk in equation (7.11) can be explained by the fact that some state vector components have random behaviour. Such components are called input disturbances. Usually, in practical applications, the following state vector decomposition can be used X1 zzz Фи Ф12 X1 4- ’0‘ x2 fc+1 . 0 Фгг. fc+i,fc X2 k

7.1. State Space Representation of Linear Systems 137 where zj. is a state subvector, which includes components de- scribed by deterministic equations; x% is a state subvector, which con- taines components with random behaviour. The last subvector can be described by a shaping filter, which can be defined as a differential equation with white noise input and output of a certain correlation function. For example, let some vector component has the following cor- relation function R(T,Xi) = A2 exp(-/?|r|) The shaping filter corresponding to that correlation function has a form Xi = — @Xi 4- Ay/2ftw(t) (7-13) where w(t) is white noise with identity intensity; A, (3 are pa- rameters of the correlation function. Note that for any type of correlation function, the corresponding shaping filter can be specified. Obviously, the level of input white noise in system model (7.11) can be interpreted as intensity of system uncertainties. It means that the accuracy of model representation depends on the level of input noise. Rough model description corresponds to high level of input (system) noise. The random part of state vector component cannot be described in deterministic bounds only. In order to improve the accuracy of system model description, influence of the random part has to be reduced. Obviously, the level of the random part depends on the knowledge of physical process taking place in the system as well as on the time scale for the system representation.

138 7. Applied Estimation Theory 7.2. Introduction to Traditional Estima- tion Methods 7.2.2. Kalman Filter Let us consider any linear system, which can be described by the following space-state model (see section 7.1) xk — $k,k-lxk-l + Gk,k-l^k-l (7-14) where з;* - (n x 1) state vector ” (n x n) transition matrix Gk,k-i - (n x r) input matrix Wfc-i - (r x 1) input white noise with zero mean and known covariance matrix = Q. A part of the state vector components or their linear combina- tions are directly observable according to the equation: zk = Hxk + vk (7.15) where zk - (m x 1) measurement vector vk ~ (m x 1) measurement white noise with zero mean and known covariance matrix ] = R. Let us estimate state vector xk using the system model and physical measurements vector zk from the condition tr M[(Tjt - xk)(xk - zfc)T] = tr M[xkxk] = min where xk - estimate of the state vector xk - estimation error. For the problem statement it is assumed that M[£o^] = 0 = 0 = 0 for any k.

7.2. Introduction to Traditional Estimation Methods 139 The algorithm of optimal Kalman filter describes the solution of the above problem. The Kalman filter equations are well known and it will be introduced here without deduction 7, 8, 9, 10]. The main Kalman equation has the following form: %k ~~ *&k,k—l^k—1 d* Kk(zk H^k,k—l^k—1) (7.16) Here xfc - optimal estimate of the state vector Kk - optimal gain matrix. Let’s form the equation for the estimation errors as — Xk %k Substituting into the above expression equations (7.14), (7.15) and (7.16), one can get %k — $k,k-l%k-l + Gk,k-l^>k-l — Фк,к—l^fc-1 Rk %к НФк,к—1&к-1) = + Gk,k-l^k-l ~ — Kk(H$k,k-lXk-l + HGk,k-lWk-i + Vk ~ НФк,к-1^к-1) = = (7 — КкН)Фк,к-л%к-1 + (7 — KkH)Gk,k-iUk-\ — Kkvk Using the above, the covariance matrix of the estimation errors can be formed as Pk = M[xkxl] = (7 - КкН)Фкл_1Рл_1Ф?л_1(7-К]кЯ)т + +(7 — KkH)Gkk_iM wfc_i^_i]Gfijb_1(7 “ KkH) + +KkM\ykv£]K% Taking into account the notification of input and measurement covariances as = Q and M= R

140 7. Applied Estimation Theory one can get pk = (/ - к*я)фм_1р*_1ф?>*_1(/ - ккн)т + +(Z - KkH)Gk^QGlk_AI - KkH)T + KkRKl = = (Z - KkH) ^.k-iPk-^^ + G^QC^^I - KkH)T + Pk/k—1 +KkRKTk (7.17) where Pk/k-i - Фк,к^Рк-1Ф^к^ + Gt.t-iQGT.t-i “ covariance matrix of a priory estimation errors. The Kalman filter provides minimization of the following crite- rion J = tr M[ffc£J] = tr Pk = min Let’s determine gain matrix Kk from minimization of the above criterion as — = = 0 (7.18) dK dK ' ’ Substituting expression (7.17) into equation (7.18) one can get = -2Рк/к^Нт + 2КкНРк/к^Нт + 2KkR = 0 (Li\ or Kk = Pklk-kHT\HPk/k^HT + J?]-1 (7.19) In order to obtain expression (7.19), the following rules for the matrix trace differentiation are used [8, 12]: d tr(AB) dA d tr(ABAT) dA~ = BT = 2ABT Using expression (7.19) in equation (7.17), it is possible to re- calculate expression (7.17) as Pk = (7 - KkH)Pk/k^

7.2. Introduction to Traditional Estimation Methods 141 As a result gain matrix Kk can be determined from the equations Pk/k-Y = ^к,к-1Рк-1^к,к-1 ^k,k-lQG^k-l Kk = Pk/k-lHT[HPk/k.1HT + R]-1 Pk = (I - KkHk)Pk/k_i (7.20) where Pk = Af[(.rt - xk)(xk — in)7-] = - covariance matrix of final estimation errors Л/*-1 = Af[(s* - - covariance matrix of a-priory estimation errors. For the Kalman filter initialization the following initial condi- tions are used: Xq = M[a?0] Po = M[a;o^ol The above algorithm has clear physical meaning. Indeed, the first component of equation (7.16) describes homogeneous solution of the system model. Forced solution is impossible to determine be- cause of unknown input disturbance vector w. Input vector u)k-i is described by white noise (absolutely random process). Such repre- sentation is mathematical fiction and used only to emphasize the fact that the system model cannot be described with absolute accuracy by deterministic equations, therefore, input noise is introduced in the right hand side of equation (7.14). The level of input noise reflects the order of uncertainties in the system model representation. In order to simulate the forced system reaction on input disturbance vector cdfc_i, the second component has to be added into equation (7.16). The mentioned component consists of the difference between actual and predicted measurements. The calculation scheme of the Kalman filter is shown in Fi- gure 7.1.

142 7. Applied Estimation Theory Figure 7.1. Calculation Scheme of Kalman Filter

7.2. Introduction to Traditional Estimation Methods 143 The first step of the calculation is based on the determination of a priory state vector estimate £k/k-i- In order to provide the above estimate, the deterministic part of the model equation (7.14) is used only. The next step of the calculation illustrates the estimation accu- racy of a priory estimate ik/k-i- On this step the covariance matrix of a priory estimation errors Pk/k-i is defined. After the determination of Pk/k-i, optimal gain matrix Kk can be calculated. The norm of Kk reflects the relationship between the level of useful signal and the inten- sity of measurement noise. That is, if ||ЯМ[£/г/*_1Ё^к_1]Нт|| »II fill > then HKk will be close to identity matrix. Conversely, ||K|| will be close to zero, if ||E|| » ||НМ[£Л/*_1£^_^ЯТ||. The fourth step is the correction of a priory estimate ik/k-i using current measurement vector zk. As a result, final estimate xk is defined. The accuracy of the final estimate is indicated by the last step of the algorithm implementation. Here, posterior covariance matrix of estimation errors Pk is determined. Let’s consider a simple example to demonstrate the Kalman filter features. Let’s introduce the constant system model as Xk = Sfc-i and measurement Zk = xk 4- vk where vk - zero mean white noise with known variance r = M[v2]. In order to estimate xk, simple averaging can be used or in a recurrent form xk ^fc-i ~r\.zk ~ к = 1,2,...

144 7. Applied Estimation Theory where jr - analog of gain coefficient. The variance of the estimation errors in this case can be de- scribed as Pi = M[(x* - £°fc)2] = M[(i£)2] = = £ The usage of the Kalman filter equations provides the following estimation equation: Po - x %k — %k—1 d* . j-j \%k %k—1) kP0 + T p where , p ?_---optimal gain coefficient of the Kalman filter Avj о ~r T Po = M[x2]. The variance of estimation errors: p£ = M[(£*)2] = Ppr _ r kP0 + r k+ — Po The comparison of Pg and Pg shows, that the Kalman filter is more accurate, than averaging, especially in case of large measurement noise. Indeed, if Po » т Pi « Р^ and if r » Po Pi < Pi The reason of the precise estimate of the Kalman filter is based on the fact that the Kalman filter is more intellectual algorithm in comparison with the averaging due to the usage of a priory knowledge of the measurement noise variance and the variance of useful signal. Generally, the accuracy of the estimation algorithm depends on a pri- ory information about the system and measurement models, which is used in its calculation procedure.

7.2. Introduction to Traditional Estimation Methods 145 Observability The estimation of the total state vector is possible if all com- ponents of the state vector are observable by current measurements. Observability of the state vector is an ability of its initial condition reconstruction using current measurements within some time interval to < t < t^. Let’s write the measurement equation for different time epochs: zi = Hxi + Vi = НФх0 + V, z2 = Hx2 + V2 = ЯФ2г0 + V2 = HX2 + V3 = НФ3х0 + V3 zn =Hxn + Vn = НФпх0 + Уп In the above equations simplified system model Xk = is used, that is not critical in the current consideration. In a matrix form the above equations can be rewritten as follows: Z2 = АФжо + v2 V3 , where A = H НФ НФп~1 If matrix A is nonsingular, state vector x is observable. In lit- erature this matrix is called the observability matrix [7, 8, 9, 10]. For non-stationary systems the condition for the uniform com- plete observability is [7, 9]: fc i=k—N for some value of N > 0, where Qi > 0, > 0. The property of the observability leads to abstract possibility of the state vector estimation. In practical applications, users do 10 - 9437

146 7. Applied Estimation Theory not deal with the abstract observability, but with the estimation convergence. In other words the duration of the estimation transition process can be an indicator of an ’’observability degree”. But it is necessary to note that such term is not strictly proper. Indeed the observability depends on the structure of transition matrix Ф and measurement matrix H only, whereas the duration of the estimation transition process depends on the level of measurement noise as well. However, the ’’observability degree” can be used as a tool for the analysis of real estimation properties for each state vector component. Convergence The estimation process of the Kalman filter contains two stages. The first stage is a transition process of the estimation, the second one is a steady state. The covariance matrix of the estimation error behaviour in case of a stationary system is shown in Figure 7.2. Figure 7.2. Covariance Matrix of Estimation Eerror Behaviour Here, within the transition process all estimates of state vector components have to converge to true values. At the steady state the Kalman filter provides the estimates with constant accuracy, which cannot be improved with time. Obviously, the time of transition pro- cess for different state vector components is different. Intuitively one can guess, that the fastest convergence have the state vector compo- nents, which are directly measured. The nearest components to them

7.2. Introduction to Traditional Estimation Methods 147 converge next. The farthest components start to converge at the end of the transition process of the other components. Using the above ter- minology, the components with the fastest convergence can be called ’’strong observable”, whereas the slowly converged components are ’’weak observable’ Let’s consider an example of the transition process estimation for the system: x— 0 or in the state space form x = Ax, where A = 1 0‘ 0 1 0 0 = x; X2 = x; X3 = x 0 0 0 The directly measured component is x^ then z = Hx + V = [1 0 0]a? + V RMSs of the estimation errors for each component are shown in Figure 7.3. It is quite obvious, that the Kalman filter has rather sequential behaviour of the convergence for different state vector components. Mathematically, such transition behaviour can be described using the estimation error equation. Let us form estimation error, as a difference between the esti- mate and true state vector: % к — % к &k Substituting the Kalman filter equation and system model into the above expression, the estimation error can be described as xk = Ф^к-i + - Ф£л_! - Kk(zk - НФхк-1) = = Фх^-i + Gwk~i — Kk(H$xk-i + HGujk-i + Vk — НФхк_1) = = (I- КкН)Фхк_1 + (I - KkH)Gcjk^ - KkVk (7.21) 10*

148 7. Applied Estimation Theory

7.2. Introduction to Traditional Estimation Methods 149 Restoring equation (7.21) in backward time, one can define k~2 ( i 1 J=0 ^»=1 ' k-1 ( i 1 j=0 f i=l [(/ - Kk4H)Gwk^ - Kt-yU-j (7.22) The first component of the above equation shows the be- haviour of initial estimation error attenuation. It was shown [9], that к || HU — Kk+i-iH)&\ tends to zero. It means that Kalman filter is in i=i principle stable. The second component shows the influence of input noise on the estimation accuracy. Third component illustrates the process of measurement noise smoothing. к Obviously, the diagonal elements of matrix П(^ — ^л+1-гЕ')Ф i=l can be used as an indicator of the ’’observability degree” individually for each state vector component. Let’s illustrate the above considera- tion by a simple example, where the system model has a form x = 0 or xk = Efc-l "h '^'Xk-1 2 _ „2 Xk — Xk-1 The measurement model is Zfc = 4 + vk

150 7. Applied Estimation Theory Introducing the observability criterion, as к diag Ц(/ - Кк+1-.Н)Ф (7.23) 1=1 one can analyze the convergence for each state vector component esti- mate. The behaviour of the above matrix diagonal elements is shown in Figure 7.4. Presented plots well demonstrate that the second com- ponent starts to converge after the termination of the transition pro- cess of the first (directly measured) one. Innovation sequence In literature the difference between current measurement and a priory estimate [9, 10] is called an innovation sequence: vk = Zk - НФжк_1 (7-24) vk - innovation sequence. Substituting into expression (7.24) the measurement equation, one can define vk = Hxk + vk - H$xk_i = = H(xk - Фя*_1) + = Hxk/k-V + vk

7.2. Introduction to Traditional Estimation Methods 151 Consequently, the innovation sequence contains the information of a priory estimation errors, but it is corrupted by measurement noise. Let’s consider the main properties of the innovation sequence: 1. vk is white noise for the optimal Kalman filter. 2. Covariance matrix of the innovation sequence can be defined as = HP^H7 + R 3. M[я*ц] = 0 for I < k. The innovation sequence play an important role for the adaptive algorithm designing (see section 7.6) and can be selected as an indica- tor of real estimation errors [9]. The Kalman filter determines within its calculation procedure covariance matrix of estimation errors Pk, which can be considered as a theoretical idea of estimation accuracy. In practical applications one can observe the gap between the real estimation errors and theoretically predicted. It can be explained by the fact, that the applied system model is never exactly correct. In order to provide the information about real estimation errors the innovation sequence can be used. Divergence Theoretically, the Kalman filter is stable. But as mentioned be- yond, the operation of the filter in practical implementations is usually degraded from the theoretical projections. This discrepancy, commonly referred as divergence, can conve- niently be separated into two categories: apparent divergence and true divergence [7]. In the apparent divergence, the estimation errors are bounded, but they increase with time and are generally greater, than those predicted by theory. In the true divergence, true estimation errors eventually become infinite. This is illustrated in Figure 7.5. As mentioned, the cause of the divergence is a gap between the true system model and its applied representation. Let’s define mathematical conditions for the true and appar- ent divergences. In order to do that, the real system model can be

152 7. Applied Estimation Theory introduced as: х{г = Фа:£*_1 + Gek-i ek = Tek-i (7-25) Let’s assume that for the algorithm implementation, the follow- ing reduced model is used (the information of e vector is absent): xk = Ф^-1 (7.26) and the measurement model is zk = Hxk + vk (7.27) In other words, Kalman filter equation (7.26) is used as a system model. In this case, the following equation for the estimation errors can be determined: 7Сь+1-»77)Ф + fc-i z 3 \ + 52] П(7 “ Wh - ф^0 (7-28) з=о 4=i ' where Ф* = (/ - К*Я)ФФк_1 + K„HGLk-ifi - GLk~lfi (7.29)

7.2. Introduction to Traditional Estimation Methods 153 with Ф(0) = 0 Ф& - matrix of vector e influence on the estimation error xk. Third additional component Ф*ео appears in equation (7.28) in comparison with the optimal case due to the incorrect model used for the estimation. Obviously, if the influence of vector cq on the estimation error has the restricted norm, i.e 11**11 < а (7.30) then the only apparent divergence can occur и 11**11 is unbounded, then the estimation errors will be unre- stricted as well. It is a condition for the true divergence. Consider an example of the true divergence. Assume that the state model is the following X = 0 or Xk = Xk-l (7.31) and the measurement model Zk = xk + vk The true state model is described by i = 0 or 4 = 4_, + x^T; x2 = (7.32) where x = x = x2. Assume that in the Kalman filter incorrect model (7.31) is used instead of true model (7.32). The expression for Фь in the considered case can be obtained as (see equation (7.29)) *k = (1 - Ш-г - (1 - K8t)T (7.33) where Kst - steady state value of the gain coefficient.

154 7. Applied Estimation Theory In the considered example, the state model is free from input noise (W = 0). As a result, Kst —> 0, then Ф* = —kT. Consequently, ||Фк|| is unbounded when к —> oo. Hence, in the above case the Kalman filter has the true divergence. Figure 7.6 illustrates the above process. In order to remove the above estimation error behaviour the introduction of input noise to the state model (7.31) is enough. It means that the pessimistic evaluation of the state model used in the Kalman filter leads to inaccurate but robust estimation. Figure 7.6. True Divergence of Kalman Filter Accuracy limit of Kalman filter Let us analyze the accuracy limit of the Kalman filter applica- tions. The covariance matrix of the estimation errors in the steady state can be described by the following equation [1]: Pst = А(ФА)п+1АРг((Фт)п+1 + А52(ФА)‘С<ЭСТ(ФТ)( i=0 where A = I — KstH Pst, К si ~ steady state matrices of Pk and Kk- The first component of the above equation attenuates with time, while the second tends to the steady state value (for the stationary sys- tem), which is defined by the level of input noise. The above deduction has clear physical meaning. The Kalman filter cannot estimate the

7.2. Introduction to Traditional Estimation Methods 155 state vector with absolute accuracy. The estimation accuracy should be in principle restricted by the level of input uncertainties Q in the system model description. The state vector of the stochastic model, which includes unpredictable white input noise can be estimated with restricted by the above noise accuracy. On the contrary, the state vec- tor of the deterministic model, which is free from input noise can be estimated, in principle, with the absolute accuracy. Hence, the filter designing skill depends on the possibility of the system model rep- resentation in more deterministic form. But the deterministic model description depends on the knowledge about the real system behaviour as well as time scale. 7.2.2. Kalman Filter with Control Signal Assume, that the system model includes known control signal as + Cufc_i + Gwk-i (7-34) where Uk - known control signal. The measurement model has a form of equation (7.15). The structure of the Kalman filter algorithm in this case is the following Xk = ^k,k-iXk-i + Cuk-i + Kjt(zfc — H$k,k-iXk-i — HCuk-i) (7.35) and Kk calculated by traditional equation (7.20). Note, that the introduction of known control signal to the sys- tem model has no influence on the calculation equations for gain ma- trix Kk and only a priory estimate of the state vector in equation (7.35) changes as Хк/к-l = $k,k-lXk-l + Cuk-l (7-36) It is easy to show, that the above deduction is correct by forming

156 7. Applied Estimation Theory the estimation error equation for this case such as %k = % к % к = = i«£fc—i 4~ Cuk—i 4~ Kk(zk НФк'к_\хк_\ HCuk_i) — $k,k-lXk — Cuk-i — GWk-1 = — ^к,к-1^к-1 + Кк(НФк,к-\%к-1 + HCUk-1 + HGWk-\ + + Vk - H$k,k-iXk-\ ~ HCuk-i) - Gwk-r = — (I — KkH)($k,k-lXk-l — GWk-1) 4- AjfeVfc The equation for the estimation errors for the considered case is the same as for the traditional Kalman filter without control signal in the system model. It means that the equation for the gain matrix remains the same. 7.2.3. Kalman Filter with Coloured Measurement and Input Noise Kalman filter assumes that measurement noise is white. But in real applications it may be coloured. In order to use the traditional Kalman filter approach, the state and the measurement models have to be rearranged. Assume that the state model has a form 4- Gci)k-i (7.37) and the measurement model is zk = Hxk 4- vk (7.38) where zk - scalar measurement, which is not critical in this consideration vk - coloured measurement noise. Let’s introduce the shaping filter (see section 7.1) for the mea- surement noise description as v = —fiv 4- Ay/2fiuJi(t)

7.2. Introduction to Traditional Estimation Methods 157 where Wi - white noise with identity intensity or in discrete time Vk = (1 - ЦТ) Vk-i + Ц* (7.39) Let’s introduce the additional component, which describes the measurement noise in the state vector. As a result, the extended state model can be rearranged as xk Vk new Xk Xk-1 Vk-1 G : 0 0 : Ay/2@ G new (7.40) According to the above modification of the state model, the measurement model can be rewritten as Zk Vk — 11new‘Lk (7.41) Pnew „new xk H 1 Using the obtained new state model and measurement model (see equations (7.40), (7.41)), the traditional Kalman filter can be implemented. But the above filter in considered case is free from mea- surement noise, i.e. R = 0. It is dangerous situation when ||Q|| is small. For this case the covariance update becomes ill-conditioned, i.e. Рк/k-i ~ Pk It means that measurements are not available to im- prove the final estimate of the state vector. To alleviate this problem, measurement differencing may be employed. The difference between

158 7. Applied Estimation Theory two neighboring measurements are 4 = = = НФхк_1 4- HGukl - (3*Hxk-i + (vk - 0*vk^) = = НФ-/УН xk^ + EGwjt-i + А^Т^к1 1 (7.42) z - - j Hnew vnew In this case instead of traditional measurement model (7.38), new model (7.42) can be used. The traditional Kalman filter for this case contains white measurement noise, but its covariance is increased. It leads to the degradation of the estimation accuracy of the Kalman filter. Another approach, which can be used in this case, is the subop- timal Kalman-Schmidt filter arranged for the coloured measurement noise (see section 7.4). Coloured input noise of the system model is a traditional case. In this situation the model of input noise is described by a shaping filter and introduced to the state model as an additional state vector component (see section 7.1). 7.3. Kalman Filter Adjustment 7.3.1. Influence of Initial Condition on Accuracy of Kalman Filter In some real applications, the information about xq = M[a?o] and Po — M[xqxJ] is not available. In this case the reasonable selection is the following: zo = 0 and Po should be large enough in order to push the estimates into the intensive transition process. Note, that the selection of initial conditions for the Kalman filter implementation has no influence on the steady state estimation accuracy (see equation (7.22)). It has influence on the duration of the transition process only.

7.3. Kalman Filter Adjustment 159 7.3.2. Influence of Noise Covariances on Accuracy of Kalman Filter In practical application of the Kalman filter a priory information about measurement noise statistics (R matrix) is usually known, because it depends on the quality of measurement devices used. The information about input (system) noise statistics (Q matrix) is quite subjective, because it depends on the model representation created by a user. The Kalman filter error behaviour does not depend on the selection of Q or R absolute levels but on their relationship As a result, the reasonable way to analyze the noise statistics influence on the estimation errors is based on Q matrix influence interpretation when R matrix is fixed. In order to do that, equation (7.22) should be considered: k—2 f j j j=o t=l J J ifc(w) The first component of the above equation attenuates over time because of the Kalman filter stability. Second component £jt(w) de- fines the influence of input (system) noise on the estimation accuracy. Third component Xk(y) demonstrates the measurement noise influ- ence. Denote the covariances of the second and third components in the above equation as sk = M[xk(w)x^w)] S% = M[xk(v)xl(v)]

160 7. Applied Estimation Theory The propagation of the above covariances can be determined using original equation (7.43) S? = (I-KkH)$S£.^T[I-KkH)T + (I-KkH)GQGT(I-KkH)T for k = 2,3,4,... (7.44) where SJ" = 0, Q = and SJ = (/ - Л'кЯ)Ф5^_1Фт(7 - KkH)T + KkRKk for k = 1,2,3,... (7.45) where SJ = 0, R = At the steady state, the Kalman filter accuracy is P.t = S” + S“ (7.46) because the first component of equation (7.43) is already attenuated. The Kalman filter is optimal from the condition tr(P,() = tr(SS) + tr(S» ) = min (7.47) Let’s optimize individually each component in criterion (7.47) tr(S'J’) = min (7-48) and tr(S£) = min (7-49) The minimization of criterion (7.48) by the selection of optimal gain matrix Kk can be determined from the condition rftr(Sr) dKk The solution of the above equation with respect to the gain matrix gives КГХ = Pk/k-iHT [HPk/k-iHT] 1 (7.50)

7.3. Kalman Filter Adjustment 161 Equation (7.50) can be determined from the general expression for Kalman filter gain matrix (see equation (7.20)) under the condition of R = 0. The above effect can be explained by the fact that the minimization of expression (7.48) has been done individually for each component which is affected by input noise only. It means that influence of the measurement noise component assumed to be zero. Obviously, that the gain matrix for the considered case has maximum norm. The minimization of criterion (7.49) by selecting optimal gain Kk can be determined from rftr(Sg) dKk as А'Г‘" = Л/*-1Нт [HPk/k^HT + Я] -1 (7.51) where Pk/k-i = ФЛ-1ФТ- Equation (7.51) can be reached from optimal gain matrix equa- tion (7.20) in the Kalman filter under the condition of Q = 0. For this case minimum norm of gain matrix Kk is achieved. Indeed, when Q = 0 then Pk —> 0, Kk —> 0 (see section 7.2.1). In order to minimize the sum of two criteria (expression (7.47)), the balance between the considered cases has to be provided as Jibril IICTI The influence of input (system) noise level of Sk can be inter- preted as the estimation error due to the time lag between the estimate and the true state vector component. The above deduction can be explained by the analysis of system model (7.14). Indeed, the above model includes deterministic part of the system which is predictable as well as random part which can be estimated through the real measurements only (because random part contains unpredictable white noise), see Figure 7.7. 11 - 9437

162 7. Applied Estimation Theory Xk — x% 4- x{ Deterministic part Random part Homogenous solution Forced solution Xhk = t x{ = J$(t,T)GW(r)d 0 System reaction on nonzero initial condition - з?о System reaction on input disturbance vector - W W = 0 Xq = 0 Figure 7.7. System Model Representation

7.3 Kalman Filter Adjustment 163 In order to reduce the influence of input noise on current es- timates (it means the minimization of the norm of S™) the weight of measurements in estimation process has to be high. It means that ||/Cfc|| in the Kalman filter has to be close to ||K£WX||. But in this case, the estimates have noisy behaviour due to the ’’free pas- sage” of measurement noise (because of large gain matrix norm). The covariance matrix S% demonstrates the results of measure- ment noise smoothing. It is expectable that for the effective measure- ment noise smoothing the gain matrix norm has to be close to ||K™,n||. But in this case, the estimates have a time lag due to the weak connec- tions between a priory estimates and current measurements. In other words the estimate remains behind the current state vector because the algorithm trusts more to the model (which includes unpredictable input white noise) rather than to measurements. Consequently, the selection of the large norm of Q matrix (with respect to fixed R matrix) leads to reducing of the estimate time lag (norm of matrix) and to amplification of the measurement noise influence (norm of matrix). Figure 7.8 illustrates the above process. Figure 7.8. Influence of Q Selection on Estimation Accuracy The optimal selection of Q matrix guarantees the balance be- tween the estimation errors due to the time lag on one hand and the measurement noise smoothing on the other hand. Using the considered analysis, it’s possible to suggest applied adjustment for Kalman filter with respect to Q selection. In practical applications, true behaviour of some state vector components is unknown. In order to select the reasonable magnitude и*

164 7. Applied Estimation Theory of Q matrix, the following procedure can be suggested. The first stage of the Kalman filter adjustment is selection of the knowingly large ele- ments of Q matrix. As a result, the noisy estimates but with the small time lag are obtained. On the second stage the magnitude of the ele- ments of Q matrix should be significantly reduced. The Kalman filter estimates in this case have substantial time lag but smooth behaviour. Varying the magnitudes of Q elements in both directions (am- plification and reduction) the reasonable compromise can be achieved. Consider an example of the INS error model estimation. The simplest single channel INS error model has a form (see equation (5.8)) 5Ve = — • dr = ~9$n dr N 4- A\/2j3w(t) or in a state space form as 0 1 R 0 -9 0 0 X2 хз A X В where хг = 5VE, x2 = x3 = Хз where v - measurement noise. X1 x2 0 1 + V The results of the Kalman filter estimation (third component of state vector) for the different magnitudes of input noise Q are shown in Figure 7.9. Here, the true gyro drift rate (third state vector com- ponent) is shown by the thickest line. The estimate of the drift rate with large Q is shown by medium thick line. The estimate with small

7A. Kalman-Schmidt Approach 165 Q is shown by dashed line. The estimates of the optimal (true) value of Q is shown by thin line. The comparison of different estimates leads to the following de- duction. If Q is large, the estimate has noisy behaviour but without time lag. For small Q the estimate has a time lag with respect to the true drift rate behaviour. Even for the optimal Q the estimate still has some time lag. The considered deduction corresponds well with the analysis made beyond. Figure 7.9. Gyro Drift Estimates for Different Q 7.4. Kalman-Schmidt Approach In practical applications some state vector components of the total system model are weakly observable or unobservable at all. In this case the total state vector model has to be reduced [8, 9, 10].

166 7. Applied Estimation Theory Consider the total system model as and the measurement model as •^k+l zk+l “ Hk+1 0 (7.53) + ^k+l Here Xk+i is a subvector, which includes the strongly observable state vector components only; fk+i is a subvector, which containes the weakly observable or unobservable state vector components. The statement of a problem is to design the reduced Kalman filter, which containes the estimates of subvector xk+1 only. The traditional equations of the Kalman filter for the total model has a form (see equation (7.16)) Фл+1,л 0 0 $fc+i,fc • t4+i,fc ~xk' LAJ 0 or for each subvector &k+l =$k+l,k%k + + ^k+Azk+i — — нмимм /fc+1 — ^fc+l,fc/fc+ ^k+1 Hk+l&k+l,k%k -^7fc4-lf7fe+l,fcA) (7-54)

7A. Kalman-Schmidt Approach 167 Assume, that subvector fk+i is not estimated at all. It means that fk = 0. Substituting the above condition into equation (7.54) it can be rearranged as ffc+i = $k+i,kXk + Kfc+i(zfc+i - Нк+1Фк+11кхк) (7.55) The covariance matrix of the estimation errors for the total state vector model has a form where Рк = М{хкх1) ск = d„ = m(A/7) Keep in mind that fk = fk ~ fk — fk because fk subvector in the considered case is not estimated (Д = 0). The gain matrix equations (see equation (7.20)) can be splitted up on Pfc, Ck, Dk covariances as Pjfc+l/fc = ^k+l,kPk^k+l,k + ^k+l,kCkU^+1 k + +ик+1^Ск фТ+1к + Uk+ijcDkUk+l k CM/k — $fc+i,к^кФк+1,к + Uk+itkDktyT+1 k Dk+l/k = ^fc+l,kDk^+ltk + rk+iikQT'k+iik (7.57) and Kl+1 = Pk+l/kHl+1[Hk+lPk+l/kHl+1 + Я]-1 (7.58) = C^+iH^+l{Hk+1Pk+t/kHl+1 + Я]-1 (7.59)

168 7. Applied Estimation Theory for final covariances Pk+i = (J — Gt+i = (7 — Kk+iHk+i)Gt+i/fc Dk+1 — Dk+l/k — ^k+l^k+lCk+1/k (7.60) where R = M and Q = M[wfcWjf]. For the considered case equation (7.59) for Kk+l matrix has to be removed because subvector fk+1 is not estimated as well as Kk+1 = 0 in equation (7.60). As a result, the reduced Kalman filter called Kalman-Schmidt algorithm is described by equations (7.55), (7.57), (7.58), (7.60). The above algorithm can be interpreted as a traditional Kalman filter implemented for subvector Xk with the reduced input noise. In- deed, the reduced Kalman filter equations can be rearranged as &k+l = $k+l,k%k + -Kfc+i(Zfc+1 — Hk+i$k+l,k£k) and Pk+l/k = $k+ltkPk+l$k+i,k + Qk Kfc+1 = Pk+l/kPk+l[Pk+lPk+l/kHk+l + ^fc+1]1 Pfc+i = (7 — A'fc+1Hfc+i)Pjfc+i/fc where Qk — $k+i,kCkUk+lk + Uk+i,kCk^k+i л + Uk+i,kDkUk+lk is the covariance matrix of the reduced input noise. Actually the reduced Kalman filter takes into account the mag- nitude of un-estimated subvector Д through covariance of the reduced input noise Qk. It is expectable that accuracy of the reduced Kalman filter, in principle, has to be less than accuracy of the Kalman filter imple- mented for the total model. It can be explained by increase of the

7.4. Kalman-Schmidt Approach 169 input noise covariance. However, in many practical applications the total Kalman filter may have poorer accuracy due to the inadequate system model as well as influence of non-observable components on the estimates of subvector x^. The cross influence of Xk (estimation error of Xk) on Д (estima- tion error of fk) - (forward) and fk on Xk (backward) in case of the total model application is illustrated by Figure 7.10. As shown before, the Kalman filter has a sequential manner of the convergence. It means that the estimates of strongly observable components of the state vector (subvector x^) converge first. After the termination of the transition process for subvector Xk the estimates of subvector fk start to converge. Consequently, the remained estimation errors of subvector Xk disturb the estimates of subvector Д. At the same time, the backward process of the fk influence on Xk is also present. ___________ Estimation error of x - subvector к backward influence Estimation error of/k- subvector ___ Зл(хЪ1) forward influence Figure 7.10. Estimation Error Influence In publication [1] the above influence is described by the

170 7. Applied Estimation Theory following expression: xk(fk) = К2Я)СД(х0) + ... + (/- KkH)Gfk(xk^) The considered process has the same behaviour as the influence of input noise on the estimation errors of subvector xk. In case of the Kalman-Schmidt algorithm, subvector fk is not estimated and reduced to input noise and, if the magnitude of fk exceeds the magnitude of fk then accuracy of the total Kalman filter degrades in comparison with the reduced Kalman filter. It is the reason for the usage of the Kalman- Schmidt approach instead of the traditional Kalman filter with the total system model. Let’s consider the Kalman-Schmidt filter application for the case, when measurement noise is coloured. Assume, that the state model has a form xk = Фхл_1 + Gwfc_i and the measurement model is zk = Hxk + vk where vk - measurement noise, which is assumed to be coloured and zk is a scalar measurement. The traditional approach in the considered case is to introduce the shaping filter for measurement noise as (see section 7.2.3) v = —flv + Ay/2j3w*(t) or for discrete time = (1 - /3T)vk_Y + A\fifiTw*k_x and to add an additional component to the state model.

7.4. Kalman-Schmidt Approach 171 As a result the extended state model has a form Ф 0 xk Vk 0 (1 - /ЗТ) xk-i Vk-l AyffiT wk-i (7.61) According to the above state model, the measurement model can be rearranged as Zk = [ H : 1 ] xk Vk The extended traditional Kalman filter for the considered case can be described as Pn : P12 P21 : P22 J L ф 0 0 : (1-/3T) Mlwk-iW^] : 0 Ф : 0 pn 0 : (1 - (3T) J L P21 P12 P22 . AfHJ2] J L 0 ; Ay/2pT . (7.62)

172 7. Applied Estimation Theory where Pll — Pk/k—1 P12 — M[zjfc/jfc_1(0jfc/fc_1)T] = Ck/k~l Р21 = •^l^k/k-l(^fc/fc-1)7'] ~ (''k/k-l p22 = ^[^Jt/Jfc-l(^Jfc/Jfc-l)T] = -Djfc/fc-l = Q MKwJ-J2] = 1 Equation (7.62) can be splitted up on P, C and D covariances as Р^-! = *Pk-PS>T + GQGT Ck/k-i = ФСк^(1-0Т) Dk/k-i = (1-(3T)2Dk^PA22^T2 (7.63) The equation for the gain matrix has a form ' КГ .Kl. — Pfc/Jfc-I z^T L °fc/fc i : Ск/к-л Dk/k-l HT 1 - X Рк/к-Л Ck/k-i - HT -1 X< H : 1 ... . > k. Dk/k-i . - 1 or Kk — (Pk/k-iHT + Ck/k-i)(H Pk/k-iHT + +KCk/k-\ + Ckik_YHT + Dk/k-iY1 (7.64) Kk — {Pk/k-\KT + ^k/k-l)(HPk/k-lHT + PPGk/k-i + Ck/k-iHT + Dk/k-\)~x (7.65)

7.4. Kalman-Schmidt Approach 173 The final covariance matrix of the estimation errors can be rep- resented as Pk = (/ - - ^kCk/k-l Ck = (I-KDCk/k-i-K'kDk/k-i Dk = -KlHCk/k-k + tl-K^Dk/k-i (7.66) The extended state vector estimate can be obtained as xk = Ф£*_1 + Kk(zk - НФхк^ - (1 -/3T)vk_i) (7.67) vk = (1 - /3T)vk_i + K%(zk - НФ^-i - (1 - ^T)vJfe_1)(7.68) Assume, that measurement noise is not estimated (vk = 0), then K% = 0 and the Kalman-Schmidt algorithm is described by (7.63), (7.64), (7.66) with Kk = 0 and (7.67) equations. If vk is weakly correlated (it means that the magnitude of (1 — /?T) is small enough), the influence of Ck can be neglected in comparison with Dk. In this case the Kalman-Schmidt algorithm equations can be rear- ranged as xk = Ф^_1 + Kk(zk - НФхкл) where Pk/k-i = $Pk-l$T + GQGT Dk/k-i = (l-pTYDk-x + A^T2 Kk = Pk/k-lHT(HPkik-lHT + Dk/k-i)-1 Pk = (I - KkH)Pk/k-! Dk — Dk/k-\ Hence, in this particular case the traditional Kalman filter equa- tions can be used, but instead of covariance matrix of white measure- ment noise 7?, actual covariance matrix of coloured measurement noise Dk/k-i is applied.

174 7. Applied Estimation Theory 7.5. Estimation Modes Let us consider different modes of the state vector estimation. Prediction The estimation of state vector Xk+N at current epoch tk+N using mea- surements in the past z*., where tk+N > tk- The Kalman predictor can be used for solution of the above problem. Filtering The real time estimation of state vector Xk using current measurement vector Zk- This problem can be solved by the traditional Kalman filter. Fixed time interval smoothing The post-mission estimation of state vector re*, where to < tk < t^ using the total number of measurements within fixed time interval to < tk < t^. For this case, the Kalman smoother can be used. It is clear by the trivial logic that smoothing provides the more accurate state vector estimates than filtering and filtering in turn is more accurate than prediction. Apparently estimation accuracy depends on the scope of the measurements used for the determination of the state vector estimates at each epoch. The traditional filtering algorithm has been considered beyond. Solution of Prediction Problem Let us consider the prediction problem solution. The prediction mode will start when the measurements of the state vector are not available. In this case, the last estimate of the state vector is an initial condition for the prediction process. It is clear by the logic that the prediction equations can be determined from the Kalman filter in case of the absense of measurements, i.e. 7? —> oo. In this case, the measurement correction equals to zero, i.e. К = 0. The above equations are %k+N = $k+N,k+N-lXk+N-l = $k+N,kXk (7.69) where $k+N,k = $k+N,k+N-l • • 3>fc+l,k-

7.5 Estimation Modes 175 The prediction accuracy depends on the precise knowledge about the mentioned initial conditions and the ’’order of determin- istic representation” of the system model (input noise). The covari- ance matrix of the prediction errors can be described by the following equation N Pk+N/k = $k+N,kPk$l+Nk + 52 (7.70) 1=1 where F*. - covariance matrix of the estimation errors at time epoch t = tk Q - covariance matrix of input noise. Obviously, for the accurate prediction, the deterministic repre- sentation of the system model is needed. Solution of Smoothing Problem Let us consider the solution of the fixed time interval (to < tk < tjv) smoothing. In order to form the state vector estimate at certain time epoch tk all future and past measurements within fixed time interval (£o < tk < t^) can be used. Measurements ^(tj, to < ti < tk can be applied in the forward time filtering procedure, while measurements z(ty), (tk < tj < tjv) can be used in the backward time filtering proce- dure. The smoothed estimates can be determined using the optimal linear combination of the forward time and backward time estimates. The estimation error behaviour for the forward time and back- ward time filtering is illustrated in Figure 7.11. Figure 7.11. Estimation Error Behaviour for Smoothing Procedure

176 7. Applied Estimation Theory In other words, the estimation for the optimal smoothing can be described as i’km = Ак£{ + (7 - Ak)ib (7.71) The optimal value of А-matrix is searched from the condition tr P£m = tr M[(xk — = mm Using the optimization procedure one can get Ak = Pk[P% + Pk]~l (7.72) where p£ - covariance matrix of the forward time estimation errors P* - covariance matrix of the backward time estimation errors. In order to use the above procedure the forward time and back- ward time estimation processes as well as their combination have to be implemented. The more convenient smoothing procedure, which includes the forward time filtering and backward time smoothing, can be repre- sented by the following equations [7, 10]: = 4 + КГ (iJ7, - $k+lJcxl} and + Q]-1 (7 73) where x[ - forward time estimate of the state vector xs™ - smoothing estimate of the state vector - smoothing optimal gain Р/ - forward time covariance matrix of the estimation errors Q - covariance matrix of input noise. Initial conditions: xff = xn (last estimate of the forward time filtering)

7.5. Estimation Modes 177 - last covariance matrix of estimation errors for the forward time filtering. By the comparison between the equations for К к and the smoothing algorithm can be interpreted as the traditional Kalman filter with the following parameters: [Covariance matrix of input noise] smoothing II Q [Covariance matrix of measurement noise] filtering II R [System transition matrix] smoothing [Observation matrix] filtering II H Ф Consequently, the above procedure is able to reduce the influ- ence of input noise (uncertainties in the system model) on the esti- mation and therefore on the restoring errors in backward time. If the system representation is free from input noise i.e. Q = 0 then &k/N ~ = (7.74) In other words, the smoothing algorithm degenerates into the simple restoring procedure if the system model does not include input noise Physical meaning of the input noise smoothing can be repre- sented through the mechanism of the input noise influence on the estimation errors. Indeed, the influence of input noise (uncertainties) can be interpreted as the estimation error due to the time lag between the estimate and the true value of the state vector component. For the forward time filtering the estimate has the time lag in one direc- tion, for the backward time filtering it has the time lag in the opposite direction. 12 - 9437

178 7. Applied Estimation Theory The combination of the forward time and backward time fil- tering procedures removes the above time lag. However, in case of high frequency behaviour of the true state vector components, the smoothed estimates lose such information due to the ” averaging of for- ward time and backward time estimates”. As a result, has smooth nature without time lag with respect to the true state vector. It is expectable that shifted forward time Kalman filter should have more accurate estimate that even the optimal smoothing procedure [1]. 7.6. Adaptive Filtering As mentioned beyond, the estimation accuracy depends on a priory knowledge about the system model and noise statistics. If the above knowledge is not accurate enough, the estimation accuracy degrades from the theoretical prediction. Let’s consider the case of unknown precise information about covariance matrix of measurement noise Rk = The covariance matrix of the innovation sequence has a form (see section 7.2): Ck = M[vkvl ] = НРк/к_кНт + Rk (7.75) Pk/k-i = ~ covariance matrix of a priory esti- mation error. Let’s define Ck from the condition of maximum probability den- sity of processes i/1} v2, Vk max p(pl, z/2, •••^) For a stationary system and noise, one can determine the fol- lowing estimate of Ck [9]: 1 - - Ck = jT 52 or in recurrent h —— 1 1 rrf , Ck = —г—Ck-i + v^k^k (7.76) rv К

7.6. Adaptive Filtering 179 Substituting the estimate Ck into equation (7.75), the covariance matrix of measurement noise can be determined as Rk = Ck- HPklk_lHT (7.77) At the beginning of the estimation process, the estimate of Rk can be negative definite due to small statistics of Ck estimation. In order to avoid this situation, a normalization procedure such as if diagRk < 0, then diagRk = 0 can be introduced. The calculation procedure of the considered adaptive algorithm is shown in Figure 7.12. For nonstationary case, the covariance matrix of the innovation sequence can be defined from the condition max р(иь) Using the formula for the Gaussian probability density, one can define [9]: Ck = M[uki%] = vkvk The above approach can be used, if the covariance matrix of measurement noise is unpredictable or environmentally dependent. For instance, it can be used for the removing of GPS (DGPS) jumps coursed by a fast change of a satellite constellation [1]. Let’s consider the case, when covariance matrix of input noise Q = M[wwT] is unknown. In order to define the estimate of Q, one can rewrite the system equation with respect to the estimates of included variables xk = + Gwk-i 12*

180 7. Applied Estimation Theory or ik - Ф£л_1 = Gwk_i (7.78) The Kalman filter estimation equation can be written in a form xk - Ф^к-! = Kkvk (7.79) Equating the right-hand parts of equations (7.78) and (7.79), one can get Gwk_i = Kkvk and _ GAf[wfc_1<_1]GT = KkM[yk^]Kl (7.80) For the estimation of the innovation sequence covariance matrix, the defined above formula can be used Ct = M[^] = ———Ck-i + (7-81) к к The above formula is valid for a stationary system. For non- stationary case, the covariance matrix of the innovation sequence can be defined as C„ = = ^Vk (7.82) As a result, the formula for Q estimation has a form GQk-iGT = KkCkKl Assuming that variation of the gain matrix elements for neigh- bouring time epochs is negligibly small, that is Kk « A\i, one can eventually get GQk^<f = (7.83) The calculation scheme of the adaptive algorithm for unknown Q matrix is shown in Figure 7.13. Generally adaptive algorithms provide lower accuracy than the traditional one, especially in case of small statistics. In order to im- prove the accuracy of the adaptive approach, the adjustment of Q and R estimates with respect to the innovation sequence can be performed, only in case when the statistics of the innovation sequence does not correspond to the reality.

7.6. Adaptive Filtering 181 Figure 7.12. Calculation Scheme of Adaptive Estimation Algorithm (1? is unknown)

182 7. Applied Estimation Theory Figure 7.13. Calculation Scheme of Adaptive Estimation Algorithm (Q is unknown)

Chapter 8 INS testing 8.1. INS Lab Testing Procedures 8.1.1. Day-to-Day Accelerometer and Gyro Bias Stability Let’s consider the procedure for the day-to-day gyro and accelerome- ter bias stability testing. For this purpose, first initial alignment for an INS (which body axes are close to the local-level axes) should be performed and attitude angles tf, 7, V’ should be stored in a file. The indications of attitude angles can be described as 1 = $true + & 71 = Itrue + 71 H1 = Htrue + H1 where у true, 4’true - pitch, roll and heading true values tf1, 71, H1 - attitude angle indications of first INS run tf1, 71, H1 - alignment errors of first INS run. The alignment errors can be expressed via accelerometer and

184 8. INS testing gyro biases, see Chapter 3 and 4 as у = 9 & = 9 where B\b, B2b - accelerometer biases in projections on the body frame - gyro drift rate bias in projection on the body frame. Before a second run, the INS should be switched off and back on respectively in order to restart the alignment procedure. At the end of the INS alignment, the attitude indications $2, 72, H2 for the second run should be stored again. The difference between the first and second run attitude indica- tions can be described as (see equation (8.1)): — R1 I - ti2 = & - & = ---------y±-. vb 9 1 ^2 _ -1 -2 &xb ~ &xb 9 . ,ldr _ . ,2dr H1 - H2 = H1 - H2 = (8.2) U costp Consequently, using two independent runs of an INS, variations of the accelerometer and gyro biases can be evaluated. In order to define variation of уь gyro bias, the INS body has to be turned by heading angle H = 90° with respect to the local-level frame and similar procedure should be repeated again. Variation of vertical gyro bias Zb can be evaluated using the increments of yaw indications from different INS runs.

8.1. INS Lab Testing Procedures 185 Variation of vertical accelerometer bias Zb can be determined using the following equation (8.3) where B}b, B2b - accelerometer bias from different INS runs az, a2 - accelerometer indications in projection on the vertical axis of the local-level frame from different INS runs C|L - transformation matrix between the local-level frame and body frame. 8.1.2. Lab Checking of Calibration Errors In order to check the accuracy of INS calibration, the following pro- cedure can be performed. Assuming, that raw accelerometer and gyro data is available, one can form a measurement model for the calibration errors as З&хЬ ’ z(axb) ‘ 0 ‘ ^Uyb — *(м) — Cb ^LL 0 (8-4) ^dzb _ z(azb) _ _ 9 . &&xb ^zb — Cb ^LL 0 U costp U sin 9? (8.5) where 6axb, 8ayb, 5azb, &^xb, ^zb _ estimates of the ac- celerometer and gyro calibration errors according to model (6.1) z(axb), z(ayb), z(azb), z(wxb), z(uyb), z(wzb) - accelerometer and gyro indications (raw data) C^L - transformation matrix between the local-level and body frames.

186 8. INS testing In order to provide the information about the elements of CbLL matrix, an INS should operate in the navigation mode. The IMU body should be turned by different angles and corresponding errors should be evaluated. Equations (8.4) and (8.5) provide a basic idea about the calibra- tion quality, but cannot provide the exact magnitude of the particular parameter in calibration model (6.1). The alternative approach for lab checking of the calibration quality is based on the analysis of velocity error behaviour with IMU rotations on a turning table. In this case ’’velocity jumps” within the rotations show the accuracy of INS calibration. 8.1.3. Calibration of Gyro Drift Bias Gyro bias is not observable during the fine calibration procedure; therefore, it should be calibrated independently. Let’s assume, that the INS body axes are close to the local-level axes. In order to estimate gyro drift bias after coarse factory calibration, SINS should firstly op- erate in the alignment and secondly in the navigation modes. Output system indications in the navigation mode are the SINS errors, since the system is unmoving with respect to the Earth. In this case, the velocity errors are used as measurements. The simplified SINS error model (see Chapter 5) for East channel can be described as follows: d^ = 0 where 6Ve - SINS velocity error Фдг - attitude error ~ gyro drift rate.

8.1. INS Lab Testing Procedures 187 Using the above system model, the Kalman filter can be used: xk = $xk-i + Kk(zk - НФхк^) Pk/k-l = Kk = Pk/k^HT[HPk/k^HT + R]~l Pk = (/ - KkH)Pk/k^i where Ф - transition matrix, which has a form ~дт о i т о i zk - current measurements (velocity indications Ve) H = [1 0 0 - measurement matrix Pk/k-i ~ covariance matrix of the a priory estimation errors Pk-i - final covariance matrix of the estimation errors Kk - gain matrix R - covariance matrix of measurement noise x - state vector, which contains velocity error (£V), attitude error (Ф) and gyro drift rate (wdr). As a result, the estimate of the gyro drift north component cor- responding approximately to is defined. Such approach can be used for the East channel only, because for the North channel 0$ (corresponds to cj^b) cannot be separated from the azimuth misalign- ment, see Chapter 3. Fbr the estimation of cu^b an INS box should be rotated with respect to the local-level frame by 90 deg and similar procedure for the East channel should be repeated, but in the new INS box position corresponds to ufy In order to estimate the drift rate of a vertical gyro. INS yaw indications in the navigation mode can be used. The differentiation of yaw indications provides the estimates of

188 8. INS testing 8.2. Effect of Accelerometer Biases and Azimuth Misalignment Influence on Output Errors During Laboratory Testing of INS Let’s consider the simplified single channel INS error model (see Chapter 5) East channel 4- Be = ф + (8.6) it North channel = g$E 4- B^ =------- Фир<^А 4- W*E (8-7) it The considered model is assumed to be accurate enough for lab testing, because the influence of the non-stationary error component is negligible (static mode). Let’s rewrite Фдг, Ф#, Фир attitude errors as a linear combination of the horizontal misalignment errors and their increments due to sensor errors as Фа(^л) = Фдг(О) 4- ДФдг^о, tk) $E<tk) = Фе(0) 4-ДФя(£о>£*:) Фир(^) = Фир(0) 4- ДФир(^о,^) (8.8) where Ф/<(0), Ф^(0), Фир(0) - misalignment errors of INS. According to the error analysis of the alignment procedure (see Chapter 3), the misalignment errors can be rearranged through the

8.2. Bias and Azimuth Misalignment Influence sensor errors as Ф„(0) = 9 189 (8-9) Фе(0) = Byb 9 Ф„р(0) = ШхЬ U cos 92 where Вхъ, Вуъ - accelerometer biases ^хь ~ ёУг0 drift rate bias. Assume, that an INS box is not moving and is aligned with respect to the North direction i.e. H « 0 (which does not affect the generality of the problem solution). In this case, the misalignment angles can be rearranged as Флг(О) Фе(0) M) Be 9 Bn 9 U cos <p (8.10) and wn = Ucosy?. Substituting expressions tions (8 6), (8.7), one can get (8.8) and (8.10) into the equa- 6VE = —рДФ^(<о, it) ФдГ = SVe + u* ~r+Un 5Vn = дДФя^о,^) Фе = wn From the new expressions of the INS errors, it is obvious that the INS output errors on an unmoving base (lab conditions) are not

190 8. INS testing disturbed by accelerometer biases. Moreover, for North channel the above errors are not affected even by gyro drift rates. It is not suprising that for a relatively stable INS North channel contains much smaller output errors than East channel for case of the unmoved INS body. 8.3. INS Field Testing Figure 8.1. Functional Scheme of INS Field Testing In order to evaluate the INS errors in real environment, field test- ing has to be performed. In this case external information is needed. A GPS receiver can be used as an external source of navigation in- formation. The functional scheme of the INS field testing is shown in Figure 8.1. Here, the differences between the INS and GPS positions and velocities are used as measurements for an estimation algorithm. An estimation algorithm, such as the Kalman filter, provides estimates of the observable components of the INS error model. Hence, using the considered procedure, the INS error behaviour in real environment can be defined. The estimation procedure of the azimuth misalignment and ac- celerometer scale factor for land applications using field data will be considered in section 9.3.

Chapter 9 INS/GPS Integration 9.1. INS/GPS Integration Methods The comparison between the INS and GPS properties is shown in Table 9.1. Table 9.1. INS and GPS Properties Comparison Properties INS GPS Output rate High (50—100 Hz) Low (1—10 Hz) Long term accuracy Low High Short term accuracy High High Independence Self-contained unit Depended on direct satellite observability Possibility of attitude measurements Compact and reliable Depended on environment From the above comparison it is obvious that a GPS receiver and an INS have complementary characteristics. An INS is self-contained

192 9. INS/GPS Integration unit, which provides position, velocity and attitude regardless of ex- ternal environment. Moreover, an INS has a high output rate, which is limited primary by the choice of computational approach and equip- ment. High frequency noise of inertial instruments is attenuated by the INS low-pass nature. As a result, the INS error has a quite pre- dictable low (Schuler) frequency behaviour, so that the relatively high accuracy can be achieved for short periods of time. However, the low frequency noise and especially sensor bias lead to poor accuracy in long term. In contrary, GPS provides accurate with respect to an INS measurements, which are corrupted by high frequency noise. The combination of the advantages of both systems motivates the design of the unique integration navigation complex. The above complex provides the reliable position, velocity and attitude information even for GPS outages as well as the output of a high rate. This chapter discusses advantages and disadvantages of the variety of different integration methods. 9.1.1. Cascade Scheme of INS/GPS Integration The cascade scheme of INS/GPS integration is shown in Figure 9.1. The INS in a stand-alone mode provides the position, veloc- ity and attitude information. The differences between INS and GPS velocity and position indications are used as measurements for the es- timation block. Using the INS error equations as a system model and the above differences as a measurement model, the estimation block provides the estimates of all observable INS errors. The estimates of INS errors are applied for the correction of INS raw measurements. In case when GPS measurements are unavailable, the prediction of INS errors is used. Moreover, the estimates of INS errors can be option- ally applied for the real-time INS calibration (shown in Figure 9.1 by dashed line). The main advantage of this approach is the simplicity of the integration as well as the universality of the integration scheme for different types of INS and GPS units. However, by simplicity this method loses in the system performance.

9.1. INS/GPS Integration Methods 193 Figure 9.1. INS/GPS Integration. Cascade Scheme 13 - 9437

194 9. INS/GPS Integration 9.1.2. Loosely Coupled Approach of INS/GPS Integration The structure of the loosely coupled integration scheme is shown in Figure 9.2. Figure 9.2. Loosely Coupled Scheme The main feature of the loosely coupled approach in comparison with the cascade scheme is the different measurement model, which instead of positions and velocities uses pseudoranges calculated by an INS and measured by a GPS receiver. In this case the measurement model is based on the difference between INS and GPS pseudoranges

9.1. INS/GPS Integration Methods 195 (see Appendix A) as j J (Xr “ Xi) s^ins , (.Уг ~ уГ) x^.ins , Pins ~ Pgps = --------------6x +--------------бу + ri ri axi ciyi + (2г ~ г?) 6zINS + cA7 - V^PS (9.1) ________ dzi where ri = \/(z- - zr)2 + (yf - yr)2 + (zf - zr)2 - range between «-satellite and receiver AT - time bias c - speed of light a?®, «/®, z® - coordinates of «-satellite in the Earth-fixed frame xr, Уri zr - coordinates of a receiver in the Earth-fixed frame V^ps - GPS errors 6xINS, dyINS, 6zINS - INS position errors in the Earth-fixed frame. The relationship between the Earth-fixed coordinates and the geographical coordinates has a form [12, 13]: x = (N + h) cos tp cos Л у = (N + h) cos sin A z — [N(l — e2) + h]sin<£> (9-2) where A, h - geographical latitude, longitude, height N - radius of curvature in the prime vertical e - eccentricity of the reference ellipsoid. The relationship between the INS errors in the Earth-fixed frame and the geographical coordinates can be determined by the variation 13*

196 9. INS/GPS Integration of equation (9.2) as <5x = — (N 4- h) sin 92 cos A592 — (N 4- h) cos 92 sin X5X 4- cos 92 cos A 3y = — (N 4-h) sin 92sin A^ 4-(N 4-h) cos 92cos A£A 4-(Vicos92 sin A 8z = [N(l — e2) 4- h] 00592692 + 6/isin 92 (9.3) Substituting expression (9.3) into measurement model (9.1), one can get P1ins ~ Pgps = [~axi(N + Л) sin 92 cos A- -ayi(N 4- h) sin sin A 4- azi(N(l - e2) 4- h) cos 92] 6924- bxi 4- [—axi(N 4- h) cos 92 sin A 4- ayi(N 4- h) cos 92 cos A] 6A4- ^yi 4- [ttjt cos 92 cos A 4- ayi cos 92 sin A 4- azi sin 92] dh 4- c/XT — VqPS bzi — ЬХг$<Р 4~ byt8X 4- bzi6h 4- сДТ — Vqps (9-4) Consequently, the measurement model in the considered case can be represented as 21 22 Zi p\ns Pgps Pins ~ Pgps P'ins ~ Pgps z VGPS —V2 VGPS H X GPS (9.5) V

9.1. INS/GPS Integration Methods 197 In the considered case, the system model in the Kalman filter application is based on the INS error equations. Hence, the Kalman filter for the loosely coupled approach has the similar form as for the cascade scheme except the new structure of H matrix (see equa- tion (9.5)). Clearly, the loosely coupled integration is more forgiving for losses in GPS measurements. In case of the cascade integration scheme, position solution de- pends on a sufficient number of satellites tracked (at least four). If this minimum condition is not satisfied, no solution is obtained and, therefore, a GPS receiver is not able to update the stand-alone INS solution. Oppositely, the loosely coupled integration makes use of any GPS signal even if it is only one measurement. For these reasons, generally the loosely coupled approach is widely applied. However, there are also advantages of the cascade integration. Should there be a sensor failure or other significant unforeseen degradation in the either system, the cascade integration, operating essentially on two independent solutions, is more likely to detect these faults or conse- quent outliers and is better able to take appropriate remedial action. The second advantage concerns the computational simplicity of the cascade scheme comparing to the loosely coupled integration. 9.1.3. Tightly Coupled Approach of INS/GPS Integration In such approach, the integrated navigation solution has a feedback to a GPS receiver to aid the carrier tracking loops. Incorporating the range acceleration information into the tracking loop, the bandwidth of the tracking loop can be significantly decreased. The reduced track- ing bandwidth increases the signal-to-noise ratio at the output of the tracking loop and makes the system more immune to interference and jamming. The main drawback comes from necessity of a system de- signer to have an access to hardware and variables used in the carrier tracking loops. Therefore this approach is not available to the general user community [12].

198 9. INS/GPS Integration 9.2. Observability of INS Errors Using GPS Measurements Let’s split up the observability analysis on different stages. First of all the observability of only the Schuler component error will be consid- ered. The above analysis corresponds to the INS error behaviour for stationary vehicle motion. For this purpose, the single-channel error model of the Schuler component can be used (see Chapter 5) E-channel r 6Ё = 6VE 8Ve — ~9$n + &e < = + (9.6) BE = 0 = 0 N-channel ' 6N = 6VN ^Vjv — 9$e + Вдг < Фе = -^-+^е Bn = 0 = 0 (9-7) Consider the case, when the INS position and velocity errors are directly measured (for instance, GPS is used). It is clear from the error model structure than observability analysis is similar for both channels. Let’s show that ^Фдг(О), ^Фе(О) are not separated from BE, BN when measurements of the position or velocity errors are used. It is clear under the consideration of the second equation in (9.6) and (9.7). Here, using the measurements of velocity error 6VE or 6VN the sum of two constants are observable: — дФд/О) 4- BE and <?Фе(0) 4- Bjv, but the separation of two constants is impossible. It means that in the estimation process one of the two above constants has to be removed from the model. Usually, the accelerometer bias is selected as unobservable component, while attitude errors Ф/v and

9.2. Observability of INS Errors Using GPS Measurements 199 ФЕ are estimated in the filter. However, in this case it is expected, that the estimates of Фдг and Фе will have a bias with respect to true values due to the existence of BE and BN. Let’s analyze the observability of the all other components of the INS error model. To simplify the consideration, the reduced model for one channel of the INS errors, when Be is removed, can be used — ~9$n 4 । dr ~ —R + bJN = 0 Measurement model z = 6Ve + V noise The above models in the space-state recurrent form are o:i ^3 1 -gT 0 T/R 1 T 0 0 1 xi Хз J fc-i X2 Яз 4- V noise where = SVe, X2 = Ф/v, #з — un- Let’s create observability matrix A (see Chapter 7) as and ‘ H НФ НФ2 1 1 0 -дт -2дТ 0 0 -дт2 det А = д2Т3 ± 0

200 9. INS/GPS Integration Consequently, the all components of the considered state vector are observable. It’s easy to understand that the above deduction is also correct of measurements of the position error. Another problem, that need to be analyzed is the ’’observability degree” of each state vector component. As was shown in 7.2, the ’’strong observable” state vector components are the nearest to the directly measured ones. The further components are ’’weak observ- able’. The physical interpretation of ’’observability degree” can be illustrated by the following diagram (see Figure 9.3). 6VE ----------------► <J>N © Be----------------------► CO* 5Vn----------► ФЕ © BN--------------► CO* Directly measured components *______Scale of “observability degree” strong weak observable observable Figure 9.3. Observability Diagram Obviously, that the longest chain of the diagram corresponds to the smallest ’’observability degree”. The similar diagram can be determined for the position error measurements (see Figure 9.4) 5E----------► 6VE ---------► On © BE------------► CO * 5N----------► 6VN----------► Фе © Bn------------► CO £ Directly measured components Figure 9.4. Observability Diagram

9 2. Observability of INS Errors Using GPS Measurements 201 The above diagram analysis confirms, that the attitude errors and gyro drift rates are stronger observable (shorter length of the chain) for the velocity error measurements than for the position er- ror measurements. Note that the level of the ’’observability degree” corresponds to the duration of the estimation transition process. In addition, some remarks concerning the features of a strap- down system have to be done. For the strapdown mechanization the own gyro drift rate bias and accelerometer bias are correlated with their projections on the local-level frame through direction cosine ma- trix C£L as (see Chapter 7) The above relationship shows that the own gyro and accelerom- eter biases are observable, if a vehicle moves out of their initial orien- tation in the local-level frame (for instance, maneuvers in the azimuth direction only). Let’s consider the observability analysis of the non-stationary components of the INS errors. Adding the azimuth misalignment angle in equations (9.6), (9.7), one can get 6Ё = + адгФир 4- Be 6N = 6VN = 9$e ~ Qe&up + &n Here, it is assumed that the accelerometer scale factor errors have been calibrated before. The above equations show that Ф«Р can be separated from Фдг, Ф# and only in case when the vehicle has a substantial acceleration change during motion (see section 5.3). Note, if Фир is observable, vertical gyro drift rate bias bJup is observable as well.

202 9. INS/GPS Integration Hence, the final diagram for illustration of the INS error observ- ability has a form shown in Figure 9.5. Finally, the observability of the non-orthogonality of the sensor installation error has to be considered. As shown in Chapter 6, the installation error influence depends on the body accelerations, angular velocities and vehicle attitude. It means, that for substantial maneu- vers of the vehicle, one can expect to estimate the above errors as well, but for such complicated error model the above possibility can be problematic. 9.3. Calibration of Non-Stationary Component of INS Error Note that the non-stationary components are weakly observable us- ing external information in real time. The reasonable way for their removing is to use a special preliminary calibration procedure. The non-stationary component of the INS velocity errors can be

9.3. Calibration of Non-Stationary Component of INS Error 203 described as (see Chapter 5) dV“at 6V£at $upVN + PeVe + P*EaE —$upVe 4- PnVn + P*NaN (9.8) where p*E, p*N - additionally introduced nonlinearities of the accelerometer scale factors. Note, that Фпр is different for each system turn and has to be estimated for each run. Accelerometer errors pE, pN, p*N car. be assumed constant at least for some time of INS exploitation. It means that the above errors can be estimated and compensated using a special calibration procedure. One of the best approaches for their calibration is make use of the special behaviour of vehicle motion. Assume, that an INS is installed on a land vehicle, which moves along the trajectory shown in Figure 9.6. t N О ZUPT3 О ZUPT2 <> ZUPT1 • ZUPTO t N <> ZUPT3 ZUPT2 I> ZUPT 1 • ZUPT 0 4 Уь a) 1st stage b) 2nd stage Figure 9.6. IMU Installation on Vehicle for Field Calibration

204 9. INS/GPS Integration Here, at the first stage of the system calibration, the IMU is installed such a way, that уь axis coincides with the longitudinal vehicle axis. The vehicle periodically stops at some points. When the vehicle is not moving, the true vehicle velocity equals to zero. Hence, the INS’s velocity indications are velocity errors. The above indications are called zero velocity updates (ZUPT). 2.5—3 min is a usual time interval between neighboring ZUPTs. The time within ZUPT is generally about 10—15 sec. Using the certain estimation procedure, the INS velocity error behaviour between ZUPTs can be restored [1, 13]. The above errors include the Schuler component only, because the non- stationary component within ZUPT equals to zero (Ve = V^ = 0). Consequently, Schuler components of INS velocity errors 6V/cf\ 8VEch can be determined using ZUPT measurements. Assume that for field calibration GPS or DGPS is used as well, then the difference between INS and DGPS velocity indications are the following: zE = v™s - VGGPS = 6V£NS - GPS = = 6V2ch + svsst - 6V°GPS Ci Ci Ci ZN = V-NS -v£GPS = 6vfrNS -w°GPS = = SV$ch + 5V£st - 6V$eps (9.9) where <5У5сЛ, iV"s< - Schuler and non-stationary components of the INS velocity errors. Using the Schuler component estimates obtained from ZUPTs, the non-stationary part can be measured as 4 = ZE - 6V^ch = SVS‘* - WPGPS z'N = ZN - SV™ = <5 - 5 VS°PS (910) Using the model for the non-stationary components of INS ve- locity errors (equation (9.8)) with respect to the vehicle traverse shown in Figure 9.6, the above measurements can be rewritten as 4 = zE - 8V^h = ФирК„ - &V°GPS z‘N = zN - 6V™ = fiNVN + v'NaN - 6VPGPS (9.11)

9.3. Calibration of Non-Stationary Component of INS Error 205 The INS velocity error behaviour between ZUPTs is illustrated in Figure 9.7. Here the Schuler component coincides with the total INS ve- locity error within ZUPT, since the non-stationary component for an unmoved vehicle is absent. Within motion for East channel, the influ- ence of the azimuth misalignment appears. For North channel in this case, except the influence of the accelerometer scale factor, one can ob- serve blunders due to the nonlinearity of the accelerometer scale factor, when the vehicle accelerates The east measurements cannot provide

206 9. INS/GPS Integration any useful information except azimuth misalignment Фир which is not interesting because it is dependent on system runs. North measure- ments can be used for the estimation of px and p*N. For this purpose, the system model is described by PN — 0 p*N — 0 and the measurement model fljv ] PN Using the traditional Kalman filter, accelerometer errors can be estimated. For IMU installation on a vehicle body as shown in Figure 9.6.a, one can get Pyb PN Pyb Pn In order to estimate the accelerometer errors for хь channel, the IMU has to be installed as shown in Figure 9.6.b and the considered test must be repeated again (2nd stage) for the new direction of хь axis. As a result, the estimates of the accelerometer errors for хь-channel can be determined as Pxb — PN Pxb — Pn Note, that the selection of vehicle traverse aligned with respect to the North-South or West-East is not necessary, but the above ori- entation provides the best observability of the estimated errors. Moreover, the considered calibration procedure can be rewritten with respect to the INS position errors. In this case, the model for non-stationary components of the INS coordinate errors has to be used. In a summary, the considered calibration procedure is able to provide the estimation and compensation of the non-stationary com- ponent of the INS errors except azimuth misalignment error Фир. The above error has to be estimated in kinematic mode in each system

9.3. Calibration of Non-Stationary Component of INS Error 207 run using external navigation information, for instance, GPS mea- surements (see section 5.3). The considered method of the accelerometer error field calibra- tion was verified by the real testing of a HG1700 (IMU). The estimates of py, p*b have been obtained. Using the above estimates, the compen- sation of the non-stationary error components in a stand-alone solution has been implemented. The INS velocity error without calibration of the non-stationary component is presented in Figure 9.8 by solid line. The velocity errors with the calibration are shown by dashed line in Figure 9.8. Obviously, the INS calibration provides the compensation of the non-stationary component, so the remained error has smooth behaviour caused by the Schuler error component only. In this case, the good predictability of the INS error, when GPS measurements are not available, can be provided. Figure 9.8 Velocity Error Behaviour Before and After Calibration

208 9. INS/GPS Integration 9.4. Kinematic Azimuth Alignment The simplified INS error model has a form (see Chapter 5) {SVe — ~g&N + aN<&np + &E ( — 9$E ~ aE<^up + Bn __ $Ve i , .dr | Я) ___ <5VjV । . ,dr + ^Nr I —---------+ ШЕг (912) where шЕг, u$r - reduced gyro drift rates, which include the own drift rates as well as the azimuth misalignment angle multiplied by the angular velocities of the local-level frame. The reason of such consideration is the fact, that the own drift rates are slightly separable from the influence of the azimuth misalignment angle (see section 5.3). In the above equations, the accelerometer scale factor and its nonlinearity are absent. Here, it is assumed, that the above errors have been calibrated before (see section 9.3). From the analysis of the above INS error model, it is obvious that the observability of azimuth misalignment Фир strongly depends on vehicle maneuvers (the magnitudes of a^, ole change). It means, that the precise estimation of Фир is questionable in case of stationary motion of carrier. The obvious approach of Фир estimation is the Kalman filter application. Equations (9.12) are used as a system model. The mea- surement model is based on the difference of the velocity indications between an INS and an external navigation device (for instance, GPS). In this case the measurement model is , _ \fINS rrGPS гт/ X\fGPS ze = vE ~ ve = oVe- ove Vе — measurement noise zN = V™s -VgPS = WN- 6V^ VN — measurement noise The above measurements can be rewritten with respect to the

9.4. Kinematic Azimuth Alignment 209 state vector of the INS error model (9.12) as (9.13) Using system model (9.12) and measurements (9.13), the Kalman filter is able to estimate the state vector, which contains az- imuth misalignment angle Фир. Note, that the transition process of the Фир estimation may be significantly long. The main drawback of the extended Kalman filter implementation lies in the large size of the total state vector, which includes not only Фир but also all other components of the INS error. As a result, the Kalman filter spends additional time for the conver- gence of the components, which are closer to the directly measured ones (see section 9.2). In order to reduce the convergence time for the Фир estimation, it is possible to create a direct approach. The idea of the above approach is to initialize the estimation process only when Фир is strongly observable. In other words, the estimation process starts, when the magnitude of the vehicle accel- eration change reaches a certain value. Moreover, in this case the estimation problem statement is reformulated so that Ф„р starts to be a directly measured component. The estimation algorithm for Фир can be determined directly from equations (9.12) as if |Aqn| or |Aue| > 6 then - EaN — g^EaE ^up — ? ; 9 aN + aE where 6Ve, 3V n - estimates of the acceleration errors 14 _ 9437

210 9. INS/GPS Integration Фе, $n ~ estimates of the horizontal attitude errors, obtained when |ДаЕ|, |Age| < <5 5 - threshold of the vehicle acceleration change. In order to additionally smooth the estimation errors, the aver- aging of Фир over some time interval can be used. The above approach should be applied in the beginning of the Фпр estimation, but after the termination of transition process, Фир can be introduced into the ex- tended Kalman filter. The results of real-time azimuth alignment using a HG1700 (IMU), are shown in Figures 9.9 and 9.10. The estimates of the az- imuth misalignment error are displayed in Figure 9.9. Note, that in the considered case, the direct approach in the beginning of the Фир estimation was used. It expedites the estimate convergence of the fil- ter. In the considered test, the vehicle periodically stopped in order to provide visible error behaviour of the INS errors caused by the azimuth misalignment error. The INS velocity error without the kinematic az- imuth alignment is presented in Figure 9.10 by thick line. The INS velocity error with the real-time kinematic azimuth alignment is shown in Figure 9.10 by thin line. As is seen from the plot, the ’’error steps” due to the azimuth misalignment error within vehicle motion between neighboring stops (see section 9.3) are removed by this alignment. 9.5. Special Vehicle Motion Behaviour for Kinematic Azimuth Alignment In present section the special maneuver of a land vehicle for the kine- matic azimuth alignment will be considered. Suppose, that a user is able to spend certain time of initial system run for azimuth calibration. Moreover, it is assumed that the accelerometer errors (scale factor and its nonlinearity) have been calibrated before, then the remained non-stationary component of INS error will be caused only by azimuth misalignment error Фир. The INS position errors can be described (see Chapter 5) as

9.5. Special Vehicle Motion Behaviour 211 t(sec) Figure 9.9. Estimates of Azimuth Misalignment Figure 9.10. Velocity Error Behaviour Before and After Compensation of Azimuth Misalignment 14*

212 9. INS/GPS Integration 5N = -$upNE + 6NSch 6E = ФирДЛГ + 6ESch where 6N, 5E - total INS position errors, i.e. 8N = $VN, 6Ё = 6VE NN, NE - increments of vehicle distance with respect to the initial point (point A), i.e. NN = NtpR^ NE = NXR\ cos <p or NN = VN NE = VE 8NSch, 8ESch - Schuler component of the INS position errors, i.e. 6NSch = 6V$ch 6ESch = 5Vgch Assume, that during motion a vehicle periodically stops in order to accumulate ZUPT measurements (see the previous section). In order to organize the precise azimuth alignment, at least three ZUPTs are needed. The approximate time between neighboring ZUPTs is 3 minutes. Using measurements within ZUPT, Schuler components of the INS velocity error 6Vfich, 6V/ch can be restored [1, 9]. The integration of restored 6V/ch gives Schuler compo- nent of the INS position error 6ESch, 5N 'ch. At the end of the cal- ibration traverse (point B), the difference between the INS and GPS (DGPS) positions is created as 6E(B) = NEins(B) - NEGPS(B) 6N(B) = NNins(B) - NNGPS(B) Using the above estimates as well as the restored Schuler com- ponents of the INS position errors, one can estimate the azimuth mis- alignment error as - [6E(B) - 6ESch(B)]NN(B, A) - [6N(B) - 6NSch(B)]NE(B, A) up ~ (NN(B, A))2 + (NE(B, A))2

9.5. Special Vehicle Motion Behaviour 213 where AN(B, A), AE(B, A) - vehicle distance in the North and East directions from initial point A to final point B, see Figure 9.Ц. It’s obvious, that accuracy of the azimuth alignment in this case depends mainly on the distance between the initial and final points of the calibration traverse. Indeed, even if the INS errors are estimated with accuracy of 1—2 m on the distance of 8—10 km, then the azimuth alignment errors will be less than 1 arc min. Consequently, the con- sidered special procedure of the kinematic azimuth alignment should be more accurate than the alignment based on velocity measurements (see the previous section). The misalignment errors in the considered procedure decrease with the distance growth, which depends only on motion time and velocity. The drawback of the considered approach is the necessity of the special calibration run with ZUPTs. Figure 9.11. Vehicle Calibration Trajectory

Chapter 10 Low-Cost INS/GPS Integration High cost of inertial units is the main obstacle for wider implemen- tation of these sensors to augment GPS for precise navigation. A low-cost inertial measurement unit which contains low precision sen- sors, is currently available on the market. Given the weak stand-alone accuracy and poor run to run stability, such devices are not applica- ble as a sole inertial navigation system. Even the integration of such inertial unit into a navigation system as a supporting device demands the development of special approaches and algorithms. 10.1. Motion Pak™/GPS Integrated System The objective of this section is to consider the certain procedure for the integration of a low-cost inertial unit with GPS or DGPS. A good example of a low-cost inertial unit is the Systron Don- ner’s series of inertial sensors. Systron Donner’s Motion Pak™ is a ’’solid-state” six degree of freedom inertial sensing system used for measuring linear accelerations and angular rates in instrumentation and control application. The unit is shown in Figure 10.1.

10.1. Motion Рак™ /GPS Integrated System 215 Figure 10.1. Systron Donner’s Motion Pak™ This is a highly reliable, compact and fully self contained mo- tion measurement package. It uses three orthogonally mounted ’’solid- state” micro-machined quartz angular rate sensors, and three linear servo accelerometers mounted in a compact, rugged package, with iner- tial power regulation and signal conditioning electronics. It dimensions are 7.75x7.75x9.15 cm and it weights less than 0.9 kg. The parameter specifications of Motion Pak™ sensors are shown in Table 10.1. Table 10.1. Motion Pak™ Parameter Specification Performance Rate channels Acceleration channels Range ±100 deg/sec 5g Bias <2 deg/sec <12.5 mg Alignment to base <1 deg <1 deg Resolution <14 deg/hr <10-4g It important to note that the equipment accuracy varies from

216 10. Low-Cost INS/GPS Integration one unit to another. Laboratory tests were conducted using different units. Table 10.2 shows the gyro accuracy variations observed. Table 10.2. Gyro Accuracies for Lab Tests Gyro Accuracy Parameter Gyro drift rate bias day to day (run to run) drift rate bias 200-500 deg/hr drift rate bias in run (averaged within 20 s) 80—200 deg/hr drift rate bias in run (averaged within 300 s) 10-50 deg/hr From the specifications and test results, the above unit could not be directly used as an inertial measurement unit (IMU) for a stand alone INS. Firstly, the gyros are not sensitive enough to sense the Earth rate, which means that a self contained azimuth alignment procedure can not be used. Secondly, the gyro bias has a large magnitude that leads to nonlinear behaviour of the INS output errors in a stand alone mode. Indeed, using the above gyro sensors within 0.5—1 hr the attitude errors (errors between platform and navigation frame - Фд, Фд, Фир) will leave the linear region. As a result, the nonlinear cross coupling in the stand alone navigation solution appears. Therefore in order to fully exploit the IMU data, the special method of operating and processing the data has to be developed. The integration scheme of the low-cost IMU with GPS (DGPS) is shown in Figure 10.2. In order to use Motion Pak™ integrated with GPS, several preliminary procedures have been developed and implemented, and these include • horizontal alignment based on the acceleration output (see the traditional applied procedure in section 4.2);

10.1. Motion Рак™/GPS Integrated System 217 Figure 10.2. Integration Scheme of Low-Cost IMU with GPS

218 10. Low-Cost INS/GPS Integration • stored azimuth alignment using a magnetic compass or any ex- ternal heading information (can be used simple rough azimuth device); • calibration of run to run gyro drift rate bias. All these procedures can be implemented in real-time and take 1—5 min. After that, the data processing switches to the navigation mode, which includes the following procedures: • INS algorithm with error damping • estimation of INS error • position, velocity correction • roll, pitch correction • heading correction • prediction mode implementation. Consider at first, the INS error damping algorithm when the GPS velocity is used. 10.2. INS Error Damping The stand alone INS error behaviour is undamped oscillations with the Schuler frequency. In order to reduce the amplitude of error os- cillation the damping procedure can be suggested. Here, the damping process of the INS errors using the external velocity information will be considered. Note, that the error damping can be organized using the internal INS information, but in this case, the INS error model will be disturbed by the nominal value of the vehicle velocity or ac- celeration. In other words, the INS will lose such important property as un-disturbance with respect to the nominal value of carrier motion

10.2. INS Error Damping 219 parameters (see section 5.2). Using external information, the INS er- ror can be extracted and necessary feedbacks for the damping can be done with respect to errors. In this case INS un-disturbing property will be saved. The scheme of the stand alone INS algorithm with damping feedback (dashed line) is shown in Figure 10.3. Two additional control signals are introduced to the stand alone INS scheme. The first one is introduced to the input on the velocity calculation block. The above signal provides damping of the velocity error itself (feedback to the first integrator). The second one is added to the absolute angular velocity of the navigation frame and introduced to the calculated image of gyro platform torque (Am - quaternion). The above control signal increases the natural oscillation frequency of the INS error. It means, that for the damped system, the error behaviour is the attenuating oscillations with a frequency higher than Schuler one. Consider the error behaviour diagram (single channel) for the damped INS, which is shown in Figure 10.4. According to the considered diagram, the INS error model can be determined as — — g$N — kiSVs + Be = ^.+ш^ + k26VE (10.1) £1 By rearranging the above equation, one can get 6Ve + kiSV^E + (^2 + ^2^)^Ve = + Ёе (10.2) The left side of equation (10.2) describes the second order oscil- lator, then the horizontal velocity errors attenuate over time. In this case, the INS errors become stable. The similar control signals have to be introduced to the another horizontal channel (North channel): -ki(V£NS — Vn) ~ to the first integrator —k2(y^NS — ~ to the gyro platform torque.

220 10. Low-Cost INS/GPS Integration Figure 10.3. Scheme of Damped INS

10.2. INS Error Damping 221 Figure 10.4. Diagram of Damped INS Errors

222 10. Low-Cost INS/GPS Integration The analysis of the error behaviour here is similar to the hori- zontal alignment case (see section 4.4). On the one hand, the selection of large magnitudes of ki, k2 provides fast attenuation of the velocity errors and the small steady state (static) error value. On the other hand, large magnitudes of ki, k2 lead to the extension of system band- width. It means, that high frequency input disturbances (for instance, random noise of gyro drift) pass through the system. The optimal selection of кг and k2 is based on the compro- mise between the two considered above cases, that provides reason- able static errors and at the same time well smoothed high frequency input disturbances. Note, that the third order oscillator for the veloc- ity error behaviour can be implemented as well, but in this case the third control signal has to be introduced into the gyro torque, which is described by ±k3 [ (V'nns - V£N)dt J to 10.3. Analysis of Error Damping Behaviour Using the Laplace transform of equations (10.1) under the condition of <5Ve(0) = 0 and Фдг(О) — 0, one can get 4?jv(s)[s2 4- k^s 4- (-^ -f- &2<?)] — un(s)(s 4- &i) 4- Be(s)(— 4- k2) ft rt Consider the attitude error reaction on the gyro drift rate as the more critical error. For this purpose the gyro drift rate is described as a linear combination of harmonical oscillations with different amplitude and frequency N 0$ = COSCJjt i=l In the Laplace transform it will be N = E EiS s2 4- tu?

10.3. Analysis of Error Damping Behaviour 223 Using the above gyro drift rate representation, the Laplace transform of the attitude error has a form = V__________+ 2^o)______________ “ (s2 + w?)(s2 + 2£w0$ + Wq) where w0 — y/dt'k + ^2) _ natural oscillation frequency of undamped oscillations £ = —j=A .... = - parameter of damping. 2V p + V rt In time domain, the above equation under the condition of op- timal £ = 0.707 has a form N $N(t) = У2----------£г^°л exp(-£wot) sin(O.7O7wo + Qi) + <=1 0.707^/^ + Afa) + 52 sin(wit + a2) (10-3) i=i v уЦ +^о where ai, a2 - initial phases. Consequently, the attitude error behaviour can be described as a linear combination of the attenuating oscillations with natural fre- quency - O.7O7uo and the undamped oscillations with disturbed fre- quencies - UJi. The amplitude Л(с^) and F(cj,) behaviour (e, is fixed) versus the disturbed frequency cj, is shown in Figure 10.5. Obviously, the selection of cjq defines the system bandwidth. For weak damping (cjo, &2 have small magnitudes) the static attitude error 1.41^ is high but the system has a small bandwidth, which leads to effective smoothing of the high frequency gyro drift rate.

224 10. Low-Cost INS/GPS Integration turbed Frequency cc? In contrary, when strong damping (large magnitude of a?0, ^2) is selected, then the static errors are small but the bandwidth is wide. As a result, the system has poor smoothability for high frequency components of the gyro drift. This effect is illustrated in Figure 10 6. For the low cost IMU application, the magnitude of the high frequency gyro drift is critical. It is a reason for the selection of w0 as small as possible. The lowest limit of u»0 can be selected from the maximum static errors, which guarantees the existence of Ф/y in a linear region. In other words, the lowest limit of cj0 can be selected as cjq > 1.46 (Ю4) |Фдг,е| - magnitude of attitude error linear region (|Фдг|, |Фе| < 2—3°). 10.4. Estimation of Low-Cost INS Errors Consider the structure of the estimation block in case of the low-cost IMU and GPS (DGPS) integration.

10.4. Estimation of Low-Cost INS Errors 225 Figure 10.6. The Amplitude of and F(wj) Versus Disturbed Frequency w, for Different Damping Parameters 15 - 9437

226 10. Low-Cost INS/GPS Integration For this purpose, the INS error equations with damping have to be used (see Chapter 5) ' 6Ё = 6VE 6N = 6VN 6VE = —g$N + + ^E + Wil < 6Vn = g$E — + Un + wi2 (10 5) Фе = — + W21 Ф.Л/ — + ^n + + W22 Фир = -jf'tgv + + w23 where Wn, W12 - input noise caused by accelerometer bias, scale factor as well as accelerometer installation errors W21, w22, w23 - input noise caused by gyro installation errors wE - first component of CbL rate on E-axis , .dr ШхЬ i >dr ШуЬ i ,dr L Uzb - second component of CbL drift rate on N-axis. - projection of gyro drift ^xb yb , ,dr ^zb J - projection of gyro The remarks about the sign of gyro drift have been done in section 5.1. uE = -k-tSVE ulN = —ki^VN — control signals for u2E = k26VE u2n = —k26VE INS error damping Note, that for civil applications, a land vehicle has a substantial change of heading angle H only. Taking into account the above, the projections of gyro drift rates on the local level frame can be rear- ranged as dr dr wdrb cos H + udrb sin H (jjdb cos H — sin H (10.6)

10.4. Estimation of Low-Cost INS Errors 227 where - own gyro drift rates, which can be described by the shaping filter: ^xb = ~^Uxb + 4* = ~^уь + A^/2^w41 (10.7) Hence, the system model for the Kalman filter is based on the application of equations (10.5)—(10.7). The differences between INS and GPS indications are used as a measurement model 2i = AEins - AEGPS = 6E —8EGPS z2 = &Nins - &Ngps = 5N —6Egps ""^7” z3 = V£NS - VGPS = 6VE -6VGps ~~V3~ 24 = V™s -VGPS = 8VN-6VGPS (10.8) ~~v7~ where Vi, v2, v3, щ - measurement noise caused by GPS errors. Using system model (10.5)—(10.7) as well as measure- ments (10.8), the Kalman filter for the INS error estimation can be implemented as %k — + Luk-l + Kfc(Zfc — H$k,k-lXk-l ~ HLUk-1) where Флд-_1 - transition matrix of the system model (equa- tion (10.5) —(10.7)) Zfc - measurement vector H - measurement matrix Uk-\ - control signal for the INS error damping. Note, that GPS error is not white, but time correlated. For this case the Kalman-Schmidt filter for colored measurement noise can be used (see Chapter 7). 15*

228 10. Low-Cost INS/GPS Integration 10.5. Roll, Pitch Correction Loop Implementation Using the Kalman filter, the estimates of Ф^ can be determined. The above angles are actually the angles between the platform and local level axes. Note, that the platform frame is the navigation frame (in considered case: local level), which is implemented in an on-board computer using real data from inertial sensors. In other words, in the navigation algorithm matrix Cfl is calculated instead of C/L. The platform frame has a small angular deviation from the navigation frame due to the presence of sensor errors. The above deviations are called platform attitude errors. In order to compensate the attitude errors, the correction of matrix C?1 has to be done as (10.9) where C? - direction cosine matrix between the body and plat- form frames calculated in the on-board computer C/L - direction cosine matrix between the body and local level frames (after the attitude error correction) Ср/ - direction cosine matrix between the platform and local level frames described as (see Chapter 5) fLL _ pi 1 Фе Ф;у —Фе 1 Фе, Ф?/ - estimates of the horizontal attitude errors obtained by the Kalman filter. Note, that the estimate of Фгф in the considered matrix is absent, because for the Фир correction the special heading correction loop is used. After the determination of C/L, roll and pitch angles can be cal- culated using the elements of corrected matrix C/L (see equation 4.22)

10.6. Heading Correction Loop Implementation 229 as C() 7cor = — tan-1(—), 7€[тг;-тг] Сзз Co = V c3i + c|3 where ci7 - elements of matrix C^L. 10.6. Heading Correction Loop Implementation For the initial azimuth alignment an external heading device has to be used. In case of rough heading information, the azimuth misalignment angle may be significantly large. In order to correct the azimuth error, GPS or DGPS measurements can be used For a land vehicle, the heading angle can be calculated using GPS velocity information according to Figure 10.7 as \/GPS HGPS = tan-1 Vn (10.10) Figure 10.7. Definition of Heading Angle

230 10. Low-Cost INS/GPS Integration Using variations of the parameters in the above equation, the variance of the heading error is determined as VN + 'E or with respect to standard deviation c(5H) = a(6VGPS) (10.11) where cr(6VGPS) - standard deviation of GPS velocity error. Consequently, the heading error in the considered case depends on GPS velocity error as well as the nominal value of vehicle speed. Assume that cr(dVGPS) = 0.1 — 0.2 m/sec and land vehicle speed is ||V|| =20 m/sec, then the heading error according to equa- tion (10.11) will be = 0.3 — 0.6 deg. Note, that the above approach can be used for an aircraft as well, but in this case the ad- ditional angle between уь and V appeares (drift angle). Using GPS measurements, the INS correction of the heading angle is given by 6H,NS = (H1NS - HGPS) (10.12) where HGPS is calculated only in case, when the vehicle has a sufficient magnitude of speed. Certainly, the estimate of 6H^.NS can be additionally smoothed by any type of low pass filter For the heading correction, the one step control signal for gyro torque (Am quaternion) can be arranged as 6HINS where hN3 - time sampling. Note, that instead of the calculation of 6HINS from equa- tion (10.12), the estimate of Фпр from the Kalman filter can be used.

10.7. Implementation of Prediction Mode 231 Figure 10.8. Estimation Behaviour of Azimuth Misalignment Angle But for the low-cost IMU, the estimate of Фпр has a noticeable time lag with respect to the true value due to the large magnitude of vertical gyro bias (see Chapter 7). The above effect is illustrated by the plots shown in Figure 10.8. The thin line displays the true value of Фир and thick line is the estimate obtained from the real test processing of Motion Pak™. 10.7. Implementation of Prediction Mode The prediction mode starts, when GPS measurements are not avail- able. In this case, the last estimates of the state vector components are used as the initial conditions for prediction. It means, that in the Kalman equation Kk = 0. For a low-cost IMU, the estimates of gyro drift rates have high frequency behaviour, which is weakly predictable. A reasonable set of the state vector components, which can be utilized for prediction, includes position, velocity and attitude errors. Conse-

232 10. Low-Cost INS/GPS Integration quently, the reduced state vector and the reduced transition matrix are applied for prediction. 10.8. Position and Velocity Correction Loop The estimates of the INS position and velocity errors can be used for correction of the INS output iiidications as Per = JNS-6<PINS Acor = XINS-SXINS K® = V™S-6VB = V^NS-6Vn where </?cor, Acor - output coordinates of the integrated system tpINS, XINS - INS coordinate indications Kot “ output velocity of the integrated system Vf75, V/NS - INS velocity <5VE, 6V^ - estimates of the INS velocity errors XXINS - estimates of the INS position errors. 10.9. Test Description and Result Analysis In order to demonstrate the accuracy of the integrated low-cost IMU/GPS system, series of tests were conducted in both a land ve- hicle and an aircraft. The test procedures and results are described below. 10.9.9 Land Vehicle Tests Test runs were carried out in Calgary, Alberta in August 1999 [15]. A Motion Pak™ and an Ashtech GG-24 GPS/GLONASS receiver were

10.9. Test Description and Result Analysis 233 installed in a passenger vehicle and driven throughout suburban areas. The test trajectory, shown in Figure 10.9, is approximately 24 km in length, which requires about 50 minutes to complete one run. The Easting (m) Figure 10.9. Calgary Test Trajectory Seven runs were conducted over a three day period. One of them was selected for the further discussion however, the results from the others were used to generate outage statistics. Figure 10.10 presents the number of visible satellites, which var- ied from 0 to 12, along with GDOP (the GDOP is set to zero, when no satellites were tracked). As seen from the plot, there were several periods of poor coverage and there was no solution 11.5% of time. The outages typically occurred, when the vehicle went under over- passes. Apparently, these results indicate one of the advantages of using an INS during GPS/GLONASS outages. From an operational point of view, it is desired to avoid the significant degradation in per- formance during outages and to have the INS accurately predicted output during this time. In order to verify the prediction capability of the integrated system, DGPS/GLONASS measurement gaps were

234 10. Low-Cost INS/GPS Integration simulated in the data set. Since the DGPS/GLONASS information was still available, it was used as a reference for comparison with the INS-predicted values. GPS Time / Local Time (Sec / Hr min) Figure 10.10. Satellite Geometry and Availability Table 10.3 presents the INS prediction accuracy obtained from the test data collected over the seven runs. As seen from the table, the results are sub-divided into two categories: (1) for periods, when the vehicle was traveling at a constant speed before and during the outage; and (2) for periods, when the vehicle underwent acceleration during the outage, hence the vehicle dynamics was changing. Table 10.3. RMS Position Accuracy of Integrated System Outage (s) Constant dynamics during prediction Acceleration during prediction 10 3 m 10 m 20 10 m 23 m The INS prediction accuracy is strongly dependent on changes

10.9. Test Description and Result Analysis 235 in the vehicle dynamics during the DGPS/DGLONASS gaps. When the vehicle acceleration changes within the prediction interval, the prediction accuracy is 10 m over 10 seconds; whereas when the ve- hicle dynamics is constant, the results improve up to 3 m over 10 seconds. For the 20 sec outage, the prediction accuracy degrades to 10 m and 23 m for the two cases. This can be explained by the fact, that the behaviour of the non-stationary inertial error, component (i.e. azimuth misalignment, accelerometer scale factors, nonorthogonality of installation errors) strongly depends on vehicle dynamics within the prediction period. This is well illustrated by an example shown in Fi- gure 10.11, where the predicted positions of the integrated system and the DGPS/DGLONASS positions are compared for two 20 sec outage periods. Figure 10.11. Position Error between Predicted and DGPS/DGLONASS Positions Direct evaluation of the system attitude accuracy has been done for the airborne tests using this integrated system. 10.9.2. Airborne Tests Flight tests were performed in 1999 in Nevada, USA (Battle Moun- tain), thanks to the Newmont Gold Company (USA). For that tests, the system was installed beside the precise local-level gimbal INS. The highly accurate aircraft attitude data was available for the entire flight

236 10. Low-Cost INS/GPS Integration was used as a reference. Two Trimble 4000 SSE receivers provided double difference (DD) GPS carrier phase solution. In this case, the DD carrier phase solution was used to update the INS. As discussed previously, an important task of an INS in the overall navigation sys- tem is its prediction capability during DGPS data losses. The north position error within the 20 sec simulated GPS outage is shown in Figure 10.12. The coordinate error during this gap does not exceed 6 m. For short term GPS measurement losses (e.g. 10 seconds), the accuracy of 1 to 1.5 m was achieved. The achieved accuracy is much better than for the land vehicle tests. It can be explained by two rea- sons. Firstly, the dynamics was more stable during the airborne tests. Secondly, the more precise carrier phase - derived solution was used to update the INS in that case. North position error (m) 1 о -1 -2 -3 -4 -5 -6 1090 1100 1110 1120 1130 Epochs (sec) Figure 10.12. GPS Outage Recovery of Integrated System in Position Domain For the attitude accuracy evaluation, the comparison between the Motion Pak™/DGPS integrated system and the precise local-level INS has been performed. The special procedure for determination of the misalignment between the Motion Pak™ and the local-level system was implemented; and the offset between the two systems was estimated and removed from the further data. Figure 10.13 shows the aircraft pitch as estimated by the Motion Pak™ and the local-level INS. The Motion Pak™ error, using the local-level system as a ref-

10.9. Test Description and Result Analysis 237 Figure 10.13. Comparison of Pitch between Motion Pak and Reference INS erence, is shown in Figure 10.14. Figures 10.15, 10.16, 10.17 and 10.18 illustrate roll and heading indications as well as roll and heading errors within the aircraft flight. The errors in attitude determined by the integrated system are shown in Table 10.4. Table 10.4. Attitude Accuracy RMS Pitch Roll Heading (arc min) 25 22 43 Finally, the analysis of test data well confirm the theoretical deduction concerning the selection of damping parameters (wo, to)- Attitude error estimates ФЕ during the flight is shown in Figure 10.19 with cjo=O.O1 (1/sec) (thin line) and with cjo=O.3 (1/sec) (thick line).

238 10. Low-Cost INS/GPS Integration Figure 10.15. Comparison of Roll between Motion Pak and Reference INS

10.9. Test Description and Result Analysis 239 Error (degree) 1.5 1.0 0.5 0 -0.5 -1.0 -1.5 2100 2150 2200 2250 2300 2350 Epochs (sec) Figure 10.16. Roll Error (Deg) Figure 10.17. Comparison of Heading between Motion Pak and Reference INS

240 10. Low-Cost INS/GPS Integration The above plots confirm, that the strong damping (o?0=0.3 (1/sec)) guarantees a small magnitude of the low frequency error com- ponent, but a large magnitude for high frequency due to the wide bandwidth of the damped system. For the weak damping (wo=O.Ol (1/sec)), the inverse situation is observed. Certainly, that the high frequency error behaviour leads to the poor prediction accuracy, when GPS measurements are absent. The plots in Figure 10.20 illustrate the prediction accuracy for different values of w0 (thick line for iv0=0.01 and thin hne for wo=O 3). 10.10. Vertical Channel of Low-Cost Integrated System The vertical channel for the IMU/GPS integrated system can be considered individually because of the different, in comparison with horizontal channels, error model. The integrated scheme of the vertical channel is shown in Figure 10.21. It has been shown in section 5.5, that the vertical channel of an INS is unstable with respect to errors. Using external altitude

10.10. Vertical Channel of Low-Cost Integrated System 241 Ф e (degree) 1100 1150 1200 1250 1300 1350 1400 1450 1500 1550 t(sec) Figure 10.19. Estimates of Attitude Errors for Different Damping Frequencies Figure 10.20. Prediction Accuracy for Different Damping Parameters 16 - 9437

242 10. Low-Cost INS/GPS Integration cor Х/СОГ Figure 10.21. Vertical Channel of Low-Cost IMU Integrated Scheme

10.10. Vertical Channel of Low-Cost Integrated System 243 information (GPS/DGPS) the error damping can be organized, see section 5.5. The error damping is implemented in the block of the INS vertical velocity and altitude calculation. The Kalman filter is implemented in the estimation block using equation (5.40) as a sys- tem model and differences between INS and GPS (DGPS) velocity and altitude indications as measurements. Consequently, the system model can be described as Sh = <5Vup + ui SVup = 2p2 6 h + 3WUp + u2 <5Wup = W,(t) where щ = —ci$h, u2 = — c26h - control signals for error dam- ping - INS error in the velocity calculation 8h - INS error in the altitude calculation 6Wup - INS error in the vertical acceleration calculation Wi - input (system) noise due to sensor errors. The measurement model is the following = hiNS — hops = 5h —Shops И Z2 = V'PNS - V°ps = SV^SVgps (10 13) V2 where Vi, V2 - measurement noise. For this case the equation of the Kalman filter has a form xk = + Ьик_1 + Kk(zk - НФхк-i ~ HLuk-i) The estimates of the INS altitude errors are used for the error damping feedback. The prediction mode starts, when GPS measurements are not available. For the low-cost IMU application, the reduced system i6*

244 10. Low-Cost INS/GPS Integration model for the error prediction is applied because of random behaviour of the estimates caused by the high level of sensor errors. The prediction accuracy of this system was analyzed as well. The altitude error RMS was on the level of 2—7 m within 10—20 sec GPS data gaps. 10.11. Ultra Compact Integrated Navigation System An integrated navigation system of a pocket size has been developed by Teknol Ltd. (www.teknol.ru), see Appendix F. This system in- cludes the following sensors: - 3 MEMS rate gyros - 3 MEMS accelerometers - high sensitivity GPS chipset - magnetometer - baroaltimeter. The above unit implements the full navigation solution in its own processor and can be applied for air, land and sea navigation, vehicle location, attitude determination (without GPS) as well as a personal navigator. Different modifications of such system are shown in Figure 10.22 and Figure 10.23. Obviously, in the above system the traditional sole navigation scheme cannot be applied because of the rough sensors used. In order to implement navigation reckoning, the different type of INS error damping can be used (see, for instance, section 10.2). In case, when GPS measurements are unavailable, the measured accelerations can be applied for the error damping; however, in this case the INS error model is disturbed by true vehicle accelerations. Nevertheless, for moderate motion this effect is not substantial (more- over it can be smoothed), especially for the attitude determination. The comparison between the attitude indications of the above unit and the reference (obtained by a high performance INS) has been carried out. In this case, GPS measurements are not used, but

10.11. Ultra Compact Integrated Navigation System 245 Figure 10.22. Companav II (Heavy Duty Implementation)

246 10. Low-Cost INS/GPS Integration Figure 10.23. Companav II (Light Implementation)

10.11. Ultra Compact Integrated Navigation System 247 for moderate motion the roll and pitch accuracy of 0.7—1 deg was achieved Angle, deg 25000 26000 27000 28000 29000 Time, sec Figure 10.24. Comparison between Integrated Ultra Compact INS and High Performance INS For the GPS integration with inertial sensors, the attitude accu- racy does not significantly depend on vehicle accelerations, because of external information (see section 10.2). In this case for moderate mo- tion the attitude accuracy (roll, pitch) of 0.2—0.3 deg was achieved. In- deed, the comparison of plots in Figure 10.24 well illustrates the above accuracy. The obtained accuracy of such unit is shown in Table 10.5.

248 10. Low-Cost INS/GPS Integration Table 10.5. Pocket Integrated GPS Specification Accuracy GPS integrated mode Sole inertial mode Moderate motion (a <0.4 m/sec2) Dynamic motion Roll, pitch Heading Velocity Position 0.5—0.7 deg 1 deg 0.2 m/sec 5-7 m 0.7-1 deg 2—3 deg 1.5—2 deg 3 deg GPS outages wit 20—30 m positior lin 40 sec: i error Data output Power RS232 10-5( 1W 12VDC Hz Fast cold start <10 sec Moreover, such pocket INS can be used even for indoor appli- cations, but it requires the additional techniques implemented in the software.

Chapter 11 Medium-Level Accuracy IMU/GPS Integration A medium-level accuracy inertial measurement unit (IMU) usually includes a rate gyro with an in-run gyro bias stability of 0.5—1 deg/hr and an accelerometer bias on the level of 10-3g to IO-4#. Such IMU can be used in a stand-alone mode with self-contained horizontal and azimuth alignment. However, the standard azimuth alignment is not accurate enough for a stand-alone mode and can provide the accuracy dr on the level of 3—6 deg (Фир = , see section 4 4). The usage of a stand-alone IMU provides poor accuracy for position and attitude. However, integration of this unit with GPS (DGPS) gives reasonable accuracy of the navigation parameters and is useful for a wide spectrum of applications. 11.1. Honeywell HG1700/GPS Integrated System A good example of a medium class IMU is Honeywell HG1700 (see Figure 11.1). This unit contains laser gyros with a ” run to run” bias stability

250 11. Medium-Level Accuracy IMU/GPS Integration Figure 11.1. Honeywell HG1700 of 1—3 deg/hr. A functional scheme of the integrated system for this case is shown in Figure 11.2. For the above unit, initialization of the navigation process con- sists of the self-contained horizontal alignment and azimuth alignment considered in section 4.2. In the navigation mode, the stand-alone navigation algorithm is implemented. For the long-term application of the integrated system, the error damping can be used as well (see section 10.2). The estima- tion block provides the estimates of all observable components of the INS error model using the difference between INS and GPS velocity and coordinate indications as measurements. The attitude is corrected by attitude error estimates obtained from the estimation block. The prediction block is implemented during the GPS outages. The stand-alone INS navigation algorithm and the correction loops are implemented in parallel procedures, which helps to provide predictable behaviour of the estimated navigation errors. Note, that the integrated system provides the low frequency (Schuler frequency) error behaviour, which leads to the good smoothing property of the estimation algorithm. Moreover, the Schuler behaviour of the INS

11.1. Honeywell HG1700/GPS Integrated System 251 Figure 11.2. Medium Accuracy IMU Integrated Scheme

252 11. Medium-Level Accuracy IMU/GPS Integration errors can be considered as a low-pass filter, which is able to smooth well random noise of sensor errors. 11.2. Estimation Block Implementation For the estimation block, the INS error model has to contain only observable components of the state vector. The INS error model for the considered case can be represented as (see Chapter 5) 6Ё = 6VE 6N = 6VN 5VE = ~~9&N + aN^up + UE + SVN = дФЕ - аЕФир + + w2 Фе = Фцр^К + Ф/^up + ШЕ 4- UN + W3 (11-1) гС Ф-V = 5 Ve — ФЕШир 4- Фир^Е 4 1- 4- u2e 4- w4 rt Ф — ^up — 6VE d -Фдг^Е 4- Фе^я + tan </? 4- 4- uup + w5 where 6E, 6N - INS error of position 6VE, 6VN - INS error of velocity Фе, $n, Фир ~ INS attitude errors Wi, w2 - input noise caused by the accelerometer bias, scale factor and accelerometer installation errors w3, w4, w5 - input noise caused by the gyro scale factor and gyro installation errors ^'e, wup - absolute angular velocities of the local-level frame ex’# , 0$, - gyro drift rates reduced to the local-level frame ize, uNi uup - vector of the control signal of the feedback from the estimation block to the navigation solution. In general case, the relationship between the above drift rates and the own gyro drift rate of the body frame are related through

11.2. Estimation Block Implementation 253 matrix CbL, i.e. . ,dr . ,dr ^xb , ,dr _ r<LL — . .dr Uyb , ,dr _ ^up _ . .dr L ^zb J (11.2) Note, that to compensate the gyro drift rates directly in raw data, the estimates of cu^rb, ufy should have the opposite sign (see section 5.1). If for the gyro drift compensation the introduction of the estimates into the gyro torque image is used, the original sign has to be saved (section 5.1). For civil applications, a vehicle usually have a substantial change of heading angle only, so equation (11.2) can be rewritten as = ccsHu^ + snH'i^ 4' = - sin H + cos H • (11.3) The model for the gyro drift rates can be defined by the shaping filter as < 4 = + Ay/tf v6(t) < % = -i3w^b + w7(t) < 1$ = + (11.4) where w6(t), w7(t), w&(t) - white noise with identity intensity A, /3 - parameters of the gyro drift rate correlation function R(wdr) = A2exp(-/3\r\). Consequently, the INS error model, which is used in the estima- tion block, can be described by equations (11.1) —(11.4). The measurement model is based on the differences between INS and GPS (DGPS) position and velocity indications Z1 = = AEINS - A.Egps =6E + vi ^2 = = &Nltis - &Neps = SN + v2 ^3 = = V^NS - V$ps = 6VE + v3 Z4 = = V'NS -VGPS = 6VN + v4 (11-5)

254 11. Medium-Level Accuracy IMU/GPS Integration ( = —6Egps where < v2 = —8NGPSV3 = —8VGPS - measurement noise ( v4 = —6Ngps (GPS errors). Using equations (11.1) —(11.4) as a system model and equa- tion (11.5) as a measurement model, the Kalman filter for this case can be created as % к — *&k,k—l&k—1 4“ Lu^-i 4~ Kjfc(Zfc 1 H LlLk—i) where Xk - estimate of the state vector (INS errors) “ transition matrix of the INS error model L - input matrix Ufc_i - vector of the control signal of the feedback from the estimation block to the navigation solution. Control signal и may include the estimates of gyro drift rates, azimuth misalignment correction as well as the damping signals if necessary (shown by dashed line in Figure 11.2). Let’s consider an example of the Honeywell 1700/DGPS inte- grated system field testing, when Uk-\ is used. Let’s analyze the possibility of the real-time estimation and com- pensation of the gyro drift rates and azimuth misalignment error. The estimates of the gyro drift rates and azimuth misalignment from the system tests on a land vehicle are shown in Figure 11.3 and Figure 11.4. Obviously, after termination of the transition process, the above esti- mates can be used for the INS real-time calibration. The INS velocity error behaviour for different values of control signal (ufc_i) is shown in Figure 11.5. The original INS velocity error for Uk-i = 0 is displayed by thick line. The velocity error, when w*._i includes the estimates of the gyro drift rates and azimuth misalign- ment correction is presented by thin line. The velocity errors, when Uk includes the estimates of the gyro drift rates, azimuth misalignment correction and damping signals (—к\8У, ±к28У) is shown by dashed line.

11.2. Estimation Block Implementation 255 t(sec) Figure 11.3. Estimates of Gyro Drift Rate Biases t(sec) Figure 11.4. Estimate of Azimuth Misalignment

256 11. Medium-Level Accuracy IMU/GPS Integration Figure 11.5 Velocity Error Behaviour for Different Control Signal иг Apparently for the INS drift rate and azimuth calibration, the velocity errors have lower amplitude than for a stand-alone mode. But the smallest velocity errors correspond to the calibrated and damped INS system. For a medium accuracy INS system, the reduced mag- nitudes of the damping parameters are used in comparison with a low-cost system. In the present case, the strategy of the cj0 selection is the same as for a low-cost INS (see section 10.2, 10.3), but the level of gyro drift rates is much smaller. The application of the error damping for such type of an INS depends on time of the system in use. For relatively short time of the system application (1—1.5 hr), the error damping should not be used, but gyro drift rate and azimuth calibration is quite useful. It is understandable, because the Schuler behaviour of the INS errors provides good smoothability of random noise. However, for the long term application, the error damping guarantees a reasonable level of the INS errors within total time of the system exploitation. Finally, the selection of input noise level for the estimation pro- cess has to be analyzed. Let’s introduce the practical way for selection of the input covariances on the example of the Фд, Ф£ estimation. The estimates of attitude error Фдг for different input noise covari-

11.2. Estimation Block Implementation 257 ances > q2 > q3 are displayed in Figure 11.6. As was shown in section 7.3.2, the real-time estimates of the Kalman filter have a time lag with respect to the true state vector components. The above time lag depends on the selection of the level of the input noise covariance. The large input noise covariance corresponds to the small time lag Assume, that magnitude of input noise covariance qr is larger than a real value. In this case, the estimate has noisy behaviour (measurement noise passes through the filter) but the small time lag. It is well illustrated by the estimate of shown in Figure 11.6. If the magnitude of input noise covariance q3 is smaller than the true value, the estimate for q3 (see Figure 11.6) will have the large time lag with respect to the estimate for Qi, but it is well smoothed. The reason- able selection of input noise covariance q2 is based on the compromise between the large time lag g3 and noisy behaviour q^. (deg) 145500 146000 146500 147000 147500 148000 t(sec) Figure 11.6. Estimates of Horizontal Attitude Error for Different Levels of Input Noise The above approach can be used for the practical selection of all input (system) noise covariances based on the analysis of the estimates obtained for different <&. 17 - 9437

258 11. Medium-Level Accuracy IMU/GPS Integration 11.3. Output INS Error Correction According to the scheme of the integrated system shown in Figure 11.2 the estimates of the INS position and velocity errors are used for the correction of the INS output indications as фсог - = (p,NS-6<prNS -Vor Vе = r COT yN = * cor = x,NS - sx,NS = V^S-6VE -- vJ,NS-svN where (pcor, Xcor - output coordinates of the integrated system <pINS, XINS - INS coordinate indication 8X - estimates of the INS position errors Кот > Kot “ outPut velocity of the integrated system V*NS, YnNS " INS velocity 514;, 8VN - estimates of the INS velocity errors. To correct the INS attitude (roll, pitch), the similar approach described in section 10.5 for a low-cost system may be used. The heading correction can be organized by the feedback from the Kalman filter to the navigation solution. In this case, the estimates of the azimuth misalignment are introduced directly into the gyro torque image (Am-quaternion). 11.4. Prediction Mode Implementation The prediction mode is initialized in case of GPS data losses. The equation for prediction is determined from the Kalman filter expres- sion when Kk = 0, i.e. % к ~ *&k,k—l%k—1 T I^k-l In the present case, all state vector components take part in the prediction mode, since all of them have sufficiently smoothed be- haviour.

11.5. HG1700/DGPS Test and Results 25g 11.5. HG1700/DGPS Test and Results The tests were carried out in Calgary, Alberta in August 1999 [14] The system was mounted on a roof of a test vehicle and a GPS antenna was hard-mounted on the box containing an IMU. The test vehicle was driven on a 15.3 km L-shaped traverse as shown in Figure 11.7. Figure 11.7. Complete GPS Trajectory The total duration of the test was 75 min. Raw measurements from a Novatel OEM-3 receiver were logged at a data rate of 1 Hz and raw data from HG1700 were logged at a rate of 100 Hz. The total number of satellites tracked during the test varied between four and nine, 10 deg elevation cut-off. Zero velocity updates ZUPTs) were applied at approximately 5 mm intervals. The self- contained horizontal and azimuth alignment was performed using the traditional SINS algorithm (see section 4.2) and took 10 min of data collection in a static mode. In order to check the attitude errors о the integrated system, an external high performance inertial system with gyro drift of 0.005 deg/hr could be used as a reference. If it not available, the following undirect method of the attitude accuracy 17’

260 11. Medium-Level Accuracy IMU/GPS Integration evaluation can be suggested. The simplified INS error model has a form (shown for the East channel, see Chapter 5) = —g$N 4- The integration of the above equation gives rt rt rt 6VE = - I g$Ndt 4- I aN$updt 4- / aEfj,Edt J to J to J to Under the condition of a constant vehicle speed and small mag- nitude of (t — to) > the following equation can be rewritten to give dVE ~ —g$N(t — t0) or SE = To asses the prediction accuracy of the system, the prediction error over a short time interval (10 to 20 sec) can be approximately described as t2 6Ё = -<zM0)-£ (u.6) where 6E - position prediction error Фдг(0) - estimation error of the horizontal misalignment angle at the beginning of prediction tpr - prediction time interval. In order to determine the prediction accuracy of the integrated system, simulation of DGPS gaps was performed. Since the DGPS information was still available it was used as a reference to define the system errors in prediction mode. The prediction accuracy for the data with 20 GPS gaps is pre- sented in Table 11.1.

11.5. HG1700/DGPS Test and Results 261 Table 11.1. Prediction Accuracy of HG1700/DGPS Integrated System Outage (sec) RMS (m) Prediction Accuracy 10 0.3 20 0.8 Using equation (11.6) and the prediction errors from Table 11.1, approximate accuracy of the horizontal attitude angles can be esti- mated as 2RMS(6E) RMS(*N(O)) =--------- gtpr Implementing this approach, the estimated accuracy of the hor- izontal attitude (roll, pitch) errors is on a level of 2—3 arcmin. Note, that the present evaluation is performed for moderate vehicle acceler- ation. The attitude accuracy for high dynamics depends of the quality of the preliminary calibration of the SINS non-stationary component. In order to evaluate the heading accuracy, the error behaviour of the SINS east non-stationary component, when vehicle moves along the north direction can be used (see section 9.3) 6V^st = aN$up The integration of the above expression gives <5V£5< = $upVN During the test ZUPTs were performed at each 4—5 min inter- val. Analyzing the error behaviour between neighboring ZUPTs it s possible to estimate the azimuth accuracy (according to Figure 9.7) after the azimuth misalignment correction, as - _ 6VE - = 6V£st (11.7) up VN VN

262 11. Medi um-Le vel Accuracy IM U/GPS Integration where <5V£st - estimate of the INS east velocity non-stationary component. Figure 11.8 displays the velocity error behaviour before (thick line) and after (thin line) the azimuth misalignment correction Using expression (11.7) one can estimate the remained azimuth error on the level of 7 arcmin. t(sec) Figure 11.8. Velocity Error Behaviour Before and After Compensation of Azimuth Misalignment Finally, the prediction accuracy within motion with moderate accelerations for two cases was compared. Firstly, the stand-alone INS without the feedback from the estimation block to the navigation so- lution (u = 0) was used The prediction error is shown in Figure 11.9 by thin line. Secondly, the INS with the drift rate and azimuth mis- alignment correction was applied. The prediction error for the second case is shown in Figure 11.9 by thick line. As it seen from the plot, the real-time correction of an INS using system error estimates is quite effective. Note, that the accuracy of such quality INS in a stand-alone mode can be even improved using the internal damping of INS errors

11.6. Vertical Channel of Medium Accuracy IMU 263 Figure 11.9. Prediction Accuracy for Different Control Signal щ (see Appendix E). 11.6. Vertical Channel of Medium Accuracy IMU Integrated System The vertical channel navigation algorithm for such type of an IMU can be implemented independently from horizontal channels because of the different error model. The integration scheme for the vertical channel of a medium accuracy IMU is similar to the algorithm, implemented for a low-cost IMU (see section 10.10). The main difference lies in the selection of different values of the damping parameters Ci, C2- The higher natural oscillation frequency in comparison with the low-cost IMU application is recommended because of the relatively small level of high frequency input noise. On one hand, the above selection provides a small magnitude of static errors. On the other hand, it guarantees, that input noise is well smoothed, see section 5.5. The above effect is illustrated by Figure 11.10. The figure presents HG1700 altitude error behaviour

264 11. Medium-Level Accuracy IMU/GPS Integration during field tests. In order to verify the prediction capability of the above inte- grated system, DGPS measurement gaps were simulated in the data set. Since the DGPS information was still available, it was used as a reference to estimate the prediction accuracy. The achieved accuracy was in the order of 0.2—0.4 m within 10—20 sec GPS data gaps. 11.7. Comparison between Low-Cost and Medium Accuracy Integrated Systems A medium accuracy IMU can be used in a stand-alone mode. It means that the considered above methods of INS calibration and compensa- tion can be implemented for such type of an inertial system, while a low-cost IMU cannot operate in a stand alone mode because of its poor sensor stability (see Chapter 10). The difference between the low-cost and medium accuracy inte- grated systems is shown in Table 11.2.

11.7. Low-Cost and Medium Accuracy Systems Comparison 265 Table 11.2. Comparative Analysis Low-cost IMU Medium accuracy IMU 1. The estimation of azimuth misalignment ($up) is possible but not accurate enough. The reliable way is to use directly heading information from GPS 2. Necessary to organize the error damping 3. Application of calibration procedures can not guarantee significantly stable estimates 1. Possible to estimate the azimuth misalignment in case of a substantial acceleration change 2. For short term applications the stand-alone INS solution can be used 3. Possible to implement the preliminary calibration procedures

266 11. Medium-Level Accuracy IMU/GPS Integration Note, that the accuracy of the sole navigation solution for a medium accuracy INS can be improved close to the level of a high performance INS. In order to provide such improvement, the special self-contained error damping procedure should be introduced into the pure navigation solution (see Appendix E). 11.8 Land Navigation Using INS/GPS and Odometer Information Odometer is a device, which calculates the increment of position along the longitudinal axis of a vehicle. It means that the local-level coor- dinate increments in north-east directions can be rearranged through the odometer measurement as shown in Figure 11.11. Figure 11.11. Vehicle Body Position AEO = &yb sin H ANO = куь cos H (11.8)

11.8. Land Navigation Using INS/GPS/Odometer Information 267 where ДЛ^, AEO - increments of a vehicle position in north-east direction Д?/ь - increment of a vehicle position along уь axis measured by an odometer H - heading. In order to provide the local-level coordinates using an odome- ter, the information about heading angle is needed, which can be ob- tained for instance from an INS. Let’s determine the odometer error model from variation of the equations (11.8). 6EO = 6Ауъ$1пН 4- Дт/bCosH6H = бАуьътН 4- AN6H 6NO = 6 Ауъ cos H — Дэд, sin H6H = <5Ayb cos H — AE6H where 8Ayb - odometer error 8H - error of a heading angle 6EO, 6NO - total odometer position errors. Odometer error, 8Ayb can be calibrated preliminary or in real- time using GPS measurements. In this case the difference between the odometer and GPS indications provides the information about heading error, i.e. ZG(E) = AEo-AEGps = 6Eo-6Egps = ANSH-6EGps Zg(N) = ANO — ANGPs $NO — 6NGps — —AE6H — 6NGps where 8EGPS, 5NGPs - position errors of GPS. Note, that the GPS errors can be smoothed preliminary by any type of a filter. Combining the measurements, one can estimate the heading error as 6Й = ZG(E)AN - ZG(N)AE 9) AN2 4- AE2

268 11. Medium-Level Accuracy IMU/GPS Integration where 6H - estimate of the INS heading error. The calculation according to formula (11.9) can be implemented in real time using the GPS and odometer information. Obviously, the estimation accuracy of 6H improves with the traveled distance. The above approach can be considered as a reliable tool for the real- time azimuth error calibration, which is not very sensitive to a vehicle acceleration change (see sections 9.4, 9.5). The functional scheme of INS/GPS/odometer integration is shown in Figure 11.12. Figure 11.12. Functional Scheme of INS/GPS/Odometer Integration In principle, the above scheme is similar to the traditional one for INS/GPS integration, however it has two advantages. Firstly, this scheme provides the more precise INS azimuth cali- bration due to the odometer data. Secondly, as a consequence, better accuracy can be achieved in prediction, when there are some gaps in

11.8. Land Navigation Using INS/GPS/Odometer Information 269 the GPS data. Actually, in the considered case, the prediction mode is actually restoring of the INS errors, because odometer measurements are constantly available. It means, that the accuracy of such scheme with gaps in the GPS data can be improved using accurate odome- ter information when the permanent real-time odometer calibration is applied.

Appendix A Mathematics of GPS Figure A.l. Pseudorange Measurements of GPS Pseudorange measurements can be made using only a code, be- cause only the use of a particular code can give the indication of a time mark sent out from a GPS satellite and detected by receiver firmware [12, 13]. Knowing the transmit time of event (a) when a signal is sent out; and the received time of event (b) when a signal is received, one

271 can compute the pseudorange p* = c[T(fr) - f(a)J where plr - pseudorange between a GPS receiver and i-satellite T(b) - received time when a signal is received t' (a) - transmit time when a signal is sent c - speed of light. Relating the two different time scales with the reference GPS time, one get plr = plr 4- cAtr where plr - geometric distance between the receiver and i- satellite Atr - receiver clock offset from the satellite system time. Arbitrary pseudorange can be rearranged through the coordi- nates of г-satellite and coordinates of the receiver as p* = у/(xi - zr)2 4- fa - yr)2 4- (z< - zr)2 4- cAtr (A.l) where xly ytZi - coordinates of г-satellite in the Earth-fixed frame з?г, Уг, zr - coordinates of the receiver in the Earth-fixed frame. The above equation includes four unknowns xr,yr,zr and Air, which can be defined, if four measurements are available (see Fi- gure A.l). It is assumed that the coordinates of all GPS satellites are known. Linearization of equation (A.l) gives (5Д = pj. - #(0) = (xr - zr(0)) 4-j- (yr ~ 3/r(Q)^ + axr >---------------------v——' dyr s-------- ад ад +^-(гг-гг(0))+сД4г (A.2) dzr "---v-----' 8zr

272 where dp' dxr dp1 dyr dp' dzT A. Mathematics of GPS — (хг — Xr) y/(xi - xr)2 4- (yi - yr)2 4- (Zi - zr)2 _______________~(^ ~ Уг)________ у/(Xi - zr)2 4- (yi - Ут)2 4- (Zi - zr)2 _______________~(zi - Zr)________________ у/(Xi - xr)2 4- (yi - Ут)2 4- (Zi - Zr)2 If a few satellites are observable, pseudorange measurements can be described by the following linear vector equation Spi 5p2 dp1 dp1 dp1 dxr dyr dzr dp2 dp2 dp2 dxr dyr dzr 6xr tyr 6zT cAtr (A.3) 1 1 H or zk = Hxk + VGPS (A.4) where VGPS - GPS errors in measurements (A.4). Using the Least Square method, solution of equation (A.4) can be obtained as x = (HTH)~1HT z (A.5) Я# where H# - pseudoinversion of measurement matrix H x - estimates of receiver coordinates z - pseudorange measurements.

Appendix В Relationship between $up and Roll, Pitch, Heading Errors In order to determine the relationship between INS errors Фж, Фу, Фг and true attitude errors 6$, fry, SH the expressions for roll, pitch, heading angles have to be used (see Chapter 4) $ = arctan— (B.l) co 7 = — arctan — (B.2) C33 H = arctan — (B.3) C22 where Cq = у/4- c|3 and Cij - elements of C^L matrix. The expressions for roll, pitch, heading errors can be determined from variations of equations (B.l) - (B.3) with respect to c^. For in- stance, the roll error can be obtained from variation of expression (B.2) 18 - 9437

274 В. Relationship Between Attitude Errors of INS as 1 Г 1 + (^)2 L сзз ' <^c31c33 — c31^c33 r2 ,r2 c33 C31 C31^C33' r2 c33 (B.4) where <5сз!, <5сз3 are the errors in calculation of the elements of matrix CbL. The above errors can be specified from the equation 6CbLL = c” - cbLL = of - c#cf' = (i - (B.5) Here, matrix Cpf is the direction cosine matrix between the platform frame and the local-level frame, introduced in Chapter 5 as 1 Фир z-xLL _ — Фир 1 —Фе Фе 1 Substituting the last expressions into equation (B.5), one can get 0 Фир Фир 0 —Фдг Фе r^Pi ФдГ —Фе 0 (B-6) where Cbl is the actual direction cosine matrix between the body and local-level frames, which is calculated in an INS on-board com- puter. It should be reminded, that a real INS unit actually calculates Cbl instead of Cb due to INS sensor errors. The similar formulas can be defined for the heading and pitch errors as 6H = Sti = <fcl2C22 — с12^с22 <^c32c0 — C32^CO C32 + C0 (B.7) (B.8)

275 where co = Vc3i + сзз C31/C31 4- сзз^сзз OCn = ------f '.... ' =-- л Л2 I r2 V c3i + сзз Hence, the relations between Ф£, Фл, Фир and the pitch, roll and heading errors can be described by equations (B.4), (B.7) (B 8). 18*

Appendix С Measurement Model for 7, H Angles Rotation in Second Calibration Procedure Measurement Model for 7, H Let us consider the measurement model for the second type of the calculation procedure, when rotations of an IMU unit by 7, H angles are used. 1. Rotation by 7 angle. Angular velocity of the body frame during rotation has a form: Mb = 0 (C.l) Assuming, that ip angles are small, the transformation matrix

277 between the body and navigation frames has a form CbN = cos 7 0 — sin 7 0 sin 7 1 0 0 cos 7 Substituting equation (C.l) into gyro error model (-6.1), one can define M = &4-&/У M = ft+fts/7 M = ft + fty7 Projections of the gyro errors on the navigation frame can be determined as 6(лЗе 6(dUp = CbN cos 7 + 5wz sin 'y —дшьх sin 7 4- 6ojx cos 7 and 5uje — fix cos 7 4- /3^7 cos 7 4- fiz sin 7 4- fizy sin 77 6uN = fiy + fiyyi Integrating the above equations, one can determine f c j, fix . p I duiEdt = —sm 7 4-pxlz sm 7 Jo 7 ft I SlOjydt — fiyt "b fiyy'Y Jo ^(cos7 — 1) — fty(cos7 - 1) 7 (C.2)

278 C. Measurement Model in Second Calibration Procedure Projections of the gravity vector on the body frame are the following «x 0 ' —(/sin 7 n 0- 1 cb — С'дг 0 — 0 _ 9 . geos 7 and using accelerometer error model (6.1), one can define pro- jections of the accelerometer errors on the body frame as 6аьх = ах - аххд sin 7 4- axzg cos 7 6aby = ay- ayxg sin 7 4- ayzg cos 7 dab — otz — azxg sin 7 4- azzg cos 7 (C.3) Projections of the accelerometer errors on the navigation (local- level) frame are 6aE = 6ab cos 7 4- 8ab sin 7 6aN = 6ab (C.4) Substituting equations (C.3) into equations (C.4), one can de- termine 6aE = Qxcos7 — axxg sin 7 cos 7 4- axzgcos2y 4- 4-q2 sin 7 — azxg sin2 7 4- otzzg cos 7 sin 7 6aN = ay - ayxg sin 7 4- ayzg cos 7 (C.5) Using equations (C.2) and (C.5) in the INS error model (6.2),

279 the velocity error measurements can be described as dVE = - Orz# + <*xC0S7 - QXIpsin7C0S7 4- +axzg cos2 7 4- olz sin 7 - a.zxg sin2 7 4- +azzg cos 7 sin 7 - g/3yt - д0ууу bVN = -fty - ayzg + Oy - ayxg sin 7 4- +ayzg cos 7 4- g~ sin 7 4- g(3xy sin 7 - 7 -6^(cos7 - 1) - £7&y(cos7 - 1) (C.6) For 7 = 90° the above equations can be rewritten as 7Г — dx &xz9 T az azxg gPyt 9Pyy~^ 8Vn = —ayz9 ~ ayx9 + 9^ + 9&xy 4- g-r- 4- gfizy and for 7 = 180° 6VE = ~2ax - g/3yt - д^уутг 2/3z 5V;v = — 2ayzg 4——g 4- 2flzyg (C.7) 2. Rotation by H angle. Angular velocity of the body frame is 0 0 H Ub = The direction cosine matrix between the body and navigation frame: cos H sin H 0 — sin H cos H 0 0 0 1

280 C. Measurement Model in Second Calibration Procedure Projections of the gyro errors on the body frame: M = Px + PzzH + M = Pz+PzzH Projections of the gyro errors on the navigation frame are: = &Л cos H 4- Sid?, sin H & J/ tfwyv = — дшьх sin H 4- bwby cos H Swup = bwb or <5оЛе = fixcosH 4- fixzH cos H 4 fiy sin Я 4- fiyzH sin H Sojfj = -fix sin H - fixzH sin H + fiy cos Я 4- fiyzH cos H Integration of the above equations leads to — sin Я 4- fixzs\nH — ^-(cosH — 1) — fiyz(cosH — 1) Я Я = ^-(cosH — 1) +fixz(cosH — 1) 4- ^-sinH + fiyz sin H Projections of the accelerometer errors on the body frame are otx 4^ &xz9 ^^y — d- ayzg 6abz = az + azzg

281 Projections of the accelerometer errors on the navigation (local- level) frame Scle = 8abx cos H 4- 8ab sin H 8a^ = —6ab sin H 4- 8ab cos H x у or 6cle = OLX cos H 4- axzg cos H 4- ay sin H + ayzg sin H 6aN = -Од. sin Я - axzg sin H 4- aycosH 4- ayzg cos H The measurement model in this case has a form 8Ve = -ax - axzg 4- cos H 4- axzg cos H 4- 4-ay sin H 4- ayzg sin H — - g^fcosH - 1) - g(3xz(cosH - 1) - - g^sinH - 0yzg sin H H 8Vn = -ay — ayzg — ax sin H — axzg sin H 4- 4-cty cos H 4- ayzg cos H 4- 4-д^Ь sin H 4- g{3xz sin H - H — g~(cos H — 1) — gfiyz(cos H — 1) (C.8)

282 C Measurement Model in Second Calibration Procedure For H = 90° dVE — O!x &xz9 + <%y "f" <-%yz9 d" +#77 + 9&xz ~9^~ 9&yz 5V/V — ^y &yz9 &x &xz9 “Ь +9^t + 9&xz + 9^ + 9&yz (C.9) H H For H = 180° 6VE = -2ax-2axzg + 20xzg + 2‘-^g H SVN = —2ay — 2ayzg + 2/Зугд + 2^д (С.10) 3. Example of calibration of the gyro installation errors. Let’s assume, that all calibration parameters except the gyro in- stallation errors have been estimated and compensated before. The following procedure can be used in order to estimate /3^-. ls< step. Rotation by d — 90° Measurement model: SVE($ = 90°) = -g/3yx + g/3zx 2nd step Rotation by d = 180° Measurement model: 6VE^ = 180°) = 2д/Згх 3rd step. Estimation of I3ZX and /3yx - <5Vb(i? = 180°)

283 - _ <5Ve(j5 = 90°) ЙЁвр = 180°) ~ 9 + 2g 4*л step. Rotation by 7 = 90° Measurement model: = 90°) = g(3xy + g/3zy 5th step. Rotation by 7 = 180° Measurement model: 5^(7 = 180°) = 2/3zyg 6th step. Estimation of (3zy and /3xy . _ ^(7 = 180°) Zy 2g Z 6VN(y = 90°) = 180°) 9 2g 7th step. Rotation by H = 180° Measurement model: SVE(H = 180°) = 2/3xzg 6VN(H = 180°) = 2/3yzg 8th step. Estimation of (3XZ1 fiyz SVE(H = 180°) ^xz 2g - dVN(H= 180°) ^yz 2g Here, to create the measurement model equa- tions (С.6), (С.7), (C.9), (C.10) were used.

Appendix D Coning Calculation The main problem of the attitude algorithm implementation is to up- date properly the quaternion and rotation vector. Quaternion update q(T + can be determined by the following quaternion multiplica- tion with respect to the inertial space (see section 2.3): я(т 4- h/уз) — q(T) * ДЛ (D.l) where ДЛ is an updating quaternion and can be described by the following expression: A . . ДФ ДФ ДА = (cos ——, — - sin——, v 2 ’ ДФ 2 ’ ДФУ1 . ДФ ДФН . ДФ. —-^-sin-—, —- sin ) ДФ 2 ДФ 2 ’ (D.2) where ДФ - rotation vector, which can be expressed using gyro output. The rotation vector can be described by the following formula (see section 2.3): дф(^) = lj + ^ДФ(г) x cj(t) 2 (D.3) where w - gyro output.

285 The solution of the above equation in Taylor series has a form ДФ(Т + hN3) = ДФ(Г) + ЛдгзД4(Г) + (^3/2!)ДФ(Т) ... (D.4) Let’s use four samples of gyro output to find solution for ДФ(£ 4- hN3). Using four points, integration of the gyro outputs within a small time interval can be described as follows: '^N3 a(t) — I cj(r)d It — at 4- bt2 4- ci3 4- dt4 (D.5) where a, b, c, d - constant vectors. Repeated differentiation of the above equation with respect to time gives the following relation at time T(t = 0): a>(T) 2b d2a>(T) dt2 d3w(T) dt3 d*w(T) dt4 24d (D.6) By differentiating equation (D.3) repeatedly over time and using

286 D. Coning Calculation equation (D.6) with initial condition ДФ(Т) = 0 one can obtain ( <1ДФ(Г) _ at ~a ^Щ = 2б at2 а3ДФ(т) ± _ r ---rv—- = 6c 4- a x b dC d4M>(T) . — ’ - 24d 4- 6 (a x c) = 36(a x d) + 12(6 x c) - ^(Г) = 120(5 X d) а7ЛФ(т) у, — 7 = 360 (c x d) < *8 W) _ dt8 ' " 0 Substituting equation (D.7) into equation (D.4), it is possible to determine ДФ(71 4* /tjv3) — 4~ 6/i^3 4~ c/i^3 4" dh^3 4~ + t(3 x b)h3N3 + i(a x c)/i^3 + -1(5 x c)h3N3 + V a 1U + Tn (a X + ~(b x 4- — (c x d)h7N3 10 о 14 (D 8) Let the incremental gyro outputs at t = h/4, h/2, 3h/4, h be «1, a2, d3, a4 respectively, then щ 4- o2 4- 63 4- 04 = d. Using this relationship in equation (D.5), one can derive four equations for determination of the values of a, b, c, d. Substituting these values into equation (D.8), one cam get ДФ(7’ 4- Ьдгз) — cl 4- k\ I di x a2 4~ С13 x q4) 4~ +k2(di x d3 4- d2 x a4) 4- fc3(di x q4) 4- k4(a2 x q3) (D.9)

287 where kx = 736/945; k2 = 334/945; k3 = 526/945* k4 = 654/945. However, the rotation vector defined by equation (D.9) contains large errors. Its estimate needs to be further improved through the er- ror analysis. It can be obtained from the optimal values of coefficients ki, k2, k3, k4. Once base motion is choosen, the corresponding exact quater- nion can be computed. In this approach, coning motion is selected as base motion, and then the corresponding true quaternion is ana- lytically computed for the chosen motion. The described above four- sample algorithm is applied to the motion to obtain an approximate quaternion. The difference between two gives an error quaternion. After the error rotation vector is calculated from the error quater- nion, fcj, k2, k3, k4 values in (D.9) can be determined to minimize the magnitude of the error vector. To analyze the performance of the attitude algorithm, coning motion of cone half-angle ДФ and coning frequency w are used. Re- sulting body quaternion q(t) due to the coning motion is given by [6]: ?(«) = cos(^) 0 sin(A^) cos(wt) sin(Ap) sin(o>t) (D.10) If ДЛ represents an incremental quaternion during update time interval hN3 of the coning motion, q(t + hw3) can be determined from q(t) using ДЛ: + hN3) = q(t) * ДЛ

288 D. Coning Calculation or AA = q(t) 1 *q(t + hN3) = A Aq AAX AA2 AA3 1 — 2sin2(A^ sin2(a;/iAr3/2) — sin2(A^) sin(w/i^3) — 8ш(АФ) sin(^) sin[w(t + /1^3/2)] зт(АФ) sin(^i^-) sin[w(t + Адг3/2)] (D.ll) where q(t) and АЛ represent the exact quaternions of coning motion. Now, the approximate quaternion can be calculated by employ- ing the proposed algorithm. The quaternion differential equation is generally written by «(t) = !?(<) * Mt)] Therefore, [0}(t)] = 2q 1(t)-q(t) = -2cu sin2(^) —cj sin f At) sin(wt) cu sin(^) cos(ut) (D.12) where [w(t)] represents gyroscope outputs in the body coordi- nates. The output vector of rate integrating gyroscopes sensing body movements between time t and t + hN3 is: rt+hN3 Г ~2uhN3 sin2 (At) a(hN3) = I [сй(т) dr — —2 8ш(ДФ) • sin(5£^La) sin[td(i-b ^) 4 2 8ш(АФ) sin(^^-) cos[w(t -I- ^)] (D.13) When four-sample sets per update period are considered, the

289 values can be obtained from (D.13). They are given by Q/V = _^sin2(^) -| sin(A$){cos[w(t 4- ^n)] - cos[w(t 4- -- hjva)]} _ sin(A$){sin[w(t + ^ta)] - sin[w(t 4- ^т^туз)]} (D.14) where N is 1,2,3,4 for ab a2, a3, o4 respectively. By substi- tuting these values into (D.9) and assuming small ДФ, one can get ДФ = [ДФжЬ ДФУ1, ДФН] (D.15) where ДФХ1 = -2w/zjv3 зт2(ДФ/2) 4- 4- 8т2(ДФ)[(4/с1 — 2к2 4- 2fc4) sin 4. 4-(—2ki 4- 4/c2 — ^3 — ^4) sin(2cuhjv3/4) 4~ +(-2fc2 4- 2fc3) sin(3wh^3/4) — k3 sin(whN3)] ДФУ1 = — 2бт(ДФ) sin(uh/v3/2) sin[cu(i 4- ^з/2)] 4- 4- (^~^) sin2 8т(ДФ)[(Л1 4- k2 4- k3) sin(wt) 4- +(—2ki — k3 4~ fc4) sin(w(t 4- h>N$/4)) 4- 4-(2fcj — 2k2 — 2/с4) sin(o;(t 4" h/y3/2)) 4~ 4-(—2A?i — fc3 4" ^4) sin(w(/ 4" З/г^з/4)) 4- 4"(fci 4" k2 4" k3} sin(tu(£ 4- ^/уз))] ДФх1 = 2 зт(ДФ) sin(o>h/v3/2) cos[w(t 4-/ijv3/2)] 4- + (^y^) s“2(^r) 81П(ДФ)[-(к! + k2 + k3) cos(wt) - -(-2/ci — fc3 4- k4) cos(cj(t 4- /1^з/4)) - — (2ki — 2k2 — 2Ar4) cos(w(t 4- hN3/2)) - — (—2ki - k3 4- fc4) cos(cj(t 4- 3/гдгз/4)) - ~(ki 4- k2 4- fc3) cos(cd(t 4- hN3))] 19 - 9437

290 D. Coning Calculation It should be pointed out that, to maintain the requird accu- racy of this approximation, the computer iteration rate must be high enough, so that ДФ remains small while a vehicle is experiencing high rotation rates. Quaternion estimate ДЛ due to the rotation vector in (D.15) is given by соз(ДФ/2) ДФ11/ДФ 8ш(ДФ/2) ДФУ1/ДФ 8ш(ДФ/2) ДФХ1/ДФ зш(ДФ/2) С ДФУ15 ДФг15 Now ДЛ is defined as follows: ДЛ = ДЛ * (ДЛ)-1 or ’ ДА0 ' ДА! ДА0С — 5(—ДА1ДФх1 — ДА2ДФ^1 — ДАзДФ^х) ДАХС - 5(ДА0ДФХ1 - ДА3ДФУ1 + ДА2ДФН) да2 ДА2С — 5(ДА3ДФХ1 + ДА0ДФУ1 — ДА1ДФи) . ДАз . ДА3С — 5(-ДА2ДФХ1 + ДА1 ДФу! + ДА0ДФг1) (D.16) where ДЛ is the true quaternion shown in (Dll). If ДЛ does not contain an error, ДЛ becomes unity. Therefore, the terms representing ДАо, ДА1, ДА2 and ДА3 in (D.16) imply an estimation error. Inspection of ДЛ and ДФ in (D.ll) and (D.15) shows, that ДАг, ДА3, ДФУ1 and ДФП are all periodic with a frequency equal to the coning frequency. Equation (D.16) shows that ДА2 and ДА3 are also periodic, and they contribute a reciprocating error. One should be interested in the non-periodic errors in ДЛ only, because they are the ones, which cause the drift rate error during the quaternion update of q(t 4- h/y3). Such terms appear in ДА0 and ДА1. Since ДФ is assumed to be small, C = 1 and S = 1/2 can be used as an approximation in (D.16). The non-periodic terms ДА0

291 in (D.16) is ДА0С, which equals to one as can be derived from (D.ll) The non-periodic term of ДАХ is (ДА1С — ЗДАоДФ^), which can be reduced to the following form: Д^ = ДА! - |дФх1 (D.17) Quaternion error ДАХ may be written in the quaternion stan- dard form such as ДА1 = ДФХ1/ДФС • 8т(ДФс/2) = 8т(ДФб/2) where ДФХ1/ДФ6 equals to one, since the error is concentrated over xl axis. Variable ДФС is a magnitude of error rotation. If ДФС is small, approximately ДФС = 2ДАТ. Substitution of the re- sult into (D.17) gives ДФС = 2ДА1 - ДФХ1 (D.18) Substituting the values of ДА1 and ДФХ, obtained from (D.ll) and (D.15), into (D 18), one can obtain: ДФе = 28т2(ДФ/2)[а?/1;7з — sin(cjA/v3)] — — зп12(ДФ)[(4&1 — 2k2 -I- 2A:4) sin(td/ijV3/4) + (2fci 4- 4fc2 — — k4) sin(cj/z^3) -t- (—2fc2 + 2&з) sin(3ojh/v3/4) — fc3 sin(cjhjvs)] (D.19) Expanding the sine terms in (D.19) by power series, ( D.19) can be rewritten as ДФе = A$2[(whN3)3(- 1/384){12fci 4- 24fc2 4- 18k3 4- 6fc4 - 32} + +(cuhN3)5(l/122.88){60fci + 360fc2 + 570fc3 + 30fc4 - 512} 4- 4-(tu/iJV3)7(-l/82.575360){252fc1 4-3864fc2 + 12.138fc3 4- 126fc4 - 8192} 4- 4-(w/in3)9(1/95.126814720)[1020fci 4- 37.320fc2 4- 223.29fc3 + 510fc4 - 131.072] (D.20) 19*

292 D. Coning Calculation Now, one can solve the above equation setting it to zero. How- ever, the preceding four equations are dependent. Hence, the coeffi- cients are not uniquely determined. In order to avoid the ambiquity in selection of the above coefficients, the following assumptions can be introduced. Let’s assume that k3 = k4 and provide the strict equality of the first two members in expression (D.20) as the biggest ones. As a result, one can obtain: , 2 8 , 7 fci — -: fco — —: — кл — — 3 15’ 15 Eventually, the final expression for the rotation vector is ^2 2 8 ДФ = ttfc + o(«l X «2) + -(«3 X «4) + —(«1 X Q3) + z' 0 3 10 fc=l (D.21) In a matrix form, the above equation can be represented by 4 2 ДФ = + -(piq(2) +p3a(4)) + fc=i +|(Pi +P2)(«(3) + a(4)) + ^-(p, - P2)(a(3) - й(4)) Z OU (D.22) where 0 -ttxl(j) 0 0 3 = 1,2,3,4 Consequently the four-step algorithm can be used for the rota- tion vector calculation. Time period hN3 is divided into four samples and 12 values of q(j) are applied for the ЛФ calculation.

Appendix E Possibility of Autonomous Mode Improvement for Medium Accuracy INS Using the internal navigation information only, the special error damp- ing procedure can be implemented for a medium accuracy INS. The above innovation leads to the accuracy improvement of the stand alone navigation solution up to the level of a high performance INS. For ex- ample, the accuracy of the stand-alone solution of a Honeywell-1700 (which gyro drift rates in the order of 1—3 deg/hr) can be improved up to 2—2.5 nautical mile. The above approach is effective for the ap- plications with moderate motion of a carrier. The test results of the above innovation for an IMU Honeywell-1700 are shown m Figure E.l. The plots demonstrate the improvement of the position accuracy using the error damping in comparison with the traditional navigation solution. The above technology has been developed in Russia, Moscow by the research group of ’’Teknol” Ltd. company (see Appendix F) and now it is implemented for medium accuracy INS units.

294 E. Possibility of Autonomous Mode Improvement Figure E.l. Comparison of Sole and Damped INS Errors

Appendix F Ultra-Compact MEMS-GPS Navigation System Teknol Ltd develops the integrated (inertial-satellite based) navigation systems for a wide range of land, air and sea applications. Fifteen year experience in designing of the integrated navigation systems and deep understanding of the inertial technology are implemented in the new original algorithms applied in the innovative technology. Teknol Ltd has developed a new line of the integrated navigation systems using low cost sensors for a ultra-compact personal navigator and using a medium accuracy INS for vehicle, airborne and marine applications.

296 F. Companav II CompaMw // ULTRA-COMPACT MEMS-&PS MAI __________________- :_____ Specification Data Sheet ТёШол. #79-2, Warshavskoye shosse, 117556 Moscow RUSSIA +7(095)718-9566 +7(095)718-9577 www. teknol. ru e-mail: contact@teknol.ru TECHNOLOGY & KNOWLEDGE

297 SPECIFICATION О A x ~ йЖ 9 ' Й 1 ' •Й ! M .. -1 J- - I < "!ГЗ?Я??PJ’J:ЯЦ Format Transfer protocol Transfer rate Alignment time ASCII (NMEA-like protocol) RS 232 10 Hz 2... 15 sec (upon operating mode) Environment Operating temperature Shock (20 msec ’/2 sine pulse) -40... +70 °C Light impl. Heavy duty impl. 10 g 20 g MBHM Input voltage Power consumption 12+3VDC >1,5 W Btfr^sical ' Dimensions, mm Weight Data connector Power connector Light impl. Heavy duty impl. 110x84,6x34,6 125x80x57 >0,15 kg >0,25 kg DB9F pin serial connector Car lighter plug

298 F. Companav II PERFORMANCE Attitude 0,2....0,5 Fitch, Roll: Resolution * Static accuracy Dynamic accuracy Low dynamic motion 0,5.. 0,7° High dynamic motion 1*5... 2° Heading Resolution Undisturbed magnetic field 2...3U 10 min of GPS outage 0,2o.0»4° 0,7... Г 0,1* U.2° Position Operating range Angular rate Acceleration Angular attitude Pitch Roll Heading ±150 deg/sec ±2g ±90° ±180° 0...3600

299 POSITION The plots demonstrate operational ability of CompaNav II in prediction during GPS outages in real-time mode. GPS data gaps were simulated for 2...3 minutes during urban area motion. The testing reference trajectory is obtained from clean GPS data. The test results are interpreted in position and time domains. Distance, m

300 F. Companav II ATTITUDE The results of a comparative test using CompaNav II and a high performance INS are presented. The side by side run of two systems demonstrates the attitude determination performance in inertial/GPS integrated mode for standard car motion with accelerations and decelerations (stop and go mode). The achieved accuracy is better then 0.5°. 25000 26000 27000 28000 29000 30000 Time, sec

301 ATTITUDE When GPS is not available Companav II is capable to determine the attitude in stand-alone inertial mode. The accuracy of autonomous attitude determination is about 2° or better. HEIGHT A barometric altimeter provides altitude increment information in real-time. When atmospheric pressure drift is compensated, the altitude accuracy is better then 2 meters. The origin of altimeter measurement assures the more sensitive and less latent data with respect to real-time GPS altitude information.

302 F. Companav II HEADING In the integrated mode CompaNav II provides accurate, not latent heading information The plot demonstrates the heading accuracy of CompaNav II in comparison with high performance INS ( better then 1°). When GPS measurements are not available, CompaNav II maintains effectively relatively high accuracy of the heading information in stand-alone mode

Bibliography [1] Salychev O.S.: Inertial Systems in Navigation and Geophysics, Bauman MSTU Press, 1998. [2] Siouris G.M.: Aerospace Avionics Systems: A modern synthesis, Academic Press, INC, 1993. [3] Britting K.P.: Inertial Navigation Systems Analysis, Wiley- In- terscience, New York, 1971. [4] Broxmeyer C.: Inertial Navigation Systems, McGraw-Hill, New York,1964. [5] Miller R.: A new strapdown attitude algorithm, Journal of Guid- ance, Control and Dinamics, vol.6 July-Aug., 1983. [6] Ignagni M.: Optimal strapdown attitude integration algorithms, Journal of Guidance, Control and Dinamics, vol. 13, #2, March- April, 1990. [7] Gelb A.: Applied optimal estimation, The M.I.T. Press, 1977. [8] Leondes C.T.: Control and dinamic systems, Academic Press, 1976. [9] Jazwinski A.H.: Stochastic Processes and Filtering Theory. Aca- demic Press, New York, 1970. [10] Sage A.P., Melsa J.L.: Estimation Theory, Academic Press, New York, 1971.

[11] Salychev O.S.: Inertial Surveying: ITC ltd. Expirience, Bauman MSTU Press, 1995. [12] Farrell J.A., Barth M.: The Global Positioning System and Iner- tial Navigation, McGraw-Hill, New York, 1998. [13] Jekeli C.: Inertial Navigation Systems with Geodetic Applica- tions, Walter de Gruyter, Berlin, New York, 2001. [14] Salychev O., Cannon M.E., Attitude determination with GPS Aided Inertial navigation system, World Congress of the Interna- tional Association of Institutes of Navigation, San Diego, 2000. [15] Salychev O., Cannon M.E., Low-cost INS/GPS integration: con- cepts and testing, ION, Anaheim, 2000.