| 1 | subroutine mtgetc(vect,dvect) |
|---|
| 2 | !----------------------------------------------------------------------* |
|---|
| 3 | ! This is exactly equivalent to mtgeti but this version is used for * |
|---|
| 4 | ! calling from C. * |
|---|
| 5 | ! * |
|---|
| 6 | ! This seems to be the only way deal with the problem that originally * |
|---|
| 7 | ! the subroutine mtgeti was called both from C and from Fortran. * |
|---|
| 8 | ! Because madX is setup with Linux-specific name-mangling, the * |
|---|
| 9 | ! adjustments made with the !DEC_dollar ATTRIBUTES do not seem able to * |
|---|
| 10 | ! cope with such a case. * |
|---|
| 11 | ! Subroutine added at 10:38:56 on 8 Apr 2003 by JMJ * |
|---|
| 12 | !----------------------------------------------------------------------* |
|---|
| 13 | implicit none |
|---|
| 14 | double precision vect(*),dvect(*) |
|---|
| 15 | call mtgeti(vect,dvect) |
|---|
| 16 | end |
|---|
| 17 | |
|---|
| 18 | subroutine mtgeti(vect,dvect) |
|---|
| 19 | use name_lenfi |
|---|
| 20 | implicit none |
|---|
| 21 | ! This subroutine should only be called from Fortran. There is an |
|---|
| 22 | ! equivalent mtgetc for the occasions when it needs to be called from C. |
|---|
| 23 | ! Modified at 10:38:56 on 8 Apr 2003 by JMJ |
|---|
| 24 | |
|---|
| 25 | |
|---|
| 26 | logical psum |
|---|
| 27 | integer j,next_vary,get_option,slope |
|---|
| 28 | double precision get_variable,vect(*),dvect(*),c_min,c_max,step, & |
|---|
| 29 | &dval,val,s_fact,valold,eps,eps2,stplim, vmax, vmin, opt |
|---|
| 30 | parameter(s_fact=5d-1) |
|---|
| 31 | parameter(eps = 1.0d-10,eps2 = 1.0d-1,stplim = 2.0d-1) |
|---|
| 32 | parameter(vmax=1.e+20,vmin=-1.e+20) |
|---|
| 33 | character*(name_len) name |
|---|
| 34 | |
|---|
| 35 | psum=get_option('match_summary ') .ne. 0 |
|---|
| 36 | 1 continue |
|---|
| 37 | j = next_vary(name,name_len,c_min,c_max,step,slope,opt) |
|---|
| 38 | if (j .ne. 0) then |
|---|
| 39 | val = get_variable(name) |
|---|
| 40 | if (val .ge. c_max) then |
|---|
| 41 | valold = val |
|---|
| 42 | dval = min(step, (val - c_max)*s_fact) |
|---|
| 43 | ! write(*,*) step, c_min, val, s_fact |
|---|
| 44 | val = c_max - 2*dval |
|---|
| 45 | write(*,*) "reset parameter:",name,"from",valold,"to",val |
|---|
| 46 | elseif (val .le. c_min) then |
|---|
| 47 | valold = val |
|---|
| 48 | dval = min(step, (c_min - val)*s_fact) |
|---|
| 49 | ! write(*,*) step, c_min, val, s_fact |
|---|
| 50 | val = c_min + 2*dval |
|---|
| 51 | write(*,831) "reset parameter:",name,"from",valold,"to",val |
|---|
| 52 | else |
|---|
| 53 | dval = step |
|---|
| 54 | endif |
|---|
| 55 | if(psum) write(*,830) name,val,c_min,c_max |
|---|
| 56 | vect(j) = val |
|---|
| 57 | dvect(j) = dval |
|---|
| 58 | goto 1 |
|---|
| 59 | endif |
|---|
| 60 | ! 830 format(a23,1x,1p,'=',e16.8,';!',2x,e16.8,2x,e16.8) |
|---|
| 61 | 830 format(a24,1x,1p,e16.8,3x,e16.8,3x,e16.8) |
|---|
| 62 | 831 format(a16,1x,a24,a4,e16.8,a4,e16.8) |
|---|
| 63 | end |
|---|
| 64 | |
|---|
| 65 | |
|---|
| 66 | |
|---|
| 67 | subroutine mtlimit(vect,ireset) |
|---|
| 68 | |
|---|
| 69 | use name_lenfi |
|---|
| 70 | implicit none |
|---|
| 71 | |
|---|
| 72 | |
|---|
| 73 | integer j,next_vary,ireset,slope |
|---|
| 74 | double precision vect(*),c_min,c_max,step, & |
|---|
| 75 | &dval,val,s_fact,valold,eps,eps2,stplim, vmax, vmin, opt |
|---|
| 76 | parameter(s_fact=5d-1) |
|---|
| 77 | parameter(eps = 1.0d-10,eps2 = 1.0d-1,stplim = 2.0d-1) |
|---|
| 78 | parameter(vmax=1.e+20,vmin=-1.e+20) |
|---|
| 79 | character*(name_len) name |
|---|
| 80 | |
|---|
| 81 | 1 continue |
|---|
| 82 | j = next_vary(name,name_len,c_min,c_max,step,slope,opt) |
|---|
| 83 | if (j .ne. 0) then |
|---|
| 84 | val = vect(j) |
|---|
| 85 | if (val .ge. c_max) then |
|---|
| 86 | valold = val |
|---|
| 87 | dval = min(step, (val - c_max)*s_fact) |
|---|
| 88 | val = c_max - 2*dval |
|---|
| 89 | write(*,831) "reset parameter:",name,"from",valold,"to",val |
|---|
| 90 | ireset = ireset + 1 |
|---|
| 91 | elseif (val .le. c_min) then |
|---|
| 92 | valold = val |
|---|
| 93 | dval = min(step, (c_min - val)*s_fact) |
|---|
| 94 | val = c_min + 2*dval |
|---|
| 95 | write(*,831) "reset parameter:",name,"from",valold,"to",val |
|---|
| 96 | ireset = ireset + 1 |
|---|
| 97 | endif |
|---|
| 98 | vect(j) = val |
|---|
| 99 | goto 1 |
|---|
| 100 | endif |
|---|
| 101 | 831 format(a16,1x,a24,a4,e16.8,a4,e16.8) |
|---|
| 102 | end |
|---|
| 103 | |
|---|
| 104 | |
|---|
| 105 | subroutine collect(ncon,fsum,fvect) |
|---|
| 106 | |
|---|
| 107 | use name_lenfi |
|---|
| 108 | use twiss0fi |
|---|
| 109 | use twisscfi |
|---|
| 110 | implicit none |
|---|
| 111 | |
|---|
| 112 | |
|---|
| 113 | logical fprt,local,psum, slow_match |
|---|
| 114 | integer ncon,next_constraint,next_global,i,j,pos,type,range(2), & |
|---|
| 115 | &flag,get_option,restart_sequ,advance_to_pos,double_from_table_row, & |
|---|
| 116 | &string_from_table_row |
|---|
| 117 | double precision fsum,fvect(*),val,valhg,c_min,c_max,weight,f_val |
|---|
| 118 | character*(name_len) name, node_name |
|---|
| 119 | integer n_pos, next_constr_namepos, advance_node |
|---|
| 120 | local=get_option('match_local ') .ne. 0 |
|---|
| 121 | fprt=get_option('match_print ') .ne. 0 |
|---|
| 122 | psum=get_option('match_summary ') .ne. 0 |
|---|
| 123 | slow_match = get_option('slow_match ') .ne. 0 |
|---|
| 124 | if(local .and. slow_match) then |
|---|
| 125 | call table_range('twiss ','#s/#e ',range) |
|---|
| 126 | j=restart_sequ() |
|---|
| 127 | do pos=range(1),range(2) |
|---|
| 128 | j=advance_to_pos('twiss ',pos) |
|---|
| 129 | 20 continue |
|---|
| 130 | i=next_constraint(name,name_len,type,valhg,c_min,c_max,weight) |
|---|
| 131 | if(i.ne.0) then |
|---|
| 132 | flag=string_from_table_row('twiss ','name ',pos, node_name) |
|---|
| 133 | flag=double_from_table_row('twiss ', name, pos, val) |
|---|
| 134 | if (type.eq.1) then |
|---|
| 135 | f_val =weight*dim(c_min,val) |
|---|
| 136 | if(fprt) write(*,880) name,weight,val,c_min,f_val**2 |
|---|
| 137 | elseif(type.eq.2) then |
|---|
| 138 | f_val=weight*dim(val,c_max) |
|---|
| 139 | if(fprt) write(*,890) name,weight,val,c_max,f_val**2 |
|---|
| 140 | elseif(type.eq.3) then |
|---|
| 141 | f_val=weight*dim(c_min,val)+weight*dim(val,c_max) |
|---|
| 142 | if(fprt) write(*,840) name,weight,val,c_min,c_max,f_val**2 |
|---|
| 143 | elseif(type.eq.4) then |
|---|
| 144 | f_val=weight*(val-valhg) |
|---|
| 145 | if(fprt) write(*,840) name,weight,val,valhg,valhg,f_val**2 |
|---|
| 146 | endif |
|---|
| 147 | ncon=ncon+1 |
|---|
| 148 | fvect(ncon)=f_val |
|---|
| 149 | fsum=fsum+f_val**2 |
|---|
| 150 | if(psum .and. type.eq.4) & |
|---|
| 151 | &write(*,830) node_name,name,type,valhg,val,f_val**2 |
|---|
| 152 | if(psum .and. type.eq.2) & |
|---|
| 153 | &write(*,830) node_name,name,type,c_max,val,f_val**2 |
|---|
| 154 | if(psum .and. type.eq.1) & |
|---|
| 155 | &write(*,830) node_name,name,type,c_min,val,f_val**2 |
|---|
| 156 | if(psum .and. type.eq.3) & |
|---|
| 157 | &write(*,832) node_name,name,type,c_min,c_max,val,f_val**2 |
|---|
| 158 | goto 20 |
|---|
| 159 | endif |
|---|
| 160 | enddo |
|---|
| 161 | else if(local) then |
|---|
| 162 | j=restart_sequ() |
|---|
| 163 | 21 continue |
|---|
| 164 | call get_twiss_data(opt_fun) |
|---|
| 165 | 22 continue |
|---|
| 166 | i=next_constraint(name,name_len,type,valhg,c_min,c_max,weight) |
|---|
| 167 | if(i.ne.0) then |
|---|
| 168 | n_pos = next_constr_namepos(name) |
|---|
| 169 | if (n_pos.eq.0) then |
|---|
| 170 | print *, ' +-+-+- fatal error' |
|---|
| 171 | print *, 'match - collect: illegal name = ', name |
|---|
| 172 | print *, ' - try with the "slow" option' |
|---|
| 173 | stop |
|---|
| 174 | endif |
|---|
| 175 | val = opt_fun(n_pos) |
|---|
| 176 | call current_node_name(node_name, name_len); |
|---|
| 177 | if (type.eq.1) then |
|---|
| 178 | f_val=weight*dim(c_min,val) |
|---|
| 179 | if(fprt) write(*,880) name,weight,val,c_min,f_val**2 |
|---|
| 180 | elseif(type.eq.2) then |
|---|
| 181 | f_val=weight*dim(val,c_max) |
|---|
| 182 | if(fprt) write(*,890) name,weight,val,c_max,f_val**2 |
|---|
| 183 | elseif(type.eq.3) then |
|---|
| 184 | f_val=weight*dim(c_min,val)+weight*dim(val,c_max) |
|---|
| 185 | if(fprt) write(*,840) name,weight,val,c_min,c_max,f_val**2 |
|---|
| 186 | elseif(type.eq.4) then |
|---|
| 187 | f_val=weight*(val-valhg) |
|---|
| 188 | if(fprt) write(*,840) name,weight,val,valhg,valhg,f_val**2 |
|---|
| 189 | endif |
|---|
| 190 | ncon=ncon+1 |
|---|
| 191 | fvect(ncon)=f_val |
|---|
| 192 | fsum=fsum+f_val**2 |
|---|
| 193 | if(psum .and. type.eq.4) & |
|---|
| 194 | &write(*,830) node_name,name,type,valhg,val,f_val**2 |
|---|
| 195 | if(psum .and. type.eq.2) & |
|---|
| 196 | &write(*,830) node_name,name,type,c_max,val,f_val**2 |
|---|
| 197 | if(psum .and. type.eq.1) & |
|---|
| 198 | &write(*,830) node_name,name,type,c_min,val,f_val**2 |
|---|
| 199 | if(psum .and. type.eq.3) & |
|---|
| 200 | &write(*,832) node_name,name,type,c_min,c_max,val,f_val**2 |
|---|
| 201 | goto 22 |
|---|
| 202 | endif |
|---|
| 203 | if(advance_node() .ne. 0) goto 21 |
|---|
| 204 | endif |
|---|
| 205 | 30 continue |
|---|
| 206 | i=next_global(name,name_len,type,valhg,c_min,c_max,weight) |
|---|
| 207 | if(i.ne.0) then |
|---|
| 208 | pos=1 |
|---|
| 209 | flag=double_from_table_row('summ ',name,pos,val) |
|---|
| 210 | if(type.eq.1) then |
|---|
| 211 | f_val=weight*dim(c_min,val) |
|---|
| 212 | if(fprt) write(*,880) name,weight,val,c_min,f_val**2 |
|---|
| 213 | elseif(type.eq.2) then |
|---|
| 214 | f_val=weight*dim(val,c_max) |
|---|
| 215 | if(fprt) write(*,890) name,weight,val,c_max,f_val**2 |
|---|
| 216 | elseif(type.eq.3) then |
|---|
| 217 | f_val=weight*dim(c_min,val)+ weight*dim(val,c_max) |
|---|
| 218 | if(fprt) write(*,840) name,weight,val,c_min,c_max,f_val**2 |
|---|
| 219 | elseif(type.eq.4) then |
|---|
| 220 | f_val=weight*(val-valhg) |
|---|
| 221 | if(fprt) write(*,840) name,weight,val,valhg,valhg,f_val**2 |
|---|
| 222 | endif |
|---|
| 223 | ncon=ncon+1 |
|---|
| 224 | fvect(ncon)=f_val |
|---|
| 225 | fsum=fsum+f_val**2 |
|---|
| 226 | |
|---|
| 227 | if(psum) & |
|---|
| 228 | &write(*,830) "Global constraint: ",name,type,valhg,val, & |
|---|
| 229 | &f_val**2 |
|---|
| 230 | |
|---|
| 231 | goto 30 |
|---|
| 232 | endif |
|---|
| 233 | ! if (psum) write(*,831) fsum |
|---|
| 234 | 830 format(a24,3x,a6,5x,i3,3x,1p,e16.8,3x,e16.8,3x,e16.8) |
|---|
| 235 | 832 format(a24,3x,a6,5x,i3,3x,1p,e16.8,3x,e16.8,3x,e16.8,3x,e16.8) |
|---|
| 236 | 831 format(//,'Final Penalty Function = ',e16.8,//) |
|---|
| 237 | 840 format(a10,3x,1p,5e16.6) |
|---|
| 238 | 880 format(a10,3x,1p,3e16.6,16x,e16.6) |
|---|
| 239 | 890 format(a10,3x,1p,2e16.6,16x,2e16.6) |
|---|
| 240 | end |
|---|
| 241 | |
|---|
| 242 | |
|---|
| 243 | subroutine mtfcn(nf,nx,x,fval,iflag) |
|---|
| 244 | implicit none |
|---|
| 245 | |
|---|
| 246 | |
|---|
| 247 | !----------------------------------------------------------------------* |
|---|
| 248 | ! Purpose: * |
|---|
| 249 | ! Compute matching functions. * |
|---|
| 250 | ! Input: * |
|---|
| 251 | ! NF (integer) Number of functions to be computed. * |
|---|
| 252 | ! NX (integer) Number of input parameters. * |
|---|
| 253 | ! X(NX) (real) Input parameters. * |
|---|
| 254 | ! Output: * |
|---|
| 255 | ! FVAL(NF) (real) Matching functions computed. * |
|---|
| 256 | ! IFLAG (integer) Stability flag. * |
|---|
| 257 | !----------------------------------------------------------------------* |
|---|
| 258 | integer iflag,nf,nx,izero,ione |
|---|
| 259 | double precision fval(nf),x(nx) |
|---|
| 260 | |
|---|
| 261 | izero = 0 |
|---|
| 262 | ione = 1 |
|---|
| 263 | !---- Store parameter values in data structure. |
|---|
| 264 | call mtputi(x) |
|---|
| 265 | |
|---|
| 266 | !---- Compute matching functions. |
|---|
| 267 | |
|---|
| 268 | call mtcond(izero, nf, fval, iflag) |
|---|
| 269 | |
|---|
| 270 | 9999 end |
|---|
| 271 | subroutine mtputi(vect) |
|---|
| 272 | |
|---|
| 273 | use name_lenfi |
|---|
| 274 | implicit none |
|---|
| 275 | |
|---|
| 276 | |
|---|
| 277 | integer j,next_vary,slope |
|---|
| 278 | double precision vect(*),c_min,c_max,step,s_fact,opt |
|---|
| 279 | parameter(s_fact=5d-1) |
|---|
| 280 | character*(name_len) name |
|---|
| 281 | |
|---|
| 282 | 1 continue |
|---|
| 283 | j=next_vary(name,name_len,c_min,c_max,step,slope,opt) |
|---|
| 284 | if(j.ne.0) then |
|---|
| 285 | call set_variable(name,vect(j)) |
|---|
| 286 | goto 1 |
|---|
| 287 | endif |
|---|
| 288 | end |
|---|
| 289 | subroutine mtlmdf(ncon,nvar,tol,calls,call_lim,vect,dvect,fun_vec,& |
|---|
| 290 | &diag,w_ifjac,w_ipvt,w_qtf,w_iwa1,w_iwa2,w_iwa3,w_iwa4,xold) |
|---|
| 291 | |
|---|
| 292 | use matchfi |
|---|
| 293 | implicit none |
|---|
| 294 | !----------------------------------------------------------------------* |
|---|
| 295 | ! Purpose: * |
|---|
| 296 | ! LMDIF command. * |
|---|
| 297 | ! Attributes: * |
|---|
| 298 | ! ncon (int) # constraints * |
|---|
| 299 | ! nvar (int) # variables * |
|---|
| 300 | ! tol (real) Final tolerance for match. * |
|---|
| 301 | ! calls (int) current call count * |
|---|
| 302 | ! call_lim (int) current call limit * |
|---|
| 303 | ! vect (real) variable values * |
|---|
| 304 | ! dvect (real) variable steps * |
|---|
| 305 | ! fun_vect (real) function values * |
|---|
| 306 | ! all other working spaces for lmdif * |
|---|
| 307 | !----------------------------------------------------------------------* |
|---|
| 308 | integer calls,call_lim,ncon,nvar,i,ipvt(nvar) |
|---|
| 309 | ! icovar: functionality still unclear HG 28.2.02 |
|---|
| 310 | ! ilevel: print level |
|---|
| 311 | double precision tol,vect(*),dvect(*),fun_vec(*),diag(*), & |
|---|
| 312 | &w_ifjac(*),w_qtf(*),w_iwa1(*),w_iwa2(*),w_iwa3(*),w_iwa4(*),one, & |
|---|
| 313 | &xold(*),w_ipvt(*) |
|---|
| 314 | parameter(one=1d0) |
|---|
| 315 | external mtfcn |
|---|
| 316 | |
|---|
| 317 | icovar = 0 |
|---|
| 318 | ilevel = 0 |
|---|
| 319 | ipvt(:) = 0 |
|---|
| 320 | call mtgeti(vect,dvect) |
|---|
| 321 | call lmdif(mtfcn,ncon,nvar,calls,call_lim,vect,fun_vec,tol,diag, & |
|---|
| 322 | &one,w_ifjac,ncon,ipvt,w_qtf,w_iwa1,w_iwa2,w_iwa3,w_iwa4,xold) |
|---|
| 323 | do i=1,nvar |
|---|
| 324 | w_ipvt(i)=ipvt(i) |
|---|
| 325 | enddo |
|---|
| 326 | 9999 end |
|---|
| 327 | |
|---|
| 328 | subroutine lmdif(fcn,m,n,calls,call_lim,x,fvec,epsfcn,diag,factor,& |
|---|
| 329 | &fjac,ldfjac,ipvt,qtf,wa1,wa2,wa3,wa4,xold) |
|---|
| 330 | |
|---|
| 331 | use matchfi |
|---|
| 332 | implicit none |
|---|
| 333 | |
|---|
| 334 | |
|---|
| 335 | !----------------------------------------------------------------------* |
|---|
| 336 | ! Purpose: * |
|---|
| 337 | ! The purpose of LMDIF is to minimize the sum of the squares of * |
|---|
| 338 | ! M nonlinear functions in N variables by a modification of * |
|---|
| 339 | ! the Levenberg-Marquardt algorithm. The user must provide a * |
|---|
| 340 | ! subroutine which calculates the functions. The Jacobian is * |
|---|
| 341 | ! then calculated by a forward-difference approximation. * |
|---|
| 342 | ! * |
|---|
| 343 | ! FCN is the name of the user-supplied subroutine which * |
|---|
| 344 | ! calculates the functions. FCN must be declared * |
|---|
| 345 | ! in an external statement in the user calling * |
|---|
| 346 | ! program, and should be written as follows: * |
|---|
| 347 | ! * |
|---|
| 348 | ! SUBROUTINE FCN(M,N,X,FVEC,IFLAG) * |
|---|
| 349 | ! DIMENSION X(N),FVEC(M) * |
|---|
| 350 | ! CALCULATE THE FUNCTIONS AT X AND * |
|---|
| 351 | ! RETURN THIS VECTOR IN FVEC. * |
|---|
| 352 | ! RETURN * |
|---|
| 353 | ! END * |
|---|
| 354 | ! * |
|---|
| 355 | ! The value of IFLAG should be set to zero, unless there * |
|---|
| 356 | ! is an error in evaluation of the function. * |
|---|
| 357 | ! * |
|---|
| 358 | ! M is a positive integer input variable set to the number * |
|---|
| 359 | ! of functions. * |
|---|
| 360 | ! * |
|---|
| 361 | ! N is a positive integer input variable set to the number * |
|---|
| 362 | ! of variables. N must not exceed M. * |
|---|
| 363 | ! * |
|---|
| 364 | ! X is an array of length N. On input X must contain * |
|---|
| 365 | ! an initial estimate of the solution vector. On output X * |
|---|
| 366 | ! contains the final estimate of the solution vector. * |
|---|
| 367 | ! * |
|---|
| 368 | ! FVEC is an output array of length M which contains * |
|---|
| 369 | ! the functions evaluated at the output X. * |
|---|
| 370 | ! * |
|---|
| 371 | ! EPSFCN is an input variable used in determining a suitable * |
|---|
| 372 | ! step length for the forward-difference approximation. This * |
|---|
| 373 | ! approximation assumes that the relative errors in the * |
|---|
| 374 | ! functions are of the order of EPSFCN. If EPSFCN is less * |
|---|
| 375 | ! than the machine precision, it is assumed that the relative * |
|---|
| 376 | ! errors in the functions are of the order of the machine * |
|---|
| 377 | ! precision. * |
|---|
| 378 | ! * |
|---|
| 379 | ! DIAG is an array of length N. If MODE = 1 (see * |
|---|
| 380 | ! below), DIAG is internally set. If MODE = 2, DIAG * |
|---|
| 381 | ! must contain positive entries that serve as * |
|---|
| 382 | ! multiplicative scale factors for the variables. * |
|---|
| 383 | ! * |
|---|
| 384 | ! FACTOR is a positive input variable used in determining the * |
|---|
| 385 | ! initial step bound. This bound is set to the product of * |
|---|
| 386 | ! FACTOR and the Euclidean norm of DIAG*X if nonzero, or else * |
|---|
| 387 | ! to FACTOR itself. In most cases FACTOR should lie in the * |
|---|
| 388 | ! interval (.1,100.). 100. Is a generally recommended value. * |
|---|
| 389 | ! * |
|---|
| 390 | ! FJAC is an output M by N array. The upper N by N submatrix * |
|---|
| 391 | ! of FJAC contains an upper triangular matrix R with * |
|---|
| 392 | ! diagonal elements of nonincreasing magnitude such that * |
|---|
| 393 | ! * |
|---|
| 394 | ! T T T * |
|---|
| 395 | ! P *(JAC *JAC)*P = R *R, * |
|---|
| 396 | ! * |
|---|
| 397 | ! where P is a permutation matrix and JAC is the final * |
|---|
| 398 | ! calculated Jacobian. column J of P is column IPVT(J) * |
|---|
| 399 | ! (see below) of the identity matrix. The lower trapezoidal * |
|---|
| 400 | ! part of FJAC contains information generated during * |
|---|
| 401 | ! the computation of R. * |
|---|
| 402 | ! * |
|---|
| 403 | ! LDFJAC is a positive integer input variable not less than M * |
|---|
| 404 | ! which specifies the leading dimension of the array FJAC. * |
|---|
| 405 | ! * |
|---|
| 406 | ! IPVT is an integer output array of length N. IPVT * |
|---|
| 407 | ! defines a permutation matrix P such that JAC*P = Q*r, * |
|---|
| 408 | ! where JAC is the final calculated Jacobian, Q is * |
|---|
| 409 | ! orthogonal (not stored), and R is upper triangular * |
|---|
| 410 | ! with diagonal elements of nonincreasing magnitude. * |
|---|
| 411 | ! column J of P is column IPVT(J) of the identity matrix. * |
|---|
| 412 | ! * |
|---|
| 413 | ! QTF is an output array of length N which contains * |
|---|
| 414 | ! the first N elements of the vector (Q transpose)*FVEC. * |
|---|
| 415 | ! * |
|---|
| 416 | ! WA1, WA2, and WA3 are work arrays of length N. * |
|---|
| 417 | ! * |
|---|
| 418 | ! WA4 is a work array of length M. * |
|---|
| 419 | ! Source: * |
|---|
| 420 | ! Argonne National Laboratory. MINPACK Project. March 1980. * |
|---|
| 421 | ! Burton S. Garbow, Kenneth E. Hillstrom, Jorge J. More. * |
|---|
| 422 | !----------------------------------------------------------------------* |
|---|
| 423 | integer izero |
|---|
| 424 | integer i,iflag,info,iter,j,l,ldfjac,level,m,n,calls,call_lim, & |
|---|
| 425 | &ipvt(n), ireset |
|---|
| 426 | double precision fmin_old,actred,delta,dirder,epsfcn,factor,fnorm,& |
|---|
| 427 | &fnorm1,ftol,gnorm,gtol,par,pnorm,prered,ratio,sum,temp,temp1, & |
|---|
| 428 | &temp2,vmod,xnorm,xtol,x(n),xold(n),fvec(m),diag(n),fjac(ldfjac,n),& |
|---|
| 429 | &qtf(n),wa1(n),wa2(n),wa3(n),wa4(m),zero,one,two,p1,p25,p5,p75,p90,& |
|---|
| 430 | &p0001,epsil,epsmch |
|---|
| 431 | parameter(zero=0d0,one=1d0,two=2d0,p1=0.1d0,p5=0.5d0,p25=0.25d0, & |
|---|
| 432 | &p75=0.75d0,p90=0.9d0,p0001=0.0001d0,epsil=1d-8,epsmch=1d-16) |
|---|
| 433 | external fcn, mtcond |
|---|
| 434 | |
|---|
| 435 | ireset = 0 |
|---|
| 436 | izero = 0 |
|---|
| 437 | info = 0 |
|---|
| 438 | level = 0 |
|---|
| 439 | ftol = epsfcn |
|---|
| 440 | gtol = epsil |
|---|
| 441 | xtol = epsil |
|---|
| 442 | !---- Compute matching functions. |
|---|
| 443 | call mtcond(izero, m, fvec, iflag) |
|---|
| 444 | ! call fcn(m,n,x,fvec,iflag) |
|---|
| 445 | calls = calls + 1 |
|---|
| 446 | if (iflag .ne. 0) then |
|---|
| 447 | call aawarn('LMDIF', ' stopped, possibly unstable') |
|---|
| 448 | info = - 1 |
|---|
| 449 | go to 300 |
|---|
| 450 | endif |
|---|
| 451 | fnorm = vmod(m, fvec) |
|---|
| 452 | fmin = fnorm**2 |
|---|
| 453 | fmin_old = fmin |
|---|
| 454 | edm = fmin |
|---|
| 455 | write(*,831) fmin |
|---|
| 456 | |
|---|
| 457 | !---- Check the input parameters for errors. |
|---|
| 458 | if (n .le. 0 .or. m .lt. n .or. ldfjac .lt. m & |
|---|
| 459 | &.or. ftol .lt. zero .or. xtol .lt. zero .or. gtol .lt. zero & |
|---|
| 460 | &.or. call_lim .le. 0 .or. factor .le. zero) go to 300 |
|---|
| 461 | |
|---|
| 462 | !---- Quit, when initial value is already OK. |
|---|
| 463 | if (fmin .le. ftol) then |
|---|
| 464 | info = 4 |
|---|
| 465 | do j = 1, n |
|---|
| 466 | xold(j) = x(j) |
|---|
| 467 | enddo |
|---|
| 468 | go to 300 |
|---|
| 469 | endif |
|---|
| 470 | if (ilevel .ge. 1) call mtprnt('old', n, x) |
|---|
| 471 | |
|---|
| 472 | !---- Initialize Levenberg-Marquardt parameter and iteration count |
|---|
| 473 | par = zero |
|---|
| 474 | iter = 1 |
|---|
| 475 | |
|---|
| 476 | !---- Beginning of the outer loop. |
|---|
| 477 | 30 continue |
|---|
| 478 | ! write(*,*) 'outer ',calls,fmin,fmin_old |
|---|
| 479 | |
|---|
| 480 | !---- Calculate the Jacobian matrix. |
|---|
| 481 | call fdjac2(fcn,m,n,x,fvec,fjac,ldfjac,iflag,xtol,wa4) |
|---|
| 482 | calls = calls + n |
|---|
| 483 | if (iflag .ne. 0) then |
|---|
| 484 | info = - 1 |
|---|
| 485 | go to 300 |
|---|
| 486 | endif |
|---|
| 487 | |
|---|
| 488 | !---- Compute the QR factorization of the Jacobian. |
|---|
| 489 | call qrfac(m,n,fjac,ldfjac,.true.,ipvt,n,wa1,wa2,wa3) |
|---|
| 490 | |
|---|
| 491 | !---- On the first iteration scale according to the norms |
|---|
| 492 | ! of the columns of the initial Jacobian. |
|---|
| 493 | ! Calculate the norm of the scaled X |
|---|
| 494 | ! and initialize the step bound delta. |
|---|
| 495 | if (iter .eq. 1) then |
|---|
| 496 | do j = 1, n |
|---|
| 497 | xold(j) = x(j) |
|---|
| 498 | diag(j) = wa2(j) |
|---|
| 499 | if (wa2(j) .eq. zero) diag(j) = one |
|---|
| 500 | wa3(j) = diag(j)*x(j) |
|---|
| 501 | enddo |
|---|
| 502 | xnorm = vmod(n, wa3) |
|---|
| 503 | delta = factor*xnorm |
|---|
| 504 | if (delta .eq. zero) delta = factor |
|---|
| 505 | endif |
|---|
| 506 | |
|---|
| 507 | !---- Form (Q transpose)*FVEC and store the first N components in QTF. |
|---|
| 508 | do i = 1, m |
|---|
| 509 | wa4(i) = fvec(i) |
|---|
| 510 | enddo |
|---|
| 511 | do j = 1, n |
|---|
| 512 | if (fjac(j,j) .ne. zero) then |
|---|
| 513 | sum = zero |
|---|
| 514 | do i = j, m |
|---|
| 515 | sum = sum + fjac(i,j)*wa4(i) |
|---|
| 516 | enddo |
|---|
| 517 | temp = -sum/fjac(j,j) |
|---|
| 518 | do i = j, m |
|---|
| 519 | wa4(i) = wa4(i) + fjac(i,j)*temp |
|---|
| 520 | enddo |
|---|
| 521 | endif |
|---|
| 522 | fjac(j,j) = wa1(j) |
|---|
| 523 | qtf(j) = wa4(j) |
|---|
| 524 | enddo |
|---|
| 525 | |
|---|
| 526 | !---- Compute the norm of the scaled gradient. |
|---|
| 527 | gnorm = zero |
|---|
| 528 | if (fnorm .ne. zero) then |
|---|
| 529 | do j = 1, n |
|---|
| 530 | l = ipvt(j) |
|---|
| 531 | if (wa2(l) .ne. zero) then |
|---|
| 532 | sum = zero |
|---|
| 533 | do i = 1, j |
|---|
| 534 | sum = sum + fjac(i,j)*(qtf(i)/fnorm) |
|---|
| 535 | enddo |
|---|
| 536 | gnorm = max(gnorm,abs(sum/wa2(l))) |
|---|
| 537 | endif |
|---|
| 538 | enddo |
|---|
| 539 | endif |
|---|
| 540 | |
|---|
| 541 | !---- Test for convergence of the gradient norm. |
|---|
| 542 | if (gnorm .le. gtol) info = 4 |
|---|
| 543 | if (info .ne. 0) go to 300 |
|---|
| 544 | |
|---|
| 545 | !---- Rescale if necessary. |
|---|
| 546 | do j = 1, n |
|---|
| 547 | diag(j) = max(diag(j),wa2(j)) |
|---|
| 548 | enddo |
|---|
| 549 | |
|---|
| 550 | !---- Beginning of the inner loop. |
|---|
| 551 | 200 continue |
|---|
| 552 | ! write(*,*) 'inner ',calls,fmin,fmin_old |
|---|
| 553 | ! if (fmin.lt.p90*fmin_old) write(*,830) calls,fmin |
|---|
| 554 | fmin_old = fmin |
|---|
| 555 | |
|---|
| 556 | !---- Determine the Levenberg-Marquardt parameter. |
|---|
| 557 | call lmpar(n,fjac,ldfjac,ipvt,diag,qtf,delta,par,wa1,wa2, & |
|---|
| 558 | &wa3,wa4) |
|---|
| 559 | |
|---|
| 560 | !---- Store the direction P and X + P. Calculate the norm of P. |
|---|
| 561 | do j = 1, n |
|---|
| 562 | wa1(j) = -wa1(j) |
|---|
| 563 | wa2(j) = x(j) + wa1(j) |
|---|
| 564 | wa3(j) = diag(j)*wa1(j) |
|---|
| 565 | enddo |
|---|
| 566 | pnorm = vmod(n, wa3) |
|---|
| 567 | |
|---|
| 568 | !---- On the first iteration, adjust the initial step bound. |
|---|
| 569 | if (iter .eq. 1) delta = min(delta,pnorm) |
|---|
| 570 | |
|---|
| 571 | !---- Evaluate the function at X + P and calculate its norm. |
|---|
| 572 | ! 23.5.2002: Oliver Bruening: inserted a routine that checks for |
|---|
| 573 | ! minimum and maximum values of the parameters: |
|---|
| 574 | call mtlimit(wa2,ireset) |
|---|
| 575 | call fcn(m,n,wa2,wa4,iflag) |
|---|
| 576 | calls = calls + 1 |
|---|
| 577 | if (iflag .ne. 0) then |
|---|
| 578 | fnorm1 = two * fnorm |
|---|
| 579 | else |
|---|
| 580 | fnorm1 = vmod(m, wa4) |
|---|
| 581 | endif |
|---|
| 582 | |
|---|
| 583 | !---- Compute the scaled actual reduction. |
|---|
| 584 | actred = -one |
|---|
| 585 | if (p1*fnorm1 .lt. fnorm) actred = one - (fnorm1/fnorm)**2 |
|---|
| 586 | |
|---|
| 587 | !---- Compute the scaled predicted reduction and |
|---|
| 588 | ! the scaled directional derivative. |
|---|
| 589 | do j = 1, n |
|---|
| 590 | wa3(j) = zero |
|---|
| 591 | l = ipvt(j) |
|---|
| 592 | temp = wa1(l) |
|---|
| 593 | do i = 1, j |
|---|
| 594 | wa3(i) = wa3(i) + fjac(i,j)*temp |
|---|
| 595 | enddo |
|---|
| 596 | enddo |
|---|
| 597 | temp1 = vmod(n, wa3)/fnorm |
|---|
| 598 | temp2 = (sqrt(par)*pnorm)/fnorm |
|---|
| 599 | prered = temp1**2 + temp2**2/p5 |
|---|
| 600 | dirder = -(temp1**2 + temp2**2) |
|---|
| 601 | |
|---|
| 602 | !---- Compute the ratio of the actual to the predicted reduction. |
|---|
| 603 | ratio = zero |
|---|
| 604 | if (prered .ne. zero) ratio = actred/prered |
|---|
| 605 | |
|---|
| 606 | !---- Update the step bound. |
|---|
| 607 | if (ratio .le. p25) then |
|---|
| 608 | if (actred .ge. zero) temp = p5 |
|---|
| 609 | if (actred .lt. zero) & |
|---|
| 610 | &temp = p5*dirder/(dirder + p5*actred) |
|---|
| 611 | if (p1*fnorm1 .ge. fnorm .or. temp .lt. p1) temp = p1 |
|---|
| 612 | delta = temp*min(delta,pnorm/p1) |
|---|
| 613 | par = par/temp |
|---|
| 614 | else if (par .eq. zero .or. ratio .ge. p75) then |
|---|
| 615 | delta = pnorm/p5 |
|---|
| 616 | par = p5*par |
|---|
| 617 | endif |
|---|
| 618 | |
|---|
| 619 | !---- Test for successful iteration. |
|---|
| 620 | if (ratio .ge. p0001) then |
|---|
| 621 | |
|---|
| 622 | !---- Successful iteration. Update X, FVEC, and their norms. |
|---|
| 623 | do j = 1, n |
|---|
| 624 | x(j) = wa2(j) |
|---|
| 625 | xold(j) = wa2(j) |
|---|
| 626 | wa2(j) = diag(j)*x(j) |
|---|
| 627 | enddo |
|---|
| 628 | do i = 1, m |
|---|
| 629 | fvec(i) = wa4(i) |
|---|
| 630 | enddo |
|---|
| 631 | xnorm = vmod(n, wa2) |
|---|
| 632 | fnorm = fnorm1 |
|---|
| 633 | iter = iter + 1 |
|---|
| 634 | |
|---|
| 635 | !---- If requested, print iterates. |
|---|
| 636 | fmin = fnorm**2 |
|---|
| 637 | edm = gnorm * fmin |
|---|
| 638 | level = 3 |
|---|
| 639 | if (mod(iter,10) .eq. 0) level = 2 |
|---|
| 640 | if (ilevel .ge. level) call mtprnt('inter', n, x) |
|---|
| 641 | write(*,830) calls,fmin |
|---|
| 642 | endif |
|---|
| 643 | |
|---|
| 644 | !---- Tests for convergence. |
|---|
| 645 | if (abs(actred) .le. ftol .and. prered .le. ftol & |
|---|
| 646 | &.and. p5*ratio .le. one) info = 1 |
|---|
| 647 | if (delta .le. xtol*xnorm) info = 3 |
|---|
| 648 | if (abs(actred) .le. ftol .and. prered .le. ftol & |
|---|
| 649 | &.and. p5*ratio .le. one .and. info .eq. 2) info = 2 |
|---|
| 650 | if (fmin .le. ftol) info = 4 |
|---|
| 651 | if (info .ne. 0) go to 300 |
|---|
| 652 | |
|---|
| 653 | !---- Tests for termination and stringent tolerances. |
|---|
| 654 | if (calls .ge. call_lim) info = 5 |
|---|
| 655 | if (abs(actred) .le. epsmch .and. prered .le. epsmch & |
|---|
| 656 | &.and. p5*ratio .le. one) info = 6 |
|---|
| 657 | if (delta .le. epsmch*xnorm) info = 7 |
|---|
| 658 | if (gnorm .le. epsmch) info = 8 |
|---|
| 659 | if (ireset .gt. 20) info = 10 |
|---|
| 660 | if (info .ne. 0) go to 300 |
|---|
| 661 | |
|---|
| 662 | !---- End of the inner loop. Repeat if iteration unsuccessful. |
|---|
| 663 | if (ratio .lt. p0001) go to 200 |
|---|
| 664 | if(info.eq.10) goto 300 |
|---|
| 665 | |
|---|
| 666 | !---- End of the outer loop. |
|---|
| 667 | go to 30 |
|---|
| 668 | |
|---|
| 669 | !---- Termination, either normal or user imposed. |
|---|
| 670 | 300 continue |
|---|
| 671 | if (info .lt. 0) then |
|---|
| 672 | print *, '++++++++++ LMDIF ended: unstable' |
|---|
| 673 | else if (info .eq. 0) then |
|---|
| 674 | print *, '++++++++++ LMDIF ended: error' |
|---|
| 675 | else if (info .eq. 4) then |
|---|
| 676 | print *, '++++++++++ LMDIF ended: converged successfully' |
|---|
| 677 | else if (info .eq. 3) then |
|---|
| 678 | print *, '++++++++++ LMDIF ended: converged without success' |
|---|
| 679 | else if (info .lt. 3) then |
|---|
| 680 | print *, '++++++++++ LMDIF ended: converged: info = ', info |
|---|
| 681 | else if (info .eq. 5) then |
|---|
| 682 | print *, '++++++++++ LMDIF ended: call limit' |
|---|
| 683 | else if (info .eq. 10) then |
|---|
| 684 | print *, '++++++++++' |
|---|
| 685 | print *, '++++++++++ LMDIF ended: variables too close to limit' |
|---|
| 686 | print *, '++++++++++' |
|---|
| 687 | else |
|---|
| 688 | print *, '++++++++++ LMDIF ended: accuracy limit' |
|---|
| 689 | endif |
|---|
| 690 | |
|---|
| 691 | call fcn(m,n,xold,fvec,iflag) |
|---|
| 692 | fnorm = vmod(m, fvec) |
|---|
| 693 | fmin = fnorm**2 |
|---|
| 694 | ! call mtputi(xold) |
|---|
| 695 | write(*,830) calls,fmin |
|---|
| 696 | |
|---|
| 697 | if (ilevel .ge. 1) call mtprnt('last',n, x) |
|---|
| 698 | |
|---|
| 699 | 831 format('Initial Penalty Function = ',e16.8,//) |
|---|
| 700 | 830 format('call:',I8,3x,'Penalty function = ',e16.8) |
|---|
| 701 | end |
|---|
| 702 | subroutine fdjac2(fcn,m,n,x,fvec,fjac,ldfjac,iflag,epsfcn,wa) |
|---|
| 703 | implicit none |
|---|
| 704 | !----------------------------------------------------------------------* |
|---|
| 705 | ! Purpose: * |
|---|
| 706 | ! This subroutine computes a forward-difference approximation * |
|---|
| 707 | ! to the M by N Jacobian matrix associated with a specified * |
|---|
| 708 | ! problem of M functions in N variables. * |
|---|
| 709 | ! Input: * |
|---|
| 710 | ! FCN is the name of the user-supplied subroutine which * |
|---|
| 711 | ! calculates the functions. FCN must be declared * |
|---|
| 712 | ! in an external statement in the user calling * |
|---|
| 713 | ! program, and should be written as follows: * |
|---|
| 714 | ! * |
|---|
| 715 | ! SUBROUTINE FCN(M,N,X,FVEC,IFLAG) * |
|---|
| 716 | ! DIMENSION X(N),FVEC(M) * |
|---|
| 717 | ! CALCULATE THE FUNCTIONS AT X AND * |
|---|
| 718 | ! RETURN THIS VECTOR IN FVEC. * |
|---|
| 719 | ! RETURN * |
|---|
| 720 | ! END * |
|---|
| 721 | ! * |
|---|
| 722 | ! The value of IFLAG should be set to zero, unless there * |
|---|
| 723 | ! is an error in evaluation of the function. * |
|---|
| 724 | ! * |
|---|
| 725 | ! M is a positive integer input variable set to the number * |
|---|
| 726 | ! of functions. * |
|---|
| 727 | ! * |
|---|
| 728 | ! N is a positive integer input variable set to the number * |
|---|
| 729 | ! of variables. N must not exceed M. * |
|---|
| 730 | ! * |
|---|
| 731 | ! X is an input array of length N. * |
|---|
| 732 | ! * |
|---|
| 733 | ! FVEC is an input array of length M which must contain the * |
|---|
| 734 | ! functions evaluated at X. * |
|---|
| 735 | ! * |
|---|
| 736 | ! FJAC is an output M by N array which contains the * |
|---|
| 737 | ! approximation to the Jacobian matrix evaluated at X. * |
|---|
| 738 | ! * |
|---|
| 739 | ! LDFJAC is a positive integer input variable not less than M * |
|---|
| 740 | ! which specifies the leading dimension of the array FJAC. * |
|---|
| 741 | ! * |
|---|
| 742 | ! IFLAG is an integer variable which tells the calling program * |
|---|
| 743 | ! wether the approximation is valid. * |
|---|
| 744 | ! * |
|---|
| 745 | ! EPSFCN is an input variable used in determining a suitable * |
|---|
| 746 | ! step length for the forward-difference approximation. This * |
|---|
| 747 | ! approximation assumes that the relative errors in the * |
|---|
| 748 | ! functions are of the order of EPSFCN. If EPSFCN is less * |
|---|
| 749 | ! than the machine precision, it is assumed that the relative * |
|---|
| 750 | ! errors in the functions are of the order of the machine * |
|---|
| 751 | ! precision. * |
|---|
| 752 | ! * |
|---|
| 753 | ! WA is a work array of length M. * |
|---|
| 754 | ! Source: * |
|---|
| 755 | ! Argonne National Laboratory. MINPACK Project. March 1980. * |
|---|
| 756 | ! Burton S. Garbow, Kenneth E. Hillstrom, Jorge J. More. * |
|---|
| 757 | !----------------------------------------------------------------------* |
|---|
| 758 | integer i,iflag,j,ldfjac,m,n |
|---|
| 759 | double precision eps,epsfcn,fjac(ldfjac,n),fvec(m),h,temp,wa(m), & |
|---|
| 760 | &x(n),zero,epsmch |
|---|
| 761 | parameter(zero=0d0,epsmch=1d-16) |
|---|
| 762 | external fcn |
|---|
| 763 | |
|---|
| 764 | eps = sqrt(max(epsfcn,epsmch)) |
|---|
| 765 | iflag = 0 |
|---|
| 766 | |
|---|
| 767 | do j = 1, n |
|---|
| 768 | temp = x(j) |
|---|
| 769 | h = eps*abs(temp) |
|---|
| 770 | if (h .eq. zero) h = eps |
|---|
| 771 | x(j) = temp + h |
|---|
| 772 | call fcn(m,n,x,wa,iflag) |
|---|
| 773 | x(j) = temp |
|---|
| 774 | if (iflag .ne. 0) go to 30 |
|---|
| 775 | do i = 1, m |
|---|
| 776 | fjac(i,j) = (wa(i) - fvec(i))/h |
|---|
| 777 | enddo |
|---|
| 778 | enddo |
|---|
| 779 | 30 continue |
|---|
| 780 | |
|---|
| 781 | end |
|---|
| 782 | subroutine qrfac(m,n,a,lda,pivot,ipvt,lipvt,rdiag,acnorm,wa) |
|---|
| 783 | |
|---|
| 784 | implicit none |
|---|
| 785 | |
|---|
| 786 | |
|---|
| 787 | !----------------------------------------------------------------------* |
|---|
| 788 | ! Purpose: * |
|---|
| 789 | ! This subroutine uses Householder transformations with column * |
|---|
| 790 | ! pivoting (optional) to compute a QR factorization of the * |
|---|
| 791 | ! M by N matrix a. That is, QRFAC determines an orthogonal * |
|---|
| 792 | ! matrix Q, a permutation matrix P, and an upper trapezoidal * |
|---|
| 793 | ! matrix R with diagonal elements of nonincreasing magnitude, * |
|---|
| 794 | ! such that A*P = Q*R. The Householder transformation for * |
|---|
| 795 | ! column K, K = 1,2,...,min(M,N), is of the form * |
|---|
| 796 | ! * |
|---|
| 797 | ! T * |
|---|
| 798 | ! I - (1/U(K))*U*U * |
|---|
| 799 | ! * |
|---|
| 800 | ! where U has zeros in the first K-1 positions. The form of * |
|---|
| 801 | ! this transformation and the method of pivoting first * |
|---|
| 802 | ! appeared in the corresponding LINPACK subroutine. * |
|---|
| 803 | ! * |
|---|
| 804 | ! M is a positive integer input variable set to the number * |
|---|
| 805 | ! of rows of A. * |
|---|
| 806 | ! * |
|---|
| 807 | ! N is a positive integer input variable set to the number * |
|---|
| 808 | ! of columns of A. * |
|---|
| 809 | ! * |
|---|
| 810 | ! A is an M by N array. On input A contains the matrix for * |
|---|
| 811 | ! which the QR factorization is to be computed. On output * |
|---|
| 812 | ! The strict upper trapezoidal part of A contains the strict * |
|---|
| 813 | ! upper trapezoidal part of R, and the lower trapezoidal * |
|---|
| 814 | ! part of A contains a factored form of Q (the non-trivial * |
|---|
| 815 | ! elements of the U vectors described above). * |
|---|
| 816 | ! * |
|---|
| 817 | ! LDA is a positive integer input variable not less than M * |
|---|
| 818 | ! which specifies the leading dimension of the array A. * |
|---|
| 819 | ! * |
|---|
| 820 | ! PIVOT is a logical input variable. If PIVOT is set true, * |
|---|
| 821 | ! then column pivoting is enforced. If PIVOT is set false, * |
|---|
| 822 | ! then no column pivoting is done. * |
|---|
| 823 | ! * |
|---|
| 824 | ! IPVT is an integer output array of length LIPVT. Ipvt * |
|---|
| 825 | ! defines the permutation matrix P such that a*p = Q*r. * |
|---|
| 826 | ! column J of P is column IPVT(J) of the identity matrix. * |
|---|
| 827 | ! if PIVOT is false, IPVT is not referenced. * |
|---|
| 828 | ! * |
|---|
| 829 | ! LIPVT is a positive integer input variable. If PIVOT is false, * |
|---|
| 830 | ! then LIPVT may be as small as 1. If PIVOT is true, then * |
|---|
| 831 | ! LIPVT must be at least N. * |
|---|
| 832 | ! * |
|---|
| 833 | ! RDIAG is an output array of length N which contains the * |
|---|
| 834 | ! diagonal elements of R. * |
|---|
| 835 | ! * |
|---|
| 836 | ! ACNORM is an output array of length N which contains the * |
|---|
| 837 | ! norms of the corresponding columns of the input matrix A. * |
|---|
| 838 | ! If this information is not needed, then ACNORM can coincide * |
|---|
| 839 | ! with RDIAG. * |
|---|
| 840 | ! * |
|---|
| 841 | ! WA is a work array of length N. If PIVOT is false, then WA * |
|---|
| 842 | ! can coincide with RDIAG. * |
|---|
| 843 | ! Source: * |
|---|
| 844 | ! Argonne National Laboratory. MINPACK Project. March 1980. * |
|---|
| 845 | ! Burton S. Garbow, Kenneth E. Hillstrom, Jorge J. More. * |
|---|
| 846 | !----------------------------------------------------------------------* |
|---|
| 847 | logical pivot |
|---|
| 848 | integer i,j,k,kmax,lda,lipvt,m,minmn,n,ipvt(lipvt) |
|---|
| 849 | double precision ajnorm,sum,temp,vmod,a(lda,n),rdiag(n),acnorm(n),& |
|---|
| 850 | &wa(n),zero,one,p05,epsmch |
|---|
| 851 | parameter(zero=0d0,one=1d0,p05=0.05d0,epsmch=1d-16) |
|---|
| 852 | |
|---|
| 853 | !---- Compute the initial column norms and initialize several arrays. |
|---|
| 854 | do j = 1, n |
|---|
| 855 | acnorm(j) = vmod(m, a(1,j)) |
|---|
| 856 | rdiag(j) = acnorm(j) |
|---|
| 857 | wa(j) = rdiag(j) |
|---|
| 858 | if (pivot) ipvt(j) = j |
|---|
| 859 | enddo |
|---|
| 860 | !---- Reduce A to R with Householder transformations. |
|---|
| 861 | minmn = min(m,n) |
|---|
| 862 | do j = 1, minmn |
|---|
| 863 | if (pivot) then |
|---|
| 864 | |
|---|
| 865 | !---- Bring the column of largest norm into the pivot position. |
|---|
| 866 | kmax = j |
|---|
| 867 | do k = j, n |
|---|
| 868 | if (rdiag(k) .gt. rdiag(kmax)) kmax = k |
|---|
| 869 | enddo |
|---|
| 870 | if (kmax .ne. j) then |
|---|
| 871 | do i = 1, m |
|---|
| 872 | temp = a(i,j) |
|---|
| 873 | a(i,j) = a(i,kmax) |
|---|
| 874 | a(i,kmax) = temp |
|---|
| 875 | enddo |
|---|
| 876 | rdiag(kmax) = rdiag(j) |
|---|
| 877 | wa(kmax) = wa(j) |
|---|
| 878 | k = ipvt(j) |
|---|
| 879 | ipvt(j) = ipvt(kmax) |
|---|
| 880 | ipvt(kmax) = k |
|---|
| 881 | endif |
|---|
| 882 | endif |
|---|
| 883 | |
|---|
| 884 | !---- Compute the Householder transformation to reduce the |
|---|
| 885 | ! J-th column of A to a multiple of the J-th unit vector. |
|---|
| 886 | ajnorm = vmod(m-j+1, a(j,j)) |
|---|
| 887 | if (ajnorm .ne. zero) then |
|---|
| 888 | if (a(j,j) .lt. zero) ajnorm = -ajnorm |
|---|
| 889 | do i = j, m |
|---|
| 890 | a(i,j) = a(i,j)/ajnorm |
|---|
| 891 | enddo |
|---|
| 892 | a(j,j) = a(j,j) + one |
|---|
| 893 | |
|---|
| 894 | !---- Apply the transformation to the remaining columns |
|---|
| 895 | ! and update the norms. |
|---|
| 896 | do k = j + 1, n |
|---|
| 897 | sum = zero |
|---|
| 898 | do i = j, m |
|---|
| 899 | sum = sum + a(i,j)*a(i,k) |
|---|
| 900 | enddo |
|---|
| 901 | temp = sum/a(j,j) |
|---|
| 902 | do i = j, m |
|---|
| 903 | a(i,k) = a(i,k) - temp*a(i,j) |
|---|
| 904 | enddo |
|---|
| 905 | if (pivot .and. rdiag(k) .ne. zero) then |
|---|
| 906 | temp = a(j,k)/rdiag(k) |
|---|
| 907 | rdiag(k) = rdiag(k)*sqrt(max(zero,one-temp**2)) |
|---|
| 908 | if (p05*(rdiag(k)/wa(k))**2 .le. epsmch) then |
|---|
| 909 | rdiag(k) = vmod(m-j, a(j+1,k)) |
|---|
| 910 | wa(k) = rdiag(k) |
|---|
| 911 | endif |
|---|
| 912 | endif |
|---|
| 913 | enddo |
|---|
| 914 | endif |
|---|
| 915 | rdiag(j) = -ajnorm |
|---|
| 916 | enddo |
|---|
| 917 | |
|---|
| 918 | end |
|---|
| 919 | subroutine lmpar(n,r,ldr,ipvt,diag,qtb,delta,par,x,sdiag,wa1,wa2) |
|---|
| 920 | |
|---|
| 921 | |
|---|
| 922 | implicit none |
|---|
| 923 | |
|---|
| 924 | |
|---|
| 925 | !----------------------------------------------------------------------* |
|---|
| 926 | ! Purpose: * |
|---|
| 927 | ! Given an M by N matrix A, an N by N nonsingular diagonal * |
|---|
| 928 | ! matrix D, an M-vector B, and a positive number DELTA, * |
|---|
| 929 | ! the problem is to determine a value for the parameter * |
|---|
| 930 | ! PAR such that if X solves the system * |
|---|
| 931 | ! * |
|---|
| 932 | ! A*X = B, SQRT(PAR)*D*X = 0, * |
|---|
| 933 | ! * |
|---|
| 934 | ! in the least squares sense, and DXNORM is the Euclidean * |
|---|
| 935 | ! norm of D*X, then either PAR is zero and * |
|---|
| 936 | ! * |
|---|
| 937 | ! (DXNORM-DELTA) .LE. 0.1*DELTA, * |
|---|
| 938 | ! * |
|---|
| 939 | ! or PAR is positive and * |
|---|
| 940 | ! * |
|---|
| 941 | ! ABS(DXNORM-DELTA) .LE. 0.1*DELTA. * |
|---|
| 942 | ! * |
|---|
| 943 | ! This subroutine completes the solution of the problem * |
|---|
| 944 | ! if it is provided with the necessary information from the * |
|---|
| 945 | ! QR factorization, with column pivoting, of A. That is, if * |
|---|
| 946 | ! A*P = Q*R, where P is a permutation matrix, Q has orthogonal * |
|---|
| 947 | ! columns, and R is an upper triangular matrix with diagonal * |
|---|
| 948 | ! elements of nonincreasing magnitude, then LMPAR expects * |
|---|
| 949 | ! the full upper triangle of R, the permutation matrix P, * |
|---|
| 950 | ! and the first N components of (Q transpose)*B. On output * |
|---|
| 951 | ! LMPAR also provides an upper triangular matrix S such that * |
|---|
| 952 | ! * |
|---|
| 953 | ! T T T * |
|---|
| 954 | ! P *(A *A + PAR*D*D)*P = S *S. * |
|---|
| 955 | ! * |
|---|
| 956 | ! S is employed within LMPAR and may be of separate interest. * |
|---|
| 957 | ! * |
|---|
| 958 | ! Only a few iterations are generally needed for convergence * |
|---|
| 959 | ! of the algorithm. If, however, the limit of 10 iterations * |
|---|
| 960 | ! is reached, then the output PAR will contain the best * |
|---|
| 961 | ! value obtained so far. * |
|---|
| 962 | ! * |
|---|
| 963 | ! N is a positive integer input variable set to the order of R. * |
|---|
| 964 | ! * |
|---|
| 965 | ! R is an N by N array. On input the full upper triangle * |
|---|
| 966 | ! must contain the full upper triangle of the matrix R. * |
|---|
| 967 | ! on output the full upper triangle is unaltered, and the * |
|---|
| 968 | ! strict lower triangle contains the strict upper triangle * |
|---|
| 969 | ! (transposed) of the upper triangular matrix S. * |
|---|
| 970 | ! * |
|---|
| 971 | ! LDR is a positive integer input variable not less than N * |
|---|
| 972 | ! which specifies the leading dimension of the array R. * |
|---|
| 973 | ! * |
|---|
| 974 | ! IPVT is an integer input array of length N which defines the * |
|---|
| 975 | ! permutation matrix P such that A*P = Q*R. column J of P * |
|---|
| 976 | ! is column IPVT(J) of the identity matrix. * |
|---|
| 977 | ! * |
|---|
| 978 | ! DIAG is an input array of length N which must contain the * |
|---|
| 979 | ! diagonal elements of the matrix D. * |
|---|
| 980 | ! * |
|---|
| 981 | ! QTB is an input array of length N which must contain the first * |
|---|
| 982 | ! N elements of the vector (Q transpose)*B. * |
|---|
| 983 | ! * |
|---|
| 984 | ! DELTA is a positive input variable which specifies an upper * |
|---|
| 985 | ! bound on the Euclidean norm of D*X. * |
|---|
| 986 | ! * |
|---|
| 987 | ! PAR is a nonnegative variable. On input PAR contains an * |
|---|
| 988 | ! initial estimate of the Levenberg-Marquardt parameter. * |
|---|
| 989 | ! on output PAR contains the final estimate. * |
|---|
| 990 | ! * |
|---|
| 991 | ! X is an output array of length N which contains the least * |
|---|
| 992 | ! squares solution of the system A*X = B, SQRT(PAR)*D*X = 0, * |
|---|
| 993 | ! for the output PAR. * |
|---|
| 994 | ! * |
|---|
| 995 | ! SDIAG is an output array of length N which contains the * |
|---|
| 996 | ! diagonal elements of the upper triangular matrix S. * |
|---|
| 997 | ! * |
|---|
| 998 | ! WA1 and WA2 are work arrays of length N. * |
|---|
| 999 | ! Source: * |
|---|
| 1000 | ! Argonne National Laboratory. MINPACK Project. March 1980. * |
|---|
| 1001 | ! Burton S. Garbow, Kenneth E. Hillstrom, Jorge J. More. * |
|---|
| 1002 | !----------------------------------------------------------------------* |
|---|
| 1003 | integer i,iter,j,k,l,ldr,n,nsing,ipvt(n) |
|---|
| 1004 | double precision delta,diag(n),dxnorm,fp,gnorm,par,parc,parl,paru,& |
|---|
| 1005 | &qtb(n),r(ldr,n),sdiag(n),sum,temp,vmod,wa1(n),wa2(n),x(n),zero, & |
|---|
| 1006 | &fltmin,p1,p001 |
|---|
| 1007 | parameter(zero=0d0,fltmin=1d-35,p1=0.1d0,p001=0.001d0) |
|---|
| 1008 | |
|---|
| 1009 | !---- Compute and store in X the Gauss-Newton direction. If the |
|---|
| 1010 | ! Jacobian is rank-deficient, obtain a least squares solution. |
|---|
| 1011 | nsing = n |
|---|
| 1012 | do j = 1, n |
|---|
| 1013 | wa1(j) = qtb(j) |
|---|
| 1014 | if (r(j,j) .eq. zero .and. nsing .eq. n) nsing = j - 1 |
|---|
| 1015 | if (nsing .lt. n) wa1(j) = zero |
|---|
| 1016 | enddo |
|---|
| 1017 | do k = 1, nsing |
|---|
| 1018 | j = nsing - k + 1 |
|---|
| 1019 | wa1(j) = wa1(j)/r(j,j) |
|---|
| 1020 | temp = wa1(j) |
|---|
| 1021 | do i = 1, j - 1 |
|---|
| 1022 | wa1(i) = wa1(i) - r(i,j)*temp |
|---|
| 1023 | enddo |
|---|
| 1024 | enddo |
|---|
| 1025 | do j = 1, n |
|---|
| 1026 | l = ipvt(j) |
|---|
| 1027 | x(l) = wa1(j) |
|---|
| 1028 | enddo |
|---|
| 1029 | |
|---|
| 1030 | !---- Initialize the iteration counter. |
|---|
| 1031 | ! Evaluate the function at the origin, and test |
|---|
| 1032 | ! for acceptance of the Gauss-Newton direction. |
|---|
| 1033 | iter = 0 |
|---|
| 1034 | do j = 1, n |
|---|
| 1035 | wa2(j) = diag(j)*x(j) |
|---|
| 1036 | enddo |
|---|
| 1037 | dxnorm = vmod(n, wa2) |
|---|
| 1038 | fp = dxnorm - delta |
|---|
| 1039 | if (fp .le. p1*delta) go to 220 |
|---|
| 1040 | |
|---|
| 1041 | !---- If the Jacobian is not rank deficient, the Newton |
|---|
| 1042 | ! step provides a lower bound, PARL, for the zero of |
|---|
| 1043 | ! the function. Otherwise set this bound to zero. |
|---|
| 1044 | parl = zero |
|---|
| 1045 | if (nsing .ge. n) then |
|---|
| 1046 | do j = 1, n |
|---|
| 1047 | l = ipvt(j) |
|---|
| 1048 | wa1(j) = diag(l)*(wa2(l)/dxnorm) |
|---|
| 1049 | enddo |
|---|
| 1050 | do j = 1, n |
|---|
| 1051 | sum = zero |
|---|
| 1052 | do i = 1, j - 1 |
|---|
| 1053 | sum = sum + r(i,j)*wa1(i) |
|---|
| 1054 | enddo |
|---|
| 1055 | wa1(j) = (wa1(j) - sum)/r(j,j) |
|---|
| 1056 | enddo |
|---|
| 1057 | temp = vmod(n, wa1) |
|---|
| 1058 | parl = ((fp/delta)/temp)/temp |
|---|
| 1059 | endif |
|---|
| 1060 | |
|---|
| 1061 | !---- Calculate an upper bound, PARU, for the zero of the function. |
|---|
| 1062 | do j = 1, n |
|---|
| 1063 | sum = zero |
|---|
| 1064 | do i = 1, j |
|---|
| 1065 | sum = sum + r(i,j)*qtb(i) |
|---|
| 1066 | enddo |
|---|
| 1067 | l = ipvt(j) |
|---|
| 1068 | wa1(j) = sum/diag(l) |
|---|
| 1069 | enddo |
|---|
| 1070 | gnorm = vmod(n, wa1) |
|---|
| 1071 | paru = gnorm/delta |
|---|
| 1072 | if (paru .eq. zero) paru = fltmin/min(delta,p1) |
|---|
| 1073 | |
|---|
| 1074 | !---- If the input PAR lies outside of the interval (PARL,PARU), |
|---|
| 1075 | ! set PAR to the closer endpoint. |
|---|
| 1076 | par = max(par,parl) |
|---|
| 1077 | par = min(par,paru) |
|---|
| 1078 | if (par .eq. zero) par = gnorm/dxnorm |
|---|
| 1079 | |
|---|
| 1080 | !---- Beginning of an iteration. |
|---|
| 1081 | 150 continue |
|---|
| 1082 | iter = iter + 1 |
|---|
| 1083 | |
|---|
| 1084 | !---- Evaluate the function at the current value of PAR. |
|---|
| 1085 | if (par .eq. zero) par = max(fltmin,p001*paru) |
|---|
| 1086 | temp = sqrt(par) |
|---|
| 1087 | do j = 1, n |
|---|
| 1088 | wa1(j) = temp*diag(j) |
|---|
| 1089 | enddo |
|---|
| 1090 | call qrsolv(n,r,ldr,ipvt,wa1,qtb,x,sdiag,wa2) |
|---|
| 1091 | do j = 1, n |
|---|
| 1092 | wa2(j) = diag(j)*x(j) |
|---|
| 1093 | enddo |
|---|
| 1094 | dxnorm = vmod(n, wa2) |
|---|
| 1095 | temp = fp |
|---|
| 1096 | fp = dxnorm - delta |
|---|
| 1097 | |
|---|
| 1098 | !---- If the function is small enough, accept the current value |
|---|
| 1099 | ! of PAR. also test for the exceptional cases where PARL |
|---|
| 1100 | ! is zero or the number of iterations has reached 10. |
|---|
| 1101 | if (abs(fp) .le. p1*delta & |
|---|
| 1102 | &.or. parl .eq. zero .and. fp .le. temp & |
|---|
| 1103 | &.and. temp .lt. zero .or. iter .eq. 10) go to 220 |
|---|
| 1104 | |
|---|
| 1105 | !---- Compute the Newton correction. |
|---|
| 1106 | do j = 1, n |
|---|
| 1107 | l = ipvt(j) |
|---|
| 1108 | wa1(j) = diag(l)*(wa2(l)/dxnorm) |
|---|
| 1109 | enddo |
|---|
| 1110 | do j = 1, n |
|---|
| 1111 | wa1(j) = wa1(j)/sdiag(j) |
|---|
| 1112 | temp = wa1(j) |
|---|
| 1113 | do i = j + 1, n |
|---|
| 1114 | wa1(i) = wa1(i) - r(i,j)*temp |
|---|
| 1115 | enddo |
|---|
| 1116 | enddo |
|---|
| 1117 | temp = vmod(n, wa1) |
|---|
| 1118 | parc = ((fp/delta)/temp)/temp |
|---|
| 1119 | |
|---|
| 1120 | !---- Depending on the sign of the function, update PARL or PARU. |
|---|
| 1121 | if (fp .gt. zero) parl = max(parl,par) |
|---|
| 1122 | if (fp .lt. zero) paru = min(paru,par) |
|---|
| 1123 | |
|---|
| 1124 | !---- Compute an improved estimate for PAR. |
|---|
| 1125 | par = max(parl,par+parc) |
|---|
| 1126 | |
|---|
| 1127 | !---- End of an iteration. |
|---|
| 1128 | go to 150 |
|---|
| 1129 | 220 continue |
|---|
| 1130 | |
|---|
| 1131 | !---- Termination. |
|---|
| 1132 | if (iter .eq. 0) par = zero |
|---|
| 1133 | |
|---|
| 1134 | end |
|---|
| 1135 | subroutine qrsolv(n,r,ldr,ipvt,diag,qtb,x,sdiag,wa) |
|---|
| 1136 | implicit none |
|---|
| 1137 | !----------------------------------------------------------------------* |
|---|
| 1138 | ! Purpose: * |
|---|
| 1139 | ! Given an M by N matrix A, an N by N diagonal matrix D, * |
|---|
| 1140 | ! and an M-vector B, the problem is to determine an X which * |
|---|
| 1141 | ! solves the system * |
|---|
| 1142 | ! * |
|---|
| 1143 | ! A*X = B, D*X = 0, * |
|---|
| 1144 | ! * |
|---|
| 1145 | ! in the least squares sense. * |
|---|
| 1146 | ! * |
|---|
| 1147 | ! This subroutine completes the solution of the problem * |
|---|
| 1148 | ! if it is provided with the necessary information from the * |
|---|
| 1149 | ! QR factorization, with column pivoting, of A. That is, if * |
|---|
| 1150 | ! A*P = Q*R, where P is a permutation matrix, Q has orthogonal * |
|---|
| 1151 | ! columns, and R is an upper triangular matrix with diagonal * |
|---|
| 1152 | ! elements of nonincreasing magnitude, then QRSOLV expects * |
|---|
| 1153 | ! the full upper triangle of R, the permutation matrix P, * |
|---|
| 1154 | ! and the first N components of (Q transpose)*B. The system * |
|---|
| 1155 | ! A*X = B, D*X = 0, is then equivalent to * |
|---|
| 1156 | ! * |
|---|
| 1157 | ! T T * |
|---|
| 1158 | ! R*Z = Q *B, P *D*P*Z = 0, * |
|---|
| 1159 | ! * |
|---|
| 1160 | ! where X = P*Z. If this system does not have full rank, * |
|---|
| 1161 | ! then a least squares solution is obtained. On output QRSOLV * |
|---|
| 1162 | ! also provides an upper triangular matrix S such that * |
|---|
| 1163 | ! * |
|---|
| 1164 | ! T T T * |
|---|
| 1165 | ! P *(A *A + D*D)*P = S *S. * |
|---|
| 1166 | ! * |
|---|
| 1167 | ! S is computed within QRSOLV and may be of separate interest. * |
|---|
| 1168 | ! * |
|---|
| 1169 | ! N is a positive integer input variable set to the order of R. * |
|---|
| 1170 | ! * |
|---|
| 1171 | ! R is an N by N array. On input the full upper triangle * |
|---|
| 1172 | ! must contain the full upper triangle of the matrix R. * |
|---|
| 1173 | ! On output the full upper triangle is unaltered, and the * |
|---|
| 1174 | ! strict lower triangle contains the strict upper triangle * |
|---|
| 1175 | ! (transposed) of the upper triangular matrix S. * |
|---|
| 1176 | ! * |
|---|
| 1177 | ! LDR is a positive integer input variable not less than N * |
|---|
| 1178 | ! which specifies the leading dimension of the array R. * |
|---|
| 1179 | ! * |
|---|
| 1180 | ! IPVT is an integer input array of length N which defines the * |
|---|
| 1181 | ! permutation matrix P such that A*P = Q*R. Column J of P * |
|---|
| 1182 | ! is column IPVT(J) of the identity matrix. * |
|---|
| 1183 | ! * |
|---|
| 1184 | ! DIAG is an input array of length N which must contain the * |
|---|
| 1185 | ! diagonal elements of the matrix D. * |
|---|
| 1186 | ! * |
|---|
| 1187 | ! QTB is an input array of length N which must contain the first * |
|---|
| 1188 | ! N elements of the vector (Q transpose)*B. * |
|---|
| 1189 | ! * |
|---|
| 1190 | ! X is an output array of length N which contains the least * |
|---|
| 1191 | ! squares solution of the system A*X = B, D*X = 0. * |
|---|
| 1192 | ! * |
|---|
| 1193 | ! SDIAG is an output array of length N which contains the * |
|---|
| 1194 | ! diagonal elements of the upper triangular matrix S. * |
|---|
| 1195 | ! * |
|---|
| 1196 | ! WA is a work array of length N. * |
|---|
| 1197 | ! Source: * |
|---|
| 1198 | ! Argonne National Laboratory. MINPACK Project. March 1980. * |
|---|
| 1199 | ! Burton S. Garbow, Kenneth E. Hillstrom, Jorge J. More. * |
|---|
| 1200 | !----------------------------------------------------------------------* |
|---|
| 1201 | integer i,j,k,l,ldr,n,nsing,ipvt(n) |
|---|
| 1202 | double precision cos,cotan,diag(n),qtb(n),qtbpj,r(ldr,n),sdiag(n),& |
|---|
| 1203 | &sin,sum,tan,temp,wa(n),x(n),zero,p5,p25 |
|---|
| 1204 | parameter(zero=0d0,p5=0.5d0,p25=0.25d0) |
|---|
| 1205 | |
|---|
| 1206 | !---- Copy R and (Q transpose)*B to preserve input and initialize S. |
|---|
| 1207 | ! In particular, save the diagonal elements of R in X. |
|---|
| 1208 | do j = 1, n |
|---|
| 1209 | do i = j, n |
|---|
| 1210 | r(i,j) = r(j,i) |
|---|
| 1211 | enddo |
|---|
| 1212 | x(j) = r(j,j) |
|---|
| 1213 | wa(j) = qtb(j) |
|---|
| 1214 | enddo |
|---|
| 1215 | |
|---|
| 1216 | !---- Eliminate the diagonal matrix D using a Givens rotation. |
|---|
| 1217 | do j = 1, n |
|---|
| 1218 | |
|---|
| 1219 | !---- Prepare the row of D to be eliminated, locating the |
|---|
| 1220 | ! diagonal element using P from the QR factorization. |
|---|
| 1221 | l = ipvt(j) |
|---|
| 1222 | if (diag(l) .ne. zero) then |
|---|
| 1223 | do k = j, n |
|---|
| 1224 | sdiag(k) = zero |
|---|
| 1225 | enddo |
|---|
| 1226 | sdiag(j) = diag(l) |
|---|
| 1227 | |
|---|
| 1228 | !---- The transformations to eliminate the row of D |
|---|
| 1229 | ! modify only a single element of (Q transpose)*B |
|---|
| 1230 | ! beyond the first N, which is initially zero. |
|---|
| 1231 | qtbpj = zero |
|---|
| 1232 | do k = j, n |
|---|
| 1233 | |
|---|
| 1234 | !---- Determine a Givens rotation which eliminates the |
|---|
| 1235 | ! appropriate element in the current row of D. |
|---|
| 1236 | if (sdiag(k) .ne. zero) then |
|---|
| 1237 | if (abs(r(k,k)) .lt. abs(sdiag(k))) then |
|---|
| 1238 | cotan = r(k,k)/sdiag(k) |
|---|
| 1239 | sin = p5/sqrt(p25+p25*cotan**2) |
|---|
| 1240 | cos = sin*cotan |
|---|
| 1241 | else |
|---|
| 1242 | tan = sdiag(k)/r(k,k) |
|---|
| 1243 | cos = p5/sqrt(p25+p25*tan**2) |
|---|
| 1244 | sin = cos*tan |
|---|
| 1245 | endif |
|---|
| 1246 | |
|---|
| 1247 | !---- Compute the modified diagonal element of R and |
|---|
| 1248 | ! the modified element of ((Q transpose)*b,0). |
|---|
| 1249 | r(k,k) = cos*r(k,k) + sin*sdiag(k) |
|---|
| 1250 | temp = cos*wa(k) + sin*qtbpj |
|---|
| 1251 | qtbpj = -sin*wa(k) + cos*qtbpj |
|---|
| 1252 | wa(k) = temp |
|---|
| 1253 | |
|---|
| 1254 | !---- Accumulate the tranformation in the row of S. |
|---|
| 1255 | do i = k + 1, n |
|---|
| 1256 | temp = cos*r(i,k) + sin*sdiag(i) |
|---|
| 1257 | sdiag(i) = -sin*r(i,k) + cos*sdiag(i) |
|---|
| 1258 | r(i,k) = temp |
|---|
| 1259 | enddo |
|---|
| 1260 | endif |
|---|
| 1261 | enddo |
|---|
| 1262 | endif |
|---|
| 1263 | |
|---|
| 1264 | !---- Store the diagonal element of S and restore |
|---|
| 1265 | ! the corresponding diagonal element of R. |
|---|
| 1266 | sdiag(j) = r(j,j) |
|---|
| 1267 | r(j,j) = x(j) |
|---|
| 1268 | enddo |
|---|
| 1269 | |
|---|
| 1270 | !---- Solve the triangular system for z. If the system is |
|---|
| 1271 | ! singular, then obtain a least squares solution. |
|---|
| 1272 | nsing = n |
|---|
| 1273 | do j = 1, n |
|---|
| 1274 | if (sdiag(j) .eq. zero .and. nsing .eq. n) nsing = j - 1 |
|---|
| 1275 | if (nsing .lt. n) wa(j) = zero |
|---|
| 1276 | enddo |
|---|
| 1277 | do j = nsing, 1, - 1 |
|---|
| 1278 | sum = zero |
|---|
| 1279 | do i = j + 1, nsing |
|---|
| 1280 | sum = sum + r(i,j)*wa(i) |
|---|
| 1281 | enddo |
|---|
| 1282 | wa(j) = (wa(j) - sum)/sdiag(j) |
|---|
| 1283 | enddo |
|---|
| 1284 | |
|---|
| 1285 | !---- Permute the components of Z back to components of X. |
|---|
| 1286 | do j = 1, n |
|---|
| 1287 | l = ipvt(j) |
|---|
| 1288 | x(l) = wa(j) |
|---|
| 1289 | enddo |
|---|
| 1290 | |
|---|
| 1291 | end |
|---|
| 1292 | subroutine mtprnt(text,n,x) |
|---|
| 1293 | implicit none |
|---|
| 1294 | integer n |
|---|
| 1295 | double precision x(n) |
|---|
| 1296 | character*(*) text |
|---|
| 1297 | |
|---|
| 1298 | print *, text, ' variable values: ', x |
|---|
| 1299 | end |
|---|
| 1300 | subroutine mtmigr(ncon,nvar,strategy,tol,calls,call_lim,vect, & |
|---|
| 1301 | &dvect,fun_vect,w_iwa1,w_iwa2,w_iwa3,w_iwa4,w_iwa5,w_iwa6,w_iwa7, & |
|---|
| 1302 | &w_iwa8) |
|---|
| 1303 | |
|---|
| 1304 | use matchfi |
|---|
| 1305 | implicit none |
|---|
| 1306 | |
|---|
| 1307 | |
|---|
| 1308 | !----------------------------------------------------------------------* |
|---|
| 1309 | ! Purpose: * |
|---|
| 1310 | ! MIGRAD command. * |
|---|
| 1311 | ! ncon (int) # constraints * |
|---|
| 1312 | ! nvar (int) # variables * |
|---|
| 1313 | ! strategy (int) strategy selection (see minuit manual). * |
|---|
| 1314 | ! tol (real) Final tolerance for match. * |
|---|
| 1315 | ! calls (int) current call count * |
|---|
| 1316 | ! call_lim (int) current call limit * |
|---|
| 1317 | ! vect (real) variable values * |
|---|
| 1318 | ! dvect (real) variable steps * |
|---|
| 1319 | ! fun_vect (real) function values * |
|---|
| 1320 | ! all other working spaces for mtmig1 * |
|---|
| 1321 | !----------------------------------------------------------------------* |
|---|
| 1322 | integer calls,call_lim,ncon,nvar,strategy |
|---|
| 1323 | ! icovar: functionality still unclear HG 28.2.02 |
|---|
| 1324 | ! ilevel: print level |
|---|
| 1325 | double precision tol,vect(*),dvect(*),fun_vect(*),w_iwa1(*), & |
|---|
| 1326 | &w_iwa2(*),w_iwa3(*),w_iwa4(*),w_iwa5(*),w_iwa6(*),w_iwa7(*), & |
|---|
| 1327 | &w_iwa8(*) |
|---|
| 1328 | external mtfcn |
|---|
| 1329 | |
|---|
| 1330 | icovar = 0 |
|---|
| 1331 | ilevel = 0 |
|---|
| 1332 | !---- Too many variable parameters? |
|---|
| 1333 | if (nvar .gt. ncon) then |
|---|
| 1334 | call aawarn('MTMIGR','More variables than constraints seen.') |
|---|
| 1335 | call aawarn('MTMIGR', & |
|---|
| 1336 | &'MIGRAD may not converge to optimal solution.') |
|---|
| 1337 | endif |
|---|
| 1338 | |
|---|
| 1339 | !---- Call minimization routine. |
|---|
| 1340 | call mtgeti(vect, dvect) |
|---|
| 1341 | call mtmig1(mtfcn, ncon, nvar, strategy, tol, calls, call_lim, & |
|---|
| 1342 | &vect, dvect, fun_vect, w_iwa1, w_iwa2, w_iwa3, w_iwa4, w_iwa5, & |
|---|
| 1343 | &w_iwa6, w_iwa7, w_iwa8) |
|---|
| 1344 | |
|---|
| 1345 | 9999 end |
|---|
| 1346 | subroutine mtmig1(fcn,nf,nx,strategy,tol,calls,call_lim,x,dx,fvec,& |
|---|
| 1347 | &covar, wa,work_1,work_2,work_3,work_4,work_5,work_6) |
|---|
| 1348 | |
|---|
| 1349 | use matchfi |
|---|
| 1350 | implicit none |
|---|
| 1351 | |
|---|
| 1352 | |
|---|
| 1353 | !----------------------------------------------------------------------* |
|---|
| 1354 | ! Purpose: * |
|---|
| 1355 | ! Minimization by MIGRAD method by Davidon/Fletcher/Powell. * |
|---|
| 1356 | ! (Computer Journal 13, 317 (1970). * |
|---|
| 1357 | ! Input: * |
|---|
| 1358 | ! FCN (subr) Returns value of penalty function. * |
|---|
| 1359 | ! NF (integer) Number of functions. * |
|---|
| 1360 | ! NX (integer) Number of parameters. * |
|---|
| 1361 | ! X(NX) (real) Parameter values. On output, best estimate. * |
|---|
| 1362 | ! DX(NX) (real) Parameter errors. On output, error estimate. * |
|---|
| 1363 | ! Output: * |
|---|
| 1364 | ! FVEC(NF) (real) Vector of function values in best point. * |
|---|
| 1365 | ! Working arrays: * |
|---|
| 1366 | ! COVAR(NX,NX) Covariance matrix. * |
|---|
| 1367 | ! WA(NX,7) Working vectors. * |
|---|
| 1368 | !----------------------------------------------------------------------* |
|---|
| 1369 | logical eflag |
|---|
| 1370 | integer i,iflag,improv,iter,j,level,nf,npsdf,nrstrt,nx,strategy, & |
|---|
| 1371 | &calls,call_lim,mgrd,mg2,mvg,mflnu,mgsave,mxsave |
|---|
| 1372 | parameter(mgrd=1,mg2=2,mvg=3,mflnu=5,mgsave=6,mxsave=7) |
|---|
| 1373 | ! ilevel: print level |
|---|
| 1374 | double precision covar(nx,nx),d,delgam,dgi,dx(nx),fvec(nf),gdel, & |
|---|
| 1375 | &gssq,gvg,sum,vdot,vgi,wa(nx,7),x(nx),tol,work_1(nx),work_2(nx), & |
|---|
| 1376 | &work_3(nx),work_4(*),work_5(*),work_6(*),zero,one,two,half,epsmch,& |
|---|
| 1377 | &eps1,eps2 |
|---|
| 1378 | parameter(zero=0d0,one=1d0,two=2d0,half=0.5d0,epsmch=1d-16, & |
|---|
| 1379 | &eps1=1d-3,eps2=1d-4) |
|---|
| 1380 | external fcn |
|---|
| 1381 | |
|---|
| 1382 | !---- Initialize penalty function. |
|---|
| 1383 | call fcn(nf, nx, x, fvec, iflag) |
|---|
| 1384 | calls = calls + 1 |
|---|
| 1385 | if (iflag .ne. 0) then |
|---|
| 1386 | call aawarn('MTMIG1', & |
|---|
| 1387 | &'Matching stopped -- start point seems to be unstable') |
|---|
| 1388 | go to 500 |
|---|
| 1389 | endif |
|---|
| 1390 | fmin = vdot(nf, fvec, fvec) |
|---|
| 1391 | edm = fmin |
|---|
| 1392 | |
|---|
| 1393 | !---- Start MIGRAD algorithm. |
|---|
| 1394 | nrstrt = 0 |
|---|
| 1395 | npsdf = 0 |
|---|
| 1396 | |
|---|
| 1397 | !---- Come here to restart algorithm. |
|---|
| 1398 | 100 continue |
|---|
| 1399 | if (strategy .eq. 2 .or. strategy.gt.2 .and. icovar.lt.2) then |
|---|
| 1400 | call mthess(fcn, nf, nx, calls, covar, x, & |
|---|
| 1401 | &wa(1,mgrd), wa(1,mg2), fvec, wa(1,mvg), work_1, work_2, work_3, & |
|---|
| 1402 | &work_4, work_5,work_6) |
|---|
| 1403 | npsdf = 0 |
|---|
| 1404 | else |
|---|
| 1405 | call mtderi(fcn, nf, nx, calls, x, wa(1,mgrd), wa(1,mg2), fvec) |
|---|
| 1406 | if (icovar .lt. 2) then |
|---|
| 1407 | do i = 1, nx |
|---|
| 1408 | do j = 1, nx |
|---|
| 1409 | covar(i,j) = zero |
|---|
| 1410 | enddo |
|---|
| 1411 | if (wa(i,mg2) .eq. zero) wa(i,mg2) = one |
|---|
| 1412 | covar(i,i) = one / wa(i,mg2) |
|---|
| 1413 | enddo |
|---|
| 1414 | endif |
|---|
| 1415 | endif |
|---|
| 1416 | |
|---|
| 1417 | !---- Initialize for first iteration. |
|---|
| 1418 | improv = 0 |
|---|
| 1419 | edm = zero |
|---|
| 1420 | do i = 1, nx |
|---|
| 1421 | sum = zero |
|---|
| 1422 | do j = 1, nx |
|---|
| 1423 | sum = sum + covar(i,j) * wa(j,mgrd) |
|---|
| 1424 | enddo |
|---|
| 1425 | edm = edm + sum * wa(i,mgrd) |
|---|
| 1426 | enddo |
|---|
| 1427 | edm = min(half * edm, fmin) |
|---|
| 1428 | |
|---|
| 1429 | !---- Print after initialization. |
|---|
| 1430 | if (ilevel .ge. 1) then |
|---|
| 1431 | call mtprnt('init', nx, x) |
|---|
| 1432 | endif |
|---|
| 1433 | iter = 0 |
|---|
| 1434 | |
|---|
| 1435 | !==== Start main iteration loop: Check for call limit. |
|---|
| 1436 | 200 if (calls .lt. call_lim) then |
|---|
| 1437 | |
|---|
| 1438 | !---- Find step size according to Newton's method. |
|---|
| 1439 | gdel = zero |
|---|
| 1440 | gssq = zero |
|---|
| 1441 | do i = 1, nx |
|---|
| 1442 | sum = zero |
|---|
| 1443 | wa(i,mgsave) = wa(i,mgrd) |
|---|
| 1444 | gssq = gssq + wa(i,mgrd)**2 |
|---|
| 1445 | do j = 1, nx |
|---|
| 1446 | sum = sum + covar(i,j) * wa(j,mgrd) |
|---|
| 1447 | enddo |
|---|
| 1448 | dx(i) = - sum |
|---|
| 1449 | gdel = gdel + dx(i) * wa(i,mgrd) |
|---|
| 1450 | enddo |
|---|
| 1451 | |
|---|
| 1452 | !---- First derivatives all zero? |
|---|
| 1453 | if (gssq .eq. zero) go to 400 |
|---|
| 1454 | |
|---|
| 1455 | !---- If GDEL .GE. 0 matrix is not positive definite. |
|---|
| 1456 | if (gdel .ge. zero) then |
|---|
| 1457 | if (npsdf .eq. 0) then |
|---|
| 1458 | call symsol(covar, nx, eflag, work_1, work_2, work_3) |
|---|
| 1459 | call mtpsdf(covar,nx,work_4,work_5,work_6) |
|---|
| 1460 | call symsol(covar, nx, eflag, work_1, work_2, work_3) |
|---|
| 1461 | npsdf = 1 |
|---|
| 1462 | go to 200 |
|---|
| 1463 | else |
|---|
| 1464 | nrstrt = nrstrt + 1 |
|---|
| 1465 | if (nrstrt .gt. strategy) go to 500 |
|---|
| 1466 | go to 100 |
|---|
| 1467 | endif |
|---|
| 1468 | endif |
|---|
| 1469 | |
|---|
| 1470 | !---- Search for minimum along predicted line. |
|---|
| 1471 | call mtline(fcn, nf, nx, calls, x, dx, fvec, & |
|---|
| 1472 | &wa(1,mxsave), iflag) |
|---|
| 1473 | |
|---|
| 1474 | !---- No improvement found. |
|---|
| 1475 | if (iflag .ne. 0) then |
|---|
| 1476 | if (edm .lt. eps1 * tol) go to 400 |
|---|
| 1477 | print *, 'accuracy limit' |
|---|
| 1478 | if (edm .lt. two * epsmch * fmin) go to 500 |
|---|
| 1479 | if (strategy .eq. 0 .and. nrstrt .eq. 0) then |
|---|
| 1480 | strategy = 1 |
|---|
| 1481 | nrstrt = 1 |
|---|
| 1482 | go to 100 |
|---|
| 1483 | endif |
|---|
| 1484 | print *, 'failed' |
|---|
| 1485 | go to 500 |
|---|
| 1486 | endif |
|---|
| 1487 | |
|---|
| 1488 | !---- Find gradient in new point. |
|---|
| 1489 | call mtderi(fcn, nf, nx, calls, x, wa(1,mgrd), wa(1,mg2), fvec) |
|---|
| 1490 | npsdf = 0 |
|---|
| 1491 | |
|---|
| 1492 | !---- Estimated distance to minimum. |
|---|
| 1493 | 300 continue |
|---|
| 1494 | edm = zero |
|---|
| 1495 | gvg = zero |
|---|
| 1496 | delgam = zero |
|---|
| 1497 | do i = 1, nx |
|---|
| 1498 | vgi = zero |
|---|
| 1499 | sum = zero |
|---|
| 1500 | do j = 1, nx |
|---|
| 1501 | vgi = vgi + covar(i,j) * (wa(j,mgrd) - wa(j,mgsave)) |
|---|
| 1502 | sum = sum + covar(i,j) * wa(j,mgrd) |
|---|
| 1503 | enddo |
|---|
| 1504 | wa(i,mvg) = vgi |
|---|
| 1505 | dgi = wa(i,mgrd) - wa(i,mgsave) |
|---|
| 1506 | gvg = gvg + vgi * dgi |
|---|
| 1507 | delgam = delgam + dx(i) * dgi |
|---|
| 1508 | edm = edm + sum * wa(i,mgrd) |
|---|
| 1509 | enddo |
|---|
| 1510 | edm = min(half * edm, fmin) |
|---|
| 1511 | |
|---|
| 1512 | !---- Test for convergence and print-out. |
|---|
| 1513 | if (edm .ge. zero .and. edm .lt. eps2 * tol) go to 400 |
|---|
| 1514 | iter = iter + 1 |
|---|
| 1515 | level = 3 |
|---|
| 1516 | if (mod(iter,10) .eq. 0) level = 2 |
|---|
| 1517 | if (ilevel .ge. level) then |
|---|
| 1518 | call mtprnt('progress', nx, x) |
|---|
| 1519 | endif |
|---|
| 1520 | write(*,830) calls,fmin |
|---|
| 1521 | |
|---|
| 1522 | !---- Force positive definiteness. |
|---|
| 1523 | if (edm .lt. zero .or. gvg .le. zero) then |
|---|
| 1524 | icovar = 0 |
|---|
| 1525 | if (npsdf .eq. 1) go to 500 |
|---|
| 1526 | call symsol(covar, nx, eflag, work_1, work_2, work_3) |
|---|
| 1527 | call mtpsdf(covar,nx,work_4,work_5,work_6) |
|---|
| 1528 | call symsol(covar, nx, eflag, work_1, work_2, work_3) |
|---|
| 1529 | npsdf = 1 |
|---|
| 1530 | go to 300 |
|---|
| 1531 | endif |
|---|
| 1532 | |
|---|
| 1533 | !---- Update covariance matrix. |
|---|
| 1534 | do i = 1, nx |
|---|
| 1535 | do j = 1, nx |
|---|
| 1536 | d = dx(i) * dx(j) / delgam - wa(i,mvg) * wa(j,mvg) / gvg |
|---|
| 1537 | covar(i,j) = covar(i,j) + d |
|---|
| 1538 | enddo |
|---|
| 1539 | enddo |
|---|
| 1540 | if (delgam .gt. gvg) then |
|---|
| 1541 | do i = 1, nx |
|---|
| 1542 | wa(i,mflnu) = dx(i) / delgam - wa(i,mvg) / gvg |
|---|
| 1543 | enddo |
|---|
| 1544 | do i = 1, nx |
|---|
| 1545 | do j = 1, nx |
|---|
| 1546 | d = gvg * wa(i,mflnu) * wa(j,mflnu) |
|---|
| 1547 | covar(i,j) = covar(i,j) + d + d |
|---|
| 1548 | enddo |
|---|
| 1549 | enddo |
|---|
| 1550 | endif |
|---|
| 1551 | improv = improv + 1 |
|---|
| 1552 | if (improv .ge. nx) icovar = 3 |
|---|
| 1553 | go to 200 |
|---|
| 1554 | endif |
|---|
| 1555 | |
|---|
| 1556 | !---- Call limit reached. |
|---|
| 1557 | print *, 'call limit' |
|---|
| 1558 | go to 500 |
|---|
| 1559 | |
|---|
| 1560 | !==== End of main iteration loop; Check covariance matrix. |
|---|
| 1561 | 400 continue |
|---|
| 1562 | if (strategy .ge. 2 .or. (strategy.eq.1 .and. icovar.lt.3)) then |
|---|
| 1563 | print *, 'verify' |
|---|
| 1564 | call mthess(fcn, nf, nx, calls, covar, x, & |
|---|
| 1565 | &wa(1,mgrd), wa(1,mg2), fvec, wa(1,mvg), work_1, work_2, work_3, & |
|---|
| 1566 | &work_4, work_5,work_6) |
|---|
| 1567 | npsdf = 0 |
|---|
| 1568 | if (edm .gt. eps1 * tol) go to 100 |
|---|
| 1569 | endif |
|---|
| 1570 | print *, 'converged' |
|---|
| 1571 | |
|---|
| 1572 | !---- Common exit point; final print-out. |
|---|
| 1573 | 500 continue |
|---|
| 1574 | call mtputi(x) |
|---|
| 1575 | if (ilevel .ge. 1) call mtprnt('final', nx, x) |
|---|
| 1576 | |
|---|
| 1577 | write(*,830) calls,fmin |
|---|
| 1578 | |
|---|
| 1579 | 830 format('call:',I8,3x,'Penalty function = ',e16.8) |
|---|
| 1580 | end |
|---|
| 1581 | subroutine mthess(fcn,nf,nx,calls,covar,x,grd,g2,fvec,wa,work_1, & |
|---|
| 1582 | &work_2,work_3,work_4,work_5,work_6) |
|---|
| 1583 | |
|---|
| 1584 | use matchfi |
|---|
| 1585 | implicit none |
|---|
| 1586 | |
|---|
| 1587 | |
|---|
| 1588 | !----------------------------------------------------------------------* |
|---|
| 1589 | ! Purpose: * |
|---|
| 1590 | ! Build covariance matrix. * |
|---|
| 1591 | ! input: * |
|---|
| 1592 | ! fcn (subr) returns value of penalty function. * |
|---|
| 1593 | ! nf (integer) number of functions. * |
|---|
| 1594 | ! nx (integer) number of parameters. * |
|---|
| 1595 | ! calls (integer) number of calls (increased) * |
|---|
| 1596 | ! x(nx) (real) parameter values. on output, best estimate. * |
|---|
| 1597 | ! output: * |
|---|
| 1598 | ! covar(nx,nx) covariance matrix. * |
|---|
| 1599 | ! grd(nx) (real) gradient of penalty function * |
|---|
| 1600 | ! w.r.t. internal parameter values. * |
|---|
| 1601 | ! g2(nx) (real) second derivatives of penalty function * |
|---|
| 1602 | ! w.r.t. internal parameter values. * |
|---|
| 1603 | ! working array: * |
|---|
| 1604 | ! fvec(nf) (real) function values. * |
|---|
| 1605 | ! wa(nx,2) (real) working vectors. * |
|---|
| 1606 | !----------------------------------------------------------------------* |
|---|
| 1607 | logical eflag |
|---|
| 1608 | integer i,icycle,iflag,j,nf,nx,calls |
|---|
| 1609 | ! icovar: functionality still unclear HG 28.2.02 |
|---|
| 1610 | ! ilevel: print level |
|---|
| 1611 | double precision eps,f1,f2,fij,vdot,xs1,xs2,xsave,xstep, & |
|---|
| 1612 | &covar(nx,nx),x(nx),grd(nx),g2(nx),wa(nx,2),fvec(nf),work_1(nx), & |
|---|
| 1613 | &work_2(nx),work_3(nx),work_4(*),work_5(*),work_6(*),zero,one,two, & |
|---|
| 1614 | &half,epsmch |
|---|
| 1615 | parameter(zero=0d0,one=1d0,two=2d0,half=0.5d0,epsmch=1d-16) |
|---|
| 1616 | external fcn |
|---|
| 1617 | |
|---|
| 1618 | eps = sqrt(epsmch) |
|---|
| 1619 | |
|---|
| 1620 | do i = 1, nx |
|---|
| 1621 | xsave = x(i) |
|---|
| 1622 | xstep = eps * max(abs(xsave), one) |
|---|
| 1623 | do icycle = 1, 10 |
|---|
| 1624 | x(i) = xsave + xstep |
|---|
| 1625 | call fcn(nf, nx, x, fvec, iflag) |
|---|
| 1626 | calls = calls + 1 |
|---|
| 1627 | if (iflag .eq. 0) then |
|---|
| 1628 | f2 = vdot(nf, fvec, fvec) |
|---|
| 1629 | x(i) = xsave - xstep |
|---|
| 1630 | call fcn(nf, nx, x, fvec, iflag) |
|---|
| 1631 | calls = calls + 1 |
|---|
| 1632 | if (iflag .eq. 0) then |
|---|
| 1633 | f1 = vdot(nf, fvec, fvec) |
|---|
| 1634 | go to 40 |
|---|
| 1635 | endif |
|---|
| 1636 | endif |
|---|
| 1637 | xstep = half * xstep |
|---|
| 1638 | enddo |
|---|
| 1639 | f1 = fmin |
|---|
| 1640 | f2 = fmin |
|---|
| 1641 | 40 continue |
|---|
| 1642 | grd(i) = (f2 - f1) / (two * xstep) |
|---|
| 1643 | g2(i) = (f2 - two * fmin + f1) / xstep**2 |
|---|
| 1644 | if (g2(i) .eq. zero) g2(i) = one |
|---|
| 1645 | x(i) = xsave |
|---|
| 1646 | covar(i,i) = g2(i) |
|---|
| 1647 | wa(i,1) = f2 |
|---|
| 1648 | wa(i,2) = xstep |
|---|
| 1649 | enddo |
|---|
| 1650 | |
|---|
| 1651 | !---- Off-diagonal elements. |
|---|
| 1652 | do i = 1, nx - 1 |
|---|
| 1653 | xs1 = x(i) |
|---|
| 1654 | x(i) = xs1 + wa(i,2) |
|---|
| 1655 | do j = i + 1, nx |
|---|
| 1656 | xs2 = x(j) |
|---|
| 1657 | x(j) = xs2 + wa(j,2) |
|---|
| 1658 | call fcn(nf, nx, x, fvec, iflag) |
|---|
| 1659 | calls = calls + 1 |
|---|
| 1660 | if (iflag .eq. 0) then |
|---|
| 1661 | fij = vdot(nf, fvec, fvec) |
|---|
| 1662 | covar(i,j) = (fij+fmin-wa(i,1)-wa(j,1)) / (wa(i,2)*wa(j,2)) |
|---|
| 1663 | covar(j,i) = covar(i,j) |
|---|
| 1664 | else |
|---|
| 1665 | covar(i,j) = zero |
|---|
| 1666 | covar(j,i) = zero |
|---|
| 1667 | endif |
|---|
| 1668 | x(j) = xs2 |
|---|
| 1669 | enddo |
|---|
| 1670 | x(i) = xs1 |
|---|
| 1671 | enddo |
|---|
| 1672 | |
|---|
| 1673 | !---- Restore original point. |
|---|
| 1674 | call mtputi(x) |
|---|
| 1675 | |
|---|
| 1676 | !---- Ensure positive definiteness and invert. |
|---|
| 1677 | call mtpsdf(covar,nx,work_4,work_5,work_6) |
|---|
| 1678 | call symsol(covar, nx, eflag, work_1, work_2, work_3) |
|---|
| 1679 | |
|---|
| 1680 | end |
|---|
| 1681 | subroutine mtderi(fcn,nf,nx,calls,x,grd,g2,fvec) |
|---|
| 1682 | |
|---|
| 1683 | use matchfi |
|---|
| 1684 | implicit none |
|---|
| 1685 | |
|---|
| 1686 | |
|---|
| 1687 | !----------------------------------------------------------------------* |
|---|
| 1688 | ! Purpose: * |
|---|
| 1689 | ! Find first derivatives of penalty function. * |
|---|
| 1690 | ! Input: * |
|---|
| 1691 | ! FCN (subr) Returns value of penalty function. * |
|---|
| 1692 | ! NF (integer) Number of functions. * |
|---|
| 1693 | ! NX (integer) Number of parameters. * |
|---|
| 1694 | ! X(NX) (real) Parameter values. On output, best estimate. * |
|---|
| 1695 | ! Output: * |
|---|
| 1696 | ! GRD(*) (real) Gradient of penalty function * |
|---|
| 1697 | ! w.r.t. internal parameter values. * |
|---|
| 1698 | ! G2(*) (real) Second derivatives of penalty function * |
|---|
| 1699 | ! w.r.t. internal parameter values. * |
|---|
| 1700 | ! Working array: * |
|---|
| 1701 | ! FVEC(NF) (real) Function values. * |
|---|
| 1702 | !----------------------------------------------------------------------* |
|---|
| 1703 | integer i,icycle,iflag,nf,nx,calls |
|---|
| 1704 | ! icovar: functionality still unclear HG 28.2.02 |
|---|
| 1705 | ! ilevel: print level |
|---|
| 1706 | double precision eps,f1,f2,fvec(nf),g2(nx),grd(nx),vdot,x(nx), & |
|---|
| 1707 | &xsave,xstep,zero,one,two,half,epsmch |
|---|
| 1708 | parameter(zero=0d0,one=1d0,two=2d0,half=0.5d0,epsmch=1d-16) |
|---|
| 1709 | external fcn |
|---|
| 1710 | |
|---|
| 1711 | eps = sqrt(epsmch) |
|---|
| 1712 | |
|---|
| 1713 | do i = 1, nx |
|---|
| 1714 | xsave = x(i) |
|---|
| 1715 | xstep = eps * abs(xsave) |
|---|
| 1716 | if (xstep .eq. zero) xstep = eps |
|---|
| 1717 | do icycle = 1, 10 |
|---|
| 1718 | x(i) = xsave + xstep |
|---|
| 1719 | call fcn(nf, nx, x, fvec, iflag) |
|---|
| 1720 | calls = calls + 1 |
|---|
| 1721 | if (iflag .eq. 0) then |
|---|
| 1722 | f2 = vdot(nf, fvec, fvec) |
|---|
| 1723 | x(i) = xsave - xstep |
|---|
| 1724 | call fcn(nf, nx, x, fvec, iflag) |
|---|
| 1725 | calls = calls + 1 |
|---|
| 1726 | if (iflag .eq. 0) then |
|---|
| 1727 | f1 = vdot(nf, fvec, fvec) |
|---|
| 1728 | go to 60 |
|---|
| 1729 | endif |
|---|
| 1730 | endif |
|---|
| 1731 | xstep = half * xstep |
|---|
| 1732 | enddo |
|---|
| 1733 | f2 = fmin |
|---|
| 1734 | f1 = fmin |
|---|
| 1735 | 60 continue |
|---|
| 1736 | grd(i) = (f2 - f1) / (two * xstep) |
|---|
| 1737 | g2(i) = (f2 - two * fmin + f1) / xstep**2 |
|---|
| 1738 | if (g2(i) .eq. zero) g2(i) = one |
|---|
| 1739 | x(i) = xsave |
|---|
| 1740 | enddo |
|---|
| 1741 | |
|---|
| 1742 | call mtputi(x) |
|---|
| 1743 | |
|---|
| 1744 | end |
|---|
| 1745 | subroutine mtpsdf(covar,nx,work_4,work_5,work_6) |
|---|
| 1746 | implicit none |
|---|
| 1747 | !----------------------------------------------------------------------* |
|---|
| 1748 | ! Purpose: * |
|---|
| 1749 | ! Force covariance matrix to be positive definite. * |
|---|
| 1750 | ! Updated: * |
|---|
| 1751 | ! COVAR(*,*) Covariance matrix. * |
|---|
| 1752 | !----------------------------------------------------------------------* |
|---|
| 1753 | integer i,j,nval,nx |
|---|
| 1754 | double precision add,pmax,pmin,covar(nx,nx),work_4(nx,nx), & |
|---|
| 1755 | &work_5(nx),work_6(nx),one,eps,epsmch |
|---|
| 1756 | parameter(one=1d0,eps=1d-3,epsmch=1d-16) |
|---|
| 1757 | |
|---|
| 1758 | !---- Copy matrix and find eigenvalues. |
|---|
| 1759 | do i = 1, nx |
|---|
| 1760 | do j = 1, nx |
|---|
| 1761 | work_4(j,i) = covar(j,i) |
|---|
| 1762 | enddo |
|---|
| 1763 | enddo |
|---|
| 1764 | call symeig(work_4,nx,nx,work_5,nval,work_6) |
|---|
| 1765 | |
|---|
| 1766 | !---- Enforce positive definiteness. |
|---|
| 1767 | pmin = work_5(1) |
|---|
| 1768 | pmax = work_5(1) |
|---|
| 1769 | do i = 1, nx |
|---|
| 1770 | if (work_5(i) .lt. pmin) pmin = work_5(i) |
|---|
| 1771 | if (work_5(i) .gt. pmax) pmax = work_5(i) |
|---|
| 1772 | enddo |
|---|
| 1773 | pmax = max(abs(pmax), one) |
|---|
| 1774 | if (pmin .le. epsmch * pmax) then |
|---|
| 1775 | add = eps * pmax - pmin |
|---|
| 1776 | do i = 1, nx |
|---|
| 1777 | covar(i,i) = covar(i,i) + add |
|---|
| 1778 | enddo |
|---|
| 1779 | print *, 'not posdef' |
|---|
| 1780 | endif |
|---|
| 1781 | |
|---|
| 1782 | end |
|---|
| 1783 | subroutine mtline(fcn,nf,nx,calls,x,dx,fvec,xsave,iflag) |
|---|
| 1784 | |
|---|
| 1785 | use matchfi |
|---|
| 1786 | implicit none |
|---|
| 1787 | |
|---|
| 1788 | |
|---|
| 1789 | !----------------------------------------------------------------------* |
|---|
| 1790 | ! Purpose: * |
|---|
| 1791 | ! Search for minimum along predicted direction. * |
|---|
| 1792 | ! Input: * |
|---|
| 1793 | ! FCN (subr) Returns value of penalty function. * |
|---|
| 1794 | ! NF (integer) Number of functions. * |
|---|
| 1795 | ! NX (integer) Number of parameters. * |
|---|
| 1796 | ! X(NX) (real) Parameter values. On output, best estimate. * |
|---|
| 1797 | ! DX(NX) (real) Initial direction. * |
|---|
| 1798 | ! FVEC(NF) (real) Function values. * |
|---|
| 1799 | ! IFLAG (integer) Error flag. * |
|---|
| 1800 | ! Working array: * |
|---|
| 1801 | ! XSAVE(NX) (real) Save area for initial point. * |
|---|
| 1802 | !----------------------------------------------------------------------* |
|---|
| 1803 | integer i,iflag,ipt,nf,npts,nvmax,nx,calls,maxpt |
|---|
| 1804 | parameter(maxpt=12) |
|---|
| 1805 | ! icovar: functionality still unclear HG 28.2.02 |
|---|
| 1806 | ! ilevel: print level |
|---|
| 1807 | double precision c1,c2,den,dx(nx),f3,fval(3),fvec(nf),fvmin, & |
|---|
| 1808 | &overal,ratio,s13,s21,s32,slam,slamax,slamin,sval(3),svmin,tol9, & |
|---|
| 1809 | &undral,vdot,x(nx),xsave(nx),zero,one,two,half,tol8,alpha,slambg, & |
|---|
| 1810 | &p100,p1000,epsmch |
|---|
| 1811 | parameter(zero=0d0,one=1d0,two=2d0,half=0.5d0,tol8=5d-2,alpha=2d0,& |
|---|
| 1812 | &slambg=5d0,p100=1d2,p1000=1d3,epsmch=1d-16) |
|---|
| 1813 | external fcn |
|---|
| 1814 | |
|---|
| 1815 | !---- Initialize. |
|---|
| 1816 | overal = p1000 |
|---|
| 1817 | undral = - p100 |
|---|
| 1818 | sval(1) = zero |
|---|
| 1819 | fval(1) = fmin |
|---|
| 1820 | svmin = zero |
|---|
| 1821 | fvmin = fmin |
|---|
| 1822 | npts = 0 |
|---|
| 1823 | |
|---|
| 1824 | slamin = zero |
|---|
| 1825 | do i = 1, nx |
|---|
| 1826 | xsave(i) = x(i) |
|---|
| 1827 | if (dx(i) .ne. zero) then |
|---|
| 1828 | ratio = abs(x(i) / dx(i)) |
|---|
| 1829 | if (slamin .eq. zero .or. ratio .lt. slamin) slamin = ratio |
|---|
| 1830 | endif |
|---|
| 1831 | enddo |
|---|
| 1832 | if (slamin .eq. zero) slamin = epsmch |
|---|
| 1833 | slamin = slamin * epsmch |
|---|
| 1834 | slamax = slambg |
|---|
| 1835 | |
|---|
| 1836 | !---- Compute function for move by DX. |
|---|
| 1837 | slam = one |
|---|
| 1838 | 20 continue |
|---|
| 1839 | sval(2) = slam |
|---|
| 1840 | do i = 1, nx |
|---|
| 1841 | x(i) = xsave(i) + slam * dx(i) |
|---|
| 1842 | enddo |
|---|
| 1843 | call fcn(nf, nx, x, fvec, iflag) |
|---|
| 1844 | calls = calls + 1 |
|---|
| 1845 | npts = npts + 1 |
|---|
| 1846 | |
|---|
| 1847 | !---- If machine becomes unstable, cut step. |
|---|
| 1848 | if (iflag .ne. 0) then |
|---|
| 1849 | slam = half * slam |
|---|
| 1850 | if (slam .gt. slamin) go to 20 |
|---|
| 1851 | go to 400 |
|---|
| 1852 | endif |
|---|
| 1853 | fval(2) = vdot(nf, fvec, fvec) |
|---|
| 1854 | if (fval(2) .lt. fvmin) then |
|---|
| 1855 | svmin = sval(2) |
|---|
| 1856 | fvmin = fval(2) |
|---|
| 1857 | endif |
|---|
| 1858 | if (slam .lt. one) go to 400 |
|---|
| 1859 | |
|---|
| 1860 | !---- Compute function for move by 1/2 DX. |
|---|
| 1861 | slam = half * slam |
|---|
| 1862 | sval(3) = slam |
|---|
| 1863 | do i = 1, nx |
|---|
| 1864 | x(i) = xsave(i) + slam * dx(i) |
|---|
| 1865 | enddo |
|---|
| 1866 | call fcn(nf, nx, x, fvec, iflag) |
|---|
| 1867 | calls = calls + 1 |
|---|
| 1868 | npts = npts + 1 |
|---|
| 1869 | if (iflag .ne. 0) go to 400 |
|---|
| 1870 | fval(3) = vdot(nf, fvec, fvec) |
|---|
| 1871 | if (fval(3) .lt. fvmin) then |
|---|
| 1872 | svmin = sval(3) |
|---|
| 1873 | fvmin = fval(3) |
|---|
| 1874 | endif |
|---|
| 1875 | |
|---|
| 1876 | !---- Begin iteration. |
|---|
| 1877 | 200 continue |
|---|
| 1878 | slamax = max(slamax, alpha * abs(svmin)) |
|---|
| 1879 | |
|---|
| 1880 | !---- Quadratic interpolation using three points. |
|---|
| 1881 | s21 = sval(2) - sval(1) |
|---|
| 1882 | s32 = sval(3) - sval(2) |
|---|
| 1883 | s13 = sval(1) - sval(3) |
|---|
| 1884 | den = s21 * s32 * s13 |
|---|
| 1885 | c2 = (s32 * fval(1) + s13 * fval(2) + s21 * fval(3)) / den |
|---|
| 1886 | c1 = ((sval(3) + sval(2)) * s32 * fval(1) + & |
|---|
| 1887 | &(sval(1) + sval(3)) * s13 * fval(2) + & |
|---|
| 1888 | &(sval(2) + sval(1)) * s21 * fval(3)) / den |
|---|
| 1889 | if (c2 .ge. zero) then |
|---|
| 1890 | slam = svmin + sign(slamax, c1 - two * c2 * svmin) |
|---|
| 1891 | else |
|---|
| 1892 | slam = c1 / (two * c2) |
|---|
| 1893 | if (slam .gt. svmin + slamax) slam = svmin + slamax |
|---|
| 1894 | if (slam .le. svmin - slamax) slam = svmin - slamax |
|---|
| 1895 | endif |
|---|
| 1896 | if (slam .gt. zero) then |
|---|
| 1897 | if (slam .gt. overal) slam = overal |
|---|
| 1898 | else |
|---|
| 1899 | if (slam .lt. undral) slam = undral |
|---|
| 1900 | endif |
|---|
| 1901 | |
|---|
| 1902 | !---- See if new point coincides with a previous one. |
|---|
| 1903 | 300 continue |
|---|
| 1904 | tol9 = tol8 * max(one, slam) |
|---|
| 1905 | do ipt = 1, 3 |
|---|
| 1906 | if (abs(slam - sval(ipt)) .lt. tol9) go to 400 |
|---|
| 1907 | enddo |
|---|
| 1908 | |
|---|
| 1909 | !---- Compute function for interpolated point. |
|---|
| 1910 | do i = 1, nx |
|---|
| 1911 | x(i) = xsave(i) + slam * dx(i) |
|---|
| 1912 | enddo |
|---|
| 1913 | call fcn(nf, nx, x, fvec, iflag) |
|---|
| 1914 | calls = calls + 1 |
|---|
| 1915 | npts = npts + 1 |
|---|
| 1916 | if (iflag .ne. 0) go to 400 |
|---|
| 1917 | f3 = vdot(nf, fvec, fvec) |
|---|
| 1918 | |
|---|
| 1919 | !---- Find worst point of previous three. |
|---|
| 1920 | nvmax = 1 |
|---|
| 1921 | if (fval(2) .gt. fval(nvmax)) nvmax = 2 |
|---|
| 1922 | if (fval(3) .gt. fval(nvmax)) nvmax = 3 |
|---|
| 1923 | |
|---|
| 1924 | !---- If no improvement, cut interval. |
|---|
| 1925 | if (f3 .ge. fval(nvmax)) then |
|---|
| 1926 | if (npts .ge. maxpt) go to 400 |
|---|
| 1927 | if (slam .gt. svmin) overal = min(overal, slam - tol8) |
|---|
| 1928 | if (slam .le. svmin) undral = max(undral, slam + tol8) |
|---|
| 1929 | slam = half * (slam + svmin) |
|---|
| 1930 | go to 300 |
|---|
| 1931 | endif |
|---|
| 1932 | |
|---|
| 1933 | !---- Accept new point; replace previous worst point. |
|---|
| 1934 | sval(nvmax) = slam |
|---|
| 1935 | fval(nvmax) = f3 |
|---|
| 1936 | if (f3 .lt. fvmin) then |
|---|
| 1937 | svmin = slam |
|---|
| 1938 | fvmin = f3 |
|---|
| 1939 | else |
|---|
| 1940 | if (slam .gt. svmin) overal = min(overal, slam - tol8) |
|---|
| 1941 | if (slam .lt. svmin) undral = max(undral, slam + tol8) |
|---|
| 1942 | endif |
|---|
| 1943 | if (npts .lt. maxpt) go to 200 |
|---|
| 1944 | |
|---|
| 1945 | !---- Common exit point: Return best point and step used. |
|---|
| 1946 | 400 continue |
|---|
| 1947 | fmin = fvmin |
|---|
| 1948 | do i = 1, nx |
|---|
| 1949 | dx(i) = svmin * dx(i) |
|---|
| 1950 | x(i) = xsave(i) + dx(i) |
|---|
| 1951 | enddo |
|---|
| 1952 | call mtputi(x) |
|---|
| 1953 | |
|---|
| 1954 | !---- Return Failure indication. |
|---|
| 1955 | iflag = 0 |
|---|
| 1956 | if (svmin .eq. zero) iflag = 2 |
|---|
| 1957 | |
|---|
| 1958 | end |
|---|
| 1959 | subroutine mtsimp(ncon,nvar,tol,calls,call_lim,vect,dvect, & |
|---|
| 1960 | &fun_vect,w_iwa1,w_iwa2,w_iwa3) |
|---|
| 1961 | |
|---|
| 1962 | use matchfi |
|---|
| 1963 | implicit none |
|---|
| 1964 | |
|---|
| 1965 | |
|---|
| 1966 | !----------------------------------------------------------------------* |
|---|
| 1967 | ! Purpose: * |
|---|
| 1968 | ! Control routine for simplex minimization; SIMPLEX command. * |
|---|
| 1969 | ! Attributes: * |
|---|
| 1970 | !----------------------------------------------------------------------* |
|---|
| 1971 | integer calls,call_lim,ncon,nvar |
|---|
| 1972 | ! icovar: functionality still unclear HG 28.2.02 |
|---|
| 1973 | ! ilevel: print level |
|---|
| 1974 | double precision tol,vect(*),dvect(*),fun_vect(*),w_iwa1(*), & |
|---|
| 1975 | &w_iwa2(*),w_iwa3(*) |
|---|
| 1976 | external mtfcn |
|---|
| 1977 | |
|---|
| 1978 | icovar = 0 |
|---|
| 1979 | ilevel = 0 |
|---|
| 1980 | |
|---|
| 1981 | !---- Too many variable parameters? |
|---|
| 1982 | if (nvar .gt. ncon) then |
|---|
| 1983 | call aawarn('MTSIMP','More variables than constraints seen.') |
|---|
| 1984 | call aawarn('MTSIMP', & |
|---|
| 1985 | &'SIMPLEX may not converge to optimal solution.') |
|---|
| 1986 | endif |
|---|
| 1987 | |
|---|
| 1988 | !---- Call minimization routine. |
|---|
| 1989 | call mtgeti(vect, dvect) |
|---|
| 1990 | call mtsim1(mtfcn, ncon, nvar, calls, call_lim, tol, & |
|---|
| 1991 | &vect, dvect, fun_vect, w_iwa1, w_iwa2, w_iwa3) |
|---|
| 1992 | |
|---|
| 1993 | 9999 end |
|---|
| 1994 | |
|---|
| 1995 | subroutine mtsim1(fcn,nf,nx,calls,call_lim,tol,x,dx,fvec,psim, & |
|---|
| 1996 | &fsim,wa) |
|---|
| 1997 | |
|---|
| 1998 | use matchfi |
|---|
| 1999 | implicit none |
|---|
| 2000 | |
|---|
| 2001 | |
|---|
| 2002 | !----------------------------------------------------------------------* |
|---|
| 2003 | ! Purpose: * |
|---|
| 2004 | ! Minimization using the SIMPLEX method by Nelder and Mead. * |
|---|
| 2005 | ! (Computer Journal 7, 308 (1965). * |
|---|
| 2006 | ! Input: * |
|---|
| 2007 | ! FCN (subr) Returns value of function to be minimized. * |
|---|
| 2008 | ! NF (integer) Number of functions. * |
|---|
| 2009 | ! NX (integer) Number of parameters. * |
|---|
| 2010 | ! X(NX) (real) Parameter values. On output, best estimate. * |
|---|
| 2011 | ! DX(NX) (real) Parameter errors. On output, error estimate. * |
|---|
| 2012 | ! Output: * |
|---|
| 2013 | ! FVEC(NF) (real) Vector of function values in best point. * |
|---|
| 2014 | ! Working arrays: * |
|---|
| 2015 | ! PSIM(NX,0:NX) Coordinates of simplex vertices. * |
|---|
| 2016 | ! FSIM(0:NX) Function values in simplex vertices. * |
|---|
| 2017 | ! WA(NX,4) Working vectors. * |
|---|
| 2018 | !----------------------------------------------------------------------* |
|---|
| 2019 | integer i,idir,iflag,j,jh,jhold,jl,k,level,ncycl,nf,nrstrt,ns,nx, & |
|---|
| 2020 | &calls,call_lim,mbar,mstar,mstst,mrho, izero |
|---|
| 2021 | parameter(mbar=1,mstar=2,mstst=3,mrho=4) |
|---|
| 2022 | ! icovar: functionality still unclear HG 28.2.02 |
|---|
| 2023 | ! ilevel: print level |
|---|
| 2024 | double precision f,f1,f2,fbar,fbest,frho,fstar,fstst,pb,pbest, & |
|---|
| 2025 | &pmax,pmin,rho,step,vdot,tol,x(nx),dx(nx),fvec(nf),psim(nx,0:nx), & |
|---|
| 2026 | &fsim(0:nx),wa(nx,4),alpha,beta,gamma,rhomin,rhomax,rho1,rho2,zero,& |
|---|
| 2027 | &two,three,half,p01,eps,epsmch |
|---|
| 2028 | parameter(alpha=1d0,beta=0.5d0,gamma=2d0,rhomin=4d0,rhomax=8d0, & |
|---|
| 2029 | &rho1=1d0+alpha,rho2=rho1+alpha*gamma,zero=0d0,two=2d0,three=3d0, & |
|---|
| 2030 | &half=5d-1,p01=1d-1,eps=1d-8,epsmch=1d-16) |
|---|
| 2031 | external fcn |
|---|
| 2032 | |
|---|
| 2033 | izero = 0 |
|---|
| 2034 | !---- Initialize penalty function. |
|---|
| 2035 | call mtcond(izero, nf, fvec, iflag) |
|---|
| 2036 | ! call fcn(nf, nx, x, fvec, iflag) |
|---|
| 2037 | calls = calls + 1 |
|---|
| 2038 | if (iflag .ne. 0) then |
|---|
| 2039 | call aawarn('MTSIMP', ' stopped, possibly unstable') |
|---|
| 2040 | go to 400 |
|---|
| 2041 | endif |
|---|
| 2042 | fmin = vdot(nf, fvec, fvec) |
|---|
| 2043 | edm = fmin |
|---|
| 2044 | nrstrt = 0 |
|---|
| 2045 | |
|---|
| 2046 | !---- Choose the initial simplex using single-parameter searches. |
|---|
| 2047 | ! Keep initial point in PBAR. |
|---|
| 2048 | 100 continue |
|---|
| 2049 | if (ilevel .ge. 1) call mtprnt('init', nx, x) |
|---|
| 2050 | fbar = fmin |
|---|
| 2051 | do i = 1, nx |
|---|
| 2052 | wa(i,mbar) = x(i) |
|---|
| 2053 | pbest = x(i) |
|---|
| 2054 | fbest = fmin |
|---|
| 2055 | step = dx(i) |
|---|
| 2056 | |
|---|
| 2057 | !---- Find proper initial direction and step. |
|---|
| 2058 | do idir = 1, 12 |
|---|
| 2059 | x(i) = pbest + step |
|---|
| 2060 | call fcn(nf, nx, x, fvec, iflag) |
|---|
| 2061 | calls = calls + 1 |
|---|
| 2062 | if (iflag .eq. 0) then |
|---|
| 2063 | f = vdot(nf, fvec, fvec) |
|---|
| 2064 | if (f .le. fbest) go to 120 |
|---|
| 2065 | endif |
|---|
| 2066 | if (mod(idir,2) .eq. 0) step = p01 * step |
|---|
| 2067 | step = - step |
|---|
| 2068 | enddo |
|---|
| 2069 | go to 160 |
|---|
| 2070 | |
|---|
| 2071 | !---- Improvement found; attempt increasing steps. |
|---|
| 2072 | 120 continue |
|---|
| 2073 | do ns = 1, 3 |
|---|
| 2074 | pbest = x(i) |
|---|
| 2075 | fbest = f |
|---|
| 2076 | step = step * three |
|---|
| 2077 | x(i) = x(i) + step |
|---|
| 2078 | call fcn(nf, nx, x, fvec, iflag) |
|---|
| 2079 | calls = calls + 1 |
|---|
| 2080 | if (iflag .ne. 0) go to 140 |
|---|
| 2081 | f = vdot(nf, fvec, fvec) |
|---|
| 2082 | if (f .gt. fbest) go to 140 |
|---|
| 2083 | enddo |
|---|
| 2084 | go to 160 |
|---|
| 2085 | |
|---|
| 2086 | !---- Backtrack to best point. |
|---|
| 2087 | 140 continue |
|---|
| 2088 | x(i) = pbest |
|---|
| 2089 | f = fbest |
|---|
| 2090 | |
|---|
| 2091 | !---- Store local minimum in i'th direction. |
|---|
| 2092 | 160 continue |
|---|
| 2093 | fsim(i) = f |
|---|
| 2094 | do k = 1, nx |
|---|
| 2095 | psim(k,i) = x(k) |
|---|
| 2096 | enddo |
|---|
| 2097 | enddo |
|---|
| 2098 | |
|---|
| 2099 | !---- Store initial point as 0'th vertex. |
|---|
| 2100 | jh = 0 |
|---|
| 2101 | call mtrazz(nx, fbar, wa(1,mbar), fsim, psim, jh, jl) |
|---|
| 2102 | |
|---|
| 2103 | !---- Extract best point. |
|---|
| 2104 | do i = 1, nx |
|---|
| 2105 | x(i) = psim(i,jl) |
|---|
| 2106 | enddo |
|---|
| 2107 | |
|---|
| 2108 | !---- Print-out after setting up simplex. |
|---|
| 2109 | if (ilevel .ge. 2) then |
|---|
| 2110 | call mtprnt('progress',nx, x) |
|---|
| 2111 | endif |
|---|
| 2112 | ncycl = 0 |
|---|
| 2113 | |
|---|
| 2114 | !==== Start main loop. |
|---|
| 2115 | 200 continue |
|---|
| 2116 | if (edm .lt. tol) then |
|---|
| 2117 | print *, 'converged' |
|---|
| 2118 | else if (calls .gt. call_lim) then |
|---|
| 2119 | print *, 'call limit' |
|---|
| 2120 | else |
|---|
| 2121 | |
|---|
| 2122 | !---- Calculate PBAR and P*. |
|---|
| 2123 | do i = 1, nx |
|---|
| 2124 | pb = psim(i,0) |
|---|
| 2125 | do j = 1, nx |
|---|
| 2126 | pb = pb + psim(i,j) |
|---|
| 2127 | enddo |
|---|
| 2128 | wa(i,mbar) = (pb - psim(i,jh)) / float(nx) |
|---|
| 2129 | wa(i,mstar) = wa(i,mbar) + alpha * (wa(i,mbar) - psim(i,jh)) |
|---|
| 2130 | enddo |
|---|
| 2131 | call fcn(nf, nx, wa(1,mstar), fvec, iflag) |
|---|
| 2132 | calls = calls + 1 |
|---|
| 2133 | if (iflag .ne. 0) then |
|---|
| 2134 | fstar = two * fsim(jh) |
|---|
| 2135 | else |
|---|
| 2136 | fstar = vdot(nf, fvec, fvec) |
|---|
| 2137 | endif |
|---|
| 2138 | |
|---|
| 2139 | !---- Point P* is better than point PSIM(*,JL). |
|---|
| 2140 | jhold = jh |
|---|
| 2141 | if (fstar .lt. fsim(jl)) then |
|---|
| 2142 | |
|---|
| 2143 | !---- Try expanded point P**. |
|---|
| 2144 | do i = 1, nx |
|---|
| 2145 | wa(i,mstst) = wa(i,mbar) + gamma * (wa(i,mstar)-wa(i,mbar)) |
|---|
| 2146 | enddo |
|---|
| 2147 | call fcn(nf, nx, wa(1,mstst), fvec, iflag) |
|---|
| 2148 | calls = calls + 1 |
|---|
| 2149 | if (iflag .ne. 0) then |
|---|
| 2150 | fstst = two * fsim(jh) |
|---|
| 2151 | rho = zero |
|---|
| 2152 | else |
|---|
| 2153 | fstst = vdot(nf, fvec, fvec) |
|---|
| 2154 | |
|---|
| 2155 | !---- Fit a parabola through FSIM(JH), F*, F**; minimum = RHO. |
|---|
| 2156 | f1 = (fstar - fsim(jh)) * rho2 |
|---|
| 2157 | f2 = (fstst - fsim(jh)) * rho1 |
|---|
| 2158 | rho = half * (rho2 * f1 - rho1 * f2) / (f1 - f2) |
|---|
| 2159 | endif |
|---|
| 2160 | |
|---|
| 2161 | !---- Minimum inexistent ot too close to PBAR; |
|---|
| 2162 | ! Use P** if it gives improvement; otherwise use P*. |
|---|
| 2163 | if (rho .lt. rhomin) then |
|---|
| 2164 | if (fstst .lt. fsim(jl)) then |
|---|
| 2165 | call mtrazz(nx, fstst, wa(1,mstst), fsim, psim, & |
|---|
| 2166 | &jh, jl) |
|---|
| 2167 | else |
|---|
| 2168 | call mtrazz(nx, fstar, wa(1,mstar), fsim, psim, & |
|---|
| 2169 | &jh, jl) |
|---|
| 2170 | endif |
|---|
| 2171 | |
|---|
| 2172 | !---- Usable minimum found. |
|---|
| 2173 | else |
|---|
| 2174 | if (rho .gt. rhomax) rho = rhomax |
|---|
| 2175 | do i = 1, nx |
|---|
| 2176 | wa(i,mrho) = psim(i,jh) + rho * (wa(i,mbar) - psim(i,jh)) |
|---|
| 2177 | enddo |
|---|
| 2178 | call fcn(nf, nx, wa(1,mrho), fvec, iflag) |
|---|
| 2179 | calls = calls + 1 |
|---|
| 2180 | if (iflag .ne. 0) then |
|---|
| 2181 | frho = two * fsim(jh) |
|---|
| 2182 | else |
|---|
| 2183 | frho = vdot(nf, fvec, fvec) |
|---|
| 2184 | endif |
|---|
| 2185 | |
|---|
| 2186 | !---- Select farthest point which gives decent improvement. |
|---|
| 2187 | if (frho .lt. fsim(jl) .and. frho .lt. fstst) then |
|---|
| 2188 | call mtrazz(nx, frho, wa(1,mrho), fsim, psim, jh, jl) |
|---|
| 2189 | else if (fstst .lt. fsim(jl)) then |
|---|
| 2190 | call mtrazz(nx, fstst, wa(1,mstst), fsim, psim, & |
|---|
| 2191 | &jh, jl) |
|---|
| 2192 | else |
|---|
| 2193 | call mtrazz(nx, fstar, wa(1,mstar), fsim, psim, & |
|---|
| 2194 | &jh, jl) |
|---|
| 2195 | endif |
|---|
| 2196 | endif |
|---|
| 2197 | |
|---|
| 2198 | !---- F* is higher than FSIM(JL). |
|---|
| 2199 | else |
|---|
| 2200 | if (fstar .lt. fsim(jh)) then |
|---|
| 2201 | call mtrazz(nx, fstar, wa(1,mstar), fsim, psim, jh, jl) |
|---|
| 2202 | endif |
|---|
| 2203 | |
|---|
| 2204 | !---- If F* is still highest value, try contraction, |
|---|
| 2205 | ! giving point P** = PWRK(*,3). |
|---|
| 2206 | if (jhold .eq. jh) then |
|---|
| 2207 | do i = 1, nx |
|---|
| 2208 | wa(i,mstst) = wa(i,mbar) & |
|---|
| 2209 | &+ beta * (psim(i,jh) - wa(i,mbar)) |
|---|
| 2210 | enddo |
|---|
| 2211 | call fcn(nf, nx, wa(1,mstst), fvec, iflag) |
|---|
| 2212 | calls = calls + 1 |
|---|
| 2213 | if (iflag .ne. 0) then |
|---|
| 2214 | fstst = two * fsim(jh) |
|---|
| 2215 | else |
|---|
| 2216 | fstst = vdot(nf, fvec, fvec) |
|---|
| 2217 | endif |
|---|
| 2218 | |
|---|
| 2219 | !---- Restart algorithm, if F** is higher; otherwise use it. |
|---|
| 2220 | if (fstst .gt. fsim(jh)) then |
|---|
| 2221 | print *, 'failed' |
|---|
| 2222 | if (nrstrt .ne. 0) go to 300 |
|---|
| 2223 | nrstrt = 1 |
|---|
| 2224 | do j = 1, nx |
|---|
| 2225 | x(j) = psim(j,jl) |
|---|
| 2226 | enddo |
|---|
| 2227 | go to 100 |
|---|
| 2228 | endif |
|---|
| 2229 | call mtrazz(nx, fstst, wa(1,mstst), fsim, psim, jh, jl) |
|---|
| 2230 | endif |
|---|
| 2231 | endif |
|---|
| 2232 | |
|---|
| 2233 | !---- New minimum found. |
|---|
| 2234 | if (jl .eq. jhold) then |
|---|
| 2235 | nrstrt = 0 |
|---|
| 2236 | ncycl = ncycl + 1 |
|---|
| 2237 | level = 3 |
|---|
| 2238 | if (mod(ncycl,10) .eq. 0) level = 2 |
|---|
| 2239 | if (ilevel .ge. level) then |
|---|
| 2240 | call mtprnt('progress',nx, x) |
|---|
| 2241 | endif |
|---|
| 2242 | ! if(calls.gt.(calls_old+dcalls)) then |
|---|
| 2243 | write(*,830) calls,fmin |
|---|
| 2244 | ! calls_old=calls |
|---|
| 2245 | ! endif |
|---|
| 2246 | endif |
|---|
| 2247 | go to 200 |
|---|
| 2248 | endif |
|---|
| 2249 | |
|---|
| 2250 | !==== End main loop: Try central point of simplex. |
|---|
| 2251 | 300 continue |
|---|
| 2252 | do i = 1, nx |
|---|
| 2253 | pmin = psim(i,0) |
|---|
| 2254 | pmax = psim(i,0) |
|---|
| 2255 | pb = psim(i,0) |
|---|
| 2256 | do j = 1, nx |
|---|
| 2257 | pb = pb + psim(i,j) |
|---|
| 2258 | enddo |
|---|
| 2259 | wa(i,mbar) = (pb - psim(i,jh)) / float(nx) |
|---|
| 2260 | dx(i) = pmax - pmin |
|---|
| 2261 | enddo |
|---|
| 2262 | call fcn(nf, nx, wa(1,mbar), fvec, iflag) |
|---|
| 2263 | calls = calls + 1 |
|---|
| 2264 | if (iflag .eq. 0) then |
|---|
| 2265 | fbar = vdot(nf, fvec, fvec) |
|---|
| 2266 | if (fbar .lt. fsim(jl)) then |
|---|
| 2267 | call mtrazz(nx, fbar, wa(1,mbar), fsim, psim, jh, jl) |
|---|
| 2268 | endif |
|---|
| 2269 | endif |
|---|
| 2270 | |
|---|
| 2271 | !---- Recompute step sizes and extract best point. |
|---|
| 2272 | do i = 1, nx |
|---|
| 2273 | pmin = psim(i,0) |
|---|
| 2274 | pmax = psim(i,0) |
|---|
| 2275 | do j = 1, nx |
|---|
| 2276 | pmin = min(psim(i,j),pmin) |
|---|
| 2277 | pmax = max(psim(i,j),pmax) |
|---|
| 2278 | enddo |
|---|
| 2279 | dx(i) = pmax - pmin |
|---|
| 2280 | x(i) = psim(i,jl) |
|---|
| 2281 | enddo |
|---|
| 2282 | fmin = fsim(jl) |
|---|
| 2283 | call mtputi(x) |
|---|
| 2284 | |
|---|
| 2285 | !---- Check for necessity to restart after final change. |
|---|
| 2286 | if (calls + 3 * nx .lt. call_lim .and. nrstrt .eq. 0 .and. & |
|---|
| 2287 | &fmin .gt. two * (tol + epsmch)) then |
|---|
| 2288 | nrstrt = 1 |
|---|
| 2289 | go to 100 |
|---|
| 2290 | endif |
|---|
| 2291 | |
|---|
| 2292 | !---- Final print-out. |
|---|
| 2293 | 400 continue |
|---|
| 2294 | if (ilevel .ge. 1) then |
|---|
| 2295 | call mtprnt('final',nx, x) |
|---|
| 2296 | endif |
|---|
| 2297 | |
|---|
| 2298 | write(*,830) calls,fmin |
|---|
| 2299 | |
|---|
| 2300 | 830 format('call:',I8,3x,'Penalty function = ',e16.8) |
|---|
| 2301 | end |
|---|
| 2302 | subroutine mtrazz(nvrr,fnew,pnew,fsim,psim,jh,jl) |
|---|
| 2303 | |
|---|
| 2304 | use matchfi |
|---|
| 2305 | implicit none |
|---|
| 2306 | !----------------------------------------------------------------------* |
|---|
| 2307 | ! Purpose: * |
|---|
| 2308 | ! Replace vertex in simplex whose function is highest. * |
|---|
| 2309 | ! Return indices of new best and worst points. * |
|---|
| 2310 | ! Input: * |
|---|
| 2311 | ! NVAR (integer) Number of parameters. * |
|---|
| 2312 | ! FNEW (real) Function value for new vertex. * |
|---|
| 2313 | ! PNEW(*) (real) Coordinates of new vertex. * |
|---|
| 2314 | ! Updated: * |
|---|
| 2315 | ! FSIM(*) (real) Function values in (NVAR + 1) vertices. * |
|---|
| 2316 | ! PSIM(*,*) (real) Coordinates of (NVAR + 1) vertices. * |
|---|
| 2317 | ! JH (integer) Index of highest function value. * |
|---|
| 2318 | ! JL (integer) Index of lowest function value. * |
|---|
| 2319 | !----------------------------------------------------------------------* |
|---|
| 2320 | integer i,jh,jl,nvrr |
|---|
| 2321 | ! icovar: functionality still unclear HG 28.2.02 |
|---|
| 2322 | ! ilevel: print level |
|---|
| 2323 | double precision fnew,fsim(0:nvrr),pnew(nvrr),psim(nvrr,0:nvrr), & |
|---|
| 2324 | &ten |
|---|
| 2325 | parameter(ten=1d1) |
|---|
| 2326 | |
|---|
| 2327 | !---- Replace vertex with highest function value. |
|---|
| 2328 | do i = 1, nvrr |
|---|
| 2329 | psim(i,jh) = pnew(i) |
|---|
| 2330 | enddo |
|---|
| 2331 | fsim(jh) = fnew |
|---|
| 2332 | |
|---|
| 2333 | !---- Find indices of lowest and highest function value. |
|---|
| 2334 | jl = 0 |
|---|
| 2335 | jh = 0 |
|---|
| 2336 | do i = 1, nvrr |
|---|
| 2337 | if (fsim(i) .lt. fsim(jl)) jl = i |
|---|
| 2338 | if (fsim(i) .gt. fsim(jh)) jh = i |
|---|
| 2339 | enddo |
|---|
| 2340 | |
|---|
| 2341 | !---- Get best value and estimated distance to minimum. |
|---|
| 2342 | fmin = fsim(jl) |
|---|
| 2343 | edm = min(ten * (fsim(jh) - fmin), fmin) |
|---|
| 2344 | |
|---|
| 2345 | end |
|---|
![(please configure the [header_logo] section in trac.ini)](/PSPA/chrome/site/pspa.gif)
