matrix.f90

CLM集合卡曼滤波数据同化算法

!
!  ------------------------------------------------------------------
! |  In the following codes, some subroutines are introduced         |
! |  in order to perform matrix computations.                        |
! |                                                                  |
! |  1)SUBROUTINE MATMULT(A,B,C,NA,L,NB) is used for calculating     |
! |    the product of two matrices A and B and the computing         |
! |    result is stored in C. A has dimensions of NA*L and B         |
! |    has dimensions of L*NB. Of course, C has ones of NA*NB        | 
! |                                                                  |
! |  2)SUBROUTINE MATINV(A,N) is used to obtain the inverse of       |
! |    a matrix. A stands for input square matrix, N represents      |
! |    its dimension. Furthermore, the computing result is also      |
! |    stored in A.                                                  |
! |                                                                  |
! |  3)SUBROUTINE PDSQRT(A,S,N) is used for acquiring the square     |
! |     root of a postive definite symetric matrix. A is input       |     
! |     matrix and S is output matrix.N denotes their dimensions     |  
! |                                                                  |
! |  4)SUBROUTINE MATADD(A,B,NA,NB) is used to calculate the sum     |
! |    of two matrices A and B. Here, NA and NB stand for their      |
! |    rows and columns,respectively. The final result is stored     |
! |    in the matrix A.                                              |
! |  5)SUBROUTINE MATSUB(A,B,NA,NB) is used to calculate the difference
! |    of two matrices A and B. Here, NA and NB stand for their      |
! |    rows and columns,respectively. The final result is stored     |
! |    in the matrix A.                                              |
! |                                                                  |
! |  For more details, see notes inside these subroutines            | 
! |                                                                  |           
! |    author:Qinjun                                                 |
! |    date  :5/25/2004                                              |
! |                                                                  |
!  -----------------------------------------------------------------     

     
      SUBROUTINE MATMULT(A,B,C,NA,L,NB)

!   -----------------------------------------------------
!  |    matrix multiplcation                             |
!  |                                                     |
!  |   input:  A   --matrix with NA rows and L  columns  |
!  |           B   --matrix with L  rows and NB columns  |
!  |   output: C   --matrix with NA rows and NB columns  |
!  |                                                     |
!   -----------------------------------------------------

      REAL A(NA,L),B(L,NB),C(NA,NB)

      DO 10 I=1,NA

      DO 5 J=1,NB

  5   C(I,J)=DOT(A(I,1),B(1,J),NA,L)

  10  CONTINUE

      RETURN

      END SUBROUTINE MATMULT  

!=========================================================
 
 
 
 
      REAL FUNCTION DOT(X,Y,LX,L)

!     -------------------------------
!    |       vector dot product      |
!     -------------------------------

      REAL X(LX,L),Y(L)

      DOT=0.

      DO 10 K=1,L

      DOT=DOT+X(1,K)*Y(K)

 10   CONTINUE

      RETURN

      END FUNCTION DOT


 !========================================================================
   
     
     
      SUBROUTINE MATINV(A,N)

!     -------------------------------------------------
!    |              invert a square matrix             |
!    |   input:                                        |
!    |           A   -- a square matrix                |
!    |           N   -- dimension for matrix A         |
!    |   output:                                       |
!    |           A   -- inverse of matrix A            |
!     -------------------------------------------------	
	implicit none
	
	integer::N

	integer::i,j

	real,dimension(1:N,1:N)::A

	real,dimension(1:N*N)::U

	do i=1,N

	   do j=1,N

	      U((i-1)*N+j)=A(i,j)

	   end do

	end do

	call KVERT(U,N)

	do i=1,N

	   do j=1,N

	      A(i,j)=U((i-1)*N+j)

	   end do

	end do

      END SUBROUTINE MATINV
 
 
 
 !========================================================================
 
      SUBROUTINE KVERT(V,N)
