Ambiguity Decorrelation algorithm

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