PROGRAM TEST C C FUNCTION: TEST Z_MATRIX SUBROUTINE FOR GPS SOLUTION - GPS TOOLBOX COLUMN C C SUBROUTINE CALLED: C Z_MATRIX (Q,N,M,Z) C C VARIABLES: C Q: Variance-covarinace matrix of the estimated real-value C ambiguities before calling Z_MATRIX C variance-covariance matrix of the transformed real-value C ambiguities after calling Z_MATRIX. C Z: Integer transformation matrix C N: Defined row and column number of Q C M: Number of ambiguities C IMPLICIT REAL*8 (A-H,O-Z) IMPLICIT INTEGER*4 (I-N) PARAMETER(N=16) REAL*8 Q(N,N),Z(N,N) C OPEN(12,FILE='Q_X.TXT',STATUS='OLD') OPEN(13,FILE='Z_Q.TXT',STATUS='UNKNOWN') C READ(12,*) M DO I=1,M READ(12,*) (Q(I,J),J=1,M) ENDDO C C COMPUTE THE INTEGER TRANSFORMATION MATRIX C CALL Z_MATRIX (Q,N,M,Z) C WRITE(13,*) 'RESULTS:' WRITE(13,*) WRITE(13,*) 'Z=' DO I=1,M WRITE(13,'(16F15.6)') (Z(I,J),J=1,M) ENDDO WRITE(13,*) 'Q=' DO I=1,M WRITE(13,'(16F15.6)') (Q(I,J),J=1,M) ENDDO C STOP END C C SUBROUTINE Z_MATRIX (Q,N,M,Z) C C FUNCTION: INTEGER TRANSFORMATION MATRIX DETERMINATION USING C LDL' AND UDU' DECOMPOSITION METHOD (REF. HAN & RIZOS PAPER, C TO BE APPEARED IN ION GPS-95 PROCEEDINGS). C C AUTHOR : SHAOWEI HAN C DATE : JANUARY 10, 1995 C C VARIABLES: C Q: Variance-covarinace matrix of the estimated real-value C ambiguities before calling Z_MATRIX; C Variance-covariance matrix of the transformed real-value C ambiguities after calling Z_MATRIX. C Z: Integer transformation matrix C N: Defined row and column number of Q from the parent program. C It should be smaller than 30. Otherwise some subroutines C (INT_TRIA, LOW_UDU, UP_UDU) need a minor modifications. C M: Number of ambiguities, and smaller than N. C N1: Choose the same constant value as N which was defined in the C parent program. C C SUBROUTINES CALLED: C INT_MATRIX: make every elements of the matrix round to nearest integer C INV_TRIA: inverse of triangular matrix C LOW_UDU: U-D decomposition Q=U*D*U', U is lower triangle matrix. C MULTIPLE: multiplication of A and B => C. C UP_UDU: U-D decomposition Q=U*D*U', U is upper triangle matrix. C IMPLICIT REAL*8 (A-H,O-Z) IMPLICIT INTEGER*4 (I-N) C PARAMETER(N1=16) REAL*8 Q(N,N),Z(N,N) REAL*8 C(N1,N1),D(N1),U(N1,N1),Z1(N1,N1) C IZERO=0 IONE=1 NUMBER1=1 NUMBER2=1 C DO I=1,M Z(I,I)=1.D0 ENDDO C DO ITER=1,50 C C 1. LOW_UDU DECOMPOSITION C 60 CALL LOW_UDU(N,M,Q,U,D) C C 1.1 Round elements of U to integers (Z1) C CALL INT_MATRIX(N,M,Z1,U,NUMBER1) C C NUMBER1=0 means Z1 is identical matrix C IF ((NUMBER1.EQ.0).AND.(NUMBER2.EQ.0)) GOTO 100 IF (NUMBER1.GT.0) THEN C C 1.2 Inverse of Z1 C CALL INV_TRIA(N,M,Z1,IZERO) CALL MULTIPLE(Z1,Z,C,N,N,N,N,N,N,M,M,M,IZERO) DO I=1,M DO J=1,M Z(I,J)=C(I,J) ENDDO ENDDO ELSE GOTO 70 ENDIF C C 1.3 Update transformation matrix Z and C CALL MULTIPLE(Z1,Q,C,N,N,N,N,N,N,M,M,M,IZERO) CALL MULTIPLE(C,Z1,Q,N,N,N,N,N,N,M,M,M,IONE) C C 2. UP_UDU DECOMPOSITION C 70 CALL UP_UDU(N,M,Q,U,D) C CALL INT_MATRIX(N,M,Z1,U,NUMBER2) C IF ((NUMBER1.EQ.0).AND.(NUMBER2.EQ.0)) GOTO 100 IF (NUMBER2.GT.0) THEN CALL INV_TRIA(N,M,Z1,IONE) CALL MULTIPLE(Z1,Z,C,N,N,N,N,N,N,M,M,M,IZERO) DO I=1,M DO J=1,M Z(I,J)=C(I,J) ENDDO ENDDO ELSE GOTO 60 ENDIF C CALL MULTIPLE(Z1,Q,C,N,N,N,N,N,N,M,M,M,IZERO) CALL MULTIPLE(C,Z1,Q,N,N,N,N,N,N,M,M,M,IONE) C ENDDO C 100 CONTINUE C RETURN END C C SUBROUTINE INT_MATRIX(N,NR,Z,D,NUMBER) C C FUNCTION: MAKE EVERY ELEMENTS OF D ROUND TO NEAREST INTEGER C C AUTHOR : SHAOWEI HAN C DATE : JANUARY 10, 1995 C IMPLICIT REAL*8 (A-H) IMPLICIT INTEGER*4 (I-N) REAL*8 D(N,N),Z(N,N) C C DO I=1,NR C DO J=1,NR C Z(I,J)=0. C ENDDO C ENDDO C NUMBER=0 DO I=1,NR DO J=1,NR Z(I,J)=DNINT(D(I,J)) IF (Z(I,J).NE.0.) THEN NUMBER=NUMBER+1 ENDIF ENDDO ENDDO NUMBER=NUMBER-NR RETURN END C C SUBROUTINE INV_TRIA(IP,N,P,IT) C C FUNCTION: INVERSE OF TRIANGULAR MATRIX C C AUTHOR : SHAOWEI HAN C DATE : JANUARY 10, 1995 C C VARIABLES: C IT:=1 UPPER TRIANGULAR MATRIX C =0 LOWER TRIANGULAR MATRIX C IP: DEFINED DIMENSION OF Q C N: THE REAL DIMENSION OF TRIANGLE MATRIX C C IMPLICIT REAL*8(A-H,O-Z) IMPLICIT INTEGER*4 (I-N) REAL*8 P(IP,IP),Q(30,30) C DO I=1,30 DO J=1,30 Q(I,J)=0.D0 ENDDO ENDDO C C INVERSE OF UPPER TRIANGLUAR MATRIX C IF (IT.EQ.1) THEN DO K=1,N Q(K,K)=1.D0/P(K,K) DO I=K-1,1,-1 SUM=.0D0 DO J=I+1,K SUM=SUM+P(I,J)*Q(J,K) ENDDO Q(I,K)=-SUM/P(I,I) ENDDO ENDDO C ELSE C C INVERSE OF LOW TRIANGLUAR MATRIX C DO K=1,N Q(K,K)=1.D0/P(K,K) DO I=K-1,1,-1 SUM=.0D0 DO J=I+1,K SUM=SUM+P(J,I)*Q(K,J) ENDDO Q(K,I)=-SUM/P(I,I) ENDDO ENDDO C ENDIF C DO I=1,N DO J=1,N P(I,J)=Q(I,J) ENDDO ENDDO RETURN END C C SUBROUTINE LOW_UDU(N,NR,Q,U,D) C C FUNCTION: U-D DECOMPOSITION Q=U*D*U' FROM P(1,1)=>P(N,N) C U IS LOWER TRIANGLE MATRIX C C AUTHOR : SHAOWEI HAN C DATE : 8TH JANUARY, 1995 C C INPUT VARIABLES: C N: DEFINED DIMENSION OF ARRAYS Q,U AND D C NR: REAL DIMENSION OF ARRAYS Q,U AND D C Q: Q WILL BE U-D DEPOSITION C C OUTPUT VARIABLES: C U: LOW-TRIANGLE MATRIX C D: DIAGONAL MATRIX C IMPLICIT REAL*8 (A-H,O-Z) IMPLICIT INTEGER*4 (I-N) REAL*8 Q(N,N),D(N),U(N,N),P(30,30) C DO J=1,N D(J)=0.D0 DO K=1,N U(J,K)=0.D0 ENDDO ENDDO C DO J=1,N DO K=1,N P(J,K)=Q(J,K) ENDDO ENDDO C DO 5 J=1,NR-1 D(J)=P(J,J) U(J,J)=1. ALPHA=1./D(J) DO 5 K=NR,J+1,-1 BETA=P(K,J) U(K,J)=ALPHA*BETA DO 5 I=K,NR 5 P(I,K)=P(I,K)-BETA*U(I,J) U(NR,NR)=1. D(NR)=P(NR,NR) C RETURN END C C SUBROUTINE MULTIPLE(A,B,C,IA,JA,IB,JB,IC,JC,M,N,L,IT) C C FUNCTION: MULTIPLICATION OF A AND B. C IT=0: A(M,N)*B(N,L) => C(M,L) C IT=1: A(M,N)*Transposed(B(L,N)) => C(M,L) C AUTHOR : SHAOWEI HAN C DATE : JANUARY 10, 1995 C C VARIABLES: C (IA, JA) IS THE DEFINED DIMENSION OF A C (IB, JB) IS THE DEFINED DIMENSION OF B C (IC, JC) IS THE DEFINED DIMENSION OF C C C A(M,N) IS THE MAIN SUBMATRIX FROM (1,1) OF A(IA,JA) C B(N,L) IS THE MAIN SUBMATRIX FROM (1,1) OF B(IB,JB) C C(M,L) IS THE MAIN SUBMATRIX FROM (1,1) OF C(IC,JC) C IMPLICIT REAL*8 (A-H) IMPLICIT INTEGER*4 (I-N) REAL*8 A(IA,JA),B(IB,JB),C(IC,JC) IF (IT.EQ.0) THEN DO I=1,M DO J=1,L H=0.D0 DO K=1,N H=H+A(I,K)*B(K,J) ENDDO C(I,J)=H ENDDO ENDDO C ELSE C DO I=1,M DO J=1,L H=0.D0 DO K=1,N H=H+A(I,K)*B(J,K) ENDDO C(I,J)=H ENDDO ENDDO ENDIF RETURN END C C SUBROUTINE UP_UDU(N,NR,Q,U,D) C C FUNCTION: U-D DECOMPOSITION P=U*D*U' FROM P(N,N)=>P(1,1) C U IS UPPER TRIANGLE MATRIX C AUTHOR : SHAOWEI HAN C DATE : 8TH JANUARY, 1995 C C INPUT VARIABLES: C N: DEFINED DIMENSION OF ARRAYS Q,U AND D C NR: REAL DIMENSION OF ARRAYS Q,U AND D C Q: Q WILL BE U-D DEPOSITION C C OUTPUT VARIABLES: C U: UP-TRIANGLE MATRIX C D: DIAGONAL MATRIX C IMPLICIT REAL*8 (A-H,O-Z) IMPLICIT INTEGER*4 (I-N) REAL*8 Q(N,N),D(N),U(N,N),P(30,30) C DO J=1,N D(J)=0.D0 DO K=1,N U(J,K)=0.D0 ENDDO ENDDO C DO J=1,N DO K=1,N P(J,K)=Q(J,K) ENDDO ENDDO C DO 5 J=NR,2,-1 D(J)=P(J,J) U(J,J)=1. ALPHA=1./D(J) DO 5 K=1,J-1 BETA=P(K,J) U(K,J)=ALPHA*BETA DO 5 I=1,K 5 P(I,K)=P(I,K)-BETA*U(I,J) U(1,1)=1. D(1)=P(1,1) RETURN END