mpfit.pro

非线性最小二乘-LM法 是用IDL语言编写的

      if iflag LT 0 then return, !values.d_nan

      if abs(dside[ifree[j]]) LE 1 then begin
          ;; COMPUTE THE ONE-SIDED DERIVATIVE
          ;; Note optimization fjac(0:*,j)
          fjac[0,j] = (fp-fvec)/h[j]

      endif else begin
          ;; COMPUTE THE TWO-SIDED DERIVATIVE
          xp[ifree[j]] = xall[ifree[j]] - h[j]

          mperr = 0
          fm = mpfit_call(fcn, xp, _EXTRA=fcnargs)
          
          iflag = mperr
          if iflag LT 0 then return, !values.d_nan
          
          ;; Note optimization fjac(0:*,j)
          fjac[0,j] = (fp-fm)/(2*h[j])
      endelse          
          
  endfor

;  profvals.fdjac2 = profvals.fdjac2 + (systime(1) - prof_start)
  return, fjac
end

function mpfit_enorm, vec

  COMPILE_OPT strictarr
  ;; NOTE: it turns out that, for systems that have a lot of data
  ;; points, this routine is a big computing bottleneck.  The extended
  ;; computations that need to be done cannot be effectively
  ;; vectorized.  The introduction of the FASTNORM configuration
  ;; parameter allows the user to select a faster routine, which is 
  ;; based on TOTAL() alone.
  common mpfit_profile, profvals
;  prof_start = systime(1)

  common mpfit_config, mpconfig
; Very simple-minded sum-of-squares
  if n_elements(mpconfig) GT 0 then if mpconfig.fastnorm then begin
      ans = sqrt(total(vec^2))
      goto, TERMINATE
  endif

  common mpfit_machar, machvals

  agiant = machvals.rgiant / n_elements(vec)
  adwarf = machvals.rdwarf * n_elements(vec)

  ;; This is hopefully a compromise between speed and robustness.
  ;; Need to do this because of the possibility of over- or underflow.
  mx = max(vec, min=mn)
  mx = max(abs([mx,mn]))
  if mx EQ 0 then return, vec[0]*0.

  if mx GT agiant OR mx LT adwarf then ans = mx * sqrt(total((vec/mx)^2))$
  else                                 ans = sqrt( total(vec^2) )

  TERMINATE:
;  profvals.enorm = profvals.enorm + (systime(1) - prof_start)
  return, ans
end

;     **********
;
;     subroutine qrfac
;
;     this subroutine uses householder transformations with column
;     pivoting (optional) to compute a qr factorization of the
;     m by n matrix a. that is, qrfac determines an orthogonal
;     matrix q, a permutation matrix p, and an upper trapezoidal
;     matrix r with diagonal elements of nonincreasing magnitude,
;     such that a*p = q*r. the householder transformation for
;     column k, k = 1,2,...,min(m,n), is of the form
;
;			    t
;	    i - (1/u(k))*u*u
;
;     where u has zeros in the first k-1 positions. the form of
;     this transformation and the method of pivoting first
;     appeared in the corresponding linpack subroutine.
;
;     the subroutine statement is
;
;	subroutine qrfac(m,n,a,lda,pivot,ipvt,lipvt,rdiag,acnorm,wa)
;
;     where
;
;	m is a positive integer input variable set to the number
;	  of rows of a.
;
;	n is a positive integer input variable set to the number
;	  of columns of a.
;
;	a is an m by n array. on input a contains the matrix for
;	  which the qr factorization is to be computed. on output
;	  the strict upper trapezoidal part of a contains the strict
;	  upper trapezoidal part of r, and the lower trapezoidal
;	  part of a contains a factored form of q (the non-trivial
;	  elements of the u vectors described above).
;
;	lda is a positive integer input variable not less than m
;	  which specifies the leading dimension of the array a.
;
;	pivot is a logical input variable. if pivot is set true,
;	  then column pivoting is enforced. if pivot is set false,
;	  then no column pivoting is done.
;
;	ipvt is an integer output array of length lipvt. ipvt
;	  defines the permutation matrix p such that a*p = q*r.
;	  column j of p is column ipvt(j) of the identity matrix.
;	  if pivot is false, ipvt is not referenced.
;
;	lipvt is a positive integer input variable. if pivot is false,
;	  then lipvt may be as small as 1. if pivot is true, then
;	  lipvt must be at least n.
;
;	rdiag is an output array of length n which contains the
;	  diagonal elements of r.
;
;	acnorm is an output array of length n which contains the
;	  norms of the corresponding columns of the input matrix a.
;	  if this information is not needed, then acnorm can coincide
;	  with rdiag.
;
;	wa is a work array of length n. if pivot is false, then wa
;	  can coincide with rdiag.
;
;     subprograms called
;
;	minpack-supplied ... dpmpar,enorm
;
;	fortran-supplied ... dmax1,dsqrt,min0
;
;     argonne national laboratory. minpack project. march 1980.
;     burton s. garbow, kenneth e. hillstrom, jorge j. more
;
;     **********
;
; PIVOTING / PERMUTING:
;
; Upon return, A(*,*) is in standard parameter order, A(*,IPVT) is in
; permuted order.
;
; RDIAG is in permuted order.
;
; ACNORM is in standard parameter order.
;
; NOTE: in IDL the factors appear slightly differently than described
; above.  The matrix A is still m x n where m >= n.  
;
; The "upper" triangular matrix R is actually stored in the strict
; lower left triangle of A under the standard notation of IDL.
;
; The reflectors that generate Q are in the upper trapezoid of A upon
; output.
;
;  EXAMPLE:  decompose the matrix [[9.,2.,6.],[4.,8.,7.]]
;    aa = [[9.,2.,6.],[4.,8.,7.]]
;    mpfit_qrfac, aa, aapvt, rdiag, aanorm
;     IDL> print, aa
;          1.81818*     0.181818*     0.545455*
;         -8.54545+      1.90160*     0.432573*
;     IDL> print, rdiag
;         -11.0000+     -7.48166+
;
; The components marked with a * are the components of the
; reflectors, and those marked with a + are components of R.
;
; To reconstruct Q and R we proceed as follows.  First R.
;    r = fltarr(m, n)
;    for i = 0, n-1 do r(0:i,i) = aa(0:i,i)  ; fill in lower diag
;    r(lindgen(n)*(m+1)) = rdiag
;
; Next, Q, which are composed from the reflectors.  Each reflector v
; is taken from the upper trapezoid of aa, and converted to a matrix
; via (I - 2 vT . v / (v . vT)).
;
;   hh = ident                                    ;; identity matrix
;   for i = 0, n-1 do begin
;    v = aa(*,i) & if i GT 0 then v(0:i-1) = 0    ;; extract reflector
;    hh = hh ## (ident - 2*(v # v)/total(v * v))  ;; generate matrix
;   endfor
;
; Test the result:
;    IDL> print, hh ## transpose(r)
;          9.00000      4.00000
;          2.00000      8.00000
;          6.00000      7.00000
;
; Note that it is usually never necessary to form the Q matrix
; explicitly, and MPFIT does not.

