Coordinate System Conversion Software User's Guide for Attitude, Quaternion, M50 and Related Applications

Manual text content

AD-A234 455 GL.IR-90-0327 COORDINATE SYSTEM CONVERSION SOFTWARE USER'S GUIDE FOR AITITUDE, QUATERNION, M50 AND RELATED APPLICATIONS M. J. Kendra N. A. Bonito Radex, Inc. -Three Preston Court Bedford, MA 01730 November 30, 1990 DTIC ,-*ELECTIn Scientific Report No. APROSD Approved for public release; distribution unlimited GEOPHYSICS LABORATORY AIR FORCE SYSTEMS COMMAND UNITED STATES AIR FORCE HANSCOM AIR FORCE BASE, MASSACHUSETIS 01731-5000 _ __ 91 4 05 089 "KThis technical report has been reviewed and is approved for publi-ation" EDWARD C. ROBINSON ROW•T Ea MINERNI¥, Chief Contract Manager Data Systems Branch Data Systcias Branch Aerospace EngineerinrqDivisiorn Aerospace Engineering Division FOR THE COMMANDER ZC. NEALON STARK, Director Aerospace Engineering Division / This report has been reviewed by the ESD Public Affairs Office (PA) and is releasable to the National Technical Information Service (NTIS). Qualified requestors may obtain additional copies from the Defense Technical Information Center. All others should apply to the National Technical Information Service. If your address has changed, or if you wish to be removed from the mailing list, or if the addressee is no longer employed by your organization, please notify GL/IMA, Hanscom AFB, MA 01731. This will assist us in maintaining a current mailing list. Do not return copies of this report unless contractual obligations or notices on a specific document requires that it be returned. Unclassified S~ICRITY CLASSIFICTION OF THIS P~ff REPOR(T DOCUMENTATION PAGE la. REPO'RT SECURITY CLASSIFICATION lb. RESTRICTIVE MARKINGS Unclassified____________________________ 2a. SECURITY CLASSIFICATION AUTHORITY 3. DISTRIBUTION/ AVAILABILITY OF REPORT ___________________________________ Approved for Public Release 2b, DECLASSIF ICAIO10N I DOWNGRADING SCHEDULE D~istribution Unlimited 4. PERFORMING ORGANIZATION REPORT NUMBER(S) S. MONITORING ORGANIZATION REPORT NUMBER(S) RX-R-901 11 OL-TR-90-0327 6 a. NAME OF PERFORMING ORGANIZATION 6b. OFFICE SYMBOL 7a. NAME OF MONITORING ORGANIZATION RADEX, Inc. (it applicable) Geophysics Laboratory ,6c. ADDRESS (City, State, and ZIP Code) 7b. ADDRESS (City, State, and ZIP Code) Three Preston Court Hanscom AFB Bediford, MA 01730) Massachusetts 01731-SW0O 8.. NAME OF FUNDING/ SPONSORING 8 b. OFFICE SYMBOL 9. PROCUREMENT INSTRUMENT IDENTIFICATION NUMBER OR AIZAION(if ppi cole)Contract F1962&-89-C-006 8c. ADDRESS (City, State, and ZIP Code) 10, SOURCE OF FUNDING NUMBERS PROGRAM IPROJECT TASK WORK UNIT ELEMENT NO. NO. NO. IACCESSION NO. I 62101F 7659 05 A 11. TITLE (include Security Classification) Coordinate System Conversion S9oftware User's.G~uide for Attitude, Quaternion, M50 and Related Applicati3ns 12. PERSONAL AUTHOR(S) M. Kendra, N. Bonito 13 TP POT13b. TIME C%)MED TO 1101.DA1 fF PORT fea~fonth, Day) IS PA~f6 COUNT 16. SUPPLEMENTARY NOTATION 17. COSATI CODES 18. SUBJECT TERMS (Continue on reverse if necessary and Identify by block number) FIELD GROUP I SUB-GROUP IQuaternions, transformations, attitude, PATH, rotations, matrices, M50, true-of-date.. coc.rdinate system conversion software, CS(S 19. ABSTRACT (Continue on reverse if necessary and identify by block number) The Coordinate System Conversion Software is a collection of general purpose subroutines and functions which may be usec to transform quantities from one coordinate system to another. The report describes these routines and gives several simpi examples of their use. A section is devoted to derivations using NASA quaternion operations. These differ from what L used in other literature. 20. DISTRIBUTION /AVAILABILITY OF ABSTRACT 21. ABSTRACT SECURITY CLASSIFICATION [3UNCLASSI FIEDiUNLl MITEO D E SAME AS RPT. C3 DTIC USERS Unclassified 22a. NAME OF RESPONSIBLE INDIVIDUAL. 22b, TELEPHONE (nld raCd)2c FIESMO E. C. Robinson (617)377-34 GLILCY DO FORM 1473,84 MAR 83 APR edition may be used until exhausted. SECURITY CLASSIFICATION OF THIS PAGE All other editions are obsolete. Unclassified TABLE OF CONTENTS 1 1. INTRODUCTION ..................................................... .1 t. NOTES ON THE USE OF QUATERNIONS ..................................... 2 3. DERIVATIONS OF COORDINATE SYSTEM TRANSFORMATIONS ................. 2 3.1 Conversion of a Quaternion into a Rotation Matrix ......................... 3 3.2 Conversion of a Rotation Matrix into a Quaternion ......................... 4 3.3 -he Product of Two Quaternions ...................................... 5 3.4 Rotate an Axis to be Collinear with a Vector .............................. 7 4. GENERAL DESCRIPTION ................................................ 9 4.1 QUAT.FOR ..................................................... 9 4.2 M50TOT.FOR ................................................... 10 5. EXAM PLES ............................................................ 11 5.1 Exam ple 1 ....................................................... 11 5.2 Exam ple 2 ....................................................... 13 5.3 Exam ple 3 ....................................................... 15 5.4 Example 4 ..... .................................................. 17 6. REFERENCES ......................................................... 22 LIST OF TABLES Table 1. STS Measurement/Stimuli Identification for Atlantis ......................... 20 A00088ion For . . . . O T IS G RA. & I ( DTIC TAB ; Unannounoed C0 Juatifiaation• By .. Distribution/ Availability Codes ilt I SpeoAel 1. INTRODUCTION The Coordinate Systems Conversion Software (CSCS) is a collection of general purpose subroutines and functions which may be used to transform vector quantities from one coordinate system to another. It is intended for use by persons with a good working knowledge of the coordinate systems and parameters of interest. The routines are low level in the sense that a single call is only a single step in the conversion process. By combining several steps, however, the user will find the CSCS to be a powerful, general purpose tool which is capable of solving a wide variety of problems. The CSCS collection contains routines which fall into five categories: quaternion conversion, rotation matrix construction, attitude manipulation, M50 to Aries true-of-date conversion, and vector manipulation. Some routines may fall entirely within a single category. Others span multiple categories, thus forming a bridge between them. By using such bridges the user may, for example, convert a quaternion into a rotation matrix, which is then used to manipulate a vector. The following sections describe the routines in the collection and give several example programs to demonstrate their application. Attitude sequences, quaternions, and rotation matrices are used by the space community to store and transmit coordinate system conversion information. Each method has its own distinct advantages and disadvantages, and the adoption of one method in preference to another takes such considerations into account. In theory, attitude sequences contain all of the conversion information in the form of three rotation angles. They are easy to understand in human terms, and translate easily into motions such as turning one's body, looking up, and tilting one's head. If the sequence of rotations is not known, however, their meaning is lost. In addition, they must be converted using trigonometric functions before they can be used for vector analysis. Quaternions store the conversion information as four unambiguous numbers. They may be converted to a form suitable for vector analysis somewhat more easily than attitude sequences. Thus they are favored in situations where speed and machine storage considerations dominate. They are not easy to understand in human terms and cannot be used directly in vector computations. Rotation matrices consist of a 3x3 array of nine numbers. They can be used directly in vector multiplication and thus are preferred for data analysis using computing machines. The CSCS is capable of working with data in any of these forms and converting it to any of the other forms to meet the needs of the user. In selecting a reference ECI coordinate system the space community has commonly employed two different systems, the M50 and the Aries true-of-date. A particular mission will generally maintain orbital and ephemeris information in one reference system but not the other. The CSCS provides the ability to convert data to the other ECI system for any date and time selected by the user. The M2000 system is likely to become more prevalent in the future, and the M50 conversion procedures may easily be modified for application to M2000. Finally, the CSCS contains vector manipulation routines. One routine allows the user to convert vectors from one coordinate system to another. A different routine gives the user the ability to reverse the process; that is, to align an axis with a vector and obtain the associated rotation matrix. With this capability the CSCS becomes a powerful tool which lets the user construct coordinate systems of interest together with their related inter-conversion. 1 The Coordinate System Conversion Software described in this document was developed for IBM PC compatible systems using Microsoft FORTRAN. All floating point variables and calculations are double precision to ensure high accuracy. Constants, such as pi, are defined by the operating environment whenever possible. ANSI FORTRAN-77 standards were adhered to so that the code may be transported to other environments with a minimum of conversion effort. 2. NOTES ON THE USE OF QUATERNIONS Two references were used in the development of the software contained in this document. The NASA PATH documentation [Cooper, et al., 1985] was referred to for all routines which use quaternions. Escobal [1965] was referred to for routines which involve attitude conversions and coordinate system rotations. The Appendices of Escobal were especially helpful in this respect. Escobal also discusses quaternions in great detail and relates them to these other areas. NASA and Escobal have defined the order of operations differently, however, and the unwary user will quickly fall victim to this unfortunate circumstance. The NASA definition, V - q V q*, works just as well as that of Escobal, V - q* V q, as long as one does not mix conventions. SIn the development of the quaternion routines presented here the NASA convention was followed. Much of the material discussed by Escobal is still relevant, but the derivations look quite different in the end. If an Escobal convention quaternion is encountered, it is recommended that the user convert it to its conjugate before proceeding. This may be done with the QTPOSE routine, which will return a quaternion consistent with the NASA convention. 3. DERIVATIONS OF COORDINATE SYSTEM TRANSFORMATIONS We introduce the quaternion, g in terms of four parameters which are components of f as follows: 4" (q 0 l,, 2,q3 ). (1) We also define the multiplication of two quaternions, z = xy [HoUinger, 1989] (2) It is sometimes more convenient to think of quaternions as being comprised of scalar and vector components. With scalar component q0 and vector components q1i + qj + q3k, we define the quaternion q [Escobal, 1965] as f - q 0 +q1i+q 2j+q3k (3) 2 where i, j, and k are the hypeiimaginaay numbers satisfying the conditions i2*.i2 P . -- 1 i. -- #- k (4) jk -kj - i ki. -ik j. Using this definition, it is now possible to carry out quaternion multiplication in the same manner as polynomial multiplication. We must take care, however, to preserve the order of operations, since the hyperimaginary numbers clearly do not commute. We now define the conjugate of a quaternion as q* . Oqiq_~.(5) I * q 0 -q 1 i-q~j-q 3k. 5 We note that a vector in three-dimensional space with components qli+ q2j+ q3k may be expressed as a quaternion with a scalar component of zero in quaternion space q - O+qli+q 2i+q 3k. (6) 3.1 CONVERSION OF A QUATERNION INTO A ROTATION MATRIX The transformation of a vector Vto V' is accomplished by the quaternion multiplication [Cooper, et al., 1985] V -fV*. (7) This is equivalent to multiplication of Vby the rotation matrix R V'" RV. (8) Carrying out the quaternion multiplication, V - (q0 +q1i+q 2i+q 3k) (vti+v'j+v 3k) (qo-qli-q1 2 -e 3k) (9) we reduce this expression to 3 2-''u + 2(q 2 - 1ro#3 )v2 +2(fof2+ i)01 Ir 0 L to +qsI- f2 - 13)v ++2(b V3 4. ' (10) + [2 (qoq + qfq2 ) v, + (f02 _-2 + f2 _-f ) V2 + 2 (2f3• -f0ql P3 (10 1 2 3 + 2(q 1q 3-q~q 2)v 1+2(t 0q 1+q 2q 3)v 2+( 2 -q2_q2 +q23 1kj. The elements of R are easily identified as '2 2 2 22( 1 2 -q f2(1 2 +f, 3 9 0 +fl -q 2 -f 3 2(q~q 2 -qoq) 2(qoq2+÷fq 3 ) Ru 2 (qoq 3.+qq2) q2 92 2(q 2q 3 -qoql) (1 0 1 2 3 2(qq 3 - q 0 q2) 2(qoq 1 + q 2q 3) 2_ - 2 +9 The transformation from the prime to the unprimed system is accomplished using the transpose of R, V - RTr'. (12) The quaternion equivalent is VW= f* I"f. (3 We see that taking the quaternion conjugate is the equivalent of matrix transposition, and use these terms interchangeably. 3.2 CONVERSION OF A ROTATION MATRIX TO A QUATERNION Using the quaternion expressions for the elements of our rotation matrix derived above, we establish R11+R22+R33+1 - 4 2 2 RII"22-R3+I 4f2(14) 2 -RI+-R22+R 33 +1 - 423 Adding these, we show 4 2 2 2 2 q2 q222 (15) a useful identity. The relationships above give the magnitude of the quaternion components, but not their signs. Changing the signs of all four quaternion components does not affect our rotation matrix, so we may make our first choice arbitrarily. If the square root of the expression for q0 above is not zero, we select the positive root. We then solve for the remaining quaternion components by referring to our rotation matrix and seeing that R32 -R 23 4 q0 A13- A3 1 (16) 4 go A2 1 - A12 4 q0 If q0 is zero, we use the square root of the expression for q, above and proceed in a similar manner. 3.3 THE PRODUCT OF TWO QUATERNIONS We are interested in finding the equivalent quaternion, q ", for the transformations q followed by q". In terms of a vector, V, two successive applications of the quaternion transformation operator give V " (. q I(V*)q*,. (17) It can be shown that the hyperimaginary numbers are associative, thus V (Wq) V(q*q*). (18) It follows that 5 fq- (io'+q 1 'i+q 2 'b~q 3 1 k)(qo+qli+q 21+q3 k) + (Ifo'fe+Of1+f2Ffv-r3dw2) 1(19) + (fq f 2 '+ q0 ' 2 - fI' 3 + 3 'fq 1 ) i + 4q 0 q3 ' -q o 3 - f, 2 ' + f,'f 2 ) k and f q' M (fq 0- fi- 2 j- q3k)(f 0'- f -i- q2 pi- 13 ok) - (00 90'- flf ,'- f2 f2'- b'3') + (-O ,1 - fO'fl- 2'w3 + t'w) j 20) + (-f~f3`-fO'q3+flq2'-fl'f2) k From these, we identify the components of f": 90 II- ff'-~2-~3 qI ff'If'-~3+32 (21) 12 Q012' ~flf3 + f2 00 f3 fl q3 IOf3'flf2'+f2fl'+f3fO' We may now use f' to operate on V directly Vola of V v"0 (22) 6 3.4 ROTATE AN AXIS TO BE COLLINEAR WITH A VECTOR This problem is treated in a general way by Escobal [1965). A single rotation about an arbitrary rotation axis is always sufficient to make one vector collinear with another. Defining a unit vector ; along the rotation axis, and a rotation angle 0, cos'+e(I-cos4') el e2(1 - cosO) + e3sint el C 3 (1 - cos)-e2sin- R * e 1e2(1-cos6)-e 3sin* cost +e2(1-os@) e2 e3(1-cos0)+ejsin4' (23) el e3(1 -cosf) + e2sint e2e3(1 -cos0) - ejsin0 cost + e•(1 -cos2) We simplify the problem by requiring that the first vector, U, be equal in magnitude to the second vector, V Furthermore, we require U to point in the direction of the n axis. The angle 4 is the direction cosine between Vand the n axis: 4aarccos-U. (24) I VI The transformation V = RU gives three equations with three unknowns, since u. =VI1 and the two other components of Uare each zero. For example, if we select U to point along the z axis (see illustration), we have 0 R VI (el e3(1 -cos')- e2sint ) V= .Rj 0 1 VI {e2 e3(1-cosO)+elsin4() 1 Vl IVl (Cos*+ 2 S+e(1-cosO)) Solving for e3 {(2-cos /(I1-cosf) (0 as expected, since the axis of rotation must lie in the xy plane. Solutions for the other components become 7 - V 2 (27) 1 VIsine and -_"2 (28) 1 VIsin* Calculating values for the elements of R is now a simple matter. v5 ,Uz V ,. y x 4. GENERAL DESCRIPTION 4.1 QUAT.FOR AEQA This function compares two attitude sequences. A value of .TRUE. is returned if they are equivalent and a value of .FALSE. if they are not. Acceptable error limits are specified in the logical function REOR. All angles are in radians. See description of subroutine ATOR for details of parameters. ATOR This subroutine converts rotation sequence about IAXIS(1), IAXIS(2), IAXIS(3) into the corresponding nine element rotation matrix. Allowed values are l-roll-x-axis, 2-pitch-y-axis, and 3-yaw-z-axis. The same rotation axis cannot be used twice. All attitudes passed to this routine must be in radians. For example, IAXIS(1)-2, IAXIS(2)-3, IAXIS(3)-1 specifies a pitch, yaw, roll sequence of angles A'IT(1), ATT7(2), ATI'(3) respectively. Returns the rotation matrix R. QEQO This function compares two quatemions. A value of .TRUE. is returned if they are equivalent and a value of .FALSE. if they are not. Absolute errors on the order of 1E-6 are acceptable. OMULTO This subroutine multiplies two quaternions, Q(3)-0(2)Q(1). It is equivalent to two successive rotations, 0(1) followed by 0(2). Note that quaternion multiplication is the equivalent of matrix multiplication R(3)-R(2)R(1) for the same rotations. QTOR This subroutine converts a four element quaternion into a nine element rotation matrix for coordinate system transformations. QTPOSE This subroutine transposes the quaternion 01 into 02. REOR This function compares two rotation matrices. A value of .TRUE. is returned if they are equal and a value of .FALSE. if they are not. Absolute errors on the order of 1E-6 are acceptable. RMULTR This subroutine multiplies two rotation matrices, R3 - (R2)(R1). It is equivalent to two 6uccessive rotations, RI followed by R2. RMULTV This subroutine multiplies a vector by a rotation matrix, V2 - (R)(Vl). 9 OUAT.FOR (CONT.) RTOA This subroutine converts a nine element rotation matrix Into the corresponding rotation sequence about IAXIS(1), IAXIS(2), IAXIS(3) where 1-rolI-x-axis, 2-pitch-y-axis, and 3-yaw-z-axis. The same rotation axis cannot be used twice. Inputs are R and IAXIS. Returns the corresponding rotation sequence ATT(1) (0 to 2PI), ATI'(2) (-PI to PI), ATT(3) (0 to 2PI) in radians. RTOQ This subroutine converts a nine element rotation matrix into a four element quaternion. RTPOSE This subroutine transposes the rotation matrix R1 into matrix R2. VTOR This subroutine converts a three element vector into a nine element rotation matrix for coordinate system transformations. The NAXIS axis is subjected to a single rotation which points it in the direction of the vector. NAXIS values of 1, 2, and 3 correspond to the x, y, and z axes respectively. The resulting rotation matrix R is returned. R may then be used to transform to the new system: V' - (R)(V). 4.2 M5OTOT.FOR ASTRO Calculates M50 to true-of-date rotation matrix by constructing the appropriate precession and nutation rotation matrices, then performing matrix multiplication. GMPRD Multiplies matrices. M50TOT Subroutine to build a rotation matrix for M50 to true-of date coordinate conversion. Input four digit year, month, day, and GMT (sec). Returns the 3x3 rotation matrix calied RESULT. MJDT Calculates a modified Julian day number. NUTATE Builds the nutation rotation matrix. PRECSS Builds the precession rotation matrix. RMAT Calculates trigonometry values, then builds rotation matrix. RMFU Builds rotation matrix. SETCON Initializes the math constants used by the MM0 converter. 10 S. EXAMPLES 5.1 Example 1. Convert a yaw, pitch, roll sequence to the corresponding pitch, yaw, roll sequence. Show that the two sequences are equivalent. This example is illustrated in the program AIT. Rotation angles are given values in a data statement and are stored as ATTL. JAXISI holds the rotation sequence associated with ATTI. The values 3,2,1 specify the z,y,x axes respectively. This corresponds to a yaw, pitch, roll sequence. Similarly, IAXIS2 holds the rotation sequence associated with ATM2. Values of 2,3,1 correspond to a pitch, yaw, roll sequence. After changing the AMTi angles from degrees to radians we are ready to begin the conversion. The attitude sequence defined by AITI and IAXISI is converted to a rotation matrix using a call to ATOR. The resulting matrix, TEMP, is then converted back to the rotation sequence specified by IAXIS2 using the call to RTOA. The values of interest are returned in ATI2. To show that these two attitude sequences are equivalent, we compare them using the logical function AEQA. A value of true or false is returned and stored as RESULT. Although the use of AEQA is somewhat trivial in this example program, knowing whether or not two attitude sequences are equivalent is a common need and so it is demonstrated here. The rotation sequences and their associated values are displayed after changing back from radians to degrees. The user should note that angles and axes are always given in the order in which they occur. It is the values of IAXIS which contain the information concerning the rotation sequence. Finally, the equality of the two sequences is displayed using RESULT. A sample output follows the program listing. STS MSID's of interest are listed in Table 1. 11 PROGRAMIATT C THIS PROGRAM CONVERTS A YAW, PITCH, ROLL SEQUBNCB TO A PITCH, YAW, ROLL C SEQUNCEs. IT THEN SHOWS THAT THE TWO SEQUNNCS ARE EQUIVALENT. C----------------------------------------- w-------------- m--------------------- C IMPLICIT RZAL,*8(A-H,O-Z) LOGICAL RESULT, AEQA DIMENSION ATTI(3), IAXI81(3), ATT2(3), IAXI82(3), TEMP(3,3) CHARACTER LEGtND(3)*8 DATA LEGEND / 'ROLL w ', 'PITCH - ', 'YAW I ' / C C IAXIS1 VALUES SPECIFY YAW, PITCH, ROLL SEQUENCE FOR ATTI. C IAXIS2 VALUES SPECIFY PITCH, YAW, ROLL SEQUENCE FOR ATT2. C ATTI DATA IS SUPPLIED IN DATA STATEMENTS, BUT COULD EASILY BE READ FROM A C DATA BASE. C DATA IAXISI / 3, 2, 1 / DATA ATTI / 0.3582767D3, 0.2380823D0, 0.896500702 / DATA IAXIS2 / 2, 3, 1 / C C CONVERT DEGREES TO RADIANS FIRST. TWOPI - 8.DO * DATAN(l.DO) DTR a TWOPI / 360.DO RTD - 1.D0 / DTR DO 100 1-1,3 ATTI(I) - ATT1(I) * DTR 100 CONTINUE C C CONVERT ATTITUDE SEQUENCE 1 TO A ROTATION MATRIX, THEN CONVERT THIS TO C ATTITUDE SEQUENCE 2. C CALL ATOR(ATTI,IAXIS1,TEMP) CALL RTOA(TEMP,ATT2,IAXIS2) RESULT - AEQA(ATT1,IAXIS1,ATT2,1AXI32) C DO 110 1-1,3 ATTI(I) - ATT1(I) * RTD ATT2(1) - ATT2(I) * RTD 110 CONTINUE C C DISPLAY RESULTS TO USER. C WRITE(*,200) (LEGEND(IAXISI(I)),ATT1(I),I-1,3), + (LEGEND(IAXIS2(I)),ATT2(I),I-1,3) IF(RESULT) THEN WRITE(*,*) # EQUAL' ELSE WRITE(*,p) ' NOT EQUL' END IF 200 FORMAT(/,/,2(lX,3(5X,A,F12.6),/,/),/) END YAW a 358.276700 PITCH = .238082 ROLL - 89.650070 PITCH - .238190 YAW - -1.723285 ROLL * 89.657233 EQUAL 12 5.2 Example 2. Convert M50 position and velocity in feet per second to Aries true-of-date position and velocity In kilometers per second. This example is illustrated in the program CNVMSO. In this example, the user Is first asked to specify input and output file names. The time, position, and velocity are then read from the specified input file. A printout of the file selected for this example, named CNVMS0.DAT, is included at the end of the program listing. The time of day is converted to seconds, then a call is made to M5OTOT. This subroutine uses the time information to calculate rotation matrix values. These values are returned as the 3x3 array M50TOD. The user may be interested to know that IMON, IDAY may be passed as either 1, day of year or month, day of month. We use this rotation matrix to multiply the vector of interest using RMULTV. In this way the M50 position is converted to TOD position, then the MSO velocity to TOD velocity. These values are converted to kilometers and kilometers per second respectively, and displayed. A sample display is shown following the program and input file listings. SIS MSID's of interest are listed in Table 1. 13 PROGRAM ONVM5O C ----------- - ---------------- --- m p ------ m-- C THIS PROGRAM CONVERTS POSITION &NO VELCITY VECTORS FROM FT/SEC IN 1450 TO C KM/SEC IN ARIES TRUE OF DATE COORDINATES. IMPLICIT P.EAL*6 (A-H,O-E) RRAL*S 950TOD DIMENSION P051450(3), VELM5O(3), POSTOD(3), VELTOD(3), M5OTOD(3,3) CHARACTER*22 IFILE, OFILE C C fl/KM CONVERSION C DATA FTKM / 3280.833D0 C C USER SELECTS INPUT AND OUTPUT FILE NAMES C WRITE(*,*) -ENTER INPUT FILE NAME READ(*,'(A)-) hFILl QPUN(UNITl, FILZuIFILE,STATU~u 'OLD' ,FORM. 'FORMATTED I) WRITE(*,*) - ENTER OUTPUT FILE NAME READ(*,'(A)') OFILE OPEN(UNIT-3, FIL~uOFILE,STATUS='NUW' FORM. 'FORMATTXD') C C READ TIME AND VECTOR INPUTS C READ(1,*) IYR,IMOMIDAY,IHR,IMIN,SEC ItEAD(l,*) (POSM5O(I)0Iul,3) READ(1,*) (VELM5O(I),1-l,3) C C CONVERT TIME TO SECONDS, THEN CONVERT VECTOR.S FROM M450 TO TRUE USING C ROTATION MATRIX. C GMTmIHR* 3600.*DO+IMIN'60.*DO+SXC CALL M5OTOT(IYR, IMON, IDAYGMT,M5OTOD) CALL RMULTV (POSM5O,M5OTOD, POSTOD) CALL RMULTV(VZLM50,M5OTOD, VBLTOD) C C CONVERT TO KM C DO 100 1-1,3 POSTOD(I) w POSTOD(I) /FTKM VELTOD(I) - VELTOD(I) /FTKM 100 CONTINUE c C WRITE RESULTS C WRITS(*,*) POSTODVELTOD WRITE(3,*) IYR,IMON,IDAY,IHR, IMIN,SEC WRITE(3,*) (POSTQD(I),1-1,3) WRITE(3,*) (VELTOD(I),I-1,3) CLOSE(UNIT-1) CLOSE (UNIT-3) END 1990 1 306 a 0.000O0O0OOOOOOOOOB+O000 750. 624415039622700 5649.784942912215000 3398.785550423236000 -5.275955101502905 -2.398301548788066 5.146722574414398 14 5.3 Example 3. Use an M50 to body axis quaternion and body axis to UVW Euler angles, construct the M50 to UVW rotation matrix. This example is illustrated in the program QM50. The quaternion values, Euler angles, and axis sequence information were taken from a NASA PATH tape. They appear as data statements at the beginning of the program. The M50 to Body quaternion is converted to a rotation matrix using OTOR. Values are returned as the 3x3 array M50B. After changing the Euler angles from degrees to radians, the Body to UVW attitude sequence is converted to a rotation matrix using ATOR. The result is returned as the 3x3 array BUVW. These two rotation matrices are multiplied using RMULTR, which returns the 3x3 array M50UVW. This last rotation matrix is the result of two successive rotations, M50B followed by BUVW, and is the desired result. These values are then displayed to the user. A sample display follows the program listing. The reader might be interested in comparing the values of the M50UVW array to those calculated using a different method and shown in the next example. STS MSID's of interest are listed in Table 1. 15 PROGRAM QH50 C THIS PROGRAM USES A QUATERNION TO CONSTRUCT AN 450 TO BODY AXIS ROTATION MATRIX. IT THEN USES EULER ANGLES TO CONSTRUCT A BODY AXIS TO UVW ROTATION MATRIX. THESE MATRICES ARE MULTIPLIED TO GIVE AN M50 TO UVw ROTATION MATRIX. C- C IMPLICIT RZAL*S(A-H,O-.Z) RZAL*8 M5OUVwD, M505 DIMENSION QMSOB(4), M50B(3,3), BUVW(3,3), M50UVW(3,3), IAXIS(3), + ATT(3) C C FOR THIS EXAMPLE, DATA I8 SUPPLIED IN DATA STATEMENTS. IT COULD EASILY BE READ IN FROM A DATA BASE INSTEAD. SOURCE OF DATA IS NASA PATH TAPE 5IF-MISSION TDRS 41D WEST - 0EG. NO. 14. IAXIR VALUES SPECIFY YAW, PITCH, ROLL SEQUENCE FOR ATT. C DATA QMS08 / 0.2599793D0, 0.5427552D-1, 0.3427433D0, -0.9011060 / DATA IAXIS / 3, 2, 1 / DATA ATT / 0.3582767D3, 0.2380823D0, 0.8965007D2 / C C CONVERT DEGREES TO RADIANS FIRST. TWOPI - 8.D0 * DATAN(1.D0) DTR - TWOPI I 360.DO DO 100 1-1,3 ATT(I) - ATT(I) * DTR 100 CONTINUE C CALL QTOR(QM50B,M5OB) CALL ATOR(ATT, IAXIS,BUVW) CALL RMULTR(MSOB,BUVW,MSOUVW) C C DISPLAY RESULTS TO USER. C WRITE(*,200) 'M50UVW - ',((MSOUVW(I,J),J=1,3),I-1,3) 200 FORMAT(/,/,1X,A,3(T15,3(5X,F1O.6),/),/,/) END M5OUVW - -. 844416 .526901 .096629 -. 282325 -. 591032 .755628 .455252 .610783 .647834 16 5.4 Example 4. Use M50 position and velocity vectors to calculate the M50 to UVW rotation matrix. Use Aries true-of- date position and velocity vectors to calculate the True to UVW rotation matrix. Combine these to obtain the M50 to True rotation matrix, then check independently. This example is illustrated in the program UVW. Values for M50 and True positions and velocities were taken from a NASA PATH tape. They appear as data statements at the beginning of the program. The M50 to UVW rotation matrix is constructed from the M50 position and velocity vectors using VTOR. In the first call to VTOR, the M50 coordinate system is rotated to point the 1, or x, axis in the direction of the position vector. This rotation makes the x' axis collinear with the U axis. The rotation matrix necessary for this operation is returned as the 3x3 array RI. The M50 velocity vector is then transformed to the new system using RMULTV, which returns the transformed velocity as TEMP. This transformed velocity is now projected onto the y'z' plane by setting the x' component to zero. Next the 2, or y', axis is pointed iP the direction of our transformed, projected velocity vector by using VTOR a second time. The rotation matrix for this transformation is returned as the 3x3 array R2. This second rotation can be desciibed as a rotation about the x' axis which results in a z" velocity component of zero. These two rotations establish conditions sufficient to define the UVW system. The rotation matrices RI and R2 are multiplied using RMULTR. The product, returned as the 3x3 array M5OUVW, is the M50 to UVW rotation matrix of interest. The true-of-date to UVW rotation matrix is constructed from True position and velocity vectors in a similar manner, and returned as the 3x3 array TODUVW. Since we are interested in going from UVW to True, RTPOSE is used to transpcse the TODUVW array. The result is returned as the 3x3 array UVWTOD. To complete the construction of the M50 to True rotation matrix, the rotation matrices M50UVW and UVWTOD are multiplied using RMULTR. The product is returned as the 3x3 array M50TOD. Next, the M50 to T'rue rotation matrix is constructed independently using the date and time associated with the position and velocity vectors. These values have been placed in a data statement at the beginning of the program. This information is converted to a rotation matrix using M50TOT, and the results are returned in the 3x3 array TEST. Finally, we check to see if the two rotation matrices are equal using the logical function REOR. A value of true or false is returned and stored as RESULT. The values of interest are then displayed to the user. A sample display follows the program listing. STS MSID's of interest are listed in Table 1. 17 PROGRAM UVW C PROGRAM TO CALCULATE N50 TO UVW AND ARIES TRUE-OF-DATE TO UVW ROTATION C MATRICES FROM POSITION AND VELOCITY VECTORS. THE TWO MATRICES ARE THEN C MULTIPLIED TO GIVE A MSO TO TRUE CONVERSION MATRIX. THIS IS CHECKED C AGAINST THE MSOTOT SUBROUTINE VALUES. C ---------------- m ---------------- m-------------------------------- m-------------- C IMPLICIT REAL*8(A-H,0-Z) LOGICAL REQR#, RESULT R3AL*8 X5OUVW, M5OTOD DIMENSION POSNSO(3), VELM50(3), POSTOD(3), VELTOD(3), TEKP(3) DIMENSION R1(3,3), R2(3,3), M5OLJVW(3,3), TODUVW(3,3), + UVWTOD(3,3), MSOTOD(3,3), TEST(3,3) C C FOR THIS EXAMPLE, DATA IS SUPPLIED IN DATA STATEMENTS. IT COULD EASILY BE C READ FROM A DATA BASE INSTEAD. SOURCE OF DATA IS NASA PATH TAPE C 51F-MISSION TDRS 41D WEST - ONG. NO. 14. C DATA IYR, IMON, IDAY, GMTSEC / 1985, 8, 1, 1001.86957D0 / C DATA POSM50 / -0.5652093D4, 0.3526012D4, 0.6467874D3 I DATA VELMS0 / -0.2178853D1, -0.4563907D1, 0.5834034D1 I DATA POSTOD / -0.5681994D4, 0.348198104, 0.6274173D3 / DATA VELTOD / -0.2162737D1, -0.4581256D0, O.5026428D1 / C C CONSTRUCT THE M50 TO UVW ROTATION MATRIX. C C FIND THE ROTATION MATRIX WHICH POINTS THE X AXIS IN THE DIRECTION OF THE C M50 RADIUS VECTOR. THIS TRANSFORMATION DEFINES THE U AXIS. CALL VTOR(POSM50,R1,1) C USE THIS MATRIX TO TRANSFORM THE VELOCITY VEL' - (Ri) (VEL). CALL RMULTV(VELM50, RI, TEMP) C NOW PROJECT VEL' ONTO V,W PLANE BY SETTING THE U COMPONENT TO ZERO. THEN C FIND ROTATION MATRIX WHICH POINTS THE V AXIS IN THE DIRECTION OF THIS C VELOCITY PROJECTION. THIS IS EQUIVALENT TO A ROTATION ABOUT THE U AXIS C WHICH RESULTS IN NO W COMPONENT OF VELOCITY. TEMP(1)O.DO CALL VTOR(TEMP,R2,2) C COMBINE THESE TWO ROTATIONS. CALL RHULTR (Ri, R2, M5OUVW) C C CONSTRUCT THE TOD TO UVW ROTATION MATRIX IN A SIMILAR MANNER. C CALL VTOR(POSTOD,R1,1) CALL RMULTV(VELTOD, R1,TEMP) TEMP(l)"0.DO CALL VTOR(TEMP,R2,2) CALL RMULTR(R1,R2,TODUVW) C C NOW CONSTRUCT THE M50 TO TOD ROTATION MATRIX. C CALL RTPOSE ( TODUVW, UVWTOD) CALL RHUL1FR(MSOL'VW, UVWTOD,M5OTOD) C C FIND THIS VALUE INDEPENDENTLY USING SUBROUTINE M5OTOT. C CALL M5OTOT(IYR, IMON, IDAY,GMTSEC, TEST) C C TEST FOR EQUALITY. ACCEPTABLE ERRORS OF 1D-6 ARE SPECIFIED IN LOGICAL C FUNCTION REQR. C 18 RESULT - REQR(TEST,MSOTOD) C C DISPLAY RESULTS TO USER. C WRITE(*,100) 'M5OUVW - ', ((MSOUVW(I,J),J-1,3),pI.,3), + 'UVWTOD - ', ((UVWTOD(IJ),J-1,3),Im,3), + 'M5OTOD -', ((M5OTOD(I,J),Jm1,3),I1i,3), + 'M50TOT - ', (( TEST(IJ),J-1,3),I-I,3) I1(RESULT) THEN WRITE(*,*) MSOTOD EQUALS MSOTOT' ELSE WRITE(*,*) ' M5OTOD DOES NOT EQUAL M50TOT' END XF 100 FORMAT(/,/,4(IX,A,3(TI5,3(SX,F1O.6),/),/)) END M5OUVW - -. 844416 .526901 .096629 -. 282325 -. 591032 .755628 .455252 .610783 .647834 UVWTOD - -. 848883 -. 280239 .448179 .520204 -. 593280 .614335 .093735 .754643 .649406 M50TOD - .999963 -. 007907 -. 003437 .007907 .999969 -. 000045 .003438 .000018 .999994 MSOTOT - .999963 -. 007907 -. 003437 .007907 .999969 -. 000045 .003438 .000018 .999994 MS0TOD EQUALS M50TOT 19 Table 1. STS MuasurementhtImull Idmdficatlos for Atdantis ID# Description Units V90W2310C M50 TO BODY QUAT TIME S V90U2240C M50-TO-MEASURED BODY QUAT ELE 1 ND V90U2241C M50-TO-MEASURED BODY QUAT ELE 2 ND V90U2242C M50-TO-MEASURED BODY QUAT ELE 3 ND V90U2243C M50-TO-MEASURED BODY QUAT ELE 4 ND V90U2641C M50 WRT LVLH QUAT 1 ND V90U2642C M50 WRT LVLH QUAT 2 ND V90U2643C M50 WRT LVLH QUAT 3 ND V90U2644C M50 WRT LVLH QUAT 4 ND V97U2218C ADI REF QUAT ELEM 1 ND V97U2219C ADI REF QUAT ELEM 2 ND V97U2220C ADI REF QUAT ELEM 3 ND V97U2221C ADI REF QUAT ELEM 4 ND V98H1248C BODY WRT INERTIAL 0 WD 1 ND V98H1249C BODY WRT INERTIAL 0 WD 2 ND V98H1250C BODY WRT INERTIAL 0 WD 3 ND V98H1251C BODY WRT INERTIAL Q WD 4 ND V90H2141C BODY ATTITUDE ERROR-PITCH DEG V90H2142C BODY ATTITUDE ERROR-YAW DEG V90H2143C BODY ATTITUDE ERROR-ROLL DEG V90H2202C BODY ROLL ATTITUDE EULER ANGLE DEG V90H2217C BODY PITCH ATTITUDE EULER ANGLE DEG V90H2230C BODY YAW ATrITUDE EULER ANGLE DEG V92H3333C POR P ATT ORB STR DEG V92H3334C POR YAW ATT ORB STR DEG V92H3335C POR R ATT ORB STR DEG V95H7467C REQD INERTIAL ROLL ANGLE RAD V95H7468C REOD INERTIAL PITCH ANGLE RAD V95H7473C CURRENT INERTIAL ROLL ANGLE RAD V95H7474C CURRENT INERTIAL PITCH ANGLE RAD V95H7475C CURRENT INERTIAL YAW ANGLE RAD V95H7484C REOD INERTIAL YAW ANGLE RAD V72R0916C LH AD! ROLL RATE DEG/S V72R0917C LH ADI PITCH RATE DEG/S V72R0918C LH ADI YAW RATE DEG/S V72RI116C AFT ADI ROLL RATE DEC/S 20 Table 1. (cont'd) ID# Description Units V72R117C AFT ADI PITCH RATE DEG/S V72R1118C AFT ADI YAW RATE DEG/S V90R2223C IMU DERIVED BODY RATE X-AXIS DEG/S 4 V90R2224C IMU DERIVED BODY RATE Y-AXIS DEG/S V90R2225C IMU DERIVED BODY RATE Z-AXIS DEG/S V92R3323C ACT POR P ROT RATE DEG/S V92R3324C ACT POR YAW ROT RATE DEG/S V92R3325C ACT POR R ROT RATE DEG/S V95R7476C IMU BODY RATE AROUND X-AXIS DEGiS V95R7477C IMU BODY RATE AROUND Y-AXIS DEG/S V95R7487C IMU BODY RATE AROUND Z-AXIS DEG/S V90H4277C X-COMP OF FLTRS CURR POS VCTR-TLM FT V90H4278C Y-COMP OF FLTRS CURR POS VCTR-TLM FT V90H4279C Z-COMP OF FLTRS CURR POS VCTR-TLM FT V92H3417C POR X-POSITION DISPLAY IN V92H3418C POR Y-POSITION DISPLAY IN V92H3419C POR Z-POSITION DISPLAY IN V95HO185C X-COMP OF CURRENT SHUTTLE POS VCTR FT V95H0186C Y-COMP OF CURRENT SHUTTLE POS VCTR FT V95H0187C Z-COMP OF CURRENT SHUTTLE POS VCTR FT V95H0862C X-COMP OF CURRENT TARGET POS VCTR FT V95H0863C Y-COMP OF CURRENT TARGET POS VC'R FT V95H0864C Z-COMP OF CURRENT TARGET POS VCTR FT V901.2557C SEL TOTAL X VEL MS0 FT/S V90L2558C SEL TOTAL Y VEL M50 FT/S V90L2559C SEL TOTAL Z VEL MS0 FT/S V92R3320C ACT POR X TRANS RATE FT/S V92R3321C ACT POR Y TRANS RATE FT/S V92R3322C ACT POR Z TRANS RATE FT/S V95LO190C X-COMP OF CURRENT SHUTTLE VEL VCTR FT/S V95L0191C Y-COMP OF CURRENT SHUTTLE VEL VCTR FT/S V95L0192C Z-COMP OF CURRENT SHUTTLE VEL VCTR FT/S 21 6. REFERENCES Cooper, D.H., Parker, K.C, and Torian, J.G., "OPS On-Oibit Postflight Attitude and Trajectory History (PATH) Product Description", JSC-18645, 1985. Escobal, P.R., "Methods of Orbit Determination", John Wiley & Sons, New York, 1965. Hollinger, H. B., Private Communication, 1989. 22