!
!      ________________________________________________________
!     |                                                        |
!     |     INVERT A GENERAL MATRIX WITH COMPLETE PIVOTING     |
!     |                                                        |
!     |    INPUT:                                              |
!     |                                                        |
!     |         V     --ARRAY CONTAINING MATRIX                |
!     |                                                        |
!     |         LV    --LEADING (ROW) DIMENSION OF ARRAY V     |
!     |                                                        |
!     |         N     --DIMENSION OF MATRIX STORED IN ARRAY V  |
!     |                                                        |
!     |         W     --WORK ARRAY WITH AT LEAST 2N ELEMENTS   |
!     |                                                        |
!     |    OUTPUT:                                             |
!     |                                                        |
!     |         V     --INVERSE                                |
!     |                                                        |
!     |    BUILTIN FUNCTIONS: ABS                              |
!     |________________________________________________________|
!
      
      REAL V(N,1),W(2*N),S,T
      INTEGER H,I,J,K,L,M,N,O,P,Q
      IF ( N .EQ. 1 ) GOTO 120
      O = N + 1
      L = 0
      M = 1
10    IF ( L .EQ. N ) GOTO 90
      K = L
      L = M
      M = M + 1
!     ---------------------------------------
!     |*** FIND PIVOT AND START ROW SWAP ***|
!     ---------------------------------------
      P = L
      Q = L
      S = ABS(V(L,L))
      DO 20 H = L,N
           DO 20 I = L,N
                T = ABS(V(I,H))
                IF ( T .LE. S ) GOTO 20
                P = I
                Q = H
                S = T
20    CONTINUE
      W(N+L) = P
      W(O-L) = Q
      DO 30 I = 1,N
           T = V(I,L)
           V(I,L) = V(I,Q)
30         V(I,Q) = T
      S = V(P,L)
      V(P,L) = V(L,L)
      IF ( S .EQ. 0. ) GOTO 130
!     -----------------------------
!     |*** COMPUTE MULTIPLIERS ***|
!     -----------------------------
      V(L,L) = -1.
      S = 1./S
      DO 40 I = 1,N
40         V(I,L) = -S*V(I,L)
      J = L
50    J = J + 1
      IF ( J .GT. N ) J = 1
      IF ( J .EQ. L ) GOTO 10
      T = V(P,J)
      V(P,J) = V(L,J)
      V(L,J) = T
      IF ( T .EQ. 0. ) GOTO 50
!     ------------------------------
!     |*** ELIMINATE BY COLUMNS ***|
!     ------------------------------
      IF ( K .EQ. 0 ) GOTO 70
      DO 60 I = 1,K
60         V(I,J) = V(I,J) + T*V(I,L)
70    V(L,J) = S*T
      IF ( M .GT. N ) GOTO 50
      DO 80 I = M,N
80         V(I,J) = V(I,J) + T*V(I,L)
      GOTO 50
!     -----------------------
!     |*** PIVOT COLUMNS ***|
!     -----------------------
90    L = W(K+N)
      DO 100 I = 1,N
           T = V(I,L)
           V(I,L) = V(I,K)
100        V(I,K) = T
      K = K - 1
      IF ( K .GT. 0 ) GOTO 90
!     --------------------
!     |*** PIVOT ROWS ***|
!     --------------------
      DO 110 J = 1,N
           DO 110 I = 2,N
                P = W(I)
                H = O - I
                T = V(P,J)
                V(P,J) = V(H,J)
                V(H,J) = T
110   CONTINUE
      RETURN
120   IF ( V(1,1) .EQ. 0. ) GOTO 130
      V(1,1) = 1./V(1,1)
      RETURN
130   WRITE(6,*) 'MATRIX HAS NO INVERSE'
      STOP
      END  SUBROUTINE KVERT






