lm.f90

开发的lm算法,很有用的一种优化算法. 对非线性优化有很大用处

!  **********

!  subroutine qrsolv

!  Given an m by n matrix a, an n by n diagonal matrix d, and an m-vector b,
!  the problem is to determine an x which solves the system

!        a*x = b ,     d*x = 0 ,

!  in the least squares sense.

!  This subroutine completes the solution of the problem if it is provided
!  with the necessary information from the qr factorization, with column
!  pivoting, of a.  That is, if a*p = q*r, where p is a permutation matrix,
!  q has orthogonal columns, and r is an upper triangular matrix with diagonal
!  elements of nonincreasing magnitude, then qrsolv expects the full upper
!  triangle of r, the permutation matrix p, and the first n components of
!  (q transpose)*b.  The system a*x = b, d*x = 0, is then equivalent to

!               t       t
!        r*z = q *b ,  p *d*p*z = 0 ,

!  where x = p*z. if this system does not have full rank,
!  then a least squares solution is obtained.  On output qrsolv
!  also provides an upper triangular matrix s such that

!         t   t               t
!        p *(a *a + d*d)*p = s *s .

!  s is computed within qrsolv and may be of separate interest.

!  the subroutine statement is

!    subroutine qrsolv(n, r, ldr, ipvt, diag, qtb, x, sdiag, wa)

! N.B. Arguments LDR and WA have been removed in this version.

!  where

!    n is a positive integer input variable set to the order of r.

!    r is an n by n array.  On input the full upper triangle must contain
!      the full upper triangle of the matrix r.
!      On output the full upper triangle is unaltered, and the strict lower
!      triangle contains the strict upper triangle (transposed) of the
!      upper triangular matrix s.

!    ldr is a positive integer input variable not less than n
!      which specifies the leading dimension of the array r.

!    ipvt is an integer input array of length n which defines the
!      permutation matrix p such that a*p = q*r.  Column j of p
!      is column ipvt(j) of the identity matrix.

!    diag is an input array of length n which must contain the
!      diagonal elements of the matrix d.

!    qtb is an input array of length n which must contain the first
!      n elements of the vector (q transpose)*b.

!    x is an output array of length n which contains the least
!      squares solution of the system a*x = b, d*x = 0.

!    sdiag is an output array of length n which contains the
!      diagonal elements of the upper triangular matrix s.

!    wa is a work array of length n.

!  subprograms called

!    fortran-supplied ... ABS,SQRT

!  argonne national laboratory. minpack project. march 1980.
!  burton s. garbow, kenneth e. hillstrom, jorge j. more

!  **********
INTEGER   :: i, j, k, kp1, l, nsing
REAL (dp) :: COS, cotan, qtbpj, SIN, sum, TAN, temp, wa(n)
REAL (dp), PARAMETER :: p5 = 0.5_dp, p25 = 0.25_dp, zero = 0.0_dp

!     Copy r and (q transpose)*b to preserve input and initialize s.
!     In particular, save the diagonal elements of r in x.

DO  j = 1, n
  r(j:n,j) = r(j,j:n)
  x(j) = r(j,j)
  wa(j) = qtb(j)
END DO

!     Eliminate the diagonal matrix d using a givens rotation.

DO  j = 1, n
  
!        Prepare the row of d to be eliminated, locating the
!        diagonal element using p from the qr factorization.
  
  l = ipvt(j)
  IF (diag(l) == zero) CYCLE
  sdiag(j:n) = zero
  sdiag(j) = diag(l)
  
!     The transformations to eliminate the row of d modify only a single
!     element of (q transpose)*b beyond the first n, which is initially zero.
  
  qtbpj = zero
  DO  k = j, n
    
!        Determine a givens rotation which eliminates the
!        appropriate element in the current row of d.
    
    IF (sdiag(k) == zero) CYCLE
    IF (ABS(r(k,k)) < ABS(sdiag(k))) THEN
      cotan = r(k,k)/sdiag(k)
      SIN = p5/SQRT(p25 + p25*cotan**2)
      COS = SIN*cotan
    ELSE
      TAN = sdiag(k)/r(k,k)
      COS = p5/SQRT(p25 + p25*TAN**2)
      SIN = COS*TAN
    END IF
    