pro mpfit_qrfac, a, ipvt, rdiag, acnorm, pivot=pivot

  COMPILE_OPT strictarr
  sz = size(a)
  m = sz[1]
  n = sz[2]

  common mpfit_machar, machvals
  common mpfit_profile, profvals
;  prof_start = systime(1)

  MACHEP0 = machvals.machep
  DWARF   = machvals.minnum
  
  ;; Compute the initial column norms and initialize arrays
  acnorm = make_array(n, value=a[0]*0.)
  for j = 0L, n-1 do $
    acnorm[j] = mpfit_enorm(a[*,j])
  rdiag = acnorm
  wa = rdiag
  ipvt = lindgen(n)

  ;; Reduce a to r with householder transformations
  minmn = min([m,n])
  for j = 0L, minmn-1 do begin
      if NOT keyword_set(pivot) then goto, HOUSE1
      
      ;; Bring the column of largest norm into the pivot position
      rmax = max(rdiag[j:*])
      kmax = where(rdiag[j:*] EQ rmax, ct) + j
      if ct LE 0 then goto, HOUSE1
      kmax = kmax[0]
      
      ;; Exchange rows via the pivot only.  Avoid actually exchanging
      ;; the rows, in case there is lots of memory transfer.  The
      ;; exchange occurs later, within the body of MPFIT, after the
      ;; extraneous columns of the matrix have been shed.
      if kmax NE j then begin
          temp     = ipvt[j]   & ipvt[j]    = ipvt[kmax] & ipvt[kmax]  = temp
          rdiag[kmax] = rdiag[j]
          wa[kmax]    = wa[j]
      endif
      
      HOUSE1:

      ;; Compute the householder transformation to reduce the jth
      ;; column of A to a multiple of the jth unit vector
      lj     = ipvt[j]
      ajj    = a[j:*,lj]
      ajnorm = mpfit_enorm(ajj)
      if ajnorm EQ 0 then goto, NEXT_ROW
      if a[j,lj] LT 0 then ajnorm = -ajnorm
      
      ajj     = ajj / ajnorm
      ajj[0]  = ajj[0] + 1
      ;; *** Note optimization a(j:*,j)
      a[j,lj] = ajj
      
      ;; Apply the transformation to the remaining columns
      ;; and update the norms

      ;; NOTE to SELF: tried to optimize this by removing the loop,
      ;; but it actually got slower.  Reverted to "for" loop to keep
      ;; it simple.
      if j+1 LT n then begin
          for k=j+1, n-1 do begin
              lk = ipvt[k]
              ajk = a[j:*,lk]
              ;; *** Note optimization a(j:*,lk) 
              ;; (corrected 20 Jul 2000)
              if a[j,lj] NE 0 then $
                a[j,lk] = ajk - ajj * total(ajk*ajj)/a[j,lj]

              if keyword_set(pivot) AND rdiag[k] NE 0 then begin
                  temp = a[j,lk]/rdiag[k]
                  rdiag[k] = rdiag[k] * sqrt((1.-temp^2) > 0)
                  temp = rdiag[k]/wa[k]
                  if 0.05D*temp*temp LE MACHEP0 then begin
                      rdiag[k] = mpfit_enorm(a[j+1:*,lk])
                      wa[k] = rdiag[k]
                  endif
              endif
          endfor
      endif

      NEXT_ROW:
      rdiag[j] = -ajnorm
  endfor

;  profvals.qrfac = profvals.qrfac + (systime(1) - prof_start)
  return
end

;     **********
;
;     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 m