!****************************************************************
!    This is the fortran 77 program for calculating
!    the square root of a symmetric positive definite matrix.
!    It requires a few routines in LAPACK (and BLAS). It
!    works on a SUN workstation. LAPACK is already included
!    in SUN's performance library. It can be compiled with
!    f77 -xlic_lib=sunperf pdsqrt.f
!    in Solaris 7 and 8. 
!    Here A is the input matrix, only the lower triangular
!    part is really needed.
!    S is the square root matrix. The lower triangular part 
!    is calculated.
!    w is a real vector for working space.
!    iw is an integer vector for working space.
!    tiny is a real error control parameter.
!*****************************************************************
      !subroutine PDSQRT(n,A,S,w,iw,tiny)
	  subroutine PDSQRT(A,S,N)
      integer n, iw(5*n)
      real A(n,n), S(n,n), w(8*n),tiny
	  integer::i,j
	  tiny = 1.0E-6
      call PDSQRW(n,A,S,w,w(n+1),w(2*n+1),w(3*n+1),w(4*n+1),iw,tiny)
	  do i=2,n
	     do j=1,i-1
		    S(j,i)=S(i,j)
		 end do
	  end do
      return
      end

!*****************************************************************
      subroutine PDSQRW(n,A,S,d,e,tau,w,work,iw,tiny)
      implicit none
      integer n
      real A(n,n),S(n,n),d(n),e(n),tau(n),w(n),work(4*n),tiny
      integer iw(5*n)

      real lams, laml, mu, rtmu, coef
!      real time(2), ttime, dtime
      integer lwork, m, info, i, j, mfound, nsplit

      lwork = 4*n

!     SSYTRD reduces a real symmetric matrix A to real symmetric
!     tridiagonal form T by an orthogonal similarity transformation:
!     Q**T * A * Q = T, where Q is is represented as a product of 
!     elementary reflectors.
!
!     It takes: 4/3 n^3 operations.

      call SSYTRD( 'L', n, A, n, d, e, tau, work, lwork, info )

!     SSTEBZ computes the eigenvalues of a symmetric tridiagonal
!     matrix T. 
!
!     It takes O(n) operations.

      call SSTEBZ( 'I', 'B', n, 0.0, 0.0, 1, 1, 0.0, d, e,&
             mfound, nsplit, w, iw, iw(n+1), work, iw(2*n+1),&
             info )
      lams = w(1)
      call SSTEBZ( 'I', 'B', n, 0.0, 0.0, n, n, 0.0, d, e,&
              mfound, nsplit, w, iw, iw(n+1), work, iw(2*n+1),&
                info )
      laml = w(1)

      call XING(laml, lams, tiny, mu, m)
      print*, 'm=', m
      rtmu = sqrt(mu)

!     We translate the tridiagonal matrix T to X.

      coef = 1.0/mu
      do i=1, n-1
         d(i) = coef*d(i) - 1.0
         e(i) = coef*e(i)
      enddo
      d(n) = coef*d(n)-1.0
      
!     It takes:   O(m n^2)

      call TRISRT(n,d,e,m,rtmu,S,work,work(n+1),work(2*n+1))

!     Given symmetric matrix S, we calculate QSQ^t
!     Q is given as in the SSYTRD, stored in elementary reflectors.

      call APPLIQ(n,A,tau,S, work, work(n+1))

      do j=1, n
         S(j,j) = S(j,j) + rtmu
      enddo

      return
      end

!****************************************************
!*     This is the IMPROVED subroutine for calculating m and mu with a given 
!*     laml, lams, tiny. The theory is given in section 3 of the paper:
!*     
!*     A Pad\'e approximation Method for Square Roots of
!*     Symmetric Positive Definite Matrices.
!*
!*     Input:  laml > lams > 0, the extreme eigenvalues of 
!*               a symmetric positive definite matrix
!*             tiny, the relative desired accuracy.
!***************************************************
      subroutine XING(laml, lams, tiny, mu, m)
      implicit none
      real laml, lams, tiny, mu
      integer m
      real kappa,rtkp,a,b,ab,tau,t,err,f,fp,mstar,s
      integer it

      kappa = laml/lams
      rtkp  = sqrt(kappa)

      a = 2.0/tiny -1.0
      b = 1.0 + 2.0/(rtkp*tiny)
      ab = a*b
      tau = ( rtkp+1.0)/( rtkp-1.0)
      t = sqrt( (rtkp-1.0)*log(a)*log(b) )
      t = 2.0/t

      err = 1.0
      it = 0
 1    if ( abs(err/t) .GT. 1.0E-5) then
         it = it + 1
         f = (ab)**t - tau*(a**t+b**t)+1.0
         fp = log(ab)*(ab)**t - tau*(log(a)*a**t+log(b)*b**t)
         err = f/fp
         t = t - err
         goto 1
      endif
      mstar  = (1.0-t)/(2.0*t)
      m = mstar
      if ( mstar .GT. m) m = m+1

!c     Now we improve our mu value for the fixed integer m
      s = (a**t-1.0)/(a**t+1.0)
      a = sqrt(rtkp)
      b = 1.0/a
      s = s*a
      err = 1.0
      it = 0
 2    if ( abs(err/s) .GT. 1.0E-5) then
         it = it + 1
!c     print*, it
         f=rtkp*(((s+b)/(s-b))**(2*m+1)-1.0)-((a+s)/(a-s))**(2*m+1)-1.0
         fp = ((s+b)/(s-b))**(2*m) / (s-b)**2 +&
               ((a+s)/(a-s))**(2*m) / (s-a)**2
         fp = -2*(2*m+1)*a*fp
         err = f/fp
         s = s - err
         goto 2
      endif
      
      mu = s*s*sqrt(laml*lams)
!c     
!c      print*, 'Number of iteration = ', it
      return
      end

!*******************************************************
!*     This is the program to calculate 
!*         S = SUM a_j sqrt(mu) ( I + b_j X)^(-1) X
!*******************************************************

      subroutine TRISRT(n,d,e,m,rtmu,S,dx,ex,x)
      implicit none
      integer n, m
      real d(n), e(n-1), S(n,n), dx(n), ex(n-1), x(n)

      integer i,j,k,info
      real theta, ak, bk, coef, pi, rtmu

      pi = 4.0*atan(1.0)

      do j=1, n
         do i=j, n
            S(i,j) = 0.0
         enddo
      enddo

      do k=1, m

         theta = k*pi/(2.0*m+1.0)
         ak = 2.0/(2.0*m+1.0)* ( sin(theta))**2
         bk = ( cos(theta) )**2
         coef = ak*rtmu

         do i=1, n-1
            dx(i) = 1.0 + bk*d(i)
            ex(i) = bk*e(i)
         enddo
         dx(n) = 1.0 + bk*d(n)

!*     SPTTRF computes the factorization of a real symmetric positive
!*     definite tridiagonal matrix A.

         call SPTTRF(n,dx,ex,info)

         do j=1, n

            do i=1, n
               x(i) = 0.0
            enddo

            x(j) = d(j)*coef
            if ( j .GT. 1) x(j-1) = coef*e(j-1)
            if ( j .LT. n) x(j+1) = e(j)*coef

!*     SPTTRS solves a system of linear equations A * X = B with a
!*     symmetric positive definite tridiagonal matrix A using the
!*     factorization A = L*D*L**T or A = U**T*D*U computed by SPTTRF.
!*     (The two forms are equivalent if A is real.)

!*            call SPTTRS(n, neq, dx, ex, x, n, info)
            call TRISPE(n, dx, ex, x, j)

!*     We will replace this by our own program that takes
!*     advantage of the zeros in x, and the symmetric of 
!*     solution matrix.
            
            do i=j, n
               S(i,j) = S(i,j) + x(i)
            enddo

         enddo
      enddo

      return
      end

!***********************************************************