!        Compute the modified diagonal element of r and
!        the modified element of ((q transpose)*b,0).
    
    r(k,k) = COS*r(k,k) + SIN*sdiag(k)
    temp = COS*wa(k) + SIN*qtbpj
    qtbpj = -SIN*wa(k) + COS*qtbpj
    wa(k) = temp
    
!        Accumulate the tranformation in the row of s.
    
    kp1 = k + 1
    DO  i = kp1, n
      temp = COS*r(i,k) + SIN*sdiag(i)
      sdiag(i) = -SIN*r(i,k) + COS*sdiag(i)
      r(i,k) = temp
    END DO
  END DO
  
!     Store the diagonal element of s and restore
!     the corresponding diagonal element of r.
  
  sdiag(j) = r(j,j)
  r(j,j) = x(j)
END DO

!     Solve the triangular system for z.  If the system is singular,
!     then obtain a least squares solution.

nsing = n
DO  j = 1, n
  IF (sdiag(j) == zero .AND. nsing == n) nsing = j - 1
  IF (nsing < n) wa(j) = zero
END DO

DO  k = 1, nsing
  j = nsing - k + 1
  sum = DOT_PRODUCT( r(j+1:nsing,j), wa(j+1:nsing) )
  wa(j) = (wa(j) - sum)/sdiag(j)
END DO

!     Permute the components of z back to components of x.

DO  j = 1, n
  l = ipvt(j)
  x(l) = wa(j)
END DO
RETURN

!     last card of subroutine qrsolv.

END SUBROUTINE qrsolv



FUNCTION enorm(n,x) RESULT(fn_val)
 
! Code converted using TO_F90 by Alan Miller
! Date: 1999-12-09  Time: 12:45:34

INTEGER, INTENT(IN)    :: n
REAL (dp), INTENT(IN)  :: x(:)
REAL (dp)              :: fn_val

!  **********

!  function enorm

!  given an n-vector x, this function calculates the euclidean norm of x.

!  the euclidean norm is computed by accumulating the sum of squares in
!  three different sums.  The sums of squares for the small and large
!  components are scaled so that no overflows occur.  Non-destructive
!  underflows are permitted.  Underflows and overflows do not occur in the
!  computation of the unscaled sum of squares for the intermediate
!  components.  The definitions of small, intermediate and large components
!  depend on two constants, rdwarf and rgiant.  The main restrictions on
!  these constants are that rdwarf**2 not underflow and rgiant**2 not
!  overflow.  The constants given here are suitable for every known computer.

!  the function statement is

!    REAL (dp) function enorm(n,x)

!  where

!    n is a positive integer input variable.

!    x is an input array of length n.

!  subprograms called

!    fortran-supplied ... ABS,SQRT

!  argonne national laboratory. minpack project. march 1980.
!  burton s. garbow, kenneth e. hillstrom, jorge j. more

!  **********
INTEGER   :: i
REAL (dp) :: agiant, floatn, s1, s2, s3, xabs, x1max, x3max
REAL (dp), PARAMETER :: one = 1.0_dp, zero = 0.0_dp, rdwarf = 3.834E-20_dp,  &
                        rgiant = 1.304E+19_dp

s1 = zero
s2 = zero
s3 = zero
x1max = zero
x3max = zero
floatn = n
agiant = rgiant/floatn
DO  i = 1, n
  xabs = ABS(x(i))
  IF (xabs > rdwarf .AND. xabs < agiant) GO TO 70
  IF (xabs <= rdwarf) GO TO 30
  
!              sum for large components.
  
  IF (xabs <= x1max) GO TO 10
  s1 = one + s1*(x1max/xabs)**2
  x1max = xabs
  GO TO 20

  10 s1 = s1 + (xabs/x1max)**2

  20 GO TO 60
  
!              sum for small components.
  
  30 IF (xabs <= x3max) GO TO 40
  s3 = one + s3*(x3max/xabs)**2
  x3max = xabs
  GO TO 60

  40 IF (xabs /= zero) s3 = s3 + (xabs/x3max)**2

  60 CYCLE
  
!           sum for intermediate components.
  
  70 s2 = s2 + xabs**2
END DO

!     calculation of norm.

IF (s1 == zero) GO TO 100
fn_val = x1max*SQRT(s1 + (s2/x1max)/x1max)
GO TO 120

100 IF (s2 == zero) GO TO 110
IF (s2 >= x3max) fn_val = SQRT(s2*(one + (x3max/s2)*(x3max*s3)))
IF (s2 < x3max) fn_val = SQRT(x3max*((s2/x3max) + (x3max*s3)))
GO TO 120

110 fn_val = x3max*SQRT(s3)

120 RETURN

!     last card of function enorm.

END FUNCTION enorm



SUBROUTINE fdjac2(fcn, m, n, x, fvec, fjac, iflag, epsfcn)
 
! Code converted using TO_F90 by Alan Miller
! Date: 1999-12-09  Time: 12:45:44

! N.B. Arguments LDFJAC & WA have been removed.

INTEGER, INTENT(IN)        :: m
INTEGER, INTENT(IN)        :: n
REAL (dp), INTENT(IN OUT)  :: x(n)
REAL (dp), INTENT(IN)      :: fvec(m)
REAL (dp), INTENT(OUT)     :: fjac(:,:)    ! fjac(ldfjac,n)
INTEGER, INTENT(IN OUT)    :: iflag
REAL (dp), INTENT(IN)      :: epsfcn

INTERFACE
  SUBROUTINE fcn(m, n, x, fvec, iflag)
    IMPLICIT NONE
    INTEGER, PARAMETER :: dp = SELECTED_REAL_KIND(12, 60)
    INTEGER, INTENT(IN)        :: m, n
    REAL (dp), INTENT(IN)      :: x(:)
    REAL (dp), INTENT(IN OUT)  :: fvec(:)
    INTEGER, INTENT(IN OUT)    :: iflag
  END SUBROUTINE fcn
END INTERFACE

!  **********

!  subroutine fdjac2

!  this subroutine computes a forward-difference approximation
!  to the m by n jacobian matrix associated with a specified
!  problem of m functions in n variables.

!  the subroutine statement is

!    subroutine fdjac2(fcn,m,n,x,fvec,fjac,ldfjac,iflag,epsfcn,wa)

!  where

!    fcn is the name of the user-supplied subroutine which calculates the
!      functions.  fcn must be declared in an external statement in the user
!      calling program, and should be written as follows.

!      subroutine fcn(m,n,x,fvec,iflag)
!      integer m,n,iflag
!      REAL (dp) x(n),fvec(m)
!      ----------
!      calculate the functions at x and
!      return this vector in fvec.
!      ----------
!      return
!      end

!      the value of iflag should not be changed by fcn unless
!      the user wants to terminate execution of fdjac2.
!      in this case set iflag to a negative integer.

!    m is a positive integer input variable set to the number of functions.

!    n is a positive integer input variable set to the number of variables.
!      n must not exceed m.

!    x is an input array of length n.

!    fvec is an input array of length m which must contain the
!      functions evaluated at x.

!    fjac is an output m by n array which contains the
!      approximation to the jacobian matrix evaluated at x.

!    ldfjac is a positive integer input variable not less than m
!      which specifies the leading dimension of the array fjac.

!    iflag is an integer variable which can be used to terminate
!      the execution of fdjac2.  see description of fcn.

!    epsfcn is an input variable used in determining a suitable step length
!      for the forward-difference approximation.  This approximation assumes
!      that the relative errors in the functions are of the order of epsfcn.
!      If epsfcn is less than the machine precision, it is assumed that the
!      relative errors in the functions are of the order of the machine
!      precision.

!    wa is a work array of length m.

!  subprograms called

!    user-supplied ...... fcn

!    minpack-supplied ... dpmpar

!    fortran-supplied ... ABS,MAX,SQRT

!  argonne national laboratory. minpack project. march 1980.
!  burton s. garbow, kenneth e. hillstrom, jorge j. more

!  **********
INTEGER   :: j
REAL (dp) :: eps, epsmch, h, temp, wa(m)
REAL (dp), PARAMETER :: zero = 0.0_dp

!     epsmch is the machine precision.

epsmch = EPSILON(zero)

eps = SQRT(MAX(epsfcn, epsmch))
DO  j = 1, n
  temp = x(j)
  h = eps*ABS(temp)
  IF (h == zero) h = eps
  x(j) = temp + h
  CALL fcn(m, n, x, wa, iflag)
  IF (iflag < 0) EXIT
  x(j) = temp
  fjac(1:m,j) = (wa(1:m) - fvec(1:m))/h
END DO

RETURN

!     last card of subroutine fdjac2.

END SUBROUTINE fdjac2


END MODULE Levenberg_Marquardt