C DATA SET COVANA AT LEVEL 028 AS OF 11/12/79 C***********************************************************************00001 C * * 00002 C * COVARIANCE ANALYSIS * 00003 C * * 00004 C***********************************************************************00005 C 00006 C A = SEMI-MAJOR AXIS OF REFERENCE ELLIPSOID (METRES) 00007 C B = SEMI-MINOR AXIS OF REFERENCE ELLIPSOID (METRES) 00008 C I = DO LOOP PARAMETER 00009 C J = DO LOOP PARAMETER 00010 C L = MEASUREMENT VECTOR DIMENSION 00011 C M = TRUTH MODEL STATE VECTOR DIMENSION 00012 C N = FILTER STATE VECTOR DIMENSION 00013 C T = TIME IN SECONDS 00014 C AH = HEIGHT (VERTICAL) ACCELERATION (METRES/SECOND**2) AT TIME T 00015 C DT = UPDATE INTERVAL IN SECONDS 00016 C HO = INPUT INITIAL HEIGHT (METRES) 00017 C MM = DIMENSIONED SIZE OF ARRAYS 00018 C RM = RADIUS OF CURVATURE IN THE MERIDIAN (METRES) 00019 C RN = RADIUS OF CURVATURE IN THE PRIME VERTICAL (METRES) 00020 C TA = TIME TO ACCELERATE TO VMAX 00021*12 C TF = FINAL TIME IN SECONDS 00022 C TI = TIME AT START OF CURRENT INTEGRATION INTERVAL 00023*12 C VH = HEIGHT (VERTICAL) VELOCITY (METRES/SECOND) AT TIME T 00024 C ACC = HORIZONTAL ACCELERATION (METRES/SECOND**2) 00025*12 C DTI = INTEGRATION INTERVAL FOR PHI AND Q (SECONDS) 00026 C RHO = CONVERSION FACTOR = ARC-SECONDS/RADIAN 00027*21 C TVA = TIME TO ACCELERATE TO VVMAX (SECONDS) 00028*12 C TVV = TIME TRAVELLING AT VVMAX TO REACH HMAX (SECONDS) 00029*12 C ACCV = VERTICAL ACCELERATION (METRES/SECOND**2) 00030*12 C ALAT = LATITUDE ACCELERATION (RADIANS/SECOND**2) AT TIME T 00031 C HMAX = MAXIMUM HEIGHT ABOVE GROUND (METRES) 00032*12 C NDTI = NUMBER OF INTEGRATION INTERVALS PER UPDATE INTERVAL 00033 C RHO2 = RHO*RHO 00034*21 C RLAT = LATITUDE (RADIANS) AT TIME T 00035 C VLAT = LATITUDE VELOCITY (RADIANS/SECOND) AT TIME T 00036 C VMAX = MAXIMUM HORIZONTAL VELOCITY (METRES/SECOND) 00037*12 C (INPUT IN KILOMETRES/HOUR) 00038*13 C ALONG = LONGITUDE ACCELERATION (RADIANS/SECOND**2) AT TIME T 00039 C ITRAV = TRAVERSE CONFIGURATION CODE 00040 C = 1 CONSTANT DYNAMICS 00041 C .GT.1 TIME VARYING 00042 C IZUPT = COUNTER OF ZUPTS USED TO REINITIALIZE CP MATRIX AT A CHOSEN Z00043*27 C NTERM = NUMBER OF TERMS TO BE USED TO CALCULATE PHI AND QK 00044*10 C NZUPT = NUMBER OF ZUPT INTERVALS IN SIMULATION RUN (MAXIMUM 30) 00045*12 C NZVOB = NUMBER OF ZERO VELOCITY OBSERVATIONS PER ZUPT 00046*21 C RLATO = INPUT INITIAL LATITUDE (RADIANS) 00047 C RLONG = LONGITUDE (RADIANS) AT TIME T 00048 C VLONG = LONGITUDE VELOCITY (RADIANS/SECOND) AT TIME T 00049 C INPUT IN KILOMETRES/HOUR IF ITRAV = 1. 00050*20 C TCRUZ = HORIZONTAL TRAVEL TIME AT CRUISE VELOCITY(VMAX) DURING A 00051*12 C ZUPT INTERVAL (SECONDS) 00052*12 C VVMAX = MAXIMUM VERTICAL VELOCITY (METRES/SECOND) 00053*12 C F(M,M) = TRUTH MODEL DYNAMICS MATRIX 00054 C HEIGHT = HEIGHT ABOVE REFERENCE ELLIPSOID (METRES) 00055 C HT(31) = ELEVATIONS AT ZUPT POINTS (METRES) 00056*12 C H(L,M) = TRUTH MODEL MEASUREMENT MATRIX 00057 C ICONST = CODE FOR DYNAMICS DURING A TRAVERSE 00058*18 C = 0 ==> NON-CONSTANT DYNAMICS MATRIX 00059*18 C = 1 ==> CONSTANT DYNAMICS:F,PHI,Q SAME AS LAST DT INTERVAL. 00060*18 C THIS OCCURS DURING ZUPT AND DURING CONSTANT VELOCITY AT00061*18 C AZIMUTHS OF 90 OR 270 AND WHEN VERTICAL VELOCITY = 0. 00062*18 C IFCODE = CODE TO SELECT F,FF MATRICES IN SUBROUTINE TVM 00063*21 C = 1 ==> BRITTING P.230 00064*21 C = 2 ==> SCHMIDT P.1-8 00065*21 C IPCODE = COMPUTED CODE FOR PRINTING 00066*15 C = IPRINT ==> PRINT - OCCURS ONCE EVERY NPRINT INTERVALS 00067*15 C # IPRINT ==> NO PRINT 00068*15 C IPRINT = COUNTER OF NUMBER OF TIMES THROUGH MAIN LOOP(ONCE PER DT) 00069*14 C FOR DETERMINING WHEN TO PRINT OUTPUT 00070*14 C IZCODE = CODE FOR ZUPT 00071*13 C = 1 ==> ZUPT TAKING PLACE ==> V=0 BEING MEASURED 00072*13 C = 2 ==> PLATFORM MOVING ==> H=HF=0 00073*13 C OR NO ZERO VELOCITY OBSERVATION IN THAT INTERVAL 00074*21 C K(N,L) = FILTER GAIN MATRIX 00075 C NPRINT = NUMBER OF DT INTERVALS BETWEEN PRINTOUTS 00076*14 C NTERMF = NUMBER OF TERMS TO BE USED TO CALCULATE PHIF AND QKF 00077*10 C P(M,M) = TRUTH MODEL COVARIANCE MATRIX BEFORE UPDATE 00078 C Q(M,M) = TRUTH MODEL PROCESS NOISE SPECTRAL DENSITY MATRIX 00079*10 C R(L,L) = TRUTH MODEL MEASUREMENT NOISE MATRIX 00080 C SE(MM) = VECTOR USED IN SUBROUTINE SYINV 00081 C TACRUZ = TIME TO ACCELERATE TO VVCRUZ FROM REST (SECONDS) 00082*12 C TVCRUZ = TRAVEL TIME AT VERTICAL CRUISE VELOCITY (SECONDS) 00083*12 C VVCRUZ = VERTICAL CRUISE VELOCITY (METRES/SECOND) 00084*12 C WEARTH = RATE OF ROTATION OF THE EARTH (RADIANS/SECOND) 00085 C ZUPDUR = DURATION OF A ZUPT (SECONDS) 00086*12 C NOTE: MUST BE EXACT MULTIPLE OF DT 00087*12 C CC(M,M) = CROSS-COVARIANCE MATRIX BETWEEN CURRENT AND NEXT ZUPT POINT00088*25 C CP(M,M) = CROSS-COVARIANCE MATRIX BETWEEN INITIAL AND CURRENT ZUPT PT00089*25 C FF(N,N) = FILTER DYNAMICS MATRIX 00090 C HF(L,N) = FILTER MEASUREMENT MATRIX 00091 C PF(N,N) = FILTER COVARIANCE BEFORE UPDATE 00092 C PP(M,M) = TRUTH MODEL COVARIANCE MATRIX AFTER UPDATE 00093 C P0(M,M) = INITIAL TRUTH MODEL COVARIANCE MATRIX 00094 C (1,1),(2,2),7,7) = VARIANCES OF INITIAL MISORIENTATIONS 00095*21 C INPUT IN ARC-SEC**2,CONVERT TO RAD**2 00096*21 C (3,3),(4,4) = VARIANCES OF INITIAL LATITUDE AND LONGITUDE 00097*21 C INPUT IN ARC-SECOND**2, CONVERTED TO RAD**2 00098*21 C (5,5),(6,6) =VARIANCES OF INITIAL NORTH AND EAST VELOCITIES00099*21 C INPUT IN ((ARC-SEC/SEC)**2, CONVERTED TO (RAD/SEC)**2 00100*21 C QF(N,N) = FILTER MODEL PROCESS NOISE SPECTRAL DENSITY MATRIX 00101*10 C QK(M,M) = TRUTH MODEL PROCESS NOISE COVARIANCE MATRIX FROM 1 INTERVAL00102*10 C RF(L,L) = FILTER MEASUREMENT NOISE MATRIX 00103 C PPF(N,N) = FILTER COVARIANCE MATRIX AFTER UPDATE 00104**9 C AZIM(30) = AZIMUTH OF SUCCESSIVE LEGS (RADIANS)(INPUT IN DEGREES) 00105*22 C PHI(M,M) = TRUTH MODEL TRANSITION MATRIX 00106 C P0F(N,N) = INITIAL FILTER COVARIANCE MATRIX 00107 C QKF(N,N) = FILTER MODEL PROCESS NOISE COVARIANCE MATRIX FROM ONE INTE00108*10 C XLAT(31) = LATITUDE AT ZUPT POINTS (RADIANS) 00109*12 C PHIF(N,N) = FILTER MODEL TRANSITION MATRIX 00110 C PHII(M,M) = TRUTH MODEL TRANSITION MATRIX FROM ITERATION I 00111**3 C QNDT(M,M) = TRUTH MODEL PROCESS NOISE COVARIANCE MATRIX FROM N INTERV00112*10 C X1(MM,MM) = WORKING MATRIX 00113 C X2(MM,MM) = WORKING MATRIX 00114 C X3(MM,MM) = WORKING MATRIX 00115 C IOCODE(10) = OUTPUT CODES: SEE 6019 FORMAT STATEMENT FOR DETAILS 00116*15 C ITITLE(20) = INPUT JOB TITLE IN 'A' FORMAT. FIRST CARD IN DATA DECK.00117*18 C PHIFI(N,N) = FILTER MODEL TRANSITION MATRIX FROM ITERATION I 00118**3 C QNDTF(N,N) = FILTER MODEL PROCESS NOISE COVARIANCE MATRIX FROM N INTE00119*10 C ZUPINT(30) = ZUPT INTERVALS (SECONDS) OF TRAVERSE 00120*12 C NOTE: MUST BE EXACT MULTIPLE OF DT 00121*12 C RMSP,RMSPP,RMSPF,RMSPPF = MATRICES OF STANDARD DEVIATIONS AT ZUPTS 00122*27 C 00123 C***********************************************************************00124 C 00125 IMPLICIT REAL*8(A-H,O-Z) 00126 REAL*8 K 00127 COMMON/TRUTH/F(20,20),H(20,20),P(20,20),PP(20,20),PHI(20,20), 00128 @ PO(20,20),Q(20,20),QK(20,20),QNDT(20,20),R(20,20) 00129*10 COMMON/FILTER/FF(20,20),HF(20,20),K(20,20),PF(20,20),PPF(20,20), 00130**9 @PHIF(20,20),POF(20,20),QF(20,20),QKF(20,20),QNDTF(20,20),RF(20,20)00131*10 COMMON/SCRTCH/X1(20,20),X2(20,20),X3(20,20) 00132 COMMON/TRAJEC/RLAT,RLONG,HEIGHT,VLAT,VLONG,VH,ALAT,ALONG,AH 00133 COMMON/TRAJIN/NZUPT,ZUPDUR,ACC,VMAX,ACCV,VVMAX,HMAX,TCRUZ,TVA,TVV 00134*12 @ ,TA,TA2,ZUPINT(30),HT(31),AZIM(30),XLAT(31) 00135*12 COMMON/INPUT/DT,DTI,TF,RLATO,HO,ITRAV,L,M,N,NDTI,NTERMP,NZVOB 00136*21 COMMON/CONST/A,B,WEARTH,PI,RADIAN,MM 00137*13 DIMENSION PHII(20,20),PHIIF(20,20) 00138*10 DIMENSION IOCODE(12),NAME(6),ITITLE(20) 00139*28 DIMENSION CC(20,20),CP(20,20) 00140*25 DATA CC/400*0.D0/,CP/400*0.D0/ 00141*25 DIMENSION RMSP(30,20),RMSPP(30,20),RMSPF(30,20),RMSPPF(30,20) 00142*27 DATA RMSP,RMSPP,RMSPF,RMSPPF/2400*0.D0/ 00143*27 C 00144 C COMMON/TRAJEC/ VARIABLES ARE DEFINED IN SR TRAJ AND USED IN SR TVM 00145 C COMMON/SCRTCH/ VARIABLES ARE USED AS SCRATCH MATRICES 00146 C COMMON/INPUT/VARIABLES ARE READ AS INPUT 00147*13 C DTI IS DEFINED IN MAIN 00148*13 C TF IS REDEFINED IN MAIN WHEN ITRAV.NE.1 00149*13 C COMMON/CONST/ VARIABLES ARE DEFINED IN MAIN AND SR BLOCK DATA 00150 C 00151 C***********************************************************************00152 C 00153 C INITIALIZE SOME VARIABLES 00154*13 C NOTE: MM MUST BE INITIALIZED HERE 00155*13 C 00156*13 DATA NAME/'Q ','QF ','PP ','PPF ','P ','PF '/ 00157*28 CALL ERRSET(208,256,-1,1,0,207) 00158*27 PI=DARCOS(-1.D0) 00159*13 RADIAN=PI/180.D0 00160*13 RHO=6.48D5/PI 00161*21 RHO2=RHO*RHO 00162*21 MM=20 00163*13 T=0.D0 00164*13 IPRINT=0 00165*14 ICONST=0 00166*18 IZUPT=0 00167*27 C 00168*13 C***********************************************************************00169*13 C 00170*13 C READ AND PRINT INPUTS 00171 C 00172 C CONSTANTS AND OPTION CODES 00173 C 00174 READ(5,5003)(ITITLE(I),I=1,20) 00175*18 READ(5,5000)ITRAV 00176*16 READ(5,5000)IFCODE 00177*21 READ(5,5000)L,M,N 00178*16 READ(5,5000)(IOCODE(I),I=1,12) 00179*21 READ(5,5001)DT 00180*16 READ(5,5000)NDTI 00181*16 READ(5,5000)NTERM,NTERMF 00182*16 READ(5,5000)NPRINT 00183*16 DTI=DT/DFLOAT(NDTI) 00184*16 WRITE(6,6041)(ITITLE(I),I=1,20) 00185*18 WRITE(6,6000)ITRAV,L,M,N 00186*16 WRITE(6,6043)IFCODE 00187*21 WRITE(6,6019)((I,IOCODE(I)),I=1,12) 00188*21 WRITE(6,6020)DT,NDTI,DTI,NTERM,NTERMF 00189*16 WRITE(6,6018)NPRINT 00190*16 C 00191 C TRAJECTORY INPUTS 00192*12 C 00193*12 READ(5,5002)ID,IM,S 00194*16 WRITE(6,6021)ID,IM,S 00195*16 RLATO=(DFLOAT(ID)+(DFLOAT(IM)+S/6.D1)/6.D1)*RADIAN 00196*12 IF(ITRAV.NE.1)GO TO 8002 00197*21 READ(5,5001)VLONG 00198*20 WRITE(6,6042)VLONG 00199*20 VLONG=VLONG/3.6D0/RADIUS(RLATO,RM,RN,A,B)/DCOS(RLATO) 00200*20 READ(5,5001)HO 00201*16 READ(5,5001)TF 00202*17 WRITE(6,6038)HO 00203*16 WRITE(6,6039)TF 00204*17 TF=TF-DT 00205*17 GO TO 8001 00206*16 8002 CONTINUE 00207*16 READ(5,5000)NZUPT 00208*16 READ(5,5001)ZUPDUR 00209*16 READ(5,5000)NZVOB 00210*21 READ(5,5001)VMAX,TA 00211*16 READ(5,5001)VVMAX,TVA 00212*16 READ(5,5001)HMAX 00213*16 VMAX=VMAX/3.6D0 00214*13 ACC=VMAX/TA 00215*13 ACCV=VVMAX/TVA 00216*13 READ(5,5001)(ZUPINT(I),I=1,NZUPT) 00217*14 NN=NZUPT+1 00218*12 READ(5,5001)(HT(I),I=1,NN) 00219*14 READ(5,5001)(AZIM(I),I=1,NZUPT) 00220*14 NN=IDINT(ZUPDUR/DT+0.001) 00221*21 ZUPDUR=DFLOAT(NN)*DT 00222*12 ZDT=ZUPDUR/NZVOB 00223*21 NZDT=ZDT/DTI 00224*21 IF(NZDT.GE.1)GO TO 8777 00225*21 WRITE(6,6045)NZDT 00226*21 STOP 00227*21 8777 CONTINUE 00228*21 ZDT=NZDT*DTI 00229*21 NZVOB=ZUPDUR/ZDT 00230*21 DO 1104 I=1,NZUPT 00231*12 NN=IDINT(ZUPINT(I)/DT) 00232*13 1104 ZUPINT(I)=DFLOAT(NN)*DT 00233*12 X=VMAX*3.6 00234*13 WRITE(6,6026)NZUPT,ZUPDUR,NZVOB,TA,ACC,X,TVA,ACCV,VVMAX,HMAX 00235*22 WRITE(6,6027)HT(1),((I,ZUPINT(I),HT(I+1),AZIM(I)),I=1,NZUPT) 00236*12 C 00237*12 C TRUTH AND FILTER MODEL INPUTS 00238*21 C 00239*21 8001 CONTINUE 00240*21 READ(5,5001)(PO(I,I),I=1,M) 00241*21 WRITE(6,6001) 00242*21 CALL MOUTD(PO,MM,M,M) 00243*21 READ(5,5001)(POF(I,I),I=1,N) 00244*21 WRITE(6,6009) 00245*21 CALL MOUTD(POF,MM,N,N) 00246*21 READ(5,5001)(Q(I,I),I=1,M) 00247*21 WRITE(6,6002) 00248*21 CALL MOUTD(Q,MM,M,M) 00249*21 READ(5,5001)(QF(I,I),I=1,N) 00250*21 WRITE(6,6010) 00251*21 CALL MOUTD(QF,MM,N,N) 00252*21 READ(5,5001)(R(I,I),I=1,L) 00253*21 WRITE(6,6003) 00254*21 CALL MOUTD(R,MM,L,L) 00255*21 READ(5,5001)(RF(I,I),I=1,L) 00256*21 WRITE(6,6011) 00257*21 CALL MOUTD(RF,MM,L,L) 00258*21 DO 1100 I=1,L 00259*21 1100 READ(5,5001)(H(I,J),J=1,M) 00260*21 WRITE(6,6004) 00261*21 CALL MOUTD(H,MM,L,M) 00262*21 DO 1101 I=1,L 00263*21 1101 READ(5,5001)(HF(I,J),J=1,N) 00264*21 WRITE(6,6012) 00265*21 CALL MOUTD(HF,MM,L,N) 00266*21 C 00267*21 C CONVERT SEC**2 TO RADIAN**2 FOR 9 BY 9 MODEL (EXCEPT H, VH IN M**2) 00268*26 C 00269*26 IF(IFCODE.NE.2)GO TO 1500 00270*26 DO 1501 I=1,7 00271*26 PO(I,I)=PO(I,I)/RHO2 00272*26 POF(I,I)=POF(I,I)/RHO2 00273*26 Q(I,I)=Q(I,I)/RHO2 00274*26 QF(I,I)=QF(I,I)/RHO2 00275*26 1501 CONTINUE 00276*26 DO 1502 I=1,2 00277*26 R(I,I)=R(I,I)/RHO2 00278*26 RF(I,I)=RF(I,I)/RHO2 00279*26 1502 CONTINUE 00280*26 WRITE(6,6050) 00281*26 1500 CONTINUE 00282*26 DO 1102 I=1,M 00283*21 C INITIALIZE P, CC, CP 00284*25 CC(I,I)=PO(I,I) 00285*25 CP(I,I)=PO(I,I) 00286*25 1102 P(I,I)=PO(I,I) 00287*21 DO 1103 I=1,N 00288*21 1103 PF(I,I)=POF(I,I) 00289*21 C***********************************************************************00290*12 C 00291*12 C INITIALIZE SOME VARIABLES 00292*12 C 00293*12 IF(ITRAV.EQ.1)GO TO 9001 00294*13 TA2=TA+TA 00295*12 X=ACC*TA*TA 00296*12 XLAT(1)=RLATO 00297*12 TCRUZ=ZUPINT(1)-TA2 00298*12 TVV=HMAX/VVMAX-ACCV*TVA*TVA 00299*12 IF(TCRUZ.LT.0.D0)GO TO 9003 00300*12 TF=0.D0 00301*22 WRITE(6,6046) 00302*22 CALL RADMS(XLAT(1),ID,IM,S) 00303*22 II=1 00304*22 WRITE(6,6047)II,ID,IM,S 00305*22 DO 1105 I=1,NZUPT 00306*12 AZIM(I)=AZIM(I)*RADIAN 00307*12 TCRUZ=ZUPINT(I)-TA2 00308*12 IF(TCRUZ.LT.0.D0)GO TO 9003 00309*12 XLAT(I+1)=XLAT(I)+(X+TCRUZ*VMAX)*DCOS(AZIM(I))/ 00310*12 @ RADIUS(XLAT(I),RM,RN,A,B) 00311*12 II=I+1 00312*22 CALL RADMS(XLAT(II),ID,IM,S) 00313*22 WRITE(6,6047)II,ID,IM,S 00314*22 TF=TF+ZUPDUR+ZUPINT(I) 00315*12 1105 CONTINUE 00316*12 WRITE(6,6039)TF 00317*17 TF=TF-DT 00318*17 C***********************************************************************00319 C 00320 C COMPUTE TRANSITION AND DISCRETE NOISE MATRICES 00321 C 00322 9001 CONTINUE 00323 DO 1204 IDTI=1,NDTI 00324*18 TI=T+DTI*DFLOAT(IDTI-1) 00325*18 CALL TVM(TI,IFCODE,IZCODE,ICONST,9004,9005) 00326*21 IF((ICONST.EQ.2).AND.(IDTI.EQ.1))GO TO 1299 00327*22 IF(IDTI.GT.1)GO TO 1212 00328*18 C SET PHI=I; PHIF=I 00329 DO 1200 I=1,M 00330**3 DO 1201 J=1,M 00331**3 QNDT(I,J)=0.D0 00332*10 1201 PHI(I,J)=0.D0 00333**3 1200 PHI(I,I)=1.D0 00334**3 DO 1202 I=1,N 00335**3 DO 1203 J=1,N 00336**3 QNDTF(I,J)=0.D0 00337*10 1203 PHIF(I,J)=0.D0 00338**3 1202 PHIF(I,I)=1.D0 00339**3 1212 CONTINUE 00340*18 IF(ICONST.GE.1)GO TO 1298 00341*22 CALL PHIQK(PHII,QK,F,Q,DTI,M,MM,NTERM,8000) 00342*11 CALL PHIQK(PHIIF,QKF,FF,QF,DTI,N,MM,NTERMF,8000) 00343*10 1298 CONTINUE 00344*22 CALL MATMPY(PHII,PHI,X1,M,M,M,MM,1) 00345*24 CALL MATMPY(PHIIF,PHIF,X2,N,N,N,MM,1) 00346*24 DO 1205 I=1,M 00347**3 DO 1205 J=1,M 00348**3 1205 PHI(I,J)=X1(I,J) 00349**3 DO 1206 I=1,N 00350**3 DO 1206 J=1,N 00351**3 1206 PHIF(I,J)=X2(I,J) 00352**3 CALL MATMPY(PHII,QNDT,X1,M,M,M,MM,1) 00353*10 CALL MATMPY(X1,PHII,QNDT,M,M,M,MM,3) 00354*10 DO 1208 I=1,M 00355*10 DO 1208 J=1,M 00356*10 1208 QNDT(I,J)=QNDT(I,J)+QK(I,J) 00357*10 CALL MATMPY(PHIIF,QNDTF,X1,N,N,N,MM,1) 00358*10 CALL MATMPY(X1,PHIIF,QNDTF,N,N,N,MM,3) 00359*10 DO 1209 I=1,N 00360*10 DO 1209 J=1,N 00361*10 1209 QNDTF(I,J)=QNDTF(I,J)+QKF(I,J) 00362*10 1204 CONTINUE 00363*14 1299 CONTINUE 00364*22 C 00365*14 C***********************************************************************00366 C 00367 C PRINT T,F,FF,PHI,PHIF,Q(=QNDT),QF(=QNDTF) 00368*15 C 00369 IPCODE=IPRINT/NPRINT 00370*14 IPCODE=IPCODE*NPRINT 00371*14 IF(IPRINT.NE.IPCODE)GO TO 1210 00372*14 WRITE(6,6023)T 00373*14 IF(IOCODE(2).EQ.0)GO TO 1400 00374*15 WRITE(6,6007) 00375*14 CALL MOUTD(F,MM,M,M) 00376*14 1400 IF(IOCODE(3).EQ.0)GO TO 1401 00377*15 WRITE(6,6015) 00378*14 CALL MOUTD(FF,MM,N,N) 00379*14 1207 CONTINUE 00380*14 1401 IF(IOCODE(4).EQ.0)GO TO 1402 00381*15 WRITE(6,6008) 00382*14 CALL MOUTD(PHI,MM,M,M) 00383*14 1402 IF(IOCODE(5).EQ.0)GO TO 1403 00384*15 WRITE(6,6016) 00385*14 CALL MOUTD(PHIF,MM,N,N) 00386*14 1403 IF(IOCODE(1).EQ.1)GO TO 1404 00387*15 IF(IOCODE(1).EQ.2)GO TO 1405 00388*15 IF(IOCODE(6).EQ.0)GO TO 1406 00389*15 WRITE(6,6002) 00390*14 CALL MOUTD(QNDT,MM,M,M) 00391*14 1406 IF(IOCODE(7).EQ.0)GO TO 1210 00392*15 WRITE(6,6010) 00393*14 CALL MOUTD(QNDTF,MM,N,N) 00394*14 GO TO 1210 00395*15 C PRINT CORRELATION MATRICES OF Q, QF 00396*15 1404 IF(IOCODE(6).EQ.0)GO TO 1408 00397*15 WRITE(6,6032) 00398*15 CALL CORREL(QNDT,X1,MM,M) 00399*15 CALL MOUTD(X1,MM,M,M) 00400*15 1408 IF(IOCODE(7).EQ.0)GO TO 1210 00401*15 WRITE(6,6035) 00402*15 CALL CORREL(QNDTF,X1,MM,N,N) 00403*15 CALL MOUTD(X1,MM,N,N) 00404*15 GO TO 1210 00405*15 C PRINT RMS VALUES OF Q, QF 00406*15 1405 IF(IOCODE(6).EQ.1)CALL RMSOUT(QNDT,MM,M,NAME(1)) 00407*15 IF(IOCODE(7).EQ.1)CALL RMSOUT(QNDTF,MM,N,NAME(2)) 00408*15 1210 CONTINUE 00409*16 C 00410 C***********************************************************************00411 C 00412 C 00413*21 C PRINT P,PF IF REQUESTED 00414*21 C 00415*21 9000 CONTINUE 00416*21 IF(IZCODE.NE.1.AND.IPRINT.NE.IPCODE)GO TO 1420 00417*24 WRITE(6,6023)T 00418*27 IF(IOCODE(11).NE.1)GO TO 1421 00419*21 WRITE(6,6005) 00420*21 CALL MOUTD(P,MM,M,M) 00421*21 WRITE(6,6033) 00422*21 CALL CORREL(P,X1,MM,M) 00423*21 CALL MOUTD(X1,MM,M,M) 00424*21 1421 CONTINUE 00425*21 IF(IOCODE(12).NE.1)GO TO 1420 00426*21 WRITE(6,6013) 00427*21 CALL MOUTD(PF,MM,N,N) 00428*21 WRITE(6,6036) 00429*21 CALL CORREL(PF,X1,MM,N) 00430*21 CALL MOUTD(X1,MM,N,N) 00431*21 1420 CONTINUE 00432*21 C 00433*22 C***********************************************************************00434*22 C 00435*22 C COMPUTE FILTER GAINS 00436 C 00437 IF(IZCODE.EQ.2)GO TO 1300 00438*13 IZUPT=IZUPT+1 00439*27 CALL MATMPY(PF,HF,X1,N,N,L,MM,3) 00440 CALL MATMPY(HF,X1,X2,L,N,L,MM,1) 00441 DO 1000 I=1,L 00442 DO 1000 J=1,L 00443 1000 X2(I,J) = X2(I,J) + RF(I,J) 00444 CALL SPIN(X2,L,MM,DET,IDEXP) 00445*23 CALL MATMPY(X1,X2,K,N,L,L,MM,1) 00446 WRITE(6,6017) 00447*24 CALL MOUTD(K,MM,N,L) 00448*24 C 00449 C***********************************************************************00450 C 00451 C UPDATE FILTER COVARIANCE 00452 C 00453 CALL MATMPY(K,HF,X1,N,L,N,MM,1) 00454 DO 1001 I=1,N 00455 X1(I,I)=X1(I,I)-1.D0 00456*20 DO 1001 J=1,N 00457 1001 X1(I,J) = -X1(I,J) 00458 CALL MATMPY(X1,PF,X2,N,N,N,MM,1) 00459 CALL MATMPY(X2,X1,PPF,N,N,N,MM,3) 00460**9 CALL MATMPY(K,RF,X1,N,L,L,MM,1) 00461 CALL MATMPY(X1,K,X2,N,L,N,MM,3) 00462 DO 1002 I=1,N 00463 DO 1002 J=1,N 00464 1002 PPF(I,J) = PPF(I,J) + X2(I,J) 00465**9 C 00466 C***********************************************************************00467 C 00468 C UPDATE SYSTEM COVARIANCE 00469 C 00470 CALL MATMPY(K,H,X1,N,L,M,MM,1) 00471 DO 1003 I=1,N 00472*12 DO 1004 J=1,M 00473*12 1004 X2(I,J)=-X1(I,J) 00474*12 1003 X2(I,I)=1.D0-X1(I,I) 00475*12 C 00476*25 C UPDATE CROSS-COVARIANCE MATRICES (CC, CP) AT END OF ZUPT INTERVAL 00477*25 CALL MATMPY(CC,PHI,X3,M,M,M,MM,3) 00478*25 CALL MATMPY(X3,X2,CC,M,M,M,MM,3) 00479*25 CALL MATMPY(CP,PHI,X3,M,M,M,MM,3) 00480*25 CALL MATMPY(X3,X2,CP,M,M,M,MM,3) 00481*25 C PRINT ZUPT CROSS-COVARIANCE MATRICES CC, CP 00482*25 WRITE(6,6048) 00483*25 CALL MOUTD(CC,MM,M,M) 00484*25 WRITE(6,6049) 00485*25 CALL MOUTD(CP,MM,M,M) 00486*25 C 00487*25 NN=N+1 00488*12 DO 1005 I=NN,M 00489*12 DO 1009 J=1,M 00490*12 X2(J,I)=0.D0 00491*23 1009 X2(I,J)=0.D0 00492*12 1005 X2(I,I)=1.D0 00493*12 CALL MATMPY(X2,P,X3,M,M,M,MM,1) 00494*12 CALL MATMPY(X3,X2,PP,M,M,M,MM,3) 00495*12 CALL MATMPY(K,R,X1,N,L,L,MM,1) 00496 CALL MATMPY(X1,K,X2,N,L,N,MM,3) 00497 DO 1006 I=1,N 00498 DO 1006 J=1,N 00499 1006 PP(I,J)=PP(I,J)+X2(I,J) 00500*12 C 00501*27 C***********************************************************************00502*27 C 00503*27 C COMPUTE STANDARD DEVIATIONS 00504*27 C 00505*27 CALL RMS(P,RMSP,M,IZUPT,RLAT,HEIGHT,IFCODE) 00506*27 CALL RMS(PP,RMSPP,M,IZUPT,RLAT,HEIGHT,IFCODE) 00507*27 CALL RMS(PF,RMSPF,N,IZUPT,RLAT,HEIGHT,IFCODE) 00508*27 CALL RMS(PPF,RMSPPF,N,IZUPT,RLAT,HEIGHT,IFCODE) 00509*27 C 00510*25 WRITE(6,6054) 00511*28 WRITE(6,6055)NAME(5),(RMSP(IZUPT,I),I=1,M) 00512*28 WRITE(6,6055)NAME(3),(RMSPP(IZUPT,I),I=1,N) 00513*28 WRITE(6,6055)NAME(6),(RMSPF(IZUPT,I),I=1,M) 00514*28 WRITE(6,6055)NAME(4),(RMSPPF(IZUPT,I),I=1,N) 00515*28 C***********************************************************************00516*25 C 00517*25 C REINITIALIZE ZUPT CROSS-COVARIANCE MATRIX AT START OF ZUPT INTERVAL 00518*26 C 00519*25 DO 1011 I=1,M 00520*25 DO 1011 J=1,M 00521*25 1011 CC(I,J)=PP(I,J) 00522*25 NZUP=0 00523*27 IF(IZUPT.NE.NZUP)GO TO 1012 00524*27 WRITE(6,6051)IZUPT 00525*27 DO 1013 I=1,7 00526*27 DO 1013 J=1,M 00527*27 1013 CP(I,J)=PP(I,J) 00528*27 1012 CONTINUE 00529*27 IF(IZUPT.NE.4)GO TO 1014 00530*27 IF(IZUPT.NE.9999)GO TO 1014 00531*27 WRITE(6,6052)IZUPT 00532*27 XYZ=Q(1,1)*10.D0 00533*27 Q(1,1)=XYZ 00534*27 Q(2,2)=XYZ 00535*27 Q(7,7)=XYZ 00536*27 QF(1,1)=XYZ 00537*27 QF(2,2)=XYZ 00538*27 QF(7,7)=XYZ 00539*27 1014 CONTINUE 00540*27 GO TO 1301 00541*13 C 00542*13 C***********************************************************************00543*13 C 00544*13 C UPDATE FILTER AND SYSTEM COVARIANCE WHEN MOVING: H=HF=K=0 00545*13 C 00546*13 1300 CONTINUE 00547*13 DO 1302 I=1,N 00548*13 DO 1302 J=1,N 00549*13 1302 PPF(I,J)=PF(I,J) 00550*13 DO 1303 I=1,M 00551*13 DO 1303 J=1,M 00552*13 1303 PP(I,J)=P(I,J) 00553*13 1301 CONTINUE 00554*13 C 00555 C***********************************************************************00556 C 00557 C PRINT K,PP,PPF 00558*15 C IF IZCODE.EQ.1, PRINT COV PP,PPF AND CORREL PP,PPF 00559*22 C 00560 IF(IZCODE.EQ.1)WRITE(6,6023)T 00561*21 IF(IZCODE.EQ.1)GO TO 1415 00562*21 IF(IPRINT.NE.IPCODE)GO TO 1304 00563*14 C PRINT K 00564*15 IF(IOCODE(10).EQ.0)GO TO 1410 00565*15 WRITE(6,6017) 00566*14 CALL MOUTD(K,MM,N,L) 00567*14 1410 IF(IOCODE(1).EQ.1)GO TO 1411 00568*15 IF(IOCODE(1).EQ.2)GO TO 1412 00569*15 C PRINT COVARIANCES MATRICES PP, PPF 00570*15 IF(IOCODE(8).EQ.0)GO TO 1413 00571*15 1415 CONTINUE 00572*16 WRITE(6,6006) 00573*14 CALL MOUTD(PP,MM,M,M) 00574*14 IF(IZCODE.EQ.1)GO TO 1416 00575*21 1413 IF(IOCODE(9).EQ.0)GO TO 1304 00576*15 1416 CONTINUE 00577*16 WRITE(6,6014) 00578*14 CALL MOUTD(PPF,MM,N,N) 00579*14 IF(IZCODE.EQ.1)GO TO 1417 00580*21 C PRINT CORRELATION MATRICES OF PP, PPF 00581*15 1411 IF(IOCODE(8).EQ.0)GO TO 1414 00582*15 1417 CONTINUE 00583*16 WRITE(6,6034) 00584*15 CALL CORREL(PP,X1,MM,M) 00585*15 CALL MOUTD(X1,MM,M,M) 00586*15 IF(IZCODE.EQ.1)GO TO 1418 00587*21 1414 IF(IOCODE(9).EQ.0)GO TO 1304 00588*15 1418 CONTINUE 00589*16 WRITE(6,6037) 00590*15 CALL CORREL(PPF,X1,MM,N) 00591*15 CALL MOUTD(X1,MM,N,N) 00592*15 IF(IOCODE(1).EQ.2)GO TO 1412 00593*16 GO TO 1304 00594*15 C PRINT RMS VALUES OF PP,PPF 00595*15 1412 IF(IOCODE(8).EQ.1)CALL RMSOUT(PP,MM,M,NAME(3)) 00596*15 IF(IOCODE(9).EQ.1)CALL RMSOUT(PPF,MM,N,NAME(4)) 00597*15 1304 CONTINUE 00598*14 C 00599 C 00600 C***********************************************************************00601 C 00602 C T=TF? 00603 C IF YES - STOP 00604 C 00605 IF((T+1.D-6).GE.TF)GO TO 8888 00606*17 C 00607 C***********************************************************************00608 C 00609 C PROPAGATE FILTER COVARIANCE 00610 C 00611 CALL MATMPY(PHIF,PPF,X1,N,N,N,MM,1) 00612**9 CALL MATMPY(X1,PHIF,PF,N,N,N,MM,3) 00613 DO 1007 I=1,N 00614 DO 1007 J=1,N 00615 1007 PF(I,J) = PF(I,J) + QNDTF(I,J) 00616*10 C 00617 C***********************************************************************00618 C 00619 C PROPAGATE SYSTEM COVARIANCE 00620 C 00621 CALL MATMPY(PHI,PP,X1,M,M,M,MM,1) 00622 CALL MATMPY(X1,PHI,P,M,M,M,MM,3) 00623 DO 1008 I=1,M 00624 DO 1008 J=1,M 00625 1008 P(I,J) = P(I,J) + QNDT(I,J) 00626*10 C 00627 C***********************************************************************00628 C 00629 C PROPAGATE ZUPT CROSS-COVARIANCE MATRICES CC, CP 00630*25 C 00631*25 CALL MATMPY(CC,PHI,X1,M,M,M,MM,3) 00632*25 CALL MATMPY(CP,PHI,X2,M,M,M,MM,3) 00633*25 DO 1010 I=1,M 00634*25 DO 1010 J=1,M 00635*25 CC(I,J)=X1(I,J) 00636*25 CP(I,J)=X2(I,J) 00637*25 1010 CONTINUE 00638*25 C 00639*25 C***********************************************************************00640*25 C 00641*25 C INCREMENT TIME 00642 C 00643 IPRINT=IPRINT+1 00644*14 T = T + DT 00645 IF(ITRAV.EQ.1)GO TO 9000 00646 IF(ITRAV.GT.1)GO TO 9001 00647 WRITE(6,6022)ITRAV 00648 C 00649 C***********************************************************************00650 9002 WRITE(6,6024) 00651*10 STOP 00652*10 9003 WRITE(6,6028)I,ZUPINT(I) 00653*12 STOP 00654*12 9004 WRITE(6,6029) 00655*12 STOP 00656*12 9005 WRITE(6,6030)IZCODE 00657*13 STOP 00658*13 8000 WRITE(6,6025) 00659*10 8888 WRITE(6,6031) 00660*15 WRITE(6,6053) 00661*27 CALL MOUTD(RMSP,30,IZUPT,M) 00662*28 CALL MOUTD(RMSPP,30,IZUPT,M) 00663*28 CALL MOUTD(RMSPF,30,IZUPT,N) 00664*28 CALL MOUTD(RMSPPF,30,IZUPT,N) 00665*28 STOP 00666*15 5000 FORMAT(10X,14I5) 00667*16 5001 FORMAT(10X,7D10.3) 00668 5002 FORMAT(10X,I5,I3,F7.3) 00669*16 5003 FORMAT(20A4) 00670*18 6000 FORMAT(' ITRAV =',I5,5X,'=1 CONSTANT DYNAMICS'/20X, 00671*21 @ '.GT.1 TIME VARYING'/ 00672 @ '0 L =',I4,' = MEASUREMENT VECTOR DIMENSION'/ 00673 @ ' M =',I4,' = TRUTH MODEL STATE VECTOR DIMENSION'/ 00674 @ ' N =',I4,' = FILTER MODEL STATE VECTOR DIMENSION') 00675 6001 FORMAT('- TRUTH MODEL INITIAL COVARIANCE MATRIX (PO)') 00676*14 6002 FORMAT('- TRUTH MODEL PROCESS NOISE MATRIX (Q)') 00677*14 6003 FORMAT('- TRUTH MODEL MEASUREMENT NOISE MATRIX (R)') 00678*14 6004 FORMAT('- TRUTH MODEL MEASUREMENT MATRIX (H)') 00679*14 6005 FORMAT('- TRUTH MODEL COVARIANCE MATRIX BEFORE UPDATE (P)') 00680*14 6006 FORMAT('- TRUTH MODEL COVARIANCE MATRIX AFTER UPDATE (PP)') 00681*14 6007 FORMAT('- TRUTH MODEL DYNAMICS MATRIX (F)') 00682*14 6008 FORMAT('- TRUTH MODEL TRANSITION MATRIX (PHI)') 00683*14 6009 FORMAT('- FILTER MODEL INITIAL COVARIANCE MATRIX (POF)') 00684*14 6010 FORMAT('- FILTER MODEL PROCESS NOISE MATRIX (QF)') 00685*14 6011 FORMAT('- FILTER MODEL MEASUREMENT NOISE MATRIX (RF)') 00686*14 6012 FORMAT('- FILTER MODEL MEASUREMENT MATRIX (HF)') 00687*14 6013 FORMAT('- FILTER MODEL COVARIANCE MATRIX BEFORE UPDATE (PF)') 00688*14 6014 FORMAT('- FILTER MODEL COVARIANCE MATRIX AFTER UPDATE (PPF)') 00689*14 6015 FORMAT('- FILTER MODEL DYNAMICS MATRIX (FF)') 00690*14 6016 FORMAT('- FILTER MODEL TRANSITION MATRIX (PHIF)') 00691*14 6017 FORMAT('- GAIN MATRIX (K)') 00692*14 6018 FORMAT(' NUMBER OF DT LOOPS BETWEEN PRINTOUTS =',I5) 00693*14 6019 FORMAT('0 OUTPUT CODES: READ INTO VECTOR IOCODE'/ 00694*15 @' ',I6,' =',I5,' 0 ==> PRINT COVARIANCE MATRICES'/ 00695*15 @' ',13X, ' 1 ==> PRINT CORRELATION MATRICES'/ 00696*15 @' ',13X, ' 2 ==> PRINT STANDARD DEVATIONS'/ 00697*15 @' ',13X, ' OF Q,QF,P,PF,PP,PPF'/ 00698*15 @ ' CODES 2 - 10 0 ==> NO PRINT'/ 00699*15 @' ',15X, '1 ==> PRINT'/ 00700*15 @' ',I6,' =',I3,' F ',I6,' =',I3,' FF'/ 00701*15 @' ',I6,' =',I3,' PHI',I6,' =',I3,' PHIF'/ 00702*15 @' ',I6,' =',I3,' Q ',I6,' =',I3,' QF'/ 00703*15 @' ',I6,' =',I3,' PP ',I6,' =',I3,' PPF'/ 00704*15 @' ',I6,' =',I3,' K'/' ',I6,' =',I3,' P ',I6,' =',I3,' PF'/) 00705*21 6020 FORMAT(' KALMAN UPDATE INTERVAL = DT =',F10.4,' SECONDS'/ 00706*17 #' NUMBER OF INTEGRATION INTERVALS PER UPDATE INTERVAL = NDTI =',00707 @I5/' INTEGRATION INTERVAL = DTI =',F10.4,' SECONDS'/ 00708*17 @' NUMBER OF ', 00709*16 @'TERMS USED IN COMPUTATION OF SYSTEM TRANSITION AND QK MATRICES ='00710*10 @,I5/' NUMBER OF TERMS USED IN COMPUTATION OF FILTER TRANSITION',00711*10 @' AND QKF MATRICES =',I5) 00712*14 6021 FORMAT(' INITIAL LATITUDE =',I5,I3,F7.3) 00713*16 6022 FORMAT(' *** TRAVERSE SELECT CODE = ITRAV =',I5,' IS LESS THAN 1')00714 6023 FORMAT('0 TIME =',F10.4,' SECONDS'/' ',21('-')) 00715*17 6024 FORMAT(' ERROR IN CHOLD - WRONG DIMENSIONS OR UNSTABLE') 00716*10 6025 FORMAT(' NUMBER OF TERMS TO COMPUTE PHI AND QK IS LESS THAN ONE')00717*10 6026 FORMAT('0 ***TRAJECTORY DATA***'/ 00718*12 @,' NUMBER OF ZUPT INTERVALS =',I5/ 00719*12 @,' ZUPT DURATION =',F10.4,' SECONDS'/ 00720*17 @,' NUMBER OF ZERO VELOCITY OBSERVATIONS PER ZUPT =',I5/ 00721*21 @,' HORIZONTAL ACCELERATION INTERVAL =',F10.4,' SECONDS'/ 00722*17 @,' HORIZONTAL ACCELERATION =',F10.4,' METRES/SECOND**2'/ 00723*17 @,' MAXIMUM HORIZONTAL VELOCITY =',F10.4,' KILOMETRES/HOUR'/ 00724*17 @,' VERTICAL ACCELERATION INTERVAL =',F10.4,' SECONDS'/ 00725*17 @,' VERTICAL ACCELERATION =',F10.4,' METRES/SECOND**2'/ 00726*17 @,' MAXIMUM VERTICAL VELOCITY =',F10.4,' METRES/SECOND'/ 00727*17 @,' MAXIMUM HEIGHT ABOVE GROUND =',F8.2,' METRES'/ ) 00728*12 6027 FORMAT('0 LEG ZUPT INTERVAL HEIGHT AZIMUTH'/ 00729*12 @ ,' (SECONDS) (METRES) (DEGREES)'/ 00730*12 @ ,' ',20X,F9.2/(' ',I5,F13.4,F11.2,F11.2)) 00731*17 6028 FORMAT(' ***ZUPINT(',I3,') =',F6.1,' IS TOO SMALL TO REACH VMAX') 00732*12 6029 FORMAT(' ***ERROR IN TRAJ: T.GT.TF***') 00733*12 6030 FORMAT(' ***ERROR IN TVM/TRAJ: IZCODE =',I5,' SHOULD BE=1 OR2')00734*13 6031 FORMAT('- RUN COMPLETED'/' ',100('*')) 00735*15 6032 FORMAT('- TRUTH MODEL PROCESS NOISE CORRELATION MATRIX(Q)') 00736*15 6033 FORMAT('- TRUTH MODEL CORRELATION MATRIX BEFORE UPDATE(P)') 00737*15 6034 FORMAT('- TRUTH MODEL CORRELATION MATRIX AFTER UPDATE(PP)') 00738*15 6035 FORMAT('- FILTER MODEL PROCESS NOISE CORRELATION MATRIX(QF)') 00739*15 6036 FORMAT('- FILTER MODEL CORRELATION MATRIX BEFORE UPDATE(PF)') 00740*15 6037 FORMAT('- FILTER MODEL CORRELATION MATRIX AFTER UPDATE(PPF)') 00741*15 6038 FORMAT(' INITIAL HEIGHT =',F10.3,' METRES') 00742*16 6039 FORMAT(' LENGTH OF RUN = FINAL TIME = TF(+DT) =',F10.4) 00743*17 6040 FORMAT('- LOGICAL PROBLEM IN TIME SAVER: ICONST =',I5,' IDTI =',I500744*18 @,' T =',F12.6,' TI =',F12.6////) 00745*18 6041 FORMAT('0 ',90('*')/' *',88X,'*'/' * ',20A4,' *'/ 00746*18 @' *',88X,'*'/' ',90('*')///) 00747*18 6042 FORMAT(' LONGITUDE VELOCITY FOR CONSTANT DYNAMICS RUN =',F8.2 00748*20 @,' KILOMETRES/HOUR') 00749*20 6043 FORMAT(' IFCODE =',I5,' = F,FF MATRICES SELECT CODE'/ 00750*21 @17X,'= 1 ==> BRITTING P.230'/17X,'= 2 ==> SCHMIDT P.1-8') 00751*21 6044 FORMAT(' NZVOB =',I5,' = NUMBER OF ZERO VELOCITY OBSERVATIONS', 00752*21 @' PER ZUPT') 00753*21 6045 FORMAT(' NZDT IS LESS THAN 1 ==> FATAL ERROR') 00754*21 6046 FORMAT(' LATITUDES OF START POINT AND ZUPT POINTS') 00755*22 6047 FORMAT(' ',I5,I7,I3,F7.3) 00756*22 6048 FORMAT(' CROSS-COVARIANCE MATRIX BETWEEN THIS AND LAST ZUPT PT')00757*25 6049 FORMAT(' CROSS-COVARIANCE MATRIX BETWEEN INITIAL & CURRENT ZUPT'00758*25 @,' POINTS') 00759*25 6050 FORMAT('- ***** INPUT UNITS IN SEC**2 (AS PRINTED ABOVE) CONVERT00760*26 &ED TO RADIAN**2 *****'/' ALL SUBSEQUENT OUTPUT IS IN RAD00761*26 &IAN**2 (EXCEPT METRE**2 FOR HEIGHT AND HEIGHT VELOCITY)') 00762*26 6051 FORMAT(' CROSS-COVARIANCE MATRIX FROM INITIAL POINT REINITIALIZE00763*27 &D AT ZUPT',I5) 00764*27 6052 FORMAT('0 Q, QF DIAGONAL ELEMENTS 1,2,7 MULTIPLIED BY 10 AT ZUPT'00765*27 &,I5) 00766*27 6053 FORMAT('0 STANDARD DEVIATIONS OF P,PP,PF,PPF') 00767*27 6054 FORMAT('0 STANDARD DEVIATIONS IN ARC-SECONDS AND METRES'/) 00768*28 6055 FORMAT(' ',2X,A4/(3X,7D14.6)) 00769*28 C 00770*15 END 00771 C***********************************************************************00772 C***********************************************************************00773 C 00774 BLOCK DATA 00775 IMPLICIT REAL*8(A-H,O-Z) 00776 REAL*8 K 00777 COMMON/TRUTH/F(20,20),H(20,20),P(20,20),PP(20,20),PHI(20,20), 00778 @ PO(20,20),Q(20,20),QK(20,20),QNDT(20,20),R(20,20) 00779*10 COMMON/FILTER/FF(20,20),HF(20,20),K(20,20),PF(20,20),PPF(20,20), 00780**9 @PHIF(20,20),POF(20,20),QF(20,20),QKF(20,20),QNDTF(20,20),RF(20,20)00781*10 COMMON/CONST/A,B,WEARTH,PI,RADIAN,MM 00782*13 DATA F,FF,H,HF,P,PF,PP,PPF,PHI,PHIF,PO,POF,Q,QF,QK,QKF, 00783*10 @ QNDT,QNDTF,R,RF,K/8400*0.D0/ 00784*10 DATA A,B,WEARTH/6378160.0,6356774.52,7.2921151467D-5/ 00785 END 00786 C 00787 C***********************************************************************00788*15 C 00789*15 SUBROUTINE PHIQK(PHI,QK,F,Q,DTI,N,MM,NTERM,*) 00790*10 C 00791*13 C INPUT: F,Q,DTI,N,MM,NTERM 00792*13 C 00793*13 C OUTPUT: PHI = TRANSITION MATRIX 00794*13 C QK = DISCRETE NOISE MATRIX 00795*13 C 00796*13 C VARIABLES: D,THETA,QDT,FDT,FN,I,J,X,Y,TERM,ITERM 00797*13 C 00798*13 C THE ALGORITHM USED HERE IS DESCRIBED BY J.A.D'APPOLITO, PROC. OF IEEE00799*23 C VOL.54,NO.12,PP.2010,2011. 00800*23 C 00801*23 IMPLICIT REAL*8(A-H,O-Z) 00802*10 COMMON/SCRTCH/X1(20,20),X2(20,20),X3(20,20) 00803*10 DIMENSION PHI(MM,MM),QK(MM,MM),F(MM,MM),Q(MM,MM) 00804*10 DIMENSION D(20,20),THETA(20,20),QDT(20,20),FDT(20,20),FN(20,20) 00805*10 IF(NTERM.LT.1)RETURN 1 00806*10 DO 1000 I=1,N 00807*10 DO 1000 J=1,N 00808*10 X=Q(I,J)*DTI 00809*10 Y=F(I,J)*DTI 00810*10 D(I,J)=X 00811*10 QDT(I,J)=X 00812*10 THETA(I,J)=X 00813*10 FDT(I,J)=Y 00814*10 FN(I,J)=Y 00815*10 PHI(I,J)=Y 00816*10 1000 CONTINUE 00817*10 DO 1001 I=1,N 00818*10 1001 PHI(I,I)=1.D0+PHI(I,I) 00819*10 IF(NTERM.EQ.1)GO TO 9000 00820*10 DO 1002 ITERM=2,NTERM 00821*10 TERM=DFLOAT(ITERM) 00822*10 CALL MATMPY(FN,QDT,X1,N,N,N,MM,1) 00823*10 CALL MATMPY(D,FDT,X2,N,N,N,MM,3) 00824*10 CALL MATMPY(FDT,FN,X3,N,N,N,MM,1) 00825*10 DO 1003 I=1,N 00826*10 DO 1003 J=1,N 00827*10 D(I,J)=(X1(I,J)-X2(I,J))/TERM 00828*10 FN(I,J)=X3(I,J)/TERM 00829*10 THETA(I,J)=THETA(I,J)+D(I,J) 00830*10 PHI(I,J)=PHI(I,J)+FN(I,J) 00831*10 1003 CONTINUE 00832*10 1002 CONTINUE 00833*10 9000 CONTINUE 00834*10 CALL MATMPY(THETA,PHI,QK,N,N,N,MM,3) 00835*10 CALL SYMMAT(QK,MM,N) 00836*12 RETURN 00837*10 END 00838*10 C 00839*15 C***********************************************************************00840*15 C 00841*15 SUBROUTINE CORREL(X,Y,MM,N) 00842*12 C THIS SUBROUTINE COMPUTES A CORRELATION MATRIX (Y) FROM AN INPUT 00843*12 C COVARIANCE MATRIX (X) 00844*12 C 00845*13 C INPUT: X = COVARIANCE MATRIX 00846*13 C MM = FULL DIMENSION OF X AND Y 00847*15 C N = DIMENSION OF PARTS OF X AND Y BEING USED 00848*15 C 00849*13 C OUTPUT: Y = CORRELATION MATRIX 00850*13 C 00851*13 REAL*8 X(MM,MM),Y(MM,MM) 00852*12 DO 1000 I=1,N 00853*12 1000 Y(I,I)=DSQRT(X(I,I)) 00854*12 NN=N-1 00855*12 DO 1001 I=1,NN 00856*12 II=I+1 00857*12 DO 1001 J=II,N 00858*12 Y(I,J)=X(I,J)/(Y(I,I)*Y(J,J)) 00859*12 1001 Y(J,I)=Y(I,J) 00860*12 DO 1002 I=1,N 00861*12 1002 Y(I,I)=1.D0 00862*12 RETURN 00863*12 END 00864*12 C 00865 C***********************************************************************00866*15 C 00867*15 SUBROUTINE TVM(T,IFCODE,IZCODE,ICONST,*,*) 00868*21 C COMPUTE TIME VARYING MATRIX ELEMENTS 00869*13 C UNSPECIFIED ELEMENTS OF F AND FF ARE INITIALIZED AS ZERO IN SR BLOCK 00870 IMPLICIT REAL*8(A-H,O-Z) 00871 REAL*8 K 00872 REAL*8 K1,K2 00873*25 COMMON/TRUTH/F(20,20),H(20,20),P(20,20),PP(20,20),PHI(20,20), 00874 @ PO(20,20),Q(20,20),QK(20,20),QNDT(20,20),R(20,20) 00875*10 COMMON/FILTER/FF(20,20),HF(20,20),K(20,20),PF(20,20),PPF(20,20), 00876**9 @PHIF(20,20),POF(20,20),QF(20,20),QKF(20,20),QNDTF(20,20),RF(20,20)00877*10 COMMON/CONST/A,B,WEARTH,PI,RADIAN,MM 00878*13 COMMON/TRAJEC/RLAT,RLONG,HEIGHT,VLAT,VLONG,VH,ALAT,ALONG,AH 00879 COMMON/INPUT/DT,DTI,TF,RLATO,HO,ITRAV,L,M,N,NDTI,NTERMP,NZVOB 00880*23 CALL TRAJ(T,IZCODE,ICONST,9004) 00881*18 C IF ICONST.GE.1 ==> F MATRIX IS SAME AS LAST INTERVAL ==> RETURN 00882*23 IF(ICONST.GE.1)RETURN 00883*22 SP=DSIN(RLAT) 00884 CP=DCOS(RLAT) 00885 G=GRAV67(RLAT,HEIGHT) 00886 RAD=RADIUS(RLAT,RM,RN,A,B)+HEIGHT 00887*22 V=VLONG+WEARTH 00888 GO TO (1000,1001),IFCODE 00889*23 1000 CONTINUE 00890*23 C 7 BY 7 MATRIX FROM BRITTING P.230 00891*23 C COMPUTE ELEMENTS OF TRUTH MODEL DYNAMICS MATRIX 00892*23 F(1,2)=-V*SP 00893 F(1,3)=-V*SP 00894**3 F(1,6)=CP 00895**3 F(2,1)=V*SP 00896**3 F(2,7)=V*CP 00897*22 F(2,5)=-1.D0 00898**3 F(7,2)=-V*CP 00899**3 F(7,3)=-V*CP 00900**3 F(7,6)=-SP 00901**3 F(3,5)=1.D0 00902**3 F(4,6)=1.D0 00903**3 F(5,2)=G/RAD 00904**3 F(6,1)=-G/RAD/CP 00905**3 C COMPUTE ELEMENTS OF FILTER MODEL DYNAMICS MATRIX 00906 FF(1,2)=-V*SP 00907 FF(1,3)=-V*SP 00908 FF(1,6)=CP 00909 FF(2,1)=V*SP 00910 FF(2,5)=-1.D0 00911 FF(3,5)=1.D0 00912 FF(4,6)=1.D0 00913 FF(5,2)=G/RAD 00914 FF(6,1)=-G/RAD/CP 00915 FF(2,7)=V*CP 00916*22 FF(7,2)=-V*CP 00917*22 FF(7,3)=-V*CP 00918*22 FF(7,6)=-SP 00919*22 RETURN 00920 1001 CONTINUE 00921*23 C 9 BY 9 F MATRIX FROM SCHMIDT 00922*23 S2P=DSIN(RLAT+RLAT) 00923*23 C2P=DCOS(RLAT+RLAT) 00924*23 TP=DTAN(RLAT) 00925*23 FN=ALAT*(RM+HEIGHT) 00926*25 FE=ALONG*(RN+HEIGHT)*CP 00927*25 FD=-AH-G 00928*25 K1=0.6D0*G/RAD 00929*25 K2=2.D0*G/RAD 00930*25 C1=VLONG*(V+WEARTH) 00931*23 F(1,2)=-V*SP 00932*23 F(1,4)= CP 00933*23 F(1,5)=-V*SP 00934*25 F(1,7)= VLAT 00935*23 F(2,1)= V*SP 00936*23 F(2,3)=-1.D0 00937*23 F(2,7)= V*CP 00938*23 F(3,2)=-FD/RAD 00939*23 F(3,3)=-2.D0*VH/RAD 00940*23 F(3,4)=-V*S2P 00941*23 F(3,5)=-C1*C2P 00942*23 F(3,7)= FE/RAD 00943*23 F(3,8)=-2.D0*VLAT/RAD 00944*23 F(3,9)=-(ALAT+0.5D0*C1*S2P)/RAD 00945*23 F(4,1)= FD/(RAD*CP) 00946*23 F(4,3)= 2.D0*V*TP 00947*23 F(4,4)=-2.D0*(VH/RAD-VLAT*TP) 00948*23 F(4,5)= (ALONG+2.D0*V*VH/RAD)*TP+2.D0*V*VLAT 00949*23 F(4,7)=-FN/(RAD*CP) 00950*23 F(4,8)=-2.D0*V/RAD 00951*23 F(4,9)=-(ALONG-2.D0*VLAT*V*TP)/RAD 00952*23 F(5,3)= 1.D0 00953*23 F(6,4)= 1.D0 00954*23 F(7,1)=-VLAT 00955*23 F(7,2)=-V*CP 00956*23 F(7,4)=-SP 00957*23 F(7,5)=-V*CP 00958*23 F(8,1)= FE 00959*23 F(8,2)=-FN 00960*23 F(8,3)= 2.D0*RAD*VLAT 00961*23 F(8,4)= 2.D0*RAD*V*CP*CP 00962*23 F(8,5)=-RAD*C1*S2P 00963*23 F(8,9)= VLAT*VLAT+C1*CP*CP+2.D0*G/RAD-K2 00964*25 F(9,8)= 1.D0 00965*23 F(9,9)=-K1 00966*25 DO 2000 I=1,N 00967*23 DO 2000 J=1,N 00968*23 2000 FF(I,J)=F(I,J) 00969*23 9004 RETURN 1 00970*12 END 00971 C 00972 C***********************************************************************00973*15 C 00974*15 SUBROUTINE TRAJ(T,IZCODE,ICONST,*) 00975*18 C COMPUTE POSITION, VELOCITY AND ACCELERATION VECTORS AT TIME T 00976*13 IMPLICIT REAL*8(A-H,O-Z) 00977 COMMON/TRAJIN/NZUPT,ZUPDUR,ACC,VMAX,ACCV,VVMAX,HMAX,TCRUZ,TVA,TVV 00978*12 @ ,TA,TA2,ZUPINT(30),HT(31),AZIM(30),XLAT(31) 00979*12 COMMON/TRAJEC/RLAT,RLONG,HEIGHT,VLAT,VLONG,VH,ALAT,ALONG,AH 00980**3 COMMON/INPUT/DT,DTI,TF,RLATO,HO,ITRAV,L,M,N,NDTI,NTERMP,NZVOB 00981*21 COMMON/CONST/A,B,WEARTH,PI,RADIAN,MM 00982*13 C 00983 GO TO (1000,1001,1002),ITRAV 00984 C 00985 C CONSTANT DYNAMICS 00986 1000 CONTINUE 00987 ICONST=0 00988*19 IZCODE=0 00989*23 RLAT=RLATO 00990 RLONG=0.D0 00991 HEIGHT=HO 00992 VLAT=0.D0 00993 VH=0.D0 00994 ALAT=0.D0 00995 ALONG=0.D0 00996 AH=0.D0 00997 RETURN 00998 C 00999*12 C TRAVERSE - MOVING PLATFORM 01000*12 1001 CONTINUE 01001 ICONST=0 01002*18 TT=T 01003*12 NZUPT1=NZUPT+1 01004*17 DO 1100 I=1,NZUPT1 01005*17 IF(TT.LT.ZUPINT(I))GO TO 1111 01006*21 TT=TT-ZUPINT(I) 01007*21 IF(TT.LE.(ZUPDUR+1.D-6))GO TO 1101 01008*17 TT=TT-ZUPDUR 01009*12 1100 CONTINUE 01010*12 RETURN 1 01011*21 C COMPUTE OUTPUT FOR ZUPT DURATION 01012*12 1101 CONTINUE 01013*17 IF(((DTI-0.001).LE.TT).AND.(TT.LE.(ZUPDUR-DTI+0.001)))ICONST=1 01014*22 IF((ICONST.EQ.1).AND.((DT-0.001).LE.TT))ICONST=2 01015*22 IZCODE=2 01016*21 ZDT=ZUPDUR/DFLOAT(NZVOB) 01017*21 X=TT/ZDT 01018*21 NX=IDINT(X) 01019*21 IF(((DFLOAT(NX)-0.001).LE.X).AND.(X.LE.(DFLOAT(NX)+(DT-DTI)/ZDT+ 01020*21 @0.001)))IZCODE=1 01021*21 AH=0.D0 01022*23 VH=0.D0 01023*23 HEIGHT=HT(I) 01024*23 ALAT=0.D0 01025*23 VLAT=0.D0 01026*23 RLAT=XLAT(I) 01027*23 ALONG=0.D0 01028*23 VLONG=0.D0 01029*23 RETURN 01030*12 C COMPUTE OUTPUT FOR ZUPT INTERVAL (DURING MOVEMENT) 01031*12 1111 CONTINUE 01032*12 TZA=ZUPINT(I)-TA2 01033*12 IZCODE=2 01034*16 C COMPUTE ELEVATION 01035*12 TVCRUZ=ZUPINT(I)-4.D0*TVA-2.D0*TVV 01036*12 VVCRUZ=(HT(I+1)-HT(I))/TVCRUZ 01037*12 TACRUZ=VVCRUZ/ACCV 01038*12 T1=TVA 01039*12 T2=T1+TVV 01040*12 T3=T2+TVA-TACRUZ 01041*12 T4=T3+TVCRUZ 01042*12 T5=T4+TVA+TACRUZ 01043*12 T6=T5+TVV 01044*12 T7=T6+TVA 01045*12 IF(TT.GT.T1)GO TO 1105 01046*12 AH=ACCV 01047*23 VH=ACCV*TT 01048*23 HEIGHT=HT(I)+0.5D0*VH*TT 01049*23 GO TO 1102 01050*23 1105 IF(TT.GT.T2)GO TO 1106 01051*12 AH=0.D0 01052*23 VH=VVMAX 01053*23 HEIGHT=HT(I)+0.5D0*ACCV*T1*T1+VVMAX*(TT-T1) 01054*23 GO TO 1102 01055*23 1106 IF(TT.GT.T3)GO TO 1107 01056*12 AH=-ACCV 01057*23 VH=VVMAX-ACCV*(TT-T2) 01058*23 HEIGHT=HT(I)+0.5D0*ACCV*(T1*T1-(TT-T2)*(TT-T2))+VVMAX*(TT-T1) 01059*23 GO TO 1102 01060*23 1107 IF(TT.GT.T4)GO TO 1108 01061*12 IF((TT.GT.(TA+DTI-0.001)).AND.(TT.LE.(TA+TZA-DTI+0.001)).AND. 01062*22 @(DABS(DCOS(AZIM(I))).LE.(1.D-5)).AND.(TT.GT.(T3+DTI-0.001)).AND. 01063*22 @(DABS(VVCRUZ).LE.(1.D-5)))ICONST=1 01064*22 IF((ICONST.EQ.1).AND.(TT.GT.(TA+DT-0.001)).AND. 01065*22 @(TT.GT.(T3+DT-0.001)))ICONST=2 01066*22 AH=0.D0 01067*23 VH=VVCRUZ 01068*23 HEIGHT=HT(I)+0.5D0*ACCV*(T1*T1-(T3-T2)*(T3-T2))+VVMAX*(T3-T1) 01069*23 @ +VVCRUZ*(TT-T3) 01070*23 GO TO 1102 01071*23 1108 IF(TT.GT.T5)GO TO 1109 01072*12 AH=-ACCV 01073*23 VH=VVCRUZ-ACCV*(TT-T4) 01074*23 HEIGHT=HT(I)+0.5D0*ACCV*(T1*T1-(T3-T2)*(T3-T2)-(TT-T4)*(TT-T4)) 01075*23 @ +VVMAX*(T3-T1)+VVCRUZ*(TT-T3) 01076*23 GO TO 1102 01077*23 1109 IF(TT.GT.T6)GO TO 1110 01078*12 AH=0.D0 01079*23 VH=-VVMAX 01080*23 HEIGHT=HT(I)+0.5D0*ACCV*(T1*T1-(T3-T2)*(T3-T2)-(T5-T4)*(T5-T4)) 01081*23 @ +VVMAX*((T3-T1)-(TT-T5))+VVCRUZ*(T5-T3) 01082*23 GO TO 1102 01083*23 1110 CONTINUE 01084*23 AH=ACCV 01085*23 VH=-VVMAX+ACCV*(TT-T6) 01086*23 HEIGHT=HT(I)+0.5D0*ACCV*(T1*T1-(T3-T2)*(T3-T2)-(T5-T4)*(T5-T4) 01087*23 @ +(TT-T6)*(TT-T6)) 01088*23 @ +VVMAX*((T3-T1)-(TT-T5))+VVCRUZ*(T5-T3) 01089*23 C COMPUTE HORIZONTAL COMPONENTS 01090*23 1102 CONTINUE 01091*23 SA=DSIN(AZIM(I)) 01092*23 CA=DCOS(AZIM(I)) 01093*23 R=RADIUS(XLAT(I),RM,RN,A,B) 01094*23 IF(TT.GT.TA)GO TO 1103 01095*23 ALAT=ACC*CA/(RM+HEIGHT) 01096*23 VLAT=ALAT*TT 01097*23 RLAT=XLAT(I)+0.5D0*ALAT*TT*TT 01098*23 ALONG=ACC*SA/(RN+HEIGHT)/DCOS(RLAT) 01099*23 VLONG=ALONG*TT 01100*23 RETURN 01101*23 1103 IF(TT.GT.TA+TZA)GO TO 1104 01102*23 ALAT=0.D0 01103*23 CA=CA/(RM+HEIGHT) 01104*23 VLAT=VMAX*CA 01105*23 RLAT=XLAT(I)+0.5D0*ACC*CA*TA*TA+VLAT*(TT-TA) 01106*23 ALONG=0.D0 01107*23 VLONG=VMAX*SA/(RN+HEIGHT)/DCOS(RLAT) 01108*23 RETURN 01109*23 1104 X=TT-TA-TZA 01110*23 CA=CA/(RM+HEIGHT) 01111*23 ALAT=-ACC*CA 01112*23 VLAT=VMAX*CA+ALAT*X 01113*23 RLAT=XLAT(I)+VMAX*CA*(TT-TA)+0.5D0*ALAT*(X*X-TA*TA) 01114*23 SA=SA/(RN+HEIGHT)/DCOS(RLAT) 01115*23 ALONG=-ACC*SA 01116*23 VLONG=VMAX*SA+ALONG*X 01117*23 RETURN 01118*24 1002 CONTINUE 01119 C 01120*13 C THIS SPACE RESERVED FOR A SECOND TRAJECTORY ROUTINE 01121*13 C 01122*13 RETURN 01123 END 01124 C 01125 C***********************************************************************01126*20 C 01127*20 SUBROUTINE MATMPY(M1,M2,M3,L,M,N,MM,ICODE) 01128 C 01129 C **********************************************************************01130 C * *01131 C * COMPUTE MATRIX PRODUCT: M3(L,N) *01132*28 C * *01133 C * INPUT ARGUMENTS: M1(L,M) = PRE-MATRIX (TRANSPOSED DIMENSIONS) *01134 C * M2(M,N) = POST-MATRIX (TANSPOSED DIMENSIONS) *01135 C * TRANSPOSED DIMENSIONS ONLY IF APPLICABLE BY CODE *01136*28 C * L,M,N = DIMENSIONS OF M1 AND M2 *01137 C * MM = ROW DIMENSION OF MATRICES M1,M2,M3 *01138*28 C * ICODE = CODE FOR TYPE OF MATRIX PRODUCT *01139 C * = 1 ==> M3(L,N)=M1(L,M)*M2(M,N) *01140*28 C * = 2 ==> M3(L,N)=M1 TRANSPOSE(L,M)*M2(M,N) *01141*28 C * = 3 ==> M3(L,N)=M1(L,M)*M2 TRANSPOSE(M,N) *01142*28 C * = 4 ==> M3(L,N)=M1 TRANSPOSE(L,M)* *01143*28 C * M2 TRANSPOSE(M,N) *01144*28 C * *01145 C * OUTPUT ARGUMENTS: M3(L,N) = PRODUCT MATRIX *01146 C * *01147 C **********************************************************************01148 C 01149 DOUBLE PRECISION M1,M2,M3 01150 INTEGER I,J,K,L,M,N 01151 DIMENSION M1(MM,MM),M2(MM,MM),M3(MM,MM) 01152 C 01153 GO TO(1,2,3,4),ICODE 01154*24 C M3=M1*M2 01155*24 1 CONTINUE 01156*24 DO 11 I=1,L 01157*24 DO 11 J=1,N 01158*24 M3(I,J)=0.D0 01159*24 DO 11 K=1,M 01160*24 11 M3(I,J)=M3(I,J)+M1(I,K)*M2(K,J) 01161*24 RETURN 01162*24 C M3=M1TRANSPOSE*M2 01163*24 2 CONTINUE 01164*24 DO 22 I=1,L 01165*24 DO 22 J=1,N 01166*24 M3(I,J)=0.D0 01167*24 DO 22 K=1,M 01168*24 22 M3(I,J)=M3(I,J)+M1(K,I)*M2(K,J) 01169*24 RETURN 01170*24 C M3=M1*M2TRANSPOSE 01171*24 3 CONTINUE 01172*24 DO 33 I=1,L 01173*24 DO 33 J=1,N 01174*24 M3(I,J)=0.D0 01175*24 DO 33 K=1,M 01176*24 33 M3(I,J)=M3(I,J)+M1(I,K)*M2(J,K) 01177*24 RETURN 01178*24 DO 44 I=1,L 01181*24 DO 44 J=1,N 01182*24 M3(I,J)=0.D0 01183*24 DO 44 K=1,M 01184*24 44 M3(I,J)=M3(I,J)+M1(K,I)*M2(J,K) 01185*24 RETURN 01186*24 END 01187 C 01188*12 C***********************************************************************01189*12 C 01190*12 SUBROUTINE SYMMAT(X,MM,N) 01191*12 REAL*8 X(MM,MM) 01192*12 C 01193*12 C THIS SUBROUTINE COMPUTES A SYMMETRIC MATRIX FROM A NON-SYMMETRIC 01194*12 C INPUT MATRIX. IT RETURNS THE MEAN OF THE INPUT MATRIX AND ITS 01195*12 C TRANSPOSE 01196*12 C X = INPUT/OUTPUT MATRIX 01197*12 C MM = DIMENSIONED SIZE OF X 01198*12 C N = SIZE OF PART OF X IN USE 01199*12 C 01200*12 NN=N-1 01201*12 DO 1000 I=1,NN 01202*12 II=I+1 01203*12 DO 1000 J=II,N 01204*12 X(I,J)=0.5D0*(X(I,J)+X(J,I)) 01205*12 X(J,I)=X(I,J) 01206*12 1000 CONTINUE 01207*12 RETURN 01208*12 END 01209*12 C 01210*10 C***********************************************************************01211*10 C 01212*10 DOUBLE PRECISION FUNCTION GRAV67(RLAT,HEIGHT) 01213 C COMPUTES NORMAL GRAVITY WITH FREE AIR CORRECTION 01214 C FOR THE I.A.G. 1967 REFERENCE ELLIPSOID 01215 C INPUT 01216 C RLAT =GEODETIC LATITUDE (RADIANS) 01217 C HEIGHT = HEIGHT ABOVE REFERENCE ELLIPSOID (METRES) 01218 C OUTPUT 01219 C GRAV67 = NORMAL GRAVITY WITH FREE AIR CORRECTION (METRES/SECOND**2)01220 IMPLICIT REAL*8(A-H,O-Z) 01221 SP2=DSIN(RLAT)**2 01222 GRAV67=9.7803185D0*(1.D0+5.278895D-3*SP2+2.3462D-5*SP2*SP2) 01223 @ -3.086D-6*HEIGHT 01224 RETURN 01225 END 01226 C 01227*15 C***********************************************************************01228*15 C 01229*15 DOUBLE PRECISION FUNCTION RADIUS(RLAT,RM,RN,A,B) 01230 C COMPUTES RADII OF CURVATURE ON A USER SPECIFIED ELLIPSOID 01231 C INPUT 01232 C RLAT= LATITUDE (RADIANS) 01233 C A = SEMI-MAJOR AXIS (METRES) 01234 C B = SEMI-MINOR AXIS (METRES) 01235 C OUTPUT 01236 C RM = RADIUS OF CURVATURE IN THE MERIDIAN (METRES) 01237 C RN = RADIUS OF CURVATURE IN THE PRIME VERTICAL (METRES) 01238 C RADIUS = EULERIAN MEAN RADIUS OF CURVATURE (METRES) 01239 IMPLICIT REAL*8(A-H,O-Z) 01240 SP2=DSIN(RLAT)**2 01241 E2=(A*A-B*B)/(A*A) 01242 X=(1.D0-E2*SP2) 01243 RN=A/DSQRT(X) 01244 RM=RN*(1.D0-E2)/X 01245 RADIUS=DSQRT(RM*RN) 01246 RETURN 01247 END 01248 C 01249*15 C***********************************************************************01250*22 C 01251*22 SUBROUTINE RADMS(R,ID,IM,S) 01252*22 IMPLICIT REAL*8(A-H,O-Z) 01253*22 D=R*180.D0/DARCOS(-1.D0) 01254*22 ID=IDINT(D) 01255*22 RM=60.D0*DABS(D-DFLOAT(ID)) 01256*22 IM=IDINT(RM) 01257*22 S=60.D0*(RM-DFLOAT(IM)) 01258*22 RETURN 01259*22 END 01260*22 C***********************************************************************01261*15 C 01262*15 SUBROUTINE RMSOUT(X,MM,N,NAME) 01263*15 C THIS SUBROUTINE COMPUTES AND PRINTS THE SQUARE ROOTS OF THE FIRST N 01264*15 C DIAGONAL ELEMENTS OF THE INPUT MATRIX X WHSE DIMENSIONED SIZE IS MM. 01265*15 C MM MUST BE LESS THAN OR EQUAL TO 20 01266*15 C THE INPUT VARIABLE 'NAME' IS PRINTED IN 'A' FORMAT 01267*15 REAL*8 X(MM,MM),Y(20) 01268*15 DO 1000 I=1,N 01269*15 1000 Y(I)=DSQRT(X(I,I)) 01270*15 WRITE(6,6100)NAME,(Y(I),I=1,N) 01271*15 6100 FORMAT(' ',2X,A4,7(D14.6)) 01272*21 RETURN 01273*15 END 01274*15 C 01275*27 C***********************************************************************01276*27 C 01277*27 SUBROUTINE RMS(X,Y,N,IROW,RLAT,HEIGHT,IFCODE) 01278*27 C 01279*27 C THIS SUBROUTINE COMPUTES STANDARD DEVIATIONS FROM THE DIAGONAL 01280*27 C ELEMENTS OF INPUT MATRIX 'X' AND RETURNS THEM IN ROW 'IROW' OF 01281*27 C MATRIX 'Y' 01282*27 C MISORIENTATIONS (1,2,7) ARE CONVERTED FROM RADIANS TO ARC-SECONDS 01283*27 C HORIZONTAL POSITION & VELOCITY ERRORS (3,4,5,6): RADIANS TO METRES 01284*27 C HEIGHT AND VERTICAL VELOCITY ERRORS ARE IN METRES (8,9) 01285*27 IMPLICIT REAL*8(A-H,O-Z) 01286*27 COMMON/CONST/A,B,WEARTH,PI,RADIAN,MM 01287*27 DIMENSION X(MM,1),Y(30,1) 01288*28 RAD=RADIUS(RLAT,RM,RN,A,B) 01289*27 RM=RM+HEIGHT 01290*27 RN=(RN+HEIGHT)*DCOS(RLAT) 01291*27 RHO=206264.8062D0 01292*27 GO TO (1000,2000),IFCODE 01293*27 1000 CONTINUE 01294*27 Y(IROW,1)=DSQRT(X(1,1)) 01295*27 Y(IROW,2)=DSQRT(X(2,2)) 01296*27 Y(IROW,3)=DSQRT(X(3,3))*RM/RHO 01297*27 Y(IROW,4)=DSQRT(X(4,4))*RN/RHO 01298*27 Y(IROW,5)=DSQRT(X(5,5))*RM/RHO 01299*27 Y(IROW,6)=DSQRT(X(6,6))*RN/RHO 01300*27 IF(N.EQ.6)RETURN 01301*27 Y(IROW,7)=DSQRT(X(7,7)) 01302*27 RETURN 01303*27 2000 CONTINUE 01304*27 Y(IROW,1)=DSQRT(X(1,1))*RHO 01305*27 Y(IROW,2)=DSQRT(X(2,2))*RHO 01306*27 Y(IROW,3)=DSQRT(X(3,3))*RM 01307*27 Y(IROW,4)=DSQRT(X(4,4))*RN 01308*27 Y(IROW,5)=DSQRT(X(5,5))*RM 01309*27 Y(IROW,6)=DSQRT(X(6,6))*RN 01310*27 IF(N.EQ.6)RETURN 01311*27 Y(IROW,7)=DSQRT(X(7,7))*RHO 01312*27 IF(N.EQ.7)RETURN 01313*27 Y(IROW,8)=DSQRT(X(8,8)) 01314*27 IF(N.EQ.8)RETURN 01315*27 Y(IROW,9)=DSQRT(X(9,9)) 01316*27 RETURN 01317*27 END 01318*27 C 01319*15 C***********************************************************************01320*15 C 01321*15 SUBROUTINE MOUTD(X,MM,M,N) 01322*19 REAL*8 X(MM,MM) 01323*19 NB=(N+6)/7 01324*25 DO 1000 IB=1,NB 01325*25 I1=IB*7-6 01326*25 I2=IB*7 01327*25 IF(N.LT.I2)I2=N 01328*25 WRITE(6,6000)(I,I=I1,I2) 01329*25 DO 1001 I=1,M 01330*25 1001 WRITE(6,6003)I,(X(I,J),J=I1,I2) 01331*25 WRITE(6,6002) 01332*25 1000 CONTINUE 01333*25 6000 FORMAT(4X,I4,6I14) 01334*21 6002 FORMAT(' ') 01335*19 6003 FORMAT(' ',I3,7D14.6) 01336*24 RETURN 01337*19 END 01338*19 C 01339*19 C***********************************************************************01340*19 C 01341*19 C DATA SET SPIN AT LEVEL 001 AS OF 10/07/79 SUBROUTINE SPIN(Q,N,MM,DET,IDEXP) 00001 C 00002 C SUBROUTINE SPIN IS A MATRIX INVERSION ROUTINE FOR SYMMETRIC 00003 C POSITIVE-DEFINITE MATRICES. THE MATRIX INVERTED IS THE UPPER 00004 C N BY N PORTION OF THE MATRIX Q WHICH IS DIMENSIONED MM BY MM 00005 C IN THE CALLING ROUTINE. 00006 C 00007 C INPUT: 00008 C Q - THE MATRIX DIMENSIONED MM BY MM WHICH CONTAINS THE 00009 C MATRIX TO BE INVERTED. 00010 C 00011 C N - THE DIMENSION OF THE ACTUAL PART( UPPER LEFT CORNER) 00012 C OF Q WHICH IS TO BE INVERTED. ( N MAY BE EQUAL BUT MUST 00013 C NOT BE LARGER THAN MM) . 00014 C MM- DIMENSIONED SIZE OF Q IN THE CALLING ROUTINE. 00015 C 00016 C 00017 C OUTPUT: 00018 C 00019 C Q - THE UPPER LEFT N BY N PORTION CONTAINS THE INVERSE OF 00020 C THE INPUT UPPER LEFT N BY N PORTION. 00021 C 00022 C DET - THE NON-EXPONENT PORTION OF THE DETERMINANT OF THE 00023 C INPUT N BY N (UPPER LEFT PORTION OF Q) MATRIX. SEE 00024 C IDEXP BELOW. 00025 C 00026 C IDEXP - THE EXPONENT (OF 10) PART OF THE DETERMINANT DESCRIBED 00027 C UNDER DET ABOVE. THUS THE DETERMINANT IS RETURNED IN 00028 C TWO PARTS CORRESPONDING TO 00029 C DETERMINANT = DET * 10 ** IDEXP . 00030 C THIS IS DONE TO AVOID UNDER OR OVERFLOW IN THE 00031 C COMPUTATION OF THE DETERMINANT. TO PRINT THE DETERM- 00032 C INANT THE USER SHOULD PRINT BOTH NUMBERS AS FOLLOWS; 00033 C (FOR EXAMPLE) 00034 C PRINT 10,DET,IDEXP 00035 C 10 FORMAT(' ','DETERMINANT= ',F7.4,'D',I4) 00036 C 00037 C R.R. STEEVES 00038 C SEPT., 1979 00039 C 00040 C 00041 REAL*8 Q(MM,MM),DET,SUM,DSQRT,DABS,RPART,APART 00042 DET=0.D0 00043 DO 4 J=1,N 00044 DO 4 I=1,J 00045 IF(I.EQ.1) GO TO 2 00046 M=I-1 00047 SUM=0.0D0 00048 DO 1 K=1,M 00049 1 SUM=SUM+Q(K,I)*Q(K,J) 00050 Q(I,J)=Q(I,J)-SUM 00051 2 IF(I.EQ.J) GO TO 3 00052 Q(I,J)=Q(I,J)/Q(I,I) 00053 GO TO 4 00054 3 CONTINUE 00055 DET=DET+DLOG10(Q(I,I)) 00056 Q(I,I)=DSQRT(Q(I,I)) 00057 4 CONTINUE 00058 IDEXP=DET 00059 RPART=DET-IDEXP 00060 APART=DABS(RPART) 00061 IF(APART.LT.1.D-20) DET=1.D0 00062 IF(APART.LT.1.D-20) GO TO 10 00063 DET=10**RPART 00064 10 CONTINUE 00065 DO 7 J=1,N 00066 DO 7 I=1,J 00067 IF(I.LT.J) GO TO 5 00068 Q(J,J)=1.0D0/Q(J,J) 00069 GO TO 7 00070 5 SUM=0.0D0 00071 M=J-1 00072 DO 6 K=I,M 00073 6 SUM=SUM-Q(I,K)*Q(K,J) 00074 Q(I,J)=SUM/Q(J,J) 00075 7 CONTINUE 00076 DO 9 J=1,N 00077 DO 9 I=1,J 00078 SUM=0.0D0 00079 DO 8 K=J,N 00080 8 SUM=SUM+Q(I,K)*Q(J,K) 00081 Q(I,J)=SUM 00082 IF(I.EQ.J) GO TO 9 00083 Q(J,I)=SUM 00084 9 CONTINUE 00085 RETURN 00086 END 00087