Upcoming SlideShare
×

# Fundamentals of inertial navigation, satellite based positioning and their integration

1,519 views

Published on

0 Likes
Statistics
Notes
• Full Name
Comment goes here.

Are you sure you want to Yes No
• Be the first to comment

• Be the first to like this

Views
Total views
1,519
On SlideShare
0
From Embeds
0
Number of Embeds
3
Actions
Shares
0
0
0
Likes
0
Embeds 0
No embeds

No notes for slide

### Fundamentals of inertial navigation, satellite based positioning and their integration

2. 2. 22 2 Basic Navigational Mathematics, Reference Frames 2 3 xk r ¼ 4 yk 5 k ð2:1Þ zk In this notation, the superscript k represents the k-frame, and the elementsðx ; yk ; zk Þ denote the coordinate components in the k-frame. For simplicity, the ksuperscript is omitted from the elements of the vector where the frame is obviousfrom the context.2.1.2 Vector Coordinate TransformationVector transformation from one reference frame to another is frequently needed ininertial navigation computations. This is achieved by a transformation matrix.A matrix is represented by a capital letter which is not written in bold. A vector ofany coordinate frame can be represented into any other frame by making a suitabletransformation. The transformation of a general k-frame vector rk into frame m isgiven as rm ¼ R m rk k ð2:2Þwhere Rm represents the matrix that transforms vector r from the k-frame to the km-frame. For a valid transformation, the superscript of the vector that is to betransformed must match the subscript of the transformation matrix (in effect theycancel each other during the transformation). The inverse of a transformation matrix Rm describes a transformation from the km-frame to the k-frame À ÁÀ1 rk ¼ Rm rm ¼ Rk rm k m ð2:3Þ If the two coordinate frames are mutually orthogonal, their transformationmatrix will also be orthogonal and its inverse is equivalent to its transpose. As allthe computational frames are orthogonal frames of references, the inverse and thetranspose of their transformation matrices are equal. Hence for a transformationmatrix Rm we see that k À ÁT À ÁÀ1 Rm ¼ Rk ¼ R k k m m ð2:4Þ A square matrix (like any transformation matrix) is orthogonal if all of itsvectors are mutually orthogonal. This means that if 2 3 r11 r12 r13 R ¼ 4 r21 r22 r23 5 ð2:5Þ r31 r32 r33
3. 3. 2.1 Basic Navigation Mathematical Techniques 23where 2 3 2 3 2 3 r11 r12 r13 r1 ¼ 4 r21 5; r2 ¼ 4 r22 5; r3 ¼ 4 r23 5 ð2:6Þ r31 r33 r33then for matrix R to be orthogonal the following should be true r1 Á r2 ¼ 0; r1 Á r3 ¼ 0; r2 Á r3 ¼ 0 ð2:7Þ2.1.3 Angular Velocity VectorsThe angular velocity of the rotation of one computational frame about another isrepresented by a three component vector x: The angular velocity of the k-framerelative to the m-frame, as resolved in the p-frame, is represented by xp as mk 2 3 xx xp ¼ 4 xy 5 mk ð2:8Þ xzwhere the subscripts of x denote the direction of rotation (the k-frame with respectto the m-frame) and the superscripts Àdenote theÁ coordinate frame in which thecomponents of the angular velocities xx ; xy ; xz are given. The rotation between two coordinate frames can be performed in two steps andexpressed as the sum of the rotations between two different coordinate frames, asshown in Eq. (2.9). The rotation of the k-frame with respect to the p-frame can beperformed in two steps: ﬁrstly a rotation of the m-frame with respect to thep-frame and then a rotation of the k-frame with respect to the m-frame xk ¼ xk þ xk pk pm mk ð2:9Þ For the above summation to be valid, the inner indices must be the same (tocancel each other) and the vectors to be added or subtracted must be in the samereference frame (i.e. their superscripts must be the same).2.1.4 Skew-Symmetric MatrixThe angular rotation between two reference frames can also be expressed by askew-symmetric matrix instead of a vector. In fact this is sometimes desired inorder to change the cross product of two vectors into the simpler case of matrixmultiplication. A vector and the corresponding skew-symmetric matrix forms of anangular velocity vector xp are denoted as mk
4. 4. 24 2 Basic Navigational Mathematics, Reference Frames 2 3 2 3 xx 0 Àxz xy xp mk ¼ 4 xy 5 ) Xp mk ¼ 4 xz 0 Àxx 5 ð2:10Þ xz Àxy xx 0 |ﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄ{zﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄ} |ﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄ{zﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄ} Angular velocity vector SkewÀsymmetric form of angular the velocity vector Similarly, a velocity vector vp can be represented in skew-symmetric form Vpas 2 3 2 3 vx 0 Àvz vy vp ¼ 4 v y 5 ) Vp ¼ 4 vz 0 Àvx 5 ð2:11Þ vz Àvy vx 0 |ﬄﬄﬄﬄﬄﬄﬄ{zﬄﬄﬄﬄﬄﬄﬄ} |ﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄ{zﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄﬄ} Velocity vector SkewÀsymmetric form of the velocity vector Note that the skew-symmetric matrix is denoted by a non-italicized capitalletter of the corresponding vector.2.1.5 Basic Operations with Skew-Symmetric MatricesSince a vector can be expressed as a corresponding skew-symmetric matrix, therules of matrix operations can be applied to most vector operations. If a, b and c arethree-dimensional vectors with corresponding skew-symmetric matrices A, B andC, then following relationships hold a Á b ¼ a T b ¼ bT a ð2:12Þ a Â b ¼ Ab ¼ BT a ¼ ÀBa ð2:13Þ ½½Ab ¼ AB À BA ð2:14Þ ða Â bÞ Á c ¼ a Á ðb Â cÞ ¼ aT Bc ð2:15Þ a Â ðb Â cÞ ¼ ABc ð2:16Þ ða Â bÞ Â c ¼ ABc À BAc ð2:17Þwhere ½½Ab in Eq. (2.14) depicts the skew-symmetric matrix of vector Ab:2.1.6 Angular Velocity Coordinate TransformationsJust like any other vector, the coordinates of an angular velocity vector can betransformed from one frame to another. Hence the transformation of an angularvelocity vector xmk from the k-frame to the p-frame can be expressed as
5. 5. 2.1 Basic Navigation Mathematical Techniques 25 xp ¼ R p xk mk k mk ð2:18Þ The equivalent transformation between two skew-symmetric matrices has thespecial form Xp ¼ Rp Xk Rk mk k mk p ð2:19Þ2.1.7 Least Squares MethodThe method of least squares is used to solve a set of equations where there aremore equations than the unknowns. The solution minimizes the sum of the squaresof the residual vector. Suppose we want to estimate a vector x of n parametersðx1 ; x2 ; . . .; xn Þ from vector z of m noisy measurements ðz1 ; z2 ; . . .; zm Þ such thatm [ n: The measurement vector is linearly related to parameter x with additiveerror vector e such that z ¼ Hx þ e ð2:20Þwhere H is a known matrix of dimension m Â n; called the design matrix, and it isof rank n (linearly independent row or columns). In the method of least square, the sum of the squares of the components of theresidual vector ðz À HxÞ is minimized in estimating vector x, and is denoted by ^: xHence minimize kz À H^k2 ¼ ðz À H^ÞT ðz À H^ÞmÂ1 x x 1Âm x ð2:21Þ This minimization is achieved by differentiating the above equation withrespect to ^ and setting it to zero. Expanding the above equation gives x kz À H^k2 ¼ zT z À zT H^ À ^T H T z þ ^T H T H^ x x x x x ð2:22Þ Using the following relationships oðxT aÞ oðaT xÞ ¼ ¼ aT ð2:23Þ ox oxand oðxT AxÞ ¼ ðAxÞT þ xT A ð2:24Þ oxthe derivative of the scalar quantity represented by Eq. (2.22) is obtained asfollows
6. 6. 26 2 Basic Navigational Mathematics, Reference Frames o kz À H^k2 x ¼ 0 À zT H À ðH T zÞT þ ðH T H^ÞT þ ^T H T H x x o^ x o kz À H^k2 x ¼ ÀzT H À zT H þ ^T H T H þ ^T H T H x x ð2:25Þ o^ x o kz À H^k2 x ¼ À2ðzT H þ ^T H T HÞ x o^ x To obtain the maximum, the derivative is set to zero and solved for ^ x À2ðzT H À ^T H T HÞ ¼ 0 x ð2:26Þ ^T H T H ¼ z T H x ð^T H T HÞT ¼ ðzT HÞT x ð2:27Þ T T H H^ ¼ H z xand ﬁnally ^ ¼ ðH T HÞÀ1 H T z x ð2:28Þ ^ We can conﬁrm that the above value of x produces the minimum value of thecost function (2.22) by differentiating Eq. (2.25) once more that results in 2H T Hwhich is positive deﬁnite.2.1.8 Linearization of Non-Linear EquationsThe non-linear differential equations of navigation must be linearized in order tobe usable by linear estimation methods such as Kalman ﬁltering. The non-linearsystem is transformed to a linear system whose states are the deviations from thenominal value of the non-linear system. This provides the estimates of the errors inthe states which are added to the estimated state. Suppose we have a non-linear differential equation x ¼ f ðx; tÞ _ ð2:29Þ ~and that we know the nominal solution to this equation is x and we let dx be theerror in the nominal solution, then the new estimated value can be written as x ¼ ~ þ dx x ð2:30Þ The time derivative of the above equation provides _ x _ x ¼ ~ þ dx _ ð2:31Þ _ Substituting the above value of x in the original Eq. (2.29) gives
7. 7. 2.1 Basic Navigation Mathematical Techniques 27 _ ~ þ dx ¼ f ð~ þ dx; tÞ x _ x ð2:32Þ Applying the Taylor series expansion to the right-hand side about the nominalvalue ~ yields x of ðx; tÞ dx þ HOT f ð~ þ dx; tÞ ¼ f ð~; tÞ þ x x ð2:33Þ ox x¼~ xwhere the HOT refers to the higher order terms that have not been considered.Substituting Eq. (2.32) for the left-hand side gives _ þ dx % f ð~; tÞ þ of ðx; tÞ dx ~ x _ x ð2:34Þ ox x¼~ xand since ~ also satisﬁes Eq. (2.29) x _ ~ ¼ f ð~; tÞ x x ð2:35Þsubstituting this in Eq. (2.34) gives _ þ dx % ~ þ of ðx; tÞ dx ~ x _ x _ ð2:36Þ ox x¼~ x The linear differential equations whose states are the errors in the original statesis give as of ðx; tÞ dx dx % _ ð2:37Þ ox x¼~ x After solving this, we get the estimated errors that are added to the estimatedstate in order to get the new estimate of the state.2.2 Coordinate FramesCoordinate frames are used to express the position of a point in relation to somereference. Some useful coordinate frames relevant to navigation and their mutualtransformations are discussed next.2.2.1 Earth-Centered Inertial FrameAn inertial frame is deﬁned to be either stationary in space or moving at constantvelocity (i.e. no acceleration). All inertial sensors produce measurements relativeto an inertial frame resolved along the instrument’s sensitive axis. Furthermore,
8. 8. 28 2 Basic Navigational Mathematics, Reference Frameswe require an inertial frame for the calculation of a satellite’s position and velocityin its orbit around the Earth. The frame of choice for near-Earth environments isthe Earth-centered inertial (ECI) frame. This is shown in Fig. 2.1 and deﬁned1 by(Grewal et al. 2007; Farrell 1998) asa. The origin is at the center of mass of the Earth.b. The z-axis is along axis of the Earth’s rotation through the conventional terrestrial pole (CTP).c. The x-axis is in the equatorial plane pointing towards the vernal equinox.2d. The y-axis completes a right-handed system. In Fig. 2.1, the axes of the ECI frame are depicted with superscript i as xi ; yi ; zi ;and in this book the ECI frame will be referred to as the i-frame.2.2.2 Earth-Centered Earth-Fixed FrameThis frame is similar to the i-frame because it shares the same origin and z-axis asthe i-frame, but it rotates along with the Earth (hence the name Earth-ﬁxed). It isdepicted in Fig. 2.1 along with the i-frame and can be deﬁned asa. The origin is at the center of mass of the Earth.b. The z-axis is through the CTP.c. The x-axis passes through the intersection of the equatorial plane and the reference meridian (i.e. the Greenwich meridian).d. The y-axis completes the right-hand coordinate system in the equatorial plane. In Fig. 2.1 the axes of the Earth-Centered Earth-Fixed Frame (ECEF) are shownas X e ; Y e ; Z e and ðt À t0 Þ represents the elapsed time since reference epoch t0 : Theterm xe represents the Earth’s rotation rate with respect to the inertial frame resolved iein the ECEF frame. In this book the ECEF frame will be referred to as the e-frame.2.2.3 Local-Level FrameA local-level frame (LLF) serves to represent a vehicle’s attitude and velocitywhen on or near the surface of the Earth. This frame is also known as the localgeodetic or navigation frame. A commonly used LLF is deﬁned as follows1 Strictly speaking this deﬁnition does not satisfy the requirement deﬁned earlier for an inertialframe because, in accordance with Kepler’s second law of planetary motion, the Earth does notorbit around the sun at a ﬁxed speed; however, for short periods of time it is satisfactory.2 The vernal equinox is the direction of intersection of the equatorial plane of the Earth with theecliptic (the plane of Earth’s orbit around the sun).
9. 9. 2.2 Coordinate Frames 29Fig. 2.1 An illustration of the ECI and ECEF coordinate framesa. The origin coincides with the center of the sensor frame (origin of inertial sensor triad).b. The y-axis points to true north.c. The x-axis points to east.d. The z-axis completes the right-handed coordinate systems by pointing up, perpendicular to reference ellipsoid. This frame is referred to as ENU since its axes are aligned with the east, north andup directions. This frame is shown in Fig. 2.2. There is another commonly used LLFthat differs from the ENU only in that the z axis completes a left-handed coordinatesystem and therefore points downwards, perpendicular to the reference ellipsoid.This is therefore known as the NED (north, east and down) frame. This book will usethe ENU convention, and the LLF frame will be referred to as the l-frame.2.2.4 Wander FrameIn the l-frame the y-axis always points towards true north, so higher rotation ratesabout the z-axis are required in order to maintain the orientation of the l-frame inthe polar regions (higher latitudes) than near the equator (lower latitudes). As isapparent in Fig. 2.3b, the l-frame must rotate at higher rates to maintain its ori-entation when moving towards the pole, reaching its maximum when it crosses thenorth pole. This rate can even become inﬁnite (a singularity condition) if thel-frame passes directly over the pole. The wander frame avoids higher rotationrates and singularity problems. Instead of always pointing northward, this rotates
10. 10. 30 2 Basic Navigational Mathematics, Reference FramesFig. 2.2 The local-level ENU reference frame in relation to the ECI and ECEF framesFig. 2.3 a The wander frame shown with respect to the local-level frame. b Rotation of they-axis of the local-level frame (shown with red/dark arrows) for a near polar crossing trajectoryat various latitudesabout the z-axis with respect to the l-frame. The angle between the y-axis of thewander frame and north is known as the wander angle a: The rotation rate of thisangle is given as _ _ a ¼ Àk sin u ð2:38Þ The wander frame (in relation to the l-frame) is shown in Fig. 2.3a, and isdeﬁned asa. The origin coincides with the center of the sensor frame (origin of inertial sensor triad).
11. 11. 2.2 Coordinate Frames 31Fig. 2.4 The body frameof a moving platformb. The z-axis is orthogonal to the reference ellipsoid pointing upward.c. The y-axis rotates by an angle a anticlockwise from north.d. The x-axis is orthogonal to the y and z axes and forms a right-handed coor- dinate frame. In this book the wander frame is referred to as the w-frame.2.2.5 Computational FrameFor the ensuing discussion, the computational frame is deﬁned to be any referenceframe used in the implementation of the equations of motion. It can be any of theabovementioned coordinate frames, and is referred to as the k-frame.2.2.6 Body FrameIn most applications, the sensitive axes of the accelerometer sensors are made tocoincide with the axes of the moving platform in which the sensors are mounted.These axes are usually known as the body frame. The body frame used in this book is shown in Fig. 2.4, and is deﬁned asa. The origin usually coincides with the center of gravity of the vehicle (this simpliﬁes derivation of kinematic equations).b. The y-axis points towards the forward direction. It is also called the roll axis as the roll angle is deﬁned around this axis using the right-hand rule.c. The x-axis points towards the transverse direction. It is also called the pitch axis, as the pitch angle corresponds to rotations around this axis using the right- hand rule.d. The z-axis points towards the vertical direction completing a right-handed coordinate system. It is also called the yaw axis as the yaw angle corresponds to rotations around this axis using the right-hand rule. In this book the body frame is referred to as b-frame.
12. 12. 32 2 Basic Navigational Mathematics, Reference FramesFig. 2.5 A depiction of a vehicle’s azimuth, pitch and roll angles. The body axes are shown inred2.2.6.1 Vehicle Attitude DescriptionApart from a vehicle’s position, we are also interested in its orientation in order todescribe its heading and tilt angles. This involves specifying its rotation about thevertical (z), transversal (x) and forward (y) axes of the b-frame with respect tothe l-frame. In general, the rotation angles about the axes of the b-frame are calledthe Euler angles. For the purpose of this book, the following convention is appliedto vehicle attitude angles (Fig. 2.5)a. Azimuth (or yaw): Azimuth is the deviation of the vehicle’s forward (y) axis from north, measured clockwise in the E-N plane. The yaw angle is similar, but is measured counter clockwise from north. In this book, the azimuth angle is denoted by ‘A’ and the yaw angle by ‘y’. Owing to this deﬁnition, the vertical axis of the b-frame is also known as the yaw axis (Fig. 2.4).b. Pitch: This is the angle that the forward (y) axis of the b-frame makes with the E-N plane (i.e. local horizontal) owing to a rotation around its transversal (x) axis. This axis is also called the pitch axis, the pitch angle is denoted by ‘p’ and follows the right-hand rule (Fig. 2.5).c. Roll: This is the rotation of the b-frame about its forward (y) axis, so the forward axis is also called the roll axis and the roll angle is denoted by ‘r’ and follows the right-hand rule.2.2.7 Orbital Coordinate SystemThis is a system of coordinates with Keplerian elements to locate a satellite ininertial space. It is deﬁned as followsa. The origin is located at the focus of an elliptical orbit that coincides with the center of the mass of the Earth.
13. 13. 2.2 Coordinate Frames 33Fig. 2.6 The orbital coordinate system for a satelliteb. The y-axis points towards the descending node, parallel to the minor axis of the orbital ellipse.c. The x-axis points to the perigee (the point in the orbit nearest the Earth’s center) and along the major axis of the elliptical orbit of the satellite.d. The z-axis is orthogonal to the orbital plane. The orbital coordinate system is illustrated in Fig. 2.6. It is mentioned here tocomplete the discussion of the frames used in navigation (it will be discussed ingreater detail in Chap. 3).2.3 Coordinate TransformationsThe techniques for transforming a vector from one coordinate frame into anothercan use direction cosines, rotation (Euler) angles or quaternions. They all involve arotation matrix which is called either the transformation matrix or the directioncosine matrices (DCM), and is represented as Rlk where the subscript represents theframe from which the vector originates and the superscript is the target frame. Forexample, a vector rk in a coordinate frame k can be represented by another vectorrl in a coordinate frame l by applying a rotation matrix Rlk as follows
14. 14. 34 2 Basic Navigational Mathematics, Reference Frames rl ¼ Rlk rk ð2:39Þ If Euler angles are used, these readily yield the elementary matrices required toconstruct the DCM.2.3.1 Euler Angles and Elementary Rotational MatricesA transformation between two coordinate frames can be accomplished by carryingout a rotation about each of the three axes. For example, a transformation from thereference frame a to the new coordinate frame b involves ﬁrst making a rotation ofangle c about the z-axis, then a rotation of an angle b about the new x-axis, andﬁnally a rotation of an angle a about the new y-axis. In these rotations, a; b and care the Euler angles. To transform a vector ra ¼ ½xa ; ya ; za  from frame a to frame d where the twoframes are orientated differently in space, we align frame a with frame d using thethree rotations speciﬁed above, each applying a suitable direction cosine matrix.The individual matrices can be obtained by considering each rotation, one by one. First we consider the x-y plane of frame a in which the projection of vector r(represented by r1 ) makes an angle h1 with the x-axis. We therefore rotate frame aaround its z-axis through an angle c to obtain the intermediate frame b; as illus-trated in Fig. 2.7. According to this ﬁgure, the new coordinates are represented by xb ; yb ; zb andcan be expressed as xb ¼ r1 cosðh1 À cÞ ð2:40Þ yb ¼ r1 sinðh1 À cÞ ð2:41Þ Since the rotation was performed around the z-axis, this remains unchanged zb ¼ za ð2:42Þ Using the following trigonometric identities sinðA Æ BÞ ¼ sin A cos B Æ cos A sin B ð2:43Þ cosðA Æ BÞ ¼ cos A cos B Ç sin A sin B Equations (2.40) and (2.41) can be written as xb ¼ r1 cos h1 cos c þ r1 sin h1 sin c ð2:44Þ yb ¼ r1 sin h1 cos c À r1 cos h1 sin c ð2:45Þ The original coordinates of vector r1 in the x-y plane can be expressed in termsof angle h1 as
15. 15. 2.3 Coordinate Transformations 35Fig. 2.7 The ﬁrst rotation offrame ‘a’ about its za-axis xa ¼ r1 cos h1 ð2:46Þ ya ¼ r1 sin h1 ð2:47Þ Substituting the above values in Eqs. (2.44) and (2.45) produces xb ¼ xa cos c þ ya sin c ð2:48Þ yb ¼ Àxa sin c þ ya cos c ð2:49Þand we have shown that zb ¼ za ð2:50Þ In matrix form, the three equations above can be written as 2 b3 2 32 a 3 x cos c sin c 0 x 4 yb 5 ¼ 4 À sin c cos c 0 54 ya 5 ð2:51Þ zb 0 0 1 za 2 b3 2 a3 x x 4 yb 5 ¼ R b 4 ya 5 ð2:52Þ a zb zawhere Rb is the elementary DCM which transforms the coordinates xa ; ya ; za to axb ; yb ; zb in a frame rotated by an angle c around the z-axis of frame a:
16. 16. 36 2 Basic Navigational Mathematics, Reference FramesFig. 2.8 The second rotationof rotated frame ‘b’ aboutxb-axis For the second rotation, we consider the y-z plane of the new coordinate frame b,and rotate it by an angle b around its x-axis to an intermediate frame c as shown inFig. 2.8. In a similar fashion it can be shown that the new coordinates xc ; yc ; zc can beexpressed in terms of xb ; yb ; zb as follows 2 c3 2 32 b 3 x 1 0 0 x 4 yc 5 ¼ 4 0 cos b sin b 54 yb 5 ð2:53Þ zc 0 À sin b cos b zb 2 c3 2 b3 x x 4 yc 5 ¼ Rc 4 yb 5 ð2:54Þ b zc z bwhere Rc is the elementary DCM which transforms the coordinates xb ; yb ; zb to bxc ; yc ; zc in a frame rotated by an angle b around the x-axis of frame b: For the third rotation, we consider the x-z plane of new coordinate frame c; androtate it by an angle a about its y-axis to align it with coordinate frame d as shownin Fig. 2.9. The ﬁnal coordinates xd ; yd ; zd can be expressed in terms of xc ; yc ; zc as follows 2 d3 2 32 c 3 x cos a 0 À sin a x 4 yd 5 ¼ 4 0 1 0 54 yc 5 ð2:55Þ zd sin a 0 cos a zc
17. 17. 2.3 Coordinate Transformations 37Fig. 2.9 The third rotation ofrotated frame ‘c’ aboutyc-axis 2 3 2 c3 xd x 4 y d 5 ¼ Rd 4 y c 5 ð2:56Þ c zd zcwhere Rd is the elementary DCM which transforms the coordinates xc ; yc ; zc to cxd ; yd ; zd in the ﬁnal desired frame d rotated by an angle a around the y-axis offrame c: We can combine all three rotations by multiplying the cosine matrices into asingle transformation matrix as R d ¼ Rd R c Rb a c b a ð2:57Þ The ﬁnal DCM for these particular set of rotations can be given as 2 32 32 3 cos a 0 À sin a 1 0 0 cos c sin c 0 Rd ¼ 4 0 a 1 0 54 0 cos b sin b 54 À sin c cos c 0 5 ð2:58Þ sin a 0 cos a 0 À sin b cos b 0 0 1 2 3 cos a cos c À sin b sin a sin c cos a sin c þ cos c sin b sin a À cos b sin aRa ¼ 4 d À cos b sin c cos b cos c sin b 5 cos c sin a þ cos a sin b sin c sin a sin c À cos a cos c sin b cos b cos a ð2:59Þ The inverse transformation from frame d to a is therefore
18. 18. 38 2 Basic Navigational Mathematics, Reference Frames À ÁÀ1 À ÁT À ÁT Ra ¼ Rd ¼ Rd ¼ Rd R c Rb d a a c b a À ÁT À ÁT À ÁT ð2:60Þ ¼ Rb R c Rd a b c It should be noted that the ﬁnal transformation matrix is contingent upon theorder of the applied rotations, as is evident from the fact that Rc Rb 6¼ Rb Rc : The b a a border of rotations is dictated by the speciﬁc application. We will see in Sect. 2.3.6that a different order of rotation is required and the elementary matrices aremultiplied in a different order to yield a different ﬁnal transformation matrix. For small values of a; b and c we can use the following approximations cos h % 1 ; sin h % h ð2:61Þ Using these approximations and ignoring the product of the small angles, wecan reduce the DCM to 2 3 1 c Àa 6 7 Rd % 4 Àc 1 a b 5 a Àb 1 2 23 3 1 0 0 0 Àc a ð2:62Þ d 6 7 6 7 Ra ¼ 4 0 1 05À 4 c 0 Àb 5 0 0 1 Àa b 0 Rd a ¼IÀWwhere W is the skew-symmetric matrix for the small Euler angles. For the small-angle approximation, the order of rotation is no longer important since in all casesthe ﬁnal result will always be the matrix of the Eq. (2.62). Similarly, it can beveriﬁed that 2 3T 1 c Àa 6 7 Ra % 4 Àc 1 d b 5 ð2:63Þ a Àb 1 R a ¼ I À WT d2.3.2 Transformation Between ECI and ECEFThe angular velocity vector between the i-frame and the e-frame as a result of therotation of the Earth is xe ¼ ð0; 0; xe ÞT ie ð2:64Þ
19. 19. 2.3 Coordinate Transformations 39Fig. 2.10 Transformationbetween the e-frame and thei-framewhere xe denotes the magnitude of the Earth’s rotation rate. The transformationfrom the i-frame to the e-frame is a simple rotation of the i-frame about the z-axisby an angle xe t where t is the time since the reference epoch (Fig. 2.10). Therotation matrix corresponds to the elementary matrix Rb ; and when denoted Re a iwhich can be expressed as 2 3 cos xe t sin xe t 0 Re ¼ 4 À sin xe t cos xe t 0 5 i ð2:65Þ 0 0 1 Transformation from the e-frame to the i-frame can be achieved through Rie ; theinverse of Re : Since rotation matrices are orthogonal i À ÁÀ1 À ÁT Rie ¼ Re ¼ Re i i ð2:66Þ2.3.3 Transformation Between LLF and ECEFFrom Fig. 2.11 it can be observed that to align the l-frame with the e-frame, thel-frame must be rotated by u À 90 degrees around its x-axis (east direction) andthen by À90 À k degrees about its z-axis (up direction). For the deﬁnition of elementary direction cosine matrices, the transformationfrom the l-frame to the e-frame is Re ¼ Rb ðÀk À 90ÞRc ðu À 90Þ l a b ð2:67Þ
20. 20. 40 2 Basic Navigational Mathematics, Reference FramesFig. 2.11 The LLF inrelation to the ECEF frame 2 32 3 cosðÀk À 90Þ sinðÀk À 90Þ 0 1 0 0Re ¼ 4 À sinðÀk À 90Þ l cosðÀk À 90Þ 0 54 0 cosðu À 90Þ sinðu À 90Þ 5 0 0 1 0 À sinðu À 90Þ cosðu À 90Þ ð2:68Þ 2 32 3 À sin k À cos k 0 1 0 0 Re ¼ 4 cos k À sin k 0 54 0 sin u À cos u 5 l ð2:69Þ 0 0 1 0 cos u sin u 2 3 À sin k À sin u cos k cos u cos k Re ¼ 4 cos k À sin u sin k cos u sin k 5 l ð2:70Þ 0 cos u sin u The reverse transformation is À ÁÀ1 À ÁT Rle ¼ Re ¼ Re l l ð2:71Þ2.3.4 Transformation Between LLF and Wander FrameThe wander frame has a rotation about the z-axis of the l-frame by a wander anglea; as depicted in Fig. 2.12. Thus the transformation matrix from the w-frame frameto the l-frame corresponds to the elementary matrix Rb with an angle Àa; and is aexpressed as
21. 21. 2.3 Coordinate Transformations 41Fig. 2.12 The relationshipbetween the l-frame and thew-frame (the third axes ofthese the frames are notshown because they coincideand point out of the pagetowards the reader) 2 3 cosðÀaÞ sinðÀaÞ 0 Rlw ¼ 4 À sinðÀaÞ cosðÀaÞ 0 5 ð2:72Þ 0 0 1 2 3 cos a À sin a 0 Rlw ¼ 4 sin a cos a 0 5 ð2:73Þ 0 0 1and À ÁÀ1 À ÁT Rw ¼ Rlw ¼ Rlw l ð2:74Þ2.3.5 Transformation Between ECEF and Wander FrameThis transformation is obtained by ﬁrst going from the w-frame to the l-frame andthen from the l-frame to the e-frame Re ¼ Re Rlw w l ð2:75Þ 2 32 3 À sin k À sin u cos k cos u cos k cos a À sin a 0 Re ¼ 4 cos k w À sin u sin k cos u sin k 54 sin a cos a 05 ð2:76Þ 0 cos u sin u 0 0 1
22. 22. 42 2 Basic Navigational Mathematics, Reference Frames 2 3 À sin k cos a À cos k sin u sin a sin k sin a À cos k sin u cos a cos k cos uRe w ¼ 4 cos k cos a À sin k sin u sin a À cos k sin a À sin k sin u cos a sin k cos u 5 cos u sin a cos u cos a sin u ð2:77Þ The inverse is À ÁÀ1 À e ÁT R w ¼ Re e w ¼ Rw ð2:78Þ2.3.6 Transformation Between Body Frame and LLFOne of the important direction cosine matrices is Rlb ; which transforms a vectorfrom the b-frame to the l-frame, a requirement during the mechanization process.This is expressed in terms of yaw, pitch and roll Euler angles. According to thedeﬁnitions of these speciﬁc angles and the elementary direction cosine matrices,Rlb can be expressed as À ÁÀ1 À ÁT À ÁT Rlb ¼ Rb ¼ Rb ¼ Rd Rc Rb l l c b a À ÁT À ÁT À ÁT ð2:79Þ ¼ Rb Rc Rd a b c Substituting the elementary matrices into this equation gives 2 3T 2 3T 2 3T cos y sin y 0 1 0 0 cos r 0 À sin r Rlb ¼ 4 À sin y cos y 0 5 4 0 cos p sin p 5 4 0 1 0 5 0 0 1 0 À sin p cos p sin r 0 cos r ð2:80Þ 2 32 32 3 cos y À sin y 0 1 0 0 cos r 0 sin r Rlb ¼ 4 sin y cos y 0 54 0 cos p À sin p 54 0 1 0 5 ð2:81Þ 0 0 1 0 sin p cos p À sin r 0 cos r 2 3 cos y cos r À sin y sin p sin r À sin y cos p cos y sin r þ sin y sin p cos rRlb ¼ 4 sin y cos r þ cos y sin p sin r cos y cos p sin y sin r À cos y sin p cos r 5 À cos p sin r sin p cos p cos r ð2:82Þwhere ‘p’, ‘r’ and ‘y’ are the pitch, roll and yaw angles. With a known Rlb ; theseangles can be calculated as
23. 23. 2.3 Coordinate Transformations 43 8 9 = À1 Rlb ð3; 2Þ p ¼ tan qﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃ ð2:83Þ Â l Ã Â : R ð1; 2Þ 2 þ Rl ð2; 2Þ 2 Ã ; b b ! À1 Rlb ð1; 2Þ y ¼ À tan ð2:84Þ Rlb ð2; 2Þ ! Rlb ð3; 1Þ r ¼ À tanÀ1 ð2:85Þ Rlb ð3; 3Þ A transformation from the l-frame to the b-frame can be achieved by the inverserotation matrix, Rlb ; as follows À ÁÀ1 À ÁT Rb ¼ Rlb ¼ Rlb l ð2:86Þ2.3.7 Transformation From Body Frame to ECEF and ECI FrameTwo other important transformations are from the b-frame to the e-frame and thei-frames. Their rotation matrices can be computed from those already deﬁned asfollows. For the body frame to the e-frame Re ¼ Re Rlb b l ð2:87Þ For the body frame to the i-frame Rib ¼ Rie Re b ð2:88Þ Their inverses are À ÁÀ1 À ÁT Rb ¼ Re ¼ R e e b b ð2:89Þ À ÁÀ1 À ÁT Rb ¼ Rib ¼ Rib i ð2:90Þ2.3.8 Time Derivative of the Transformation MatrixIf a coordinate reference frame k rotates with angular velocity x relative to anotherframe m, the transformation matrix between the two is composed of a set of time _kvariable functions. The time rate of change of the transformation matrix Rm can bedescribed using a set of differential equations. The frame in which the time dif-ferentiation occurs is usually identiﬁed by the superscript of the variable.
24. 24. 44 2 Basic Navigational Mathematics, Reference Frames At time t, the two frames m and k are related by a DCM Rm ðtÞ: After a short ktime dt; frame k rotates to a new orientation and the new DCM at time t þ dt isRm ðt þ dtÞ: The time derivative of the DCM Rm is therefore k k _k dRm Rm ¼ lim k dt!0 dt ð2:91Þ _k Rm ðt þ dtÞ À Rm ðtÞ Rm ¼ lim k k dt!0 dt The transformation at time t þ dt is the outcome of the transformation up totime t followed by the small change of the m frame that occurs during the briefinterval dt: Hence Rm ðt þ dtÞ can be written as the product of two matrices k Rm ðt þ dtÞ ¼ dRm Rm ðtÞ k k ð2:92Þ From Eq. (2.62), the small angle transformation can be given as dRm ¼ I À Wm ð2:93Þ Substituting Eq. (2.93) into (2.92) gives Rm ðt þ dtÞ ¼ ðI À Wm ÞRm ðtÞ k k ð2:94Þand substituting this back into Eq. (2.91) produces _k ðI À Wm ÞRm ðtÞ À Rm ðtÞ Rm ¼ lim k k dt!0 dt _k ðI À Wm À IÞRm ðtÞ Rm ¼ lim k dt!0 dt ð2:95Þ _k ÀWm Rm ðtÞ Rm ¼ lim k dt!0 dt m _ m ¼ À lim W Rm ðtÞ Rk k dt!0 dt When dt ! 0; Wm =dt is the skew-symmetric form of the angular velocityvector of the m frame with respect to the k frame during the time increment dt: Dueto the limiting process, the angular velocity can also be referenced to the k frame Wm lim ¼ Xm km ð2:96Þ dt!0 dt Substituting Eq. (2.96) into (2.95) gives _k Rm ¼ ÀXm Rm km k ð2:97Þ From the relation Xm ¼ ÀXm ; this becomes km mk Rm ¼ Xm Rm _k mk k ð2:98Þ
25. 25. 2.3 Coordinate Transformations 45 From Eq. (2.19) Xm ¼ Rm Xk Rk mk k mk m ð2:99Þ Substituting this into (2.98) gives Rm ¼ Rm Xk Rk Rm _k k mk m k ð2:100Þ Finally we get the important equation for the rate of change of the DCM as _k Rm ¼ R m X k k mk ð2:101Þ This implies that the time derivative of the rotation matrix is related to theangular velocity vector x of the relative rotation between the two coordinateframes. If we have the initial transformation matrix between the body and inertialframes Rib ; then we can update the change of the rotation matrix using gyroscopeoutput Xb : ib2.3.9 Time Derivative of the Position Vector in the Inertial FrameFor a position vector rb ; the transformation of its coordinates from the b-frame tothe inertial frame is ri ¼ Rib rb ð2:102Þ Differentiating both sides with respect to time leads to ri ¼ Rib rb þ Rib rb _ _ _ ð2:103Þ _ Substituting the value of Rib from Eq. (2.101) into (2.103) gives À Á ri ¼ Rib Xb rb þ Rib rb _ ib _ ð2:104Þ A rearrangement of the terms gives À Á ri ¼ Rib rb þ Xb rb _ _ ib ð2:105Þwhich describes the transformation of the velocity vector from the b-frame to theinertial frame. This is often called the Coriolis equation.2.3.10 Time Derivative of the Velocity Vector in the Inertial FrameThe time derivative of the velocity vector is obtained by differentiating Eq. (2.105)as follows
26. 26. 46 2 Basic Navigational Mathematics, Reference FramesFig. 2.13 A depiction of various surfaces of the Earth À Á _ _ _ ib _ €i ¼ Rib rb þ Rib€b þ Rib Xb rb þ Xb rb þ Xb rb Rib r r ib ib _ ð2:106Þ _ _ _ ib _ €i ¼ Rib rb þ Rib€b þ Rib Xb rb þ Rib Xb rb þ Rib Xb rb r r ib ib _ _ Substituting the value of Rib from Eq. (2.101) yields _ €i ¼ Rib Xb rb þ Rib€b þ Rib Xb Xb rb þ Rib Xb rb þ Rib Xb rb r ib _ r ib ib ib ib _ À b b Á r i i b b b b _ b b € ¼ Rb Xib r þ € þ Xib Xib r þ Xib r þ Xib r _ r b b _ ð2:107Þ À b b Á r i i b b b b € ¼ R 2X r þ € þ X X r þ X r _ r _ b b b ib ib ib iband rearrangement gives À Á _ €i ¼ Rib €b þ 2Xb rb þ Xb rb þ Xb Xb rb ð2:108Þ r r ib _ ib ib ibwhere €b r is the acceleration of the moving object in the b-frame Xbib is the angular velocity of the moving object measured by a gyroscope 2Xb rb ib _ is the Coriolis acceleration _ b rb Xib is the tangential acceleration Xb Xb rb ib ib is the centripetal acceleration2.4 The Geometry of the EarthAlthough the Earth is neither a sphere nor a perfect ellipsoid, it is approximated byan ellipsoid for computational convenience. The ellipsoid and various surfaces thatare useful for understanding the geometry of the Earth’s shape are depicted inFig. 2.13.
27. 27. 2.4 The Geometry of the Earth 472.4.1 Important DeﬁnitionsAt this point, it will be useful to describe some of the important deﬁnitions whichwill assist in understanding the ensuing analysis. For further details, the reader isreferred to (Titterton and Weston 2005; Vanícek and Krakiwsky 1986).• Physical Surface—Terrain’’: This is deﬁned as the interface between the solid and ﬂuid masses of the Earth and its atmosphere. It is the actual surface that we walk or ﬂoat on.• Geometric Figure—Geoid’’: This is the equipotential surface (surface of constant gravity) best ﬁtting the average sea level in the least squares sense (ignoring tides and other dynamical effects in the oceans). It can be thought of as the idealized mean sea level extended over the land portion of the globe. The geoid is a smooth surface but its shape is irregular and it does not provide the simple analytic expression needed for navigational computations.• Reference Ellipsoid—Ellipsoid’’: This mathematically deﬁned surface approximates the geoid by an ellipsoid that is made by rotating an ellipse about its minor axis, which is coincident with the mean rotational axis of the Earth. The center of the ellipsoid is coincident with the Earth’s center of mass. The ellipsoid is the most analytically convenient surface to work with fornavigational purposes. Its shape is deﬁned by two geometric parameters called thesemimajor axis and the semiminor axis. These are typically represented by a and brespectively, as in Fig. 2.14. The geoid height N is the distance along the ellip-soidal normal from the surface of the ellipsoid to the geoid. The orthometric heightH is the distance from the geoid to the point of interest. The geodetic height h (alsoknown as altitude) is the sum of the geoid and orthometric heights (h ¼ H þ N).Various parameter sets have been deﬁned to model the ellipsoid. This book usesthe world geodetic system (WGS)-84 whose deﬁning parameters (Torge 1980;Vanícek and Krakiwsky 1986) are Semimajor axis (equatorial radius) a ¼ 6; 378; 137:0 m Reciprocal ﬂattening 1 ¼ 298:257223563 f Earth’s rotation rate xe ¼ 7:292115 Â 10À5 rad/s Gravitational constant GM ¼ 3:986004418 Â 1014 m3 =s2Other derived parameters of interest are Flatness f ¼ aÀb ¼ 0:00335281 a Semiminor axis b ¼ að1 À f Þ ¼ 6356752.3142 m qﬃﬃﬃﬃﬃﬃﬃﬃﬃ pﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃ ﬃ 2 Àb2 Eccentricity e ¼ a a2 ¼ f ð2 À f Þ ¼ 0:08181919
29. 29. 2.5 Types of Coordinates in the ECEF Frame 49Fig. 2.15 Two types ofECEF coordinates and theirinterrelationship2.5 Types of Coordinates in the ECEF FrameIt is important to distinguish between the two common coordinate systems of thee-frame, known as the ‘rectangular’ and ‘geodetic’ systems.2.5.1 Rectangular Coordinates in the ECEF FrameRectangular coordinates are like traditional Cartesian coordinates, and representthe position of a point with its x, y and z vector components aligned parallel to thecorresponding e-frame axes (Fig. 2.15).2.5.2 Geodetic Coordinates in the ECEF FrameGeodetic (also referred to as ellipsoidal or curvilinear) coordinates are deﬁned in away that is more intuitive for positioning applications on or near the Earth. Thesecoordinates are deﬁned (Farrell 2008) asa. Latitude ðuÞ is the angle in the meridian plane from the equatorial plane to the ellipsoidal normal at the point of interest.b. Longitude ðkÞ is the angle in the equatorial plane from the prime meridian to the projection of the point of interest onto the equatorial plane.
30. 30. 50 2 Basic Navigational Mathematics, Reference Framesc. Altitude h is the distance along the ellipsoidal normal between the surface of the ellipsoid and the point of interest. The two types of e-frame coordinates and their interrelationship are illustratedin Fig. 2.15.2.5.3 Conversion From Geodetic to Rectangular Coordinates in the ECEF FrameIn navigation, it is often necessary to convert from geodetic e-frame coordinates torectangular e-frame coordinates. The following relationship (see Appendix B for aderivation) accomplishes this 2 e3 2 3 x ðRN þ hÞ cos u cos k 6 e 7 4 ðR þ hÞ cos u sin k 5 4y 5 ¼ È N É ð2:111Þ z e RN ð1 À e2 Þ þ h sin uwhere ðxe ; ye ; ze Þ are the e-frame rectangular coordinates RN is the normal radius h is the ellipsoidal height k is the longitude u is the latitude e is the eccentricity.2.5.4 Conversion From Rectangular to Geodetic Coordinates in the ECEF FrameConverting rectangular to geodetic coordinates is not straightforward, because theanalytical solution results in a fourth-order equation. There are approximate closedform solutions but an iterative scheme is usually employed.2.5.4.1 Closed-Form AlgorithmThis section will describe a closed form algorithm to calculate e-frame geodeticcoordinates directly from e-frame rectangular coordinates though series expansion(Hofmann-Wellenhof et al. 2008). An alternate method is detailed in Appendix C.
31. 31. 2.5 Types of Coordinates in the ECEF Frame 51Longitude is ye k ¼ 2 arctan qﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃ ð2:112Þ xe þ ðxe Þ2 þðye Þ2latitude is ze þ ðe0 Þ2 b sin3 h u ¼ arctan ð2:113Þ p À e2 a cos3 hwhere ze a h ¼ arctan pb rﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃ a2 À b2 e0 ¼ b2 qﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃ p ¼ ðxe Þ2 þðye Þ2and height is p h¼ ÀN ð2:114Þ cos uwhere a2 N ¼ pﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃ a2 cos2 u þ b2 sin2 u2.5.4.2 Iterative AlgorithmThe iterative algorithm is derived in Appendix D and implemented by taking thefollowing stepsa. Initialize the altitude as h0 ¼ 0 ð2:115Þb. Choose an arbitrary value of latitude either from a previous measurement (if one is available) or by using the approximation ! À1 ze u0 ¼ tan ð2:116Þ Pe ð 1 À e 2 Þ
32. 32. 52 2 Basic Navigational Mathematics, Reference Framesc. The geodetic longitude is calculated as e yÀ1 k ¼ tan ð2:117Þ xed. Starting from i ¼ 1; iterate as follows a RNi ¼ À Á1=2 ð2:118Þ 1 À e2 sin2 uiÀ1 qﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃ ðxe Þ2 þ ðye Þ2 hi ¼ À R Ni ð2:119Þ cos uiÀ1 8 9 À1 z e ð R Ni þ h i Þ = ui ¼ tan qﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃ Á ð2:120Þ : ðxe Þ2 þðye Þ2 RNi ð1 À e Þ þ hi 2 ;e. Compare ui ; uiÀ1 and hi ; hiÀ1 ; if convergence has been achieved then stop, otherwise repeat step d using the new values.2.6 Earth GravityThe gravity ﬁeld vector is different from the gravitational ﬁeld vector. Due to theEarth’s rotation, the gravity ﬁeld is used more frequently and is deﬁned as g ¼ À Xie Xie r g ð2:121Þwhere is the gravitational vector, Xie is the skew-symmetric representation of the gEarth’s rotation vector xie with respect to the i-frame, and r is the geocentricposition vector. The second term in the above equation denotes the centripetalacceleration due to the rotation of the Earth around its axis. Usually, the gravityvector is given in the l-frame. Because the normal gravity vector on the ellipsoidcoincides with the ellipsoidal normal, the east and the north components of thenormal gravity vector are zero and only third component is non-zero gl ¼ ½ 0 0 Àg T ð2:122Þ
33. 33. 2.6 Earth Gravity 53Fig. 2.16 A meridian cross section of the reference ellipsoid containing the projection of thepoint of interest P The magnitude of the normal gravity vector over the surface of the ellipsoid canbe computed as a function of latitude and height by a closed form expressionknown as the Somigliana formula (Schwarz and Wei Jan 1999), which is À Á À Á c ¼ a1 1 þ a2 sin2 u þ a3 sin4 u þ a4 þ a5 sin2 u h þ a6 h2 ð2:123Þwhere h is the height above the Earth’s surface and the coefﬁcients a1 through a6for the 1980 geographic reference system (GRS) are deﬁned as a1 ¼ 9:7803267714 m/s2 ; a4 ¼ À 0:0000030876910891=s2 ; a2 ¼0:0052790414; a5 ¼0:0000000043977311=s2 ; a3 ¼0:0000232718; a6 ¼ 0:0000000000007211=ms2Appendix ADerivation of Meridian Radius and Normal RadiusFor Earth ellipsoids, every meridian is an ellipse with equatorial radius a (calledthe semimajor axis) and polar radius b (called the semiminor axis). Figure 2.16shows a meridian cross section of one such ellipse (Rogers 2007). This ellipse can be described by the equation w 2 z2 þ ¼1 ð2:124Þ a2 b2and the slope of the tangent to point P can be derived by differentiation
34. 34. 54 2 Basic Navigational Mathematics, Reference Frames 2wdw 2zdz þ 2 ¼0 ð2:125Þ a2 b dz b2 w ¼À 2 ð2:126Þ dw az An inspection of the Fig. 2.16 shows that the derivative of the curve at point P,which is equal to the slope of the tangent to the curve at that point, is dz p ¼ tan þ u ð2:127Þ dw 2 À Á dz sin p þ u cos u 1 ¼ À2 Á¼ ¼À ð2:128Þ dw cos p þ u 2 À sin u tan u Therefore 1 b2 w ¼ 2 ð2:129Þ tan u a z From the deﬁnition of eccentricity, we have b2 e2 ¼ 1 À a2 ð2:130Þ b2 ¼ 1 À e2 a2and Eq. (2.129) becomes À Á z ¼ w 1 À e2 tan u ð2:131Þ The ellipse described by Eq. (2.124) gives 2 2 z2 w ¼a 1À 2 ð2:132Þ b Substituting the value of z from Eq. (2.131) yields 2 ! a2 w2 ð1 À e2 Þ tan2 u w2 ¼ a2 À b2 2 a2 w2 ð1 À e2 Þ tan2 u w2 þ ¼ a2 b2 ! 2 b2 þ a2 ð1 À e2 Þ tan2 u w2 ¼ a2 b2
35. 35. 2.6 Earth Gravity 55 a2 b2 w2 ¼ b2 þ a2 ð1 À e2 Þ2 tan2 u a2 ½a2 ð1 À e2 Þ w2 ¼ a2 ð1 À e2 Þ þ a2 ð1 À e2 Þ2 tan2 u a2 w2 ¼ 1 þ ð1 À e2 Þ tan2 u ð2:133Þ a2 w2 ¼ sin2 1 þ ð1 À e2 Þ cos2 u u a2 cos2 u w2 ¼ cos2 u þ ð1 À e2 Þ sin2 u a2 cos2 u w2 ¼ 1 À e2 sin2 u a cos u w¼À Á1 ð2:134Þ 1 À e2 sin2 u 2 Substituting this expression for w in Eq. (2.131) produces að1 À e2 Þ sin u z¼ ð2:135Þ À Á1 2 1 À e2 sin2 uwhich will be used later to derive the meridian radius. It can easily be proved from Fig. 2.16 that w ¼ RN cos u ð2:136Þ From this and Eq. (2.134) we have the expression for the normal radius, RN ;also known as the radius of curvature in the prime vertical a RN ¼ ð2:137Þ À 2 Á1 2 1À e2 sin u The radius of curvature of an arc of constant longitude is h À dz Á2 i3 2 1þ dw RM ¼ d2 ð2:138Þ Æ dwz2
36. 36. 56 2 Basic Navigational Mathematics, Reference Frames The second derivative of Eq. (2.126) provides À1=2 dz b2 w b2 w 2 b2 w 2 ¼À 2 ¼À 2 b À 2 dw az a a 2 2 2 2 À1=2 À3=2 2 d z b 2 bw b2 w 1 2 b2 w2 b ¼À 2 b À 2 À 2 À b À 2 À 2 2w dw2 a a a 2 a a 2 2 4 2 d z b b w 2 ¼À 2 À 4 3 dw a z a z 2 2 2 b2 w2 4 2 d z Àb2 a2 z2 À b4 w2 Àb a b À a2 À b w 2 ¼ ¼ dw2 a4 z 3 a4 z 3 2 4 2 4 2 4 2 d z Àb a þ b w À b w ¼ dw2 a4 z3which simpliﬁes to d2 z b4 ¼À 2 3 ð2:139Þ dw2 az Substituting Eqs. (2.139) and (2.126) into (2.138) yields h 4 2 i3 2 1 þ b 4w2 a z RM ¼ b4 ð2:140Þ a2 z3 b2and since a2 ¼ 1 À e2 2 !3 ð1 À e 2 Þ w2 2 1þ z2 RM ¼ b 2 ð 1 À e2 Þ z3 2 !3 z 2 þ ð 1 À e2 Þ w 2 2 z2 RM ¼ b2 ð1 À e2 Þ z3 h i3 2 2 z2 þ ð1 À e2 Þ w2 RM ¼ ð2:141Þ b2 ð 1 À e 2 Þ
37. 37. 2.6 Earth Gravity 57 Substituting the value of w from Eq. (2.134) and z from (2.135) gives 2 !3 a2 ð1 À e2 Þ sin2 u 2 a2 2 2 1 À e2 sin2 u þ ð1 À e2 Þ 1 À ecos uu 2 sin2 RM ¼ b2 ð 1 À e 2 Þ 2 2 !3 a2 ð1 À e2 Þ sin2 uþð1 À e2 Þ a2 cos2 u 2 1 À e2 sin2 u RM ¼ b2 ð1 À e2 Þ h Ái 3 2À 2 a2 ð1 À e2 Þ sin2 u þ cos2 u RM ¼ À Á3 b2 ð1 À e2 Þ 1 À e2 sin2 u 2 3 a3 ð 1 À e 2 Þ RM ¼ À Á3 b2 ð1 À e2 Þ 1 À e2 sin2 u 2 3 að 1 À e 2 Þ RM ¼ À Á3 ð1 À e2 Þð1 À e2 Þ 1 À e2 sin2 u 2leading to the meridian radius of curvature að1 À e2 Þ RM ¼ À Á3 ð2:142Þ 1 À e2 sin2 u 2Appendix BDerivation of the Conversion Equations From Geodeticto Rectangular Coordinates in the ECEF FrameFigure 2.17 shows the relationship between geodetic and rectangular coordinatesin the ECEF frame. It is evident that rP ¼ rQ þ hn ð2:143Þwhere 2 3 2 3 RN cos u cos k cos u cos k rQ ¼ 4 RN cos u sin k 5 ¼ RN 4 cos u sin k 5 ð2:144Þ RN sin u À RN e2 sin u sin u À RN e2 sin u
38. 38. 58 2 Basic Navigational Mathematics, Reference FramesFig. 2.17 The relationship between geodetic and rectangular coordinates in the ECEF frame Also, the unit vector along the ellipsoidal normal is 2 3 cos u cos k n ¼ 4 cos u sin k 5 ð2:145Þ sin u Substituting Eqs. (2.144) and (2.145) into (2.143) gives 2 3 2 3 cos u cos k cos u cos k rP ¼ R N 4 cos u sin k 5 þ h4 cos u sin k 5 ð2:146Þ sin u À RN e2 sin u sin u 2 3 ðRN þ hÞ cos u cos k rP ¼ 4 È ðRN þ hÞ cos u É k 5 sin ð2:147Þ RN ð1 À e2 Þ þ h sin u
39. 39. 2.6 Earth Gravity 59Fig. 2.18 Reference diagram for conversion of rectangular coordinates to geodetic coordinatesin the ECEF frame through a closed-form method 2 2 3 3 xe ðRN þ hÞ cos u cos k 6 e 7 4 ðR þ hÞ cos u sin k 5 4y 5 ¼ È N É ð2:148Þ ze RN ð1 À e2 Þ þ h sin uAppendix CDerivation of Closed Form Equations From Rectangularto Geodetic Coordinates in the ECEF FrameHere we derive a closed form algorithm which uses a series expansion (Schwarzand Wei Jan 1999) to compute e-frame geodetic coordinates directly from e-framerectangular coordinates. From Fig. 2.18a it can is evident that, for given rectangular coordinates, thecalculation of geocentric coordinates ðk; u0 ; r Þ is simply qﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃ r ¼ ðxe Þ2 þðye Þ2 þðze Þ2 ð2:149Þ
40. 40. 60 2 Basic Navigational Mathematics, Reference Frames e y À1 k ¼ tan ð2:150Þ xe e z u0 ¼ sinÀ1 ð2:151Þ r From the triangle POC (elaborated in Fig. 2.18b) it is apparent that the dif-ference between the geocentric latitude u0 and the geodetic latitude u is p p D þ u0 þ þ Àu ¼p 2 2 D þ u0 þ p À u ¼ p ð2:152Þ D ¼ u À u0where D is the angle between the ellipsoidal normal at Q and the normal to thesphere at P: Applying the law of sines to the triangle in Fig. 2.18b provides À Á sin D sin p À u 2 ¼ RN e2 sin u r sin D cos u ¼ RN e2 sin u r 2 ð2:153Þ RN e sin u cos u sin D ¼ r À1 RN e2 sin u cos u D ¼ sin r 21 À1 RN e 2 sin 2u D ¼ sin ð2:154Þ r Substituting the deﬁnition of the normal radius RN given in Eq. (2.137) a RN ¼ À Á1=2 ð2:155Þ 1 À e2 sin2 uinto Eq. (2.154) gives 0 a 21 1 1=2 e 2 sin 2u ð 1Àe2 sin uÞ 2 D ¼ sinÀ1 @ A ð2:156Þ r ! À1 k sin 2u D ¼ sin À Á1=2 ð2:157Þ 1 À e2 sin2 uwhere
41. 41. 2.6 Earth Gravity 61 e2 a k¼ ð2:158Þ 2 To achieve a ﬁrst approximation, if u ¼ u0 then D can be computed by usingthe geocentric latitude ! À1 k sin 2u0 Dc ¼ sin À Á1=2 ð2:159Þ 1 À e2 sin2 u0 Expanding Eq. (2.157) for this approximation, Dc ; gives ðu À u0 Þ2 D ¼ Dðu ¼ u0 Þ þ D0 ðuÞju¼u0 ðu À u0 Þ þ D00 ðuÞju¼u0 2! ðu À u 0 Þ3 þ D000 ðuÞju¼u0 þ ... ð2:160Þ 3! 1 00 0 2 1 000 0 3 D ¼ Dc þ D0 ðu0 ÞD þ D ðu ÞD þ D ðu ÞD þ . . . ð2:161Þ 2! 3! For a very small value of D (less than 0.005), it can be assumed that RN ’ r;and hence e2 D¼ sin 2u ð2:162Þ 2 2 So for this situation, let k ¼ e2 so that D ¼ k sin 2u ð2:163Þ The series used above can therefore be truncated after the fourth-order term andbe considered as a polynomial equation. Solving this polynomial provides Dc D¼ ð2:164Þ 1 À 2k cos 2u0 þ 2k2 sin2 u0 The geodetic latitude can be given as ze u ¼ u0 þ D ¼ sinÀ1 þD ð2:165Þ r The geodetic longitude is the same as shown earlier e y k ¼ tanÀ1 e ð2:166Þ x The ellipsoidal height is calculated from the triangle PRC of Fig. 2.18a Pe ¼ ðRN þ hÞ cos u ð2:167Þ
42. 42. 62 2 Basic Navigational Mathematics, Reference Frames Pe h¼ À RN ð2:168Þ cos u qﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃ ðxe Þ2 þðye Þ2 h¼ À RN ð2:169Þ cos u Equations (2.165), (2.166) and (2.169) are therefore closed-form expressions toconvert from rectangular coordinates to geodetic coordinates in the e-frame.Appendix DDerivation of the Iterative Equations From Rectangularto Geodetic Coordinates in the ECEF FrameFrom Eq. (2.148), which relates geodetic and rectangular coordinates in thee-frame, we see that xe ¼ ðRN þ hÞ cos u cos k ð2:170Þ ye ¼ ðRN þ hÞ cos u sin k ð2:171Þ È À Á É ze ¼ RN 1 À e2 þ h sin u ð2:172Þ Given the rectangular coordinates and these equations, the geodetic longitude is e y k ¼ tanÀ1 e ð2:173Þ x Equation (2.169) speciﬁes the relationship for the altitude as qﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃ ðxe Þ2 þðye Þ2 h¼ À RN ð2:174Þ cos u Also, from Eqs. (2.170) and (2.171) it can be shown that Â Ã ðxe Þ2 þðye Þ2 ¼ ðRN þ hÞ2 cos u2 cos2 k þ sin2 k ð2:175Þ qﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃ ðxe Þ2 þðye Þ2 ¼ ðRN þ hÞ cos u ð2:176Þ And ﬁnally, dividing Eq. (2.172) by Eq. (2.176) yields ze ½ RN ð 1 À e 2 Þ þ h  qﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃ ¼ tan u ð RN þ hÞ ðxe Þ2 þðye Þ2
43. 43. 2.6 Earth Gravity 63 8 9 e = z ð RN þ h Þ u ¼ tanÀ1 qﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃﬃ ð2:177Þ :½RN ð1 À e2 Þ þ h ðxe Þ2 þðye Þ2 ;ReferencesChatﬁeld AB (1997) Fundamentals of high accuracy inertial navigation. American Institute of Aeronautics and Astronautics, RestonFarrell JA (1998) The global positioning system and inertial navigation. McGraw-Hill, New YorkFarrell JA (2008) Aided navigation : GPS with high rate sensors. McGraw-Hill, New YorkGrewal MS, Weill LR, Andrews AP (2007) Global positioning systems, inertial navigation, and integration, 2nd edn. Wiley, New YorkHofmann-Wellenhof B, Lichtenegger H, Wasle E (2008) GNSS-global navigation satellite systems : GPS, GLONASS, Galileo, and more. Springer, New YorkRogers RM (2007) Applied mathematics in integrated navigation systems, 3rd edn. American Institute of Aeronautics and Astronautics, RestonSchwarz KP, Wei M (Jan 1999) ENGO 623: Partial lecture on INS/GPS integration for geodetic applications University of Calgary, Department of Geomatics EngineeringTitterton D, Weston J (2005) Strapdown inertial navigation technology IEE radar, sonar, navigation and avionics series, 2nd edn. AIAATorge W (1980) Geodesy: an introduction. De Gruyter, BerlinVanícek P, Krakiwsky EJ (1986) Geodesy, the concepts. North Holland; Sole distributors for the U.S.A. and Canada. Elsevier Science Pub. Co., Amsterdam
44. 44. http://www.springer.com/978-3-642-30465-1