/*! \fn calculatingErrors * * function calculates the errors */ void calculatingErrors(DATA_NEWTON* solverData, double* delta_x, double* delta_x_scaled, double* delta_f, double* error_f, double* scaledError_f, int* n, double* x, double* fvec) { int i=0; double scaling_factor; /* delta_x = || x_new-x_old || */ for (i=0; i<*n; i++) solverData->delta_x_vec[i] = x[i]-solverData->x_new[i]; *delta_x = enorm_(n,solverData->delta_x_vec); scaling_factor = enorm_(n,x); if (scaling_factor > 1) *delta_x_scaled = *delta_x * 1./ scaling_factor; else *delta_x_scaled = *delta_x; /* delta_f = || f_old - f_new || */ for (i=0; i<*n; i++) solverData->delta_f[i] = solverData->f_old[i]-fvec[i]; *delta_f=enorm_(n, solverData->delta_f); *error_f = enorm_(n,fvec); /* scaling residual vector */ scaling_residual_vector(solverData); for (i=0; i<*n; i++) solverData->fvecScaled[i]=fvec[i]/solverData->resScaling[i]; *scaledError_f = enorm_(n,solverData->fvecScaled); }
/*! \fn damping_heuristic * * first damping heuristic: * x_increment will be halved until the Euclidean norm of the residual function * is smaller than the Euclidean norm of the current point * * treshold for damping = 0.01 * compiler flag: -newton = damped */ void damping_heuristic(double* x, int(*f)(int*, double*, double*, void*, int), double current_fvec_enorm, int* n, double* fvec, double* lambda, int* k, DATA_NEWTON* solverData, void* userdata) { int i,j=0; double enorm_new, treshold = 1e-2; /* calculate new function values */ (*f)(n, solverData->x_new, fvec, userdata, 1); solverData->nfev++; enorm_new=enorm_(n,fvec); if (enorm_new >= current_fvec_enorm) infoStreamPrint(LOG_NLS_V, 1, "Start Damping: enorm_new : %e; current_fvec_enorm: %e ", enorm_new, current_fvec_enorm); while (enorm_new >= current_fvec_enorm) { j++; *lambda*=0.5; for (i=0; i<*n; i++) solverData->x_new[i]=x[i]-*lambda*solverData->x_increment[i]; /* calculate new function values */ (*f)(n, solverData->x_new, fvec, userdata, 1); solverData->nfev++; enorm_new=enorm_(n,fvec); if (*lambda <= treshold) { warningStreamPrint(LOG_NLS_V, 0, "Warning: lambda reached a threshold."); /* if damping is without success, trying full newton step; after 5 full newton steps try a very little step */ if (*k >= 5) for (i=0; i<*n; i++) solverData->x_new[i]=x[i]-*lambda*solverData->x_increment[i]; else for (i=0; i<*n; i++) solverData->x_new[i]=x[i]-solverData->x_increment[i]; /* calculate new function values */ (*f)(n, solverData->x_new, fvec, userdata, 1); solverData->nfev++; (*k)++; break; } } *lambda = 1; messageClose(LOG_NLS_V); }
int main() { int m, n, ldfjac, info, lwa, ipvt[3], one=1; double tol, fnorm; double x[3], fvec[15], fjac[9], wa[30]; m = 15; n = 3; /* the following starting values provide a rough fit. */ x[0] = 1.; x[1] = 1.; x[2] = 1.; ldfjac = 3; lwa = 30; /* set tol to the square root of the machine precision. unless high precision solutions are required, this is the recommended setting. */ tol = sqrt(dpmpar_(&one)); lmstr1_(&fcn, &m, &n, x, fvec, fjac, &ldfjac, &tol, &info, ipvt, wa, &lwa); fnorm = enorm_(&m, fvec); printf(" FINAL L2 NORM OF THE RESIDUALS%15.7g\n\n", fnorm); printf(" EXIT PARAMETER %10i\n\n", info); printf(" FINAL APPROXIMATE SOLUTION\n\n%15.7g%15.7g%15.7g\n", x[0], x[1], x[2]); return 0; }
/* ********** */ /* Main program */ int MAIN__(void) { /* Initialized data */ static integer nread = 5; static integer nwrite = 6; static doublereal one = 1.; static doublereal ten = 10.; /* Format strings */ static char fmt_50[] = "(3i5)"; static char fmt_60[] = "(////5x,\002 PROBLEM\002,i5,5x,\002 DIMENSION" "\002,i5,5x//)"; static char fmt_70[] = "(5x,\002 INITIAL L2 NORM OF THE RESIDUALS\002,d1" "5.7//5x,\002 FINAL L2 NORM OF THE RESIDUALS \002,d15.7//5x,\002" " NUMBER OF FUNCTION EVALUATIONS \002,i10//5x,\002 EXIT PARAMETER" "\002,18x,i10//5x,\002 FINAL APPROXIMATE SOLUTION\002//(5x,5d15.7" "))"; static char fmt_80[] = "(\0021SUMMARY OF \002,i3,\002 CALLS TO HYBRD1" "\002/)"; static char fmt_90[] = "(\002 NPROB N NFEV INFO FINAL L2 NORM\002" "/)"; static char fmt_100[] = "(i4,i6,i7,i6,1x,d15.7)"; /* System generated locals */ integer i__1, i__2; /* Builtin functions */ double sqrt(doublereal); integer s_rsfe(cilist *), do_fio(integer *, char *, ftnlen), e_rsfe(void), s_wsfe(cilist *), e_wsfe(void); /* Subroutine */ int s_stop(char *, ftnlen); /* Local variables */ static integer i__, k, n; static doublereal x[40]; static integer ic, na[60], nf[60]; static doublereal wa[2660]; static integer np[60], nx[60]; extern /* Subroutine */ int fcn_(); static doublereal fnm[60]; static integer lwa; static doublereal tol, fvec[40]; static integer info; extern doublereal enorm_(integer *, doublereal *); extern /* Subroutine */ int hybrd1_(U_fp, integer *, doublereal *, doublereal *, doublereal *, integer *, doublereal *, integer *); static doublereal fnorm1, fnorm2; extern /* Subroutine */ int vecfcn_(integer *, doublereal *, doublereal *, integer *); static doublereal factor; extern doublereal dpmpar_(integer *); static integer ntries; extern /* Subroutine */ int initpt_(integer *, doublereal *, integer *, doublereal *); /* Fortran I/O blocks */ static cilist io___8 = { 0, 0, 0, fmt_50, 0 }; static cilist io___16 = { 0, 0, 0, fmt_60, 0 }; static cilist io___25 = { 0, 0, 0, fmt_70, 0 }; static cilist io___27 = { 0, 0, 0, fmt_80, 0 }; static cilist io___28 = { 0, 0, 0, fmt_90, 0 }; static cilist io___29 = { 0, 0, 0, fmt_100, 0 }; /* LOGICAL INPUT UNIT IS ASSUMED TO BE NUMBER 5. */ /* LOGICAL OUTPUT UNIT IS ASSUMED TO BE NUMBER 6. */ tol = sqrt(dpmpar_(&c__1)); lwa = 2660; ic = 0; L10: io___8.ciunit = nread; s_rsfe(&io___8); do_fio(&c__1, (char *)&refnum_1.nprob, (ftnlen)sizeof(integer)); do_fio(&c__1, (char *)&n, (ftnlen)sizeof(integer)); do_fio(&c__1, (char *)&ntries, (ftnlen)sizeof(integer)); e_rsfe(); if (refnum_1.nprob <= 0) { goto L30; } factor = one; i__1 = ntries; for (k = 1; k <= i__1; ++k) { ++ic; initpt_(&n, x, &refnum_1.nprob, &factor); vecfcn_(&n, x, fvec, &refnum_1.nprob); fnorm1 = enorm_(&n, fvec); io___16.ciunit = nwrite; s_wsfe(&io___16); do_fio(&c__1, (char *)&refnum_1.nprob, (ftnlen)sizeof(integer)); do_fio(&c__1, (char *)&n, (ftnlen)sizeof(integer)); e_wsfe(); refnum_1.nfev = 0; hybrd1_((U_fp)fcn_, &n, x, fvec, &tol, &info, wa, &lwa); fnorm2 = enorm_(&n, fvec); np[ic - 1] = refnum_1.nprob; na[ic - 1] = n; nf[ic - 1] = refnum_1.nfev; nx[ic - 1] = info; fnm[ic - 1] = fnorm2; io___25.ciunit = nwrite; s_wsfe(&io___25); do_fio(&c__1, (char *)&fnorm1, (ftnlen)sizeof(doublereal)); do_fio(&c__1, (char *)&fnorm2, (ftnlen)sizeof(doublereal)); do_fio(&c__1, (char *)&refnum_1.nfev, (ftnlen)sizeof(integer)); do_fio(&c__1, (char *)&info, (ftnlen)sizeof(integer)); i__2 = n; for (i__ = 1; i__ <= i__2; ++i__) { do_fio(&c__1, (char *)&x[i__ - 1], (ftnlen)sizeof(doublereal)); } e_wsfe(); factor = ten * factor; /* L20: */ } goto L10; L30: io___27.ciunit = nwrite; s_wsfe(&io___27); do_fio(&c__1, (char *)&ic, (ftnlen)sizeof(integer)); e_wsfe(); io___28.ciunit = nwrite; s_wsfe(&io___28); e_wsfe(); i__1 = ic; for (i__ = 1; i__ <= i__1; ++i__) { io___29.ciunit = nwrite; s_wsfe(&io___29); do_fio(&c__1, (char *)&np[i__ - 1], (ftnlen)sizeof(integer)); do_fio(&c__1, (char *)&na[i__ - 1], (ftnlen)sizeof(integer)); do_fio(&c__1, (char *)&nf[i__ - 1], (ftnlen)sizeof(integer)); do_fio(&c__1, (char *)&nx[i__ - 1], (ftnlen)sizeof(integer)); do_fio(&c__1, (char *)&fnm[i__ - 1], (ftnlen)sizeof(doublereal)); e_wsfe(); /* L40: */ } s_stop("", (ftnlen)0); /* LAST CARD OF DRIVER. */ return 0; } /* MAIN__ */
/*! \fn solve non-linear system with hybrd method * * \param [in] [data] * [sysNumber] index of the corresponing non-linear system * * \author wbraun */ int solveHybrd(DATA *data, int sysNumber) { NONLINEAR_SYSTEM_DATA* systemData = &(data->simulationInfo.nonlinearSystemData[sysNumber]); DATA_HYBRD* solverData = (DATA_HYBRD*)systemData->solverData; /* * We are given the number of the non-linear system. * We want to look it up among all equations. */ int eqSystemNumber = systemData->equationIndex; int i, j; integer iflag = 1; double xerror, xerror_scaled; int success = 0; double local_tol = 1e-12; double initial_factor = solverData->factor; int nfunc_evals = 0; int continuous = 1; int nonContinuousCase = 0; int giveUp = 0; int retries = 0; int retries2 = 0; int retries3 = 0; int assertCalled = 0; int assertRetries = 0; int assertMessage = 0; static state mem_state; modelica_boolean* relationsPreBackup; relationsPreBackup = (modelica_boolean*) malloc(data->modelData.nRelations*sizeof(modelica_boolean)); /* debug output */ if(ACTIVE_STREAM(LOG_NLS)) { INFO2(LOG_NLS, "start solving non-linear system >>%s<< at time %g", modelInfoXmlGetEquation(&data->modelData.modelDataXml, eqSystemNumber).name, data->localData[0]->timeValue); INDENT(LOG_NLS); for(i=0; i<solverData->n; i++) { INFO2(LOG_NLS, "x[%d] = %f", i, systemData->nlsx[i]); INDENT(LOG_NLS); INFO3(LOG_NLS, "scaling = %f\nold = %f\nextrapolated = %f", systemData->nominal[i], systemData->nlsxOld[i], systemData->nlsxExtrapolation[i]); RELEASE(LOG_NLS); } RELEASE(LOG_NLS); } /* set x vector */ if(data->simulationInfo.discreteCall) memcpy(solverData->x, systemData->nlsx, solverData->n*(sizeof(double))); else memcpy(solverData->x, systemData->nlsxExtrapolation, solverData->n*(sizeof(double))); for(i=0; i<solverData->n; i++) solverData->xScalefactors[i] = fmax(fabs(solverData->x[i]), systemData->nominal[i]); /* evaluate with discontinuities */ { int scaling = solverData->useXScaling; if(scaling) solverData->useXScaling = 0; mem_state = get_memory_state(); /* try */ if(!setjmp(nonlinearJmpbuf)) { wrapper_fvec_hybrj(&solverData->n, solverData->x, solverData->fvec, solverData->fjac, &solverData->ldfjac, &iflag, data, sysNumber); restore_memory_state(mem_state); } else { /* catch */ restore_memory_state(mem_state); WARNING(LOG_STDOUT, "Non-Linear Solver try to handle a problem with a called assert."); } if(scaling) { solverData->useXScaling = 1; } } /* start solving loop */ while(!giveUp && !success) { for(i=0; i<solverData->n; i++) solverData->xScalefactors[i] = fmax(fabs(solverData->x[i]), systemData->nominal[i]); /* debug output */ if(ACTIVE_STREAM(LOG_NLS_V)) { printVector(solverData->xScalefactors, &(solverData->n), LOG_NLS_V, "scaling factors x vector"); printVector(solverData->x, &(solverData->n), LOG_NLS_V, "Iteration variable values"); } /* Scaling x vector */ if(solverData->useXScaling) { for(i=0; i<solverData->n; i++) { solverData->x[i] = (1.0/solverData->xScalefactors[i]) * solverData->x[i]; } } /* debug output */ if(ACTIVE_STREAM(LOG_NLS_V)) { printVector(solverData->x, &solverData->n, LOG_NLS_V, "Iteration variable values (scaled)"); } /* set residual function continuous */ if(continuous) { ((DATA*)data)->simulationInfo.solveContinuous = 1; } else { ((DATA*)data)->simulationInfo.solveContinuous = 0; } giveUp = 1; mem_state = get_memory_state(); /* try */ if(!setjmp(nonlinearJmpbuf)) { _omc_hybrj_(wrapper_fvec_hybrj, &solverData->n, solverData->x, solverData->fvec, solverData->fjac, &solverData->ldfjac, &solverData->xtol, &solverData->maxfev, solverData->diag, &solverData->mode, &solverData->factor, &solverData->nprint, &solverData->info, &solverData->nfev, &solverData->njev, solverData->r__, &solverData->lr, solverData->qtf, solverData->wa1, solverData->wa2, solverData->wa3, solverData->wa4, data, sysNumber); restore_memory_state(mem_state); if(assertCalled) { INFO(LOG_NLS, "After asserts was called, values reached which avoided assert call."); memcpy(systemData->nlsxOld, solverData->x, solverData->n*(sizeof(double))); } assertRetries = 0; assertCalled = 0; } else { /* catch */ restore_memory_state(mem_state); if(!assertMessage) { INDENT(LOG_STDOUT); WARNING(LOG_STDOUT, "While solving non-linear system an assert was called."); WARNING(LOG_STDOUT, "The non-linear solver tries to solve the problem that could take some time."); WARNING(LOG_STDOUT, "It could help to provide better start-values for the iteration variables."); WARNING(LOG_STDOUT, "For more information simulate with -lv LOG_NLS"); RELEASE(LOG_STDOUT); assertMessage = 1; } solverData->info = -1; xerror_scaled = 1; xerror = 1; assertCalled = 1; } /* set residual function continuous */ if(continuous) { ((DATA*)data)->simulationInfo.solveContinuous = 0; } else { ((DATA*)data)->simulationInfo.solveContinuous = 1; } /* re-scaling x vector */ if(solverData->useXScaling) for(i=0; i<solverData->n; i++) solverData->x[i] = solverData->x[i]*solverData->xScalefactors[i]; /* check for proper inputs */ if(solverData->info == 0) { printErrorEqSyst(IMPROPER_INPUT, modelInfoXmlGetEquation(&data->modelData.modelDataXml, eqSystemNumber), data->localData[0]->timeValue); } if(solverData->info != -1) { /* evaluate with discontinuities */ if(data->simulationInfo.discreteCall){ int scaling = solverData->useXScaling; if(scaling) solverData->useXScaling = 0; ((DATA*)data)->simulationInfo.solveContinuous = 0; mem_state = get_memory_state(); /* try */ if(!setjmp(nonlinearJmpbuf)) { wrapper_fvec_hybrj(&solverData->n, solverData->x, solverData->fvec, solverData->fjac, &solverData->ldfjac, &iflag, data, sysNumber); restore_memory_state(mem_state); } else { /* catch */ restore_memory_state(mem_state); WARNING(LOG_STDOUT, "Non-Linear Solver try to handle a problem with a called assert."); solverData->info = -1; xerror_scaled = 1; xerror = 1; assertCalled = 1; } if(scaling) solverData->useXScaling = 1; storeRelations(data); } } if(solverData->info != -1) { /* scaling residual vector */ { int l=0; for(i=0; i<solverData->n; i++){ solverData->resScaling[i] = 1e-16; for(j=0; j<solverData->n; j++){ solverData->resScaling[i] = (fabs(solverData->fjacobian[l]) > solverData->resScaling[i]) ? fabs(solverData->fjacobian[l]) : solverData->resScaling[i]; l++; } solverData->fvecScaled[i] = solverData->fvec[i] * (1 / solverData->resScaling[i]); } /* debug output */ if(ACTIVE_STREAM(LOG_NLS_V)) { INFO(LOG_NLS_V, "scaling factors for residual vector"); INDENT(LOG_NLS_V); for(i=0; i<solverData->n; i++) { INFO2(LOG_NLS_V, "scaled residual [%d] : %.20e", i, solverData->fvecScaled[i]); INDENT(LOG_NLS_V); INFO2(LOG_NLS_V, "scaling factor [%d] : %.20e", i, solverData->resScaling[i]); RELEASE(LOG_NLS_V); } RELEASE(LOG_NLS_V); } /* debug output */ if(ACTIVE_STREAM(LOG_NLS_JAC)) { char buffer[4096]; INFO2(LOG_NLS_JAC, "jacobian matrix [%dx%d]", (int)solverData->n, (int)solverData->n); INDENT(LOG_NLS_JAC); for(i=0; i<solverData->n; i++) { buffer[0] = 0; for(j=0; j<solverData->n; j++) sprintf(buffer, "%s%10g ", buffer, solverData->fjacobian[i*solverData->n+j]); INFO1(LOG_NLS_JAC, "%s", buffer); } RELEASE(LOG_NLS_JAC); } /* check for error */ xerror_scaled = enorm_(&solverData->n, solverData->fvecScaled); xerror = enorm_(&solverData->n, solverData->fvec); } } /* reset non-contunuousCase */ if(nonContinuousCase && xerror > local_tol && xerror_scaled > local_tol) { memcpy(data->simulationInfo.relationsPre, relationsPreBackup, sizeof(modelica_boolean)*data->modelData.nRelations); nonContinuousCase = 0; } if(solverData->info < 4 && xerror > local_tol && xerror_scaled > local_tol) solverData->info = 4; /* solution found */ if(solverData->info == 1 || xerror <= local_tol || xerror_scaled <= local_tol) { int scaling; success = 1; nfunc_evals += solverData->nfev; if(ACTIVE_STREAM(LOG_NLS)) { INFO(LOG_NLS, "system solved"); INDENT(LOG_NLS); INFO2(LOG_NLS, "%d retries\n%d restarts", retries, retries2+retries3); RELEASE(LOG_NLS); printStatus(solverData, &nfunc_evals, &xerror, &xerror_scaled, LOG_NLS); } scaling = solverData->useXScaling; if(scaling) solverData->useXScaling = 0; /* take the solution */ memcpy(systemData->nlsx, solverData->x, solverData->n*(sizeof(double))); mem_state = get_memory_state(); /* try */ if(!setjmp(nonlinearJmpbuf)) { wrapper_fvec_hybrj(&solverData->n, solverData->x, solverData->fvec, solverData->fjac, &solverData->ldfjac, &iflag, data, sysNumber); restore_memory_state(mem_state); } else { /* catch */ restore_memory_state(mem_state); WARNING(LOG_STDOUT, "Non-Linear Solver try to handle a problem with a called assert."); solverData->info = 4; xerror_scaled = 1; xerror = 1; assertCalled = 1; success = 0; giveUp = 0; } if(scaling) solverData->useXScaling = 1; } else if((solverData->info == 4 || solverData->info == 5) && assertRetries < 1+solverData->n && assertCalled) { /* case only used, when the Modelica code called an assert * then, we try to modify start values to avoid the assert call.*/ int i; memcpy(solverData->x, systemData->nlsxOld, solverData->n*(sizeof(double))); /* set all zero values to nominal values */ if(assertRetries < 1) { for(i=0; i<solverData->n; i++) { if(systemData->nlsx[i] == 0) { systemData->nlsx[i] = systemData->nominal[i]; solverData->x[i] = systemData->nominal[i]; } } } /* change initial guess values one by one */ else if(assertRetries < solverData->n+1) { i = assertRetries-1; solverData->x[i] += 0.01*systemData->nominal[i]; } giveUp = 0; nfunc_evals += solverData->nfev; assertRetries++; if(ACTIVE_STREAM(LOG_NLS)) { INFO1(LOG_NLS, " - try to handle a problem with a called assert vary initial value a bit. (Retry: %d)",assertRetries); printStatus(solverData, &nfunc_evals, &xerror, &xerror_scaled, LOG_NLS_V); } } else if((solverData->info == 4 || solverData->info == 5) && retries < 3) { /* first try to decrease factor */ /* set x vector */ if(data->simulationInfo.discreteCall) memcpy(solverData->x, systemData->nlsx, solverData->n*(sizeof(double))); else memcpy(solverData->x, systemData->nlsxExtrapolation, solverData->n*(sizeof(double))); solverData->factor = solverData->factor / 10.0; retries++; giveUp = 0; nfunc_evals += solverData->nfev; if(ACTIVE_STREAM(LOG_NLS)) { INFO1(LOG_NLS, " - iteration making no progress:\t decreasing initial step bound to %f.", solverData->factor); printStatus(solverData, &nfunc_evals, &xerror, &xerror_scaled, LOG_NLS_V); } } else if((solverData->info == 4 || solverData->info == 5) && retries < 4) { /* try to vary the initial values */ for(i = 0; i < solverData->n; i++) solverData->x[i] += systemData->nominal[i] * 0.1; solverData->factor = initial_factor; retries++; giveUp = 0; nfunc_evals += solverData->nfev; if(ACTIVE_STREAM(LOG_NLS)) { INFO(LOG_NLS, "iteration making no progress:\t vary solution point by 1%%."); printStatus(solverData, &nfunc_evals, &xerror, &xerror_scaled, LOG_NLS_V); } } else if((solverData->info == 4 || solverData->info == 5) && retries < 5) { /* try old values as x-Scaling factors */ /* set x vector */ if(data->simulationInfo.discreteCall) memcpy(solverData->x, systemData->nlsx, solverData->n*(sizeof(double))); else memcpy(solverData->x, systemData->nlsxExtrapolation, solverData->n*(sizeof(double))); for(i=0; i<solverData->n; i++) solverData->xScalefactors[i] = fmax(fabs(systemData->nlsxOld[i]), systemData->nominal[i]); retries++; giveUp = 0; nfunc_evals += solverData->nfev; if(ACTIVE_STREAM(LOG_NLS)) { INFO(LOG_NLS, "iteration making no progress:\t try old values as scaling factors."); printStatus(solverData, &nfunc_evals, &xerror, &xerror_scaled, LOG_NLS_V); } } else if((solverData->info == 4 || solverData->info == 5) && retries < 6) { int scaling = 0; /* try to disable x-Scaling */ /* set x vector */ if(data->simulationInfo.discreteCall) memcpy(solverData->x, systemData->nlsx, solverData->n*(sizeof(double))); else memcpy(solverData->x, systemData->nlsxExtrapolation, solverData->n*(sizeof(double))); scaling = solverData->useXScaling; if(scaling) solverData->useXScaling = 0; /* reset x-scalling factors */ for(i=0; i<solverData->n; i++) solverData->xScalefactors[i] = fmax(fabs(solverData->x[i]), systemData->nominal[i]); retries++; giveUp = 0; nfunc_evals += solverData->nfev; if(ACTIVE_STREAM(LOG_NLS)) { INFO(LOG_NLS, "iteration making no progress:\t try without scaling at all."); printStatus(solverData, &nfunc_evals, &xerror, &xerror_scaled, LOG_NLS_V); } } else if((solverData->info == 4 || solverData->info == 5) && retries < 7 && data->simulationInfo.discreteCall) { /* try to solve non-continuous * work-a-round: since other wise some model does * stuck in event iteration. e.g.: Modelica.Mechanics.Rotational.Examples.HeatLosses */ memcpy(solverData->x, systemData->nlsxOld, solverData->n*(sizeof(double))); retries++; /* try to solve a discontinuous system */ continuous = 0; nonContinuousCase = 1; memcpy(relationsPreBackup, data->simulationInfo.relationsPre, sizeof(modelica_boolean)*data->modelData.nRelations); giveUp = 0; nfunc_evals += solverData->nfev; if(ACTIVE_STREAM(LOG_NLS)) { INFO(LOG_NLS, " - iteration making no progress:\t try to solve a discontinuous system."); printStatus(solverData, &nfunc_evals, &xerror, &xerror_scaled, LOG_NLS_V); } /* Then try with old values (instead of extrapolating )*/ } else if((solverData->info == 4 || solverData->info == 5) && retries2 < 1) { int scaling = 0; /* set x vector */ memcpy(solverData->x, systemData->nlsxOld, solverData->n*(sizeof(double))); scaling = solverData->useXScaling; if(!scaling) solverData->useXScaling = 1; continuous = 1; solverData->factor = initial_factor; retries = 0; retries2++; giveUp = 0; nfunc_evals += solverData->nfev; if(ACTIVE_STREAM(LOG_NLS)) { INFO(LOG_NLS, " - iteration making no progress:\t use old values instead extrapolated."); printStatus(solverData, &nfunc_evals, &xerror, &xerror_scaled, LOG_NLS_V); } /* try to vary the initial values */ } else if((solverData->info == 4 || solverData->info == 5) && retries2 < 2) { /* set x vector */ if(data->simulationInfo.discreteCall) memcpy(solverData->x, systemData->nlsx, solverData->n*(sizeof(double))); else memcpy(solverData->x, systemData->nlsxExtrapolation, solverData->n*(sizeof(double))); for(i = 0; i < solverData->n; i++) { solverData->x[i] *= 1.01; }; retries = 0; retries2++; giveUp = 0; nfunc_evals += solverData->nfev; if(ACTIVE_STREAM(LOG_NLS)) { INFO(LOG_NLS, " - iteration making no progress:\t vary initial point by adding 1%%."); printStatus(solverData, &nfunc_evals, &xerror, &xerror_scaled, LOG_NLS_V); } /* try to vary the initial values */ } else if((solverData->info == 4 || solverData->info == 5) && retries2 < 3) { /* set x vector */ if(data->simulationInfo.discreteCall) memcpy(solverData->x, systemData->nlsx, solverData->n*(sizeof(double))); else memcpy(solverData->x, systemData->nlsxExtrapolation, solverData->n*(sizeof(double))); for(i = 0; i < solverData->n; i++) { solverData->x[i] *= 0.99; }; retries = 0; retries2++; giveUp = 0; nfunc_evals += solverData->nfev; if(ACTIVE_STREAM(LOG_NLS)) { INFO(LOG_NLS, " - iteration making no progress:\t vary initial point by -1%%."); printStatus(solverData, &nfunc_evals, &xerror, &xerror_scaled, LOG_NLS_V); } /* try to vary the initial values */ } else if((solverData->info == 4 || solverData->info == 5) && retries2 < 4) { /* set x vector */ memcpy(solverData->x, systemData->nominal, solverData->n*(sizeof(double))); retries = 0; retries2++; giveUp = 0; nfunc_evals += solverData->nfev; if(ACTIVE_STREAM(LOG_NLS)) { INFO(LOG_NLS, " - iteration making no progress:\t try scaling factor as initial point."); printStatus(solverData, &nfunc_evals, &xerror, &xerror_scaled, LOG_NLS_V); } /* try own scaling factors */ } else if((solverData->info == 4 || solverData->info == 5) && retries2 < 5 && !assertCalled) { /* set x vector */ if(data->simulationInfo.discreteCall) memcpy(solverData->x, systemData->nlsx, solverData->n*(sizeof(double))); else memcpy(solverData->x, systemData->nlsxExtrapolation, solverData->n*(sizeof(double))); for(i = 0; i < solverData->n; i++) { solverData->diag[i] = fabs(solverData->resScaling[i]); if(solverData->diag[i] <= 1e-16) solverData->diag[i] = 1e-16; } retries = 0; retries2++; giveUp = 0; solverData->mode = 2; nfunc_evals += solverData->nfev; if(ACTIVE_STREAM(LOG_NLS)) { INFO(LOG_NLS, " - iteration making no progress:\t try with own scaling factors."); printStatus(solverData, &nfunc_evals, &xerror, &xerror_scaled, LOG_NLS_V); } /* try without internal scaling */ } else if((solverData->info == 4 || solverData->info == 5) && retries3 < 1) { /* set x vector */ if(data->simulationInfo.discreteCall) memcpy(solverData->x, systemData->nlsx, solverData->n*(sizeof(double))); else memcpy(solverData->x, systemData->nlsxExtrapolation, solverData->n*(sizeof(double))); for(i = 0; i < solverData->n; i++) solverData->diag[i] = 1.0; solverData->useXScaling = 1; retries = 0; retries2 = 0; retries3++; solverData->mode = 2; giveUp = 0; nfunc_evals += solverData->nfev; if(ACTIVE_STREAM(LOG_NLS)) { INFO(LOG_NLS, " - iteration making no progress:\t disable solver internal scaling."); printStatus(solverData, &nfunc_evals, &xerror, &xerror_scaled, LOG_NLS_V); } /* try to reduce the tolerance a bit */ } else if((solverData->info == 4 || solverData->info == 5) && retries3 < 6) { /* set x vector */ if(data->simulationInfo.discreteCall) memcpy(solverData->x, systemData->nlsx, solverData->n*(sizeof(double))); else memcpy(solverData->x, systemData->nlsxExtrapolation, solverData->n*(sizeof(double))); /* reduce tolarance */ local_tol = local_tol*10; solverData->factor = initial_factor; solverData->mode = 1; retries = 0; retries2 = 0; retries3++; giveUp = 0; nfunc_evals += solverData->nfev; if(ACTIVE_STREAM(LOG_NLS)) { INFO1(LOG_NLS, " - iteration making no progress:\t reduce the tolerance slightly to %e.", local_tol); printStatus(solverData, &nfunc_evals, &xerror, &xerror_scaled, LOG_NLS_V); } } else if(solverData->info >= 2 && solverData->info <= 5) { /* while the initialization it's ok to every time a solution */ if(!data->simulationInfo.initial){ printErrorEqSyst(ERROR_AT_TIME, modelInfoXmlGetEquation(&data->modelData.modelDataXml, eqSystemNumber), data->localData[0]->timeValue); } if(ACTIVE_STREAM(LOG_NLS)) { RELEASE(LOG_NLS); INFO1(LOG_NLS, "### No Solution! ###\n after %d restarts", retries*retries2*retries3); printStatus(solverData, &nfunc_evals, &xerror, &xerror_scaled, LOG_NLS); } /* take the best approximation */ memcpy(systemData->nlsx, solverData->x, solverData->n*(sizeof(double))); } } /* reset some solving data */ solverData->factor = initial_factor; solverData->mode = 1; free(relationsPreBackup); return success; }
/* Subroutine */ int qrfac_(integer *m, integer *n, doublereal *a, integer * lda, logical *pivot, integer *ipvt, integer *lipvt, doublereal *rdiag, doublereal *acnorm, doublereal *wa) { /* Initialized data */ static doublereal one = 1.; static doublereal p05 = .05; static doublereal zero = 0.; /* System generated locals */ integer a_dim1, a_offset, i__1, i__2, i__3; doublereal d__1, d__2, d__3; /* Builtin functions */ double sqrt(doublereal); /* Local variables */ static integer i__, j, k, jp1; static doublereal sum; static integer kmax; static doublereal temp; static integer minmn; extern doublereal enorm_(integer *, doublereal *); static doublereal epsmch; extern doublereal dpmpar_(integer *); static doublereal ajnorm; /* ********** */ /* subroutine qrfac */ /* this subroutine uses householder transformations with column */ /* pivoting (optional) to compute a qr factorization of the */ /* m by n matrix a. that is, qrfac determines an orthogonal */ /* matrix q, a permutation matrix p, and an upper trapezoidal */ /* matrix r with diagonal elements of nonincreasing magnitude, */ /* such that a*p = q*r. the householder transformation for */ /* column k, k = 1,2,...,min(m,n), is of the form */ /* t */ /* i - (1/u(k))*u*u */ /* where u has zeros in the first k-1 positions. the form of */ /* this transformation and the method of pivoting first */ /* appeared in the corresponding linpack subroutine. */ /* the subroutine statement is */ /* subroutine qrfac(m,n,a,lda,pivot,ipvt,lipvt,rdiag,acnorm,wa) */ /* where */ /* m is a positive integer input variable set to the number */ /* of rows of a. */ /* n is a positive integer input variable set to the number */ /* of columns of a. */ /* a is an m by n array. on input a contains the matrix for */ /* which the qr factorization is to be computed. on output */ /* the strict upper trapezoidal part of a contains the strict */ /* upper trapezoidal part of r, and the lower trapezoidal */ /* part of a contains a factored form of q (the non-trivial */ /* elements of the u vectors described above). */ /* lda is a positive integer input variable not less than m */ /* which specifies the leading dimension of the array a. */ /* pivot is a logical input variable. if pivot is set true, */ /* then column pivoting is enforced. if pivot is set false, */ /* then no column pivoting is done. */ /* ipvt is an integer output array of length lipvt. ipvt */ /* defines the permutation matrix p such that a*p = q*r. */ /* column j of p is column ipvt(j) of the identity matrix. */ /* if pivot is false, ipvt is not referenced. */ /* lipvt is a positive integer input variable. if pivot is false, */ /* then lipvt may be as small as 1. if pivot is true, then */ /* lipvt must be at least n. */ /* rdiag is an output array of length n which contains the */ /* diagonal elements of r. */ /* acnorm is an output array of length n which contains the */ /* norms of the corresponding columns of the input matrix a. */ /* if this information is not needed, then acnorm can coincide */ /* with rdiag. */ /* wa is a work array of length n. if pivot is false, then wa */ /* can coincide with rdiag. */ /* subprograms called */ /* minpack-supplied ... dpmpar,enorm */ /* fortran-supplied ... dmax1,dsqrt,min0 */ /* argonne national laboratory. minpack project. march 1980. */ /* burton s. garbow, kenneth e. hillstrom, jorge j. more */ /* ********** */ /* Parameter adjustments */ --wa; --acnorm; --rdiag; a_dim1 = *lda; a_offset = 1 + a_dim1 * 1; a -= a_offset; --ipvt; /* Function Body */ /* epsmch is the machine precision. */ epsmch = dpmpar_(&c__1); /* compute the initial column norms and initialize several arrays. */ i__1 = *n; for (j = 1; j <= i__1; ++j) { acnorm[j] = enorm_(m, &a[j * a_dim1 + 1]); rdiag[j] = acnorm[j]; wa[j] = rdiag[j]; if (*pivot) { ipvt[j] = j; } /* L10: */ } /* reduce a to r with householder transformations. */ minmn = min(*m,*n); i__1 = minmn; for (j = 1; j <= i__1; ++j) { if (! (*pivot)) { goto L40; } /* bring the column of largest norm into the pivot position. */ kmax = j; i__2 = *n; for (k = j; k <= i__2; ++k) { if (rdiag[k] > rdiag[kmax]) { kmax = k; } /* L20: */ } if (kmax == j) { goto L40; } i__2 = *m; for (i__ = 1; i__ <= i__2; ++i__) { temp = a[i__ + j * a_dim1]; a[i__ + j * a_dim1] = a[i__ + kmax * a_dim1]; a[i__ + kmax * a_dim1] = temp; /* L30: */ } rdiag[kmax] = rdiag[j]; wa[kmax] = wa[j]; k = ipvt[j]; ipvt[j] = ipvt[kmax]; ipvt[kmax] = k; L40: /* compute the householder transformation to reduce the */ /* j-th column of a to a multiple of the j-th unit vector. */ i__2 = *m - j + 1; ajnorm = enorm_(&i__2, &a[j + j * a_dim1]); if (ajnorm == zero) { goto L100; } if (a[j + j * a_dim1] < zero) { ajnorm = -ajnorm; } i__2 = *m; for (i__ = j; i__ <= i__2; ++i__) { a[i__ + j * a_dim1] /= ajnorm; /* L50: */ } a[j + j * a_dim1] += one; /* apply the transformation to the remaining columns */ /* and update the norms. */ jp1 = j + 1; if (*n < jp1) { goto L100; } i__2 = *n; for (k = jp1; k <= i__2; ++k) { sum = zero; i__3 = *m; for (i__ = j; i__ <= i__3; ++i__) { sum += a[i__ + j * a_dim1] * a[i__ + k * a_dim1]; /* L60: */ } temp = sum / a[j + j * a_dim1]; i__3 = *m; for (i__ = j; i__ <= i__3; ++i__) { a[i__ + k * a_dim1] -= temp * a[i__ + j * a_dim1]; /* L70: */ } if (! (*pivot) || rdiag[k] == zero) { goto L80; } temp = a[j + k * a_dim1] / rdiag[k]; /* Computing MAX */ /* Computing 2nd power */ d__3 = temp; d__1 = zero, d__2 = one - d__3 * d__3; rdiag[k] *= sqrt((max(d__1,d__2))); /* Computing 2nd power */ d__1 = rdiag[k] / wa[k]; if (p05 * (d__1 * d__1) > epsmch) { goto L80; } i__3 = *m - j; rdiag[k] = enorm_(&i__3, &a[jp1 + k * a_dim1]); wa[k] = rdiag[k]; L80: /* L90: */ ; } L100: rdiag[j] = -ajnorm; /* L110: */ } return 0; /* last card of subroutine qrfac. */ } /* qrfac_ */
/* Subroutine */ void lmstr_(void (*fcn)(const int *m, const int *n, const double *x, double *fvec, double *fjrow, int *iflag ), const int *m, const int *n, double *x, double *fvec, double *fjac, const int *ldfjac, const double *ftol, const double *xtol, const double *gtol, const int *maxfev, double * diag, const int *mode, const double *factor, const int *nprint, int * info, int *nfev, int *njev, int *ipvt, double *qtf, double *wa1, double *wa2, double *wa3, double *wa4) { /* Table of constant values */ const int c__1 = 1; const int c_true = TRUE_; /* Initialized data */ #define p1 .1 #define p5 .5 #define p25 .25 #define p75 .75 #define p0001 1e-4 /* System generated locals */ int fjac_dim1, fjac_offset, i__1, i__2; double d__1, d__2, d__3; /* Local variables */ int i__, j, l; double par, sum; int sing; int iter; double temp, temp1, temp2; int iflag; double delta; double ratio; double fnorm, gnorm, pnorm, xnorm, fnorm1, actred, dirder, epsmch, prered; /* ********** */ /* subroutine lmstr */ /* the purpose of lmstr is to minimize the sum of the squares of */ /* m nonlinear functions in n variables by a modification of */ /* the levenberg-marquardt algorithm which uses minimal storage. */ /* the user must provide a subroutine which calculates the */ /* functions and the rows of the jacobian. */ /* the subroutine statement is */ /* subroutine lmstr(fcn,m,n,x,fvec,fjac,ldfjac,ftol,xtol,gtol, */ /* maxfev,diag,mode,factor,nprint,info,nfev, */ /* njev,ipvt,qtf,wa1,wa2,wa3,wa4) */ /* where */ /* fcn is the name of the user-supplied subroutine which */ /* calculates the functions and the rows of the jacobian. */ /* fcn must be declared in an external statement in the */ /* user calling program, and should be written as follows. */ /* subroutine fcn(m,n,x,fvec,fjrow,iflag) */ /* integer m,n,iflag */ /* double precision x(n),fvec(m),fjrow(n) */ /* ---------- */ /* if iflag = 1 calculate the functions at x and */ /* return this vector in fvec. */ /* if iflag = i calculate the (i-1)-st row of the */ /* jacobian at x and return this vector in fjrow. */ /* ---------- */ /* return */ /* end */ /* the value of iflag should not be changed by fcn unless */ /* the user wants to terminate execution of lmstr. */ /* in this case set iflag to a negative integer. */ /* m is a positive integer input variable set to the number */ /* of functions. */ /* n is a positive integer input variable set to the number */ /* of variables. n must not exceed m. */ /* x is an array of length n. on input x must contain */ /* an initial estimate of the solution vector. on output x */ /* contains the final estimate of the solution vector. */ /* fvec is an output array of length m which contains */ /* the functions evaluated at the output x. */ /* fjac is an output n by n array. the upper triangle of fjac */ /* contains an upper triangular matrix r such that */ /* t t t */ /* p *(jac *jac)*p = r *r, */ /* where p is a permutation matrix and jac is the final */ /* calculated jacobian. column j of p is column ipvt(j) */ /* (see below) of the identity matrix. the lower triangular */ /* part of fjac contains information generated during */ /* the computation of r. */ /* ldfjac is a positive integer input variable not less than n */ /* which specifies the leading dimension of the array fjac. */ /* ftol is a nonnegative input variable. termination */ /* occurs when both the actual and predicted relative */ /* reductions in the sum of squares are at most ftol. */ /* therefore, ftol measures the relative error desired */ /* in the sum of squares. */ /* xtol is a nonnegative input variable. termination */ /* occurs when the relative error between two consecutive */ /* iterates is at most xtol. therefore, xtol measures the */ /* relative error desired in the approximate solution. */ /* gtol is a nonnegative input variable. termination */ /* occurs when the cosine of the angle between fvec and */ /* any column of the jacobian is at most gtol in absolute */ /* value. therefore, gtol measures the orthogonality */ /* desired between the function vector and the columns */ /* of the jacobian. */ /* maxfev is a positive integer input variable. termination */ /* occurs when the number of calls to fcn with iflag = 1 */ /* has reached maxfev. */ /* diag is an array of length n. if mode = 1 (see */ /* below), diag is internally set. if mode = 2, diag */ /* must contain positive entries that serve as */ /* multiplicative scale factors for the variables. */ /* mode is an integer input variable. if mode = 1, the */ /* variables will be scaled internally. if mode = 2, */ /* the scaling is specified by the input diag. other */ /* values of mode are equivalent to mode = 1. */ /* factor is a positive input variable used in determining the */ /* initial step bound. this bound is set to the product of */ /* factor and the euclidean norm of diag*x if nonzero, or else */ /* to factor itself. in most cases factor should lie in the */ /* interval (.1,100.). 100. is a generally recommended value. */ /* nprint is an integer input variable that enables controlled */ /* printing of iterates if it is positive. in this case, */ /* fcn is called with iflag = 0 at the beginning of the first */ /* iteration and every nprint iterations thereafter and */ /* immediately prior to return, with x and fvec available */ /* for printing. if nprint is not positive, no special calls */ /* of fcn with iflag = 0 are made. */ /* info is an integer output variable. if the user has */ /* terminated execution, info is set to the (negative) */ /* value of iflag. see description of fcn. otherwise, */ /* info is set as follows. */ /* info = 0 improper input parameters. */ /* info = 1 both actual and predicted relative reductions */ /* in the sum of squares are at most ftol. */ /* info = 2 relative error between two consecutive iterates */ /* is at most xtol. */ /* info = 3 conditions for info = 1 and info = 2 both hold. */ /* info = 4 the cosine of the angle between fvec and any */ /* column of the jacobian is at most gtol in */ /* absolute value. */ /* info = 5 number of calls to fcn with iflag = 1 has */ /* reached maxfev. */ /* info = 6 ftol is too small. no further reduction in */ /* the sum of squares is possible. */ /* info = 7 xtol is too small. no further improvement in */ /* the approximate solution x is possible. */ /* info = 8 gtol is too small. fvec is orthogonal to the */ /* columns of the jacobian to machine precision. */ /* nfev is an integer output variable set to the number of */ /* calls to fcn with iflag = 1. */ /* njev is an integer output variable set to the number of */ /* calls to fcn with iflag = 2. */ /* ipvt is an integer output array of length n. ipvt */ /* defines a permutation matrix p such that jac*p = q*r, */ /* where jac is the final calculated jacobian, q is */ /* orthogonal (not stored), and r is upper triangular. */ /* column j of p is column ipvt(j) of the identity matrix. */ /* qtf is an output array of length n which contains */ /* the first n elements of the vector (q transpose)*fvec. */ /* wa1, wa2, and wa3 are work arrays of length n. */ /* wa4 is a work array of length m. */ /* subprograms called */ /* user-supplied ...... fcn */ /* minpack-supplied ... dpmpar,enorm,lmpar,qrfac,rwupdt */ /* fortran-supplied ... dabs,dmax1,dmin1,dsqrt,mod */ /* argonne national laboratory. minpack project. march 1980. */ /* burton s. garbow, dudley v. goetschel, kenneth e. hillstrom, */ /* jorge j. more */ /* ********** */ /* Parameter adjustments */ --wa4; --fvec; --wa3; --wa2; --wa1; --qtf; --ipvt; --diag; --x; fjac_dim1 = *ldfjac; fjac_offset = 1 + fjac_dim1 * 1; fjac -= fjac_offset; /* Function Body */ /* epsmch is the machine precision. */ epsmch = dpmpar_(&c__1); *info = 0; iflag = 0; *nfev = 0; *njev = 0; /* check the input parameters for errors. */ if (*n <= 0 || *m < *n || *ldfjac < *n || *ftol < 0. || *xtol < 0. || *gtol < 0. || *maxfev <= 0 || *factor <= 0.) { goto L340; } if (*mode != 2) { goto L20; } i__1 = *n; for (j = 1; j <= i__1; ++j) { if (diag[j] <= 0.) { goto L340; } /* L10: */ } L20: /* evaluate the function at the starting point */ /* and calculate its norm. */ iflag = 1; (*fcn)(m, n, &x[1], &fvec[1], &wa3[1], &iflag); *nfev = 1; if (iflag < 0) { goto L340; } fnorm = enorm_(m, &fvec[1]); /* initialize levenberg-marquardt parameter and iteration counter. */ par = 0.; iter = 1; /* beginning of the outer loop. */ L30: /* if requested, call fcn to enable printing of iterates. */ if (*nprint <= 0) { goto L40; } iflag = 0; if ((iter - 1) % *nprint == 0) { (*fcn)(m, n, &x[1], &fvec[1], &wa3[1], &iflag); } if (iflag < 0) { goto L340; } L40: /* compute the qr factorization of the jacobian matrix */ /* calculated one row at a time, while simultaneously */ /* forming (q transpose)*fvec and storing the first */ /* n components in qtf. */ i__1 = *n; for (j = 1; j <= i__1; ++j) { qtf[j] = 0.; i__2 = *n; for (i__ = 1; i__ <= i__2; ++i__) { fjac[i__ + j * fjac_dim1] = 0.; /* L50: */ } /* L60: */ } iflag = 2; i__1 = *m; for (i__ = 1; i__ <= i__1; ++i__) { (*fcn)(m, n, &x[1], &fvec[1], &wa3[1], &iflag); if (iflag < 0) { goto L340; } temp = fvec[i__]; rwupdt_(n, &fjac[fjac_offset], ldfjac, &wa3[1], &qtf[1], &temp, &wa1[ 1], &wa2[1]); ++iflag; /* L70: */ } ++(*njev); /* if the jacobian is rank deficient, call qrfac to */ /* reorder its columns and update the components of qtf. */ sing = FALSE_; i__1 = *n; for (j = 1; j <= i__1; ++j) { if (fjac[j + j * fjac_dim1] == 0.) { sing = TRUE_; } ipvt[j] = j; wa2[j] = enorm_(&j, &fjac[j * fjac_dim1 + 1]); /* L80: */ } if (! sing) { goto L130; } qrfac_(n, n, &fjac[fjac_offset], ldfjac, &c_true, &ipvt[1], n, &wa1[1], & wa2[1], &wa3[1]); i__1 = *n; for (j = 1; j <= i__1; ++j) { if (fjac[j + j * fjac_dim1] == 0.) { goto L110; } sum = 0.; i__2 = *n; for (i__ = j; i__ <= i__2; ++i__) { sum += fjac[i__ + j * fjac_dim1] * qtf[i__]; /* L90: */ } temp = -sum / fjac[j + j * fjac_dim1]; i__2 = *n; for (i__ = j; i__ <= i__2; ++i__) { qtf[i__] += fjac[i__ + j * fjac_dim1] * temp; /* L100: */ } L110: fjac[j + j * fjac_dim1] = wa1[j]; /* L120: */ } L130: /* on the first iteration and if mode is 1, scale according */ /* to the norms of the columns of the initial jacobian. */ if (iter != 1) { goto L170; } if (*mode == 2) { goto L150; } i__1 = *n; for (j = 1; j <= i__1; ++j) { diag[j] = wa2[j]; if (wa2[j] == 0.) { diag[j] = 1.; } /* L140: */ } L150: /* on the first iteration, calculate the norm of the scaled x */ /* and initialize the step bound delta. */ i__1 = *n; for (j = 1; j <= i__1; ++j) { wa3[j] = diag[j] * x[j]; /* L160: */ } xnorm = enorm_(n, &wa3[1]); delta = *factor * xnorm; if (delta == 0.) { delta = *factor; } L170: /* compute the norm of the scaled gradient. */ gnorm = 0.; if (fnorm == 0.) { goto L210; } i__1 = *n; for (j = 1; j <= i__1; ++j) { l = ipvt[j]; if (wa2[l] == 0.) { goto L190; } sum = 0.; i__2 = j; for (i__ = 1; i__ <= i__2; ++i__) { sum += fjac[i__ + j * fjac_dim1] * (qtf[i__] / fnorm); /* L180: */ } /* Computing MAX */ d__2 = gnorm, d__3 = (d__1 = sum / wa2[l], abs(d__1)); gnorm = max(d__2,d__3); L190: /* L200: */ ; } L210: /* test for convergence of the gradient norm. */ if (gnorm <= *gtol) { *info = 4; } if (*info != 0) { goto L340; } /* rescale if necessary. */ if (*mode == 2) { goto L230; } i__1 = *n; for (j = 1; j <= i__1; ++j) { /* Computing MAX */ d__1 = diag[j], d__2 = wa2[j]; diag[j] = max(d__1,d__2); /* L220: */ } L230: /* beginning of the inner loop. */ L240: /* determine the levenberg-marquardt parameter. */ lmpar_(n, &fjac[fjac_offset], ldfjac, &ipvt[1], &diag[1], &qtf[1], &delta, &par, &wa1[1], &wa2[1], &wa3[1], &wa4[1]); /* store the direction p and x + p. calculate the norm of p. */ i__1 = *n; for (j = 1; j <= i__1; ++j) { wa1[j] = -wa1[j]; wa2[j] = x[j] + wa1[j]; wa3[j] = diag[j] * wa1[j]; /* L250: */ } pnorm = enorm_(n, &wa3[1]); /* on the first iteration, adjust the initial step bound. */ if (iter == 1) { delta = min(delta,pnorm); } /* evaluate the function at x + p and calculate its norm. */ iflag = 1; (*fcn)(m, n, &wa2[1], &wa4[1], &wa3[1], &iflag); ++(*nfev); if (iflag < 0) { goto L340; } fnorm1 = enorm_(m, &wa4[1]); /* compute the scaled actual reduction. */ actred = -1.; if (p1 * fnorm1 < fnorm) { /* Computing 2nd power */ d__1 = fnorm1 / fnorm; actred = 1. - d__1 * d__1; } /* compute the scaled predicted reduction and */ /* the scaled directional derivative. */ i__1 = *n; for (j = 1; j <= i__1; ++j) { wa3[j] = 0.; l = ipvt[j]; temp = wa1[l]; i__2 = j; for (i__ = 1; i__ <= i__2; ++i__) { wa3[i__] += fjac[i__ + j * fjac_dim1] * temp; /* L260: */ } /* L270: */ } temp1 = enorm_(n, &wa3[1]) / fnorm; temp2 = sqrt(par) * pnorm / fnorm; /* Computing 2nd power */ d__1 = temp1; /* Computing 2nd power */ d__2 = temp2; prered = d__1 * d__1 + d__2 * d__2 / p5; /* Computing 2nd power */ d__1 = temp1; /* Computing 2nd power */ d__2 = temp2; dirder = -(d__1 * d__1 + d__2 * d__2); /* compute the ratio of the actual to the predicted */ /* reduction. */ ratio = 0.; if (prered != 0.) { ratio = actred / prered; } /* update the step bound. */ if (ratio > p25) { goto L280; } if (actred >= 0.) { temp = p5; } if (actred < 0.) { temp = p5 * dirder / (dirder + p5 * actred); } if (p1 * fnorm1 >= fnorm || temp < p1) { temp = p1; } /* Computing MIN */ d__1 = delta, d__2 = pnorm / p1; delta = temp * min(d__1,d__2); par /= temp; goto L300; L280: if (par != 0. && ratio < p75) { goto L290; } delta = pnorm / p5; par = p5 * par; L290: L300: /* test for successful iteration. */ if (ratio < p0001) { goto L330; } /* successful iteration. update x, fvec, and their norms. */ i__1 = *n; for (j = 1; j <= i__1; ++j) { x[j] = wa2[j]; wa2[j] = diag[j] * x[j]; /* L310: */ } i__1 = *m; for (i__ = 1; i__ <= i__1; ++i__) { fvec[i__] = wa4[i__]; /* L320: */ } xnorm = enorm_(n, &wa2[1]); fnorm = fnorm1; ++iter; L330: /* tests for convergence. */ if (abs(actred) <= *ftol && prered <= *ftol && p5 * ratio <= 1.) { *info = 1; } if (delta <= *xtol * xnorm) { *info = 2; } if (abs(actred) <= *ftol && prered <= *ftol && p5 * ratio <= 1. && *info == 2) { *info = 3; } if (*info != 0) { goto L340; } /* tests for termination and stringent tolerances. */ if (*nfev >= *maxfev) { *info = 5; } if (abs(actred) <= epsmch && prered <= epsmch && p5 * ratio <= 1.) { *info = 6; } if (delta <= epsmch * xnorm) { *info = 7; } if (gnorm <= epsmch) { *info = 8; } if (*info != 0) { goto L340; } /* end of the inner loop. repeat if iteration unsuccessful. */ if (ratio < p0001) { goto L240; } /* end of the outer loop. */ goto L30; L340: /* termination, either normal or user imposed. */ if (iflag < 0) { *info = iflag; } iflag = 0; if (*nprint > 0) { (*fcn)(m, n, &x[1], &fvec[1], &wa3[1], &iflag); } return; /* last card of subroutine lmstr. */ } /* lmstr_ */
/* Subroutine */ int lmpar_(integer *n, doublereal *r__, integer *ldr, integer *ipvt, doublereal *diag, doublereal *qtb, doublereal *delta, doublereal *par, doublereal *x, doublereal *sdiag, doublereal *wa1, doublereal *wa2) { /* Initialized data */ static doublereal p1 = .1; static doublereal p001 = .001; static doublereal zero = 0.; /* System generated locals */ integer r_dim1, r_offset, i__1, i__2; doublereal d__1, d__2; /* Builtin functions */ double sqrt(doublereal); /* Local variables */ static integer i__, j, k, l; static doublereal fp; static integer jm1, jp1; static doublereal sum, parc, parl; static integer iter; static doublereal temp, paru, dwarf; static integer nsing; extern doublereal enorm_(integer *, doublereal *); static doublereal gnorm; extern doublereal dpmpar_(integer *); static doublereal dxnorm; extern /* Subroutine */ int qrsolv_(integer *, doublereal *, integer *, integer *, doublereal *, doublereal *, doublereal *, doublereal *, doublereal *); /* ********** */ /* subroutine lmpar */ /* given an m by n matrix a, an n by n nonsingular diagonal */ /* matrix d, an m-vector b, and a positive number delta, */ /* the problem is to determine a value for the parameter */ /* par such that if x solves the system */ /* a*x = b , sqrt(par)*d*x = 0 , */ /* in the least squares sense, and dxnorm is the euclidean */ /* norm of d*x, then either par is zero and */ /* (dxnorm-delta) .le. 0.1*delta , */ /* or par is positive and */ /* abs(dxnorm-delta) .le. 0.1*delta . */ /* this subroutine completes the solution of the problem */ /* if it is provided with the necessary information from the */ /* qr factorization, with column pivoting, of a. that is, if */ /* a*p = q*r, where p is a permutation matrix, q has orthogonal */ /* columns, and r is an upper triangular matrix with diagonal */ /* elements of nonincreasing magnitude, then lmpar expects */ /* the full upper triangle of r, the permutation matrix p, */ /* and the first n components of (q transpose)*b. on output */ /* lmpar also provides an upper triangular matrix s such that */ /* t t t */ /* p *(a *a + par*d*d)*p = s *s . */ /* s is employed within lmpar and may be of separate interest. */ /* only a few iterations are generally needed for convergence */ /* of the algorithm. if, however, the limit of 10 iterations */ /* is reached, then the output par will contain the best */ /* value obtained so far. */ /* the subroutine statement is */ /* subroutine lmpar(n,r,ldr,ipvt,diag,qtb,delta,par,x,sdiag, */ /* wa1,wa2) */ /* where */ /* n is a positive integer input variable set to the order of r. */ /* r is an n by n array. on input the full upper triangle */ /* must contain the full upper triangle of the matrix r. */ /* on output the full upper triangle is unaltered, and the */ /* strict lower triangle contains the strict upper triangle */ /* (transposed) of the upper triangular matrix s. */ /* ldr is a positive integer input variable not less than n */ /* which specifies the leading dimension of the array r. */ /* ipvt is an integer input array of length n which defines the */ /* permutation matrix p such that a*p = q*r. column j of p */ /* is column ipvt(j) of the identity matrix. */ /* diag is an input array of length n which must contain the */ /* diagonal elements of the matrix d. */ /* qtb is an input array of length n which must contain the first */ /* n elements of the vector (q transpose)*b. */ /* delta is a positive input variable which specifies an upper */ /* bound on the euclidean norm of d*x. */ /* par is a nonnegative variable. on input par contains an */ /* initial estimate of the levenberg-marquardt parameter. */ /* on output par contains the final estimate. */ /* x is an output array of length n which contains the least */ /* squares solution of the system a*x = b, sqrt(par)*d*x = 0, */ /* for the output par. */ /* sdiag is an output array of length n which contains the */ /* diagonal elements of the upper triangular matrix s. */ /* wa1 and wa2 are work arrays of length n. */ /* subprograms called */ /* minpack-supplied ... dpmpar,enorm,qrsolv */ /* fortran-supplied ... dabs,dmax1,dmin1,dsqrt */ /* argonne national laboratory. minpack project. march 1980. */ /* burton s. garbow, kenneth e. hillstrom, jorge j. more */ /* ********** */ /* Parameter adjustments */ --wa2; --wa1; --sdiag; --x; --qtb; --diag; --ipvt; r_dim1 = *ldr; r_offset = r_dim1 + 1; r__ -= r_offset; /* Function Body */ /* dwarf is the smallest positive magnitude. */ dwarf = dpmpar_(&c__2); /* compute and store in x the gauss-newton direction. if the */ /* jacobian is rank-deficient, obtain a least squares solution. */ nsing = *n; i__1 = *n; for (j = 1; j <= i__1; ++j) { wa1[j] = qtb[j]; if (r__[j + j * r_dim1] == zero && nsing == *n) { nsing = j - 1; } if (nsing < *n) { wa1[j] = zero; } /* L10: */ } if (nsing < 1) { goto L50; } i__1 = nsing; for (k = 1; k <= i__1; ++k) { j = nsing - k + 1; wa1[j] /= r__[j + j * r_dim1]; temp = wa1[j]; jm1 = j - 1; if (jm1 < 1) { goto L30; } i__2 = jm1; for (i__ = 1; i__ <= i__2; ++i__) { wa1[i__] -= r__[i__ + j * r_dim1] * temp; /* L20: */ } L30: /* L40: */ ; } L50: i__1 = *n; for (j = 1; j <= i__1; ++j) { l = ipvt[j]; x[l] = wa1[j]; /* L60: */ } /* initialize the iteration counter. */ /* evaluate the function at the origin, and test */ /* for acceptance of the gauss-newton direction. */ iter = 0; i__1 = *n; for (j = 1; j <= i__1; ++j) { wa2[j] = diag[j] * x[j]; /* L70: */ } dxnorm = enorm_(n, &wa2[1]); fp = dxnorm - *delta; if (fp <= p1 * *delta) { goto L220; } /* if the jacobian is not rank deficient, the newton */ /* step provides a lower bound, parl, for the zero of */ /* the function. otherwise set this bound to zero. */ parl = zero; if (nsing < *n) { goto L120; } i__1 = *n; for (j = 1; j <= i__1; ++j) { l = ipvt[j]; wa1[j] = diag[l] * (wa2[l] / dxnorm); /* L80: */ } i__1 = *n; for (j = 1; j <= i__1; ++j) { sum = zero; jm1 = j - 1; if (jm1 < 1) { goto L100; } i__2 = jm1; for (i__ = 1; i__ <= i__2; ++i__) { sum += r__[i__ + j * r_dim1] * wa1[i__]; /* L90: */ } L100: wa1[j] = (wa1[j] - sum) / r__[j + j * r_dim1]; /* L110: */ } temp = enorm_(n, &wa1[1]); parl = fp / *delta / temp / temp; L120: /* calculate an upper bound, paru, for the zero of the function. */ i__1 = *n; for (j = 1; j <= i__1; ++j) { sum = zero; i__2 = j; for (i__ = 1; i__ <= i__2; ++i__) { sum += r__[i__ + j * r_dim1] * qtb[i__]; /* L130: */ } l = ipvt[j]; wa1[j] = sum / diag[l]; /* L140: */ } gnorm = enorm_(n, &wa1[1]); paru = gnorm / *delta; if (paru == zero) { paru = dwarf / min(*delta,p1); } /* if the input par lies outside of the interval (parl,paru), */ /* set par to the closer endpoint. */ *par = max(*par,parl); *par = min(*par,paru); if (*par == zero) { *par = gnorm / dxnorm; } /* beginning of an iteration. */ L150: ++iter; /* evaluate the function at the current value of par. */ if (*par == zero) { /* Computing MAX */ d__1 = dwarf, d__2 = p001 * paru; *par = max(d__1,d__2); } temp = sqrt(*par); i__1 = *n; for (j = 1; j <= i__1; ++j) { wa1[j] = temp * diag[j]; /* L160: */ } qrsolv_(n, &r__[r_offset], ldr, &ipvt[1], &wa1[1], &qtb[1], &x[1], &sdiag[ 1], &wa2[1]); i__1 = *n; for (j = 1; j <= i__1; ++j) { wa2[j] = diag[j] * x[j]; /* L170: */ } dxnorm = enorm_(n, &wa2[1]); temp = fp; fp = dxnorm - *delta; /* if the function is small enough, accept the current value */ /* of par. also test for the exceptional cases where parl */ /* is zero or the number of iterations has reached 10. */ if (abs(fp) <= p1 * *delta || (parl == zero && fp <= temp && temp < zero) || iter == 10) { goto L220; } /* compute the newton correction. */ i__1 = *n; for (j = 1; j <= i__1; ++j) { l = ipvt[j]; wa1[j] = diag[l] * (wa2[l] / dxnorm); /* L180: */ } i__1 = *n; for (j = 1; j <= i__1; ++j) { wa1[j] /= sdiag[j]; temp = wa1[j]; jp1 = j + 1; if (*n < jp1) { goto L200; } i__2 = *n; for (i__ = jp1; i__ <= i__2; ++i__) { wa1[i__] -= r__[i__ + j * r_dim1] * temp; /* L190: */ } L200: /* L210: */ ; } temp = enorm_(n, &wa1[1]); parc = fp / *delta / temp / temp; /* depending on the sign of the function, update parl or paru. */ if (fp > zero) { parl = max(parl,*par); } if (fp < zero) { paru = min(paru,*par); } /* compute an improved estimate for par. */ /* Computing MAX */ d__1 = parl, d__2 = *par + parc; *par = max(d__1,d__2); /* end of an iteration. */ goto L150; L220: /* termination. */ if (iter == 0) { *par = zero; } return 0; /* last card of subroutine lmpar. */ } /* lmpar_ */
/* DECK SCOV */ /* Subroutine */ int scov_(S_fp fcn, integer *iopt, integer *m, integer *n, real *x, real *fvec, real *r__, integer *ldr, integer *info, real * wa1, real *wa2, real *wa3, real *wa4) { /* Initialized data */ static real zero = 0.f; static real one = 1.f; /* System generated locals */ integer r_dim1, r_offset, i__1, i__2, i__3; /* Local variables */ static integer i__, j, k, kp1, nm1, idum; static logical sing; static real temp; static integer nrow, iflag; extern /* Subroutine */ int qrfac_(integer *, integer *, real *, integer * , logical *, integer *, integer *, real *, real *, real *); static real sigma; extern doublereal enorm_(integer *, real *); extern /* Subroutine */ int fdjac3_(S_fp, integer *, integer *, real *, real *, real *, integer *, integer *, real *, real *), xermsg_( char *, char *, char *, integer *, integer *, ftnlen, ftnlen, ftnlen), rwupdt_(integer *, real *, integer *, real *, real *, real *, real *, real *); /* ***BEGIN PROLOGUE SCOV */ /* ***PURPOSE Calculate the covariance matrix for a nonlinear data */ /* fitting problem. It is intended to be used after a */ /* successful return from either SNLS1 or SNLS1E. */ /* ***LIBRARY SLATEC */ /* ***CATEGORY K1B1 */ /* ***TYPE SINGLE PRECISION (SCOV-S, DCOV-D) */ /* ***KEYWORDS COVARIANCE MATRIX, NONLINEAR DATA FITTING, */ /* NONLINEAR LEAST SQUARES */ /* ***AUTHOR Hiebert, K. L., (SNLA) */ /* ***DESCRIPTION */ /* 1. Purpose. */ /* SCOV calculates the covariance matrix for a nonlinear data */ /* fitting problem. It is intended to be used after a */ /* successful return from either SNLS1 or SNLS1E. SCOV */ /* and SNLS1 (and SNLS1E) have compatible parameters. The */ /* required external subroutine, FCN, is the same */ /* for all three codes, SCOV, SNLS1, and SNLS1E. */ /* 2. Subroutine and Type Statements. */ /* SUBROUTINE SCOV(FCN,IOPT,M,N,X,FVEC,R,LDR,INFO, */ /* WA1,WA2,WA3,WA4) */ /* INTEGER IOPT,M,N,LDR,INFO */ /* REAL X(N),FVEC(M),R(LDR,N),WA1(N),WA2(N),WA3(N),WA4(M) */ /* EXTERNAL FCN */ /* 3. Parameters. */ /* FCN is the name of the user-supplied subroutine which calculates */ /* the functions. If the user wants to supply the Jacobian */ /* (IOPT=2 or 3), then FCN must be written to calculate the */ /* Jacobian, as well as the functions. See the explanation */ /* of the IOPT argument below. FCN must be declared in an */ /* EXTERNAL statement in the calling program and should be */ /* written as follows. */ /* SUBROUTINE FCN(IFLAG,M,N,X,FVEC,FJAC,LDFJAC) */ /* INTEGER IFLAG,LDFJAC,M,N */ /* REAL X(N),FVEC(M) */ /* ---------- */ /* FJAC and LDFJAC may be ignored , if IOPT=1. */ /* REAL FJAC(LDFJAC,N) , if IOPT=2. */ /* REAL FJAC(N) , if IOPT=3. */ /* ---------- */ /* IFLAG will never be zero when FCN is called by SCOV. */ /* RETURN */ /* ---------- */ /* If IFLAG=1, calculate the functions at X and return */ /* this vector in FVEC. */ /* RETURN */ /* ---------- */ /* If IFLAG=2, calculate the full Jacobian at X and return */ /* this matrix in FJAC. Note that IFLAG will never be 2 unless */ /* IOPT=2. FVEC contains the function values at X and must */ /* not be altered. FJAC(I,J) must be set to the derivative */ /* of FVEC(I) with respect to X(J). */ /* RETURN */ /* ---------- */ /* If IFLAG=3, calculate the LDFJAC-th row of the Jacobian */ /* and return this vector in FJAC. Note that IFLAG will */ /* never be 3 unless IOPT=3. FJAC(J) must be set to */ /* the derivative of FVEC(LDFJAC) with respect to X(J). */ /* RETURN */ /* ---------- */ /* END */ /* The value of IFLAG should not be changed by FCN unless the */ /* user wants to terminate execution of SCOV. In this case, set */ /* IFLAG to a negative integer. */ /* IOPT is an input variable which specifies how the Jacobian will */ /* be calculated. If IOPT=2 or 3, then the user must supply the */ /* Jacobian, as well as the function values, through the */ /* subroutine FCN. If IOPT=2, the user supplies the full */ /* Jacobian with one call to FCN. If IOPT=3, the user supplies */ /* one row of the Jacobian with each call. (In this manner, */ /* storage can be saved because the full Jacobian is not stored.) */ /* If IOPT=1, the code will approximate the Jacobian by forward */ /* differencing. */ /* M is a positive integer input variable set to the number of */ /* functions. */ /* N is a positive integer input variable set to the number of */ /* variables. N must not exceed M. */ /* X is an array of length N. On input X must contain the value */ /* at which the covariance matrix is to be evaluated. This is */ /* usually the value for X returned from a successful run of */ /* SNLS1 (or SNLS1E). The value of X will not be changed. */ /* FVEC is an output array of length M which contains the functions */ /* evaluated at X. */ /* R is an output array. For IOPT=1 and 2, R is an M by N array. */ /* For IOPT=3, R is an N by N array. On output, if INFO=1, */ /* the upper N by N submatrix of R contains the covariance */ /* matrix evaluated at X. */ /* LDR is a positive integer input variable which specifies */ /* the leading dimension of the array R. For IOPT=1 and 2, */ /* LDR must not be less than M. For IOPT=3, LDR must not */ /* be less than N. */ /* INFO is an integer output variable. If the user has terminated */ /* execution, INFO is set to the (negative) value of IFLAG. See */ /* description of FCN. Otherwise, INFO is set as follows. */ /* INFO = 0 Improper input parameters (M.LE.0 or N.LE.0). */ /* INFO = 1 Successful return. The covariance matrix has been */ /* calculated and stored in the upper N by N */ /* submatrix of R. */ /* INFO = 2 The Jacobian matrix is singular for the input value */ /* of X. The covariance matrix cannot be calculated. */ /* The upper N by N submatrix of R contains the QR */ /* factorization of the Jacobian (probably not of */ /* interest to the user). */ /* WA1 is a work array of length N. */ /* WA2 is a work array of length N. */ /* WA3 is a work array of length N. */ /* WA4 is a work array of length M. */ /* ***REFERENCES (NONE) */ /* ***ROUTINES CALLED ENORM, FDJAC3, QRFAC, RWUPDT, XERMSG */ /* ***REVISION HISTORY (YYMMDD) */ /* 810522 DATE WRITTEN */ /* 890505 REVISION DATE from Version 3.2 */ /* 891214 Prologue converted to Version 4.0 format. (BAB) */ /* 900315 CALLs to XERROR changed to CALLs to XERMSG. (THJ) */ /* 900510 Fixed an error message. (RWC) */ /* ***END PROLOGUE SCOV */ /* REVISED 820707-1100 */ /* REVISED YYMMDD HHMM */ /* Parameter adjustments */ --x; --fvec; r_dim1 = *ldr; r_offset = 1 + r_dim1; r__ -= r_offset; --wa1; --wa2; --wa3; --wa4; /* Function Body */ /* ***FIRST EXECUTABLE STATEMENT SCOV */ sing = FALSE_; iflag = 0; if (*m <= 0 || *n <= 0) { goto L300; } /* CALCULATE SIGMA = (SUM OF THE SQUARED RESIDUALS) / (M-N) */ iflag = 1; (*fcn)(&iflag, m, n, &x[1], &fvec[1], &r__[r_offset], ldr); if (iflag < 0) { goto L300; } temp = enorm_(m, &fvec[1]); sigma = one; if (*m != *n) { sigma = temp * temp / (*m - *n); } /* CALCULATE THE JACOBIAN */ if (*iopt == 3) { goto L200; } /* STORE THE FULL JACOBIAN USING M*N STORAGE */ if (*iopt == 1) { goto L100; } /* USER SUPPLIES THE JACOBIAN */ iflag = 2; (*fcn)(&iflag, m, n, &x[1], &fvec[1], &r__[r_offset], ldr); goto L110; /* CODE APPROXIMATES THE JACOBIAN */ L100: fdjac3_((S_fp)fcn, m, n, &x[1], &fvec[1], &r__[r_offset], ldr, &iflag, & zero, &wa4[1]); L110: if (iflag < 0) { goto L300; } /* COMPUTE THE QR DECOMPOSITION */ qrfac_(m, n, &r__[r_offset], ldr, &c_false, &idum, &c__1, &wa1[1], &wa1[1] , &wa1[1]); i__1 = *n; for (i__ = 1; i__ <= i__1; ++i__) { /* L120: */ r__[i__ + i__ * r_dim1] = wa1[i__]; } goto L225; /* COMPUTE THE QR FACTORIZATION OF THE JACOBIAN MATRIX CALCULATED ONE */ /* ROW AT A TIME AND STORED IN THE UPPER TRIANGLE OF R. */ /* ( (Q TRANSPOSE)*FVEC IS ALSO CALCULATED BUT NOT USED.) */ L200: i__1 = *n; for (j = 1; j <= i__1; ++j) { wa2[j] = zero; i__2 = *n; for (i__ = 1; i__ <= i__2; ++i__) { r__[i__ + j * r_dim1] = zero; /* L205: */ } /* L210: */ } iflag = 3; i__1 = *m; for (i__ = 1; i__ <= i__1; ++i__) { nrow = i__; (*fcn)(&iflag, m, n, &x[1], &fvec[1], &wa1[1], &nrow); if (iflag < 0) { goto L300; } temp = fvec[i__]; rwupdt_(n, &r__[r_offset], ldr, &wa1[1], &wa2[1], &temp, &wa3[1], & wa4[1]); /* L220: */ } /* CHECK IF R IS SINGULAR. */ L225: i__1 = *n; for (i__ = 1; i__ <= i__1; ++i__) { if (r__[i__ + i__ * r_dim1] == zero) { sing = TRUE_; } /* L230: */ } if (sing) { goto L300; } /* R IS UPPER TRIANGULAR. CALCULATE (R TRANSPOSE) INVERSE AND STORE */ /* IN THE UPPER TRIANGLE OF R. */ if (*n == 1) { goto L275; } nm1 = *n - 1; i__1 = nm1; for (k = 1; k <= i__1; ++k) { /* INITIALIZE THE RIGHT-HAND SIDE (WA1(*)) AS THE K-TH COLUMN OF THE */ /* IDENTITY MATRIX. */ i__2 = *n; for (i__ = 1; i__ <= i__2; ++i__) { wa1[i__] = zero; /* L240: */ } wa1[k] = one; r__[k + k * r_dim1] = wa1[k] / r__[k + k * r_dim1]; kp1 = k + 1; i__2 = *n; for (i__ = kp1; i__ <= i__2; ++i__) { /* SUBTRACT R(K,I-1)*R(I-1,*) FROM THE RIGHT-HAND SIDE, WA1(*). */ i__3 = *n; for (j = i__; j <= i__3; ++j) { wa1[j] -= r__[k + (i__ - 1) * r_dim1] * r__[i__ - 1 + j * r_dim1]; /* L250: */ } r__[k + i__ * r_dim1] = wa1[i__] / r__[i__ + i__ * r_dim1]; /* L260: */ } /* L270: */ } L275: r__[*n + *n * r_dim1] = one / r__[*n + *n * r_dim1]; /* CALCULATE R-INVERSE * (R TRANSPOSE) INVERSE AND STORE IN THE UPPER */ /* TRIANGLE OF R. */ i__1 = *n; for (i__ = 1; i__ <= i__1; ++i__) { i__2 = *n; for (j = i__; j <= i__2; ++j) { temp = zero; i__3 = *n; for (k = j; k <= i__3; ++k) { temp += r__[i__ + k * r_dim1] * r__[j + k * r_dim1]; /* L280: */ } r__[i__ + j * r_dim1] = temp * sigma; /* L290: */ } } *info = 1; L300: if (*m <= 0 || *n <= 0) { *info = 0; } if (iflag < 0) { *info = iflag; } if (sing) { *info = 2; } if (*info < 0) { xermsg_("SLATEC", "SCOV", "EXECUTION TERMINATED BECAUSE USER SET IFL" "AG NEGATIVE.", &c__1, &c__1, (ftnlen)6, (ftnlen)4, (ftnlen)53) ; } if (*info == 0) { xermsg_("SLATEC", "SCOV", "INVALID INPUT PARAMETER.", &c__2, &c__1, ( ftnlen)6, (ftnlen)4, (ftnlen)24); } if (*info == 2) { xermsg_("SLATEC", "SCOV", "SINGULAR JACOBIAN MATRIX, COVARIANCE MATR" "IX CANNOT BE CALCULATED.", &c__1, &c__1, (ftnlen)6, (ftnlen)4, (ftnlen)65); } return 0; } /* scov_ */
int qrfac_(int m, int n, double *a, int lda, int *ipvt, double *rdiag, double *acnorm, double *wa) { const double p05 = .05; const double epsmch = DBL_EPSILON; double ajnorm, d, sum, temp; int kmax, minmn, jp1; int i, j, k; /* compute the initial column norms and initialize several arrays */ for (j = 0; j < n; ++j) { acnorm[j] = enorm_(m, a + j * lda); rdiag[j] = acnorm[j]; wa[j] = rdiag[j]; ipvt[j] = j; } /* reduce a to r with Householder transformations */ minmn = min(m, n); for (j = 0; j < minmn; ++j) { /* bring the column of largest norm into the pivot position */ kmax = j; for (k = j; k < n; ++k) { if (rdiag[k] > rdiag[kmax]) { kmax = k; } } if (kmax != j) { for (i = 0; i < m; ++i) { temp = a[i + j * lda]; a[i + j * lda] = a[i + kmax * lda]; a[i + kmax * lda] = temp; } rdiag[kmax] = rdiag[j]; wa[kmax] = wa[j]; k = ipvt[j]; ipvt[j] = ipvt[kmax]; ipvt[kmax] = k; } /* compute the Householder transformation to reduce the j-th column of a to a multiple of the j-th unit vector */ ajnorm = enorm_(m - j, &a[j + j * lda]); if (ajnorm == 0.0) { rdiag[j] = -ajnorm; continue; } if (a[j + j * lda] < 0.0) { ajnorm = -ajnorm; } for (i = j; i < m; ++i) { a[i + j * lda] /= ajnorm; } a[j + j * lda] += 1.0; /* apply the transformation to the remaining columns and update the norms */ jp1 = j + 1; for (k = jp1; k < n; ++k) { sum = 0.0; for (i = j; i < m; ++i) { sum += a[i + j * lda] * a[i + k * lda]; } temp = sum / a[j + j * lda]; for (i = j; i < m; ++i) { a[i + k * lda] -= temp * a[i + j * lda]; } if (rdiag[k] != 0.0) { temp = a[j + k * lda] / rdiag[k]; d = 1.0 - temp * temp; if (d > 0) { rdiag[k] *= sqrt(d); } else { rdiag[k] = 0.0; } d = rdiag[k] / wa[k]; if (p05 * (d * d) <= epsmch) { rdiag[k] = enorm_(m - jp1, &a[jp1 + k * lda]); wa[k] = rdiag[k]; } } } rdiag[j] = -ajnorm; } return 0; }
/*! \fn LineSearch * * third damping heuristic: * Along the tangent 5 five points are selected. For every point the Euclidean norm of * the residual function will be calculated and the minimum is chosen for the further iteration. * * compiler flag: -newton = damped_ls */ void LineSearch(double* x, int(*f)(int*, double*, double*, void*, int), double current_fvec_enorm, int* n, double* fvec, int* k, DATA_NEWTON* solverData, void* userdata) { int i,j; double enorm_new, enorm_minimum=current_fvec_enorm, lambda_minimum=0; double lambda[5]={1.25,1,0.75,0.5,0.25}; for (j=0; j<5; j++) { for (i=0; i<*n; i++) solverData->x_new[i]=x[i]-lambda[j]*solverData->x_increment[i]; /* calculate new function values */ (*f)(n, solverData->x_new, fvec, userdata, 1); solverData->nfev++; enorm_new=enorm_(n,fvec); /* searching minimal enorm */ if (enorm_new < enorm_minimum) { enorm_minimum = enorm_new; lambda_minimum = lambda[j]; memcpy(solverData->fvec_minimum, fvec,*n*sizeof(double)); } } infoStreamPrint(LOG_NLS_V,0,"lambda_minimum = %e", lambda_minimum); if (lambda_minimum == 0) { warningStreamPrint(LOG_NLS_V, 0, "Warning: lambda_minimum = 0 "); /* if damping is without success, trying full newton step; after 5 full newton steps try a very little step */ if (*k >= 5) { lambda_minimum = 0.125; /* calculate new function values */ (*f)(n, solverData->x_new, fvec, userdata, 1); solverData->nfev++; } else { lambda_minimum = 1; /* calculate new function values */ (*f)(n, solverData->x_new, fvec, userdata, 1); solverData->nfev++; } (*k)++; } else { /* save new function values */ memcpy(fvec, solverData->fvec_minimum, *n*sizeof(double)); } for (i=0; i<*n; i++) solverData->x_new[i]=x[i]-lambda_minimum*solverData->x_increment[i]; }
/*! \fn solve system with Newton-Raphson * * \param [in] [n] size of equation * [eps] tolerance for x * [h] tolerance for f' * [k] maximum number of iterations * [work] work array size of (n*X) * [f] user provided function * [data] userdata * [info] * [calculate_jacobian] flag which decides whether Jacobian is calculated * (0) once for the first calculation * (i) every i steps (=1 means original newton method) * (-1) never, factorization has to be given in A * */ int _omc_newton(int(*f)(int*, double*, double*, void*, int), DATA_NEWTON* solverData, void* userdata) { int i, j, k = 0, l = 0, nrsh = 1; int *n = &(solverData->n); double *x = solverData->x; double *fvec = solverData->fvec; double *eps = &(solverData->ftol); double *fdeps = &(solverData->epsfcn); int * maxfev = &(solverData->maxfev); double *fjac = solverData->fjac; double *work = solverData->rwork; int *iwork = solverData->iwork; int *info = &(solverData->info); int calc_jac = 1; double error_f = 1.0 + *eps, scaledError_f = 1.0 + *eps, delta_x = 1.0 + *eps, delta_f = 1.0 + *eps, delta_x_scaled = 1.0 + *eps, lambda = 1.0; double current_fvec_enorm, enorm_new; if(ACTIVE_STREAM(LOG_NLS_V)) { infoStreamPrint(LOG_NLS_V, 1, "######### Start Newton maxfev: %d #########", (int)*maxfev); infoStreamPrint(LOG_NLS_V, 1, "x vector"); for(i=0; i<*n; i++) infoStreamPrint(LOG_NLS_V, 0, "x[%d]: %e ", i, x[i]); messageClose(LOG_NLS_V); messageClose(LOG_NLS_V); } *info = 1; /* calculate the function values */ (*f)(n, x, fvec, userdata, 1); solverData->nfev++; /* save current fvec in f_old*/ memcpy(solverData->f_old, fvec, *n*sizeof(double)); error_f = current_fvec_enorm = enorm_(n, fvec); while(error_f > *eps && scaledError_f > *eps && delta_x > *eps && delta_f > *eps && delta_x_scaled > *eps) { if(ACTIVE_STREAM(LOG_NLS_V)) { infoStreamPrint(LOG_NLS_V, 0, "\n**** start Iteration: %d *****", (int) l); /* Debug output */ infoStreamPrint(LOG_NLS_V, 1, "function values"); for(i=0; i<*n; i++) infoStreamPrint(LOG_NLS_V, 0, "fvec[%d]: %e ", i, fvec[i]); messageClose(LOG_NLS_V); } /* calculate jacobian if no matrix is given */ if (calc_jac == 1 && solverData->calculate_jacobian >= 0) { (*f)(n, x, fvec, userdata, 0); solverData->factorization = 0; calc_jac = solverData->calculate_jacobian; } else { solverData->factorization = 1; calc_jac--; } /* debug output */ if(ACTIVE_STREAM(LOG_NLS_JAC)) { char buffer[4096]; infoStreamPrint(LOG_NLS_JAC, 1, "jacobian matrix [%dx%d]", (int)*n, (int)*n); for(i=0; i<solverData->n;i++) { buffer[0] = 0; for(j=0; j<solverData->n; j++) sprintf(buffer, "%s%10g ", buffer, fjac[i*(*n)+j]); infoStreamPrint(LOG_NLS_JAC, 0, "%s", buffer); } messageClose(LOG_NLS_JAC); } if (solveLinearSystem(n, iwork, fvec, fjac, solverData) != 0) { *info=-1; break; } else { for (i =0; i<*n; i++) solverData->x_new[i]=x[i]-solverData->x_increment[i]; infoStreamPrint(LOG_NLS_V,1,"x_increment"); for(i=0; i<*n; i++) infoStreamPrint(LOG_NLS_V, 0, "x_increment[%d] = %e ", i, solverData->x_increment[i]); messageClose(LOG_NLS_V); if (solverData->newtonStrategy == NEWTON_DAMPED) { damping_heuristic(x, f, current_fvec_enorm, n, fvec, &lambda, &k, solverData, userdata); } else if (solverData->newtonStrategy == NEWTON_DAMPED2) { damping_heuristic2(0.75, x, f, current_fvec_enorm, n, fvec, &k, solverData, userdata); } else if (solverData->newtonStrategy == NEWTON_DAMPED_LS) { LineSearch(x, f, current_fvec_enorm, n, fvec, &k, solverData, userdata); } else if (solverData->newtonStrategy == NEWTON_DAMPED_BT) { Backtracking(x, f, current_fvec_enorm, n, fvec, solverData, userdata); } else { /* calculate the function values */ (*f)(n, solverData->x_new, fvec, userdata, 1); solverData->nfev++; } calculatingErrors(solverData, &delta_x, &delta_x_scaled, &delta_f, &error_f, &scaledError_f, n, x, fvec); /* updating x */ memcpy(x, solverData->x_new, *n*sizeof(double)); /* updating f_old */ memcpy(solverData->f_old, fvec, *n*sizeof(double)); current_fvec_enorm = error_f; /* check if maximum iteration is reached */ if (++l > *maxfev) { *info = -1; warningStreamPrint(LOG_NLS_V, 0, "Warning: maximal number of iteration reached but no root found"); break; } } if(ACTIVE_STREAM(LOG_NLS_V)) { infoStreamPrint(LOG_NLS_V,1,"x vector"); for(i=0; i<*n; i++) infoStreamPrint(LOG_NLS_V, 0, "x[%d] = %e ", i, x[i]); messageClose(LOG_NLS_V); printErrors(delta_x, delta_x_scaled, delta_f, error_f, scaledError_f, eps); } } solverData->numberOfIterations += l; solverData->numberOfFunctionEvaluations += solverData->nfev; return 0; }
/*< >*/ /* Subroutine */ int lmder_( void (*fcn)(v3p_netlib_integer*, v3p_netlib_integer*, v3p_netlib_doublereal*, v3p_netlib_doublereal*, v3p_netlib_doublereal*, v3p_netlib_integer*, v3p_netlib_integer*, void*), integer *m, integer *n, doublereal *x, doublereal *fvec, doublereal *fjac, integer *ldfjac, doublereal *ftol, doublereal *xtol, doublereal *gtol, integer *maxfev, doublereal * diag, integer *mode, doublereal *factor, integer *nprint, integer * info, integer *nfev, integer *njev, integer *ipvt, doublereal *qtf, doublereal *wa1, doublereal *wa2, doublereal *wa3, doublereal *wa4, void* userdata) { /* Initialized data */ static doublereal one = 1.; /* constant */ static doublereal p1 = .1; /* constant */ static doublereal p5 = .5; /* constant */ static doublereal p25 = .25; /* constant */ static doublereal p75 = .75; /* constant */ static doublereal p0001 = 1e-4; /* constant */ static doublereal zero = 0.; /* constant */ /* System generated locals */ integer fjac_dim1, fjac_offset, i__1, i__2; doublereal d__1, d__2, d__3; /* Builtin functions */ double sqrt(doublereal); /* Local variables */ integer i__, j, l; doublereal par, sum; integer iter; doublereal temp=0, temp1, temp2; integer iflag; doublereal delta; extern /* Subroutine */ int qrfac_(integer *, integer *, doublereal *, integer *, logical *, integer *, integer *, doublereal *, doublereal *, doublereal *), lmpar_(integer *, doublereal *, integer *, integer *, doublereal *, doublereal *, doublereal *, doublereal *, doublereal *, doublereal *, doublereal *, doublereal *); doublereal ratio; extern doublereal enorm_(integer *, doublereal *); doublereal fnorm, gnorm, pnorm, xnorm=0, fnorm1, actred, dirder, epsmch, prered; extern doublereal dpmpar_(integer *); /*< integer m,n,ldfjac,maxfev,mode,nprint,info,nfev,njev >*/ /*< integer ipvt(n) >*/ /*< double precision ftol,xtol,gtol,factor >*/ /*< >*/ /* ********** */ /* subroutine lmder */ /* the purpose of lmder is to minimize the sum of the squares of */ /* m nonlinear functions in n variables by a modification of */ /* the levenberg-marquardt algorithm. the user must provide a */ /* subroutine which calculates the functions and the jacobian. */ /* the subroutine statement is */ /* subroutine lmder(fcn,m,n,x,fvec,fjac,ldfjac,ftol,xtol,gtol, */ /* maxfev,diag,mode,factor,nprint,info,nfev, */ /* njev,ipvt,qtf,wa1,wa2,wa3,wa4) */ /* where */ /* fcn is the name of the user-supplied subroutine which */ /* calculates the functions and the jacobian. fcn must */ /* be declared in an external statement in the user */ /* calling program, and should be written as follows. */ /* subroutine fcn(m,n,x,fvec,fjac,ldfjac,iflag) */ /* integer m,n,ldfjac,iflag */ /* double precision x(n),fvec(m),fjac(ldfjac,n) */ /* ---------- */ /* if iflag = 1 calculate the functions at x and */ /* return this vector in fvec. do not alter fjac. */ /* if iflag = 2 calculate the jacobian at x and */ /* return this matrix in fjac. do not alter fvec. */ /* ---------- */ /* return */ /* end */ /* the value of iflag should not be changed by fcn unless */ /* the user wants to terminate execution of lmder. */ /* in this case set iflag to a negative integer. */ /* m is a positive integer input variable set to the number */ /* of functions. */ /* n is a positive integer input variable set to the number */ /* of variables. n must not exceed m. */ /* x is an array of length n. on input x must contain */ /* an initial estimate of the solution vector. on output x */ /* contains the final estimate of the solution vector. */ /* fvec is an output array of length m which contains */ /* the functions evaluated at the output x. */ /* fjac is an output m by n array. the upper n by n submatrix */ /* of fjac contains an upper triangular matrix r with */ /* diagonal elements of nonincreasing magnitude such that */ /* t t t */ /* p *(jac *jac)*p = r *r, */ /* where p is a permutation matrix and jac is the final */ /* calculated jacobian. column j of p is column ipvt(j) */ /* (see below) of the identity matrix. the lower trapezoidal */ /* part of fjac contains information generated during */ /* the computation of r. */ /* ldfjac is a positive integer input variable not less than m */ /* which specifies the leading dimension of the array fjac. */ /* ftol is a nonnegative input variable. termination */ /* occurs when both the actual and predicted relative */ /* reductions in the sum of squares are at most ftol. */ /* therefore, ftol measures the relative error desired */ /* in the sum of squares. */ /* xtol is a nonnegative input variable. termination */ /* occurs when the relative error between two consecutive */ /* iterates is at most xtol. therefore, xtol measures the */ /* relative error desired in the approximate solution. */ /* gtol is a nonnegative input variable. termination */ /* occurs when the cosine of the angle between fvec and */ /* any column of the jacobian is at most gtol in absolute */ /* value. therefore, gtol measures the orthogonality */ /* desired between the function vector and the columns */ /* of the jacobian. */ /* maxfev is a positive integer input variable. termination */ /* occurs when the number of calls to fcn with iflag = 1 */ /* has reached maxfev. */ /* diag is an array of length n. if mode = 1 (see */ /* below), diag is internally set. if mode = 2, diag */ /* must contain positive entries that serve as */ /* multiplicative scale factors for the variables. */ /* mode is an integer input variable. if mode = 1, the */ /* variables will be scaled internally. if mode = 2, */ /* the scaling is specified by the input diag. other */ /* values of mode are equivalent to mode = 1. */ /* factor is a positive input variable used in determining the */ /* initial step bound. this bound is set to the product of */ /* factor and the euclidean norm of diag*x if nonzero, or else */ /* to factor itself. in most cases factor should lie in the */ /* interval (.1,100.).100. is a generally recommended value. */ /* nprint is an integer input variable that enables controlled */ /* printing of iterates if it is positive. in this case, */ /* fcn is called with iflag = 0 at the beginning of the first */ /* iteration and every nprint iterations thereafter and */ /* immediately prior to return, with x, fvec, and fjac */ /* available for printing. fvec and fjac should not be */ /* altered. if nprint is not positive, no special calls */ /* of fcn with iflag = 0 are made. */ /* info is an integer output variable. if the user has */ /* terminated execution, info is set to the (negative) */ /* value of iflag. see description of fcn. otherwise, */ /* info is set as follows. */ /* info = 0 improper input parameters. */ /* info = 1 both actual and predicted relative reductions */ /* in the sum of squares are at most ftol. */ /* info = 2 relative error between two consecutive iterates */ /* is at most xtol. */ /* info = 3 conditions for info = 1 and info = 2 both hold. */ /* info = 4 the cosine of the angle between fvec and any */ /* column of the jacobian is at most gtol in */ /* absolute value. */ /* info = 5 number of calls to fcn with iflag = 1 has */ /* reached maxfev. */ /* info = 6 ftol is too small. no further reduction in */ /* the sum of squares is possible. */ /* info = 7 xtol is too small. no further improvement in */ /* the approximate solution x is possible. */ /* info = 8 gtol is too small. fvec is orthogonal to the */ /* columns of the jacobian to machine precision. */ /* nfev is an integer output variable set to the number of */ /* calls to fcn with iflag = 1. */ /* njev is an integer output variable set to the number of */ /* calls to fcn with iflag = 2. */ /* ipvt is an integer output array of length n. ipvt */ /* defines a permutation matrix p such that jac*p = q*r, */ /* where jac is the final calculated jacobian, q is */ /* orthogonal (not stored), and r is upper triangular */ /* with diagonal elements of nonincreasing magnitude. */ /* column j of p is column ipvt(j) of the identity matrix. */ /* qtf is an output array of length n which contains */ /* the first n elements of the vector (q transpose)*fvec. */ /* wa1, wa2, and wa3 are work arrays of length n. */ /* wa4 is a work array of length m. */ /* subprograms called */ /* user-supplied ...... fcn */ /* minpack-supplied ... dpmpar,enorm,lmpar,qrfac */ /* fortran-supplied ... dabs,dmax1,dmin1,dsqrt,mod */ /* argonne national laboratory. minpack project. march 1980. */ /* burton s. garbow, kenneth e. hillstrom, jorge j. more */ /* ********** */ /*< integer i,iflag,iter,j,l >*/ /*< >*/ /*< double precision dpmpar,enorm >*/ /*< >*/ /* Parameter adjustments */ --wa4; --fvec; --wa3; --wa2; --wa1; --qtf; --ipvt; --diag; --x; fjac_dim1 = *ldfjac; fjac_offset = 1 + fjac_dim1; fjac -= fjac_offset; /* Function Body */ /* epsmch is the machine precision. */ /*< epsmch = dpmpar(1) >*/ epsmch = dpmpar_(&c__1); /*< info = 0 >*/ *info = 0; /*< iflag = 0 >*/ iflag = 0; /*< nfev = 0 >*/ *nfev = 0; /*< njev = 0 >*/ *njev = 0; /* check the input parameters for errors. */ /*< >*/ if (*n <= 0 || *m < *n || *ldfjac < *m || *ftol < zero || *xtol < zero || *gtol < zero || *maxfev <= 0 || *factor <= zero) { goto L300; } /*< if (mode .ne. 2) go to 20 >*/ if (*mode != 2) { goto L20; } /*< do 10 j = 1, n >*/ i__1 = *n; for (j = 1; j <= i__1; ++j) { /*< if (diag(j) .le. zero) go to 300 >*/ if (diag[j] <= zero) { goto L300; } /*< 10 continue >*/ /* L10: */ } /*< 20 continue >*/ L20: /* evaluate the function at the starting point */ /* and calculate its norm. */ /*< iflag = 1 >*/ iflag = 1; /*< call fcn(m,n,x,fvec,fjac,ldfjac,iflag) >*/ (*fcn)(m, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac, &iflag, userdata); /*< nfev = 1 >*/ *nfev = 1; /*< if (iflag .lt. 0) go to 300 >*/ if (iflag < 0) { goto L300; } /*< fnorm = enorm(m,fvec) >*/ fnorm = enorm_(m, &fvec[1]); /* initialize levenberg-marquardt parameter and iteration counter. */ /*< par = zero >*/ par = zero; /*< iter = 1 >*/ iter = 1; /* beginning of the outer loop. */ /*< 30 continue >*/ L30: /* calculate the jacobian matrix. */ /*< iflag = 2 >*/ iflag = 2; /*< call fcn(m,n,x,fvec,fjac,ldfjac,iflag) >*/ (*fcn)(m, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac, &iflag, userdata); /*< njev = njev + 1 >*/ ++(*njev); /*< if (iflag .lt. 0) go to 300 >*/ if (iflag < 0) { goto L300; } /* if requested, call fcn to enable printing of iterates. */ /*< if (nprint .le. 0) go to 40 >*/ if (*nprint <= 0) { goto L40; } /*< iflag = 0 >*/ iflag = 0; /*< >*/ if ((iter - 1) % *nprint == 0) { (*fcn)(m, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac, &iflag, userdata); } /*< if (iflag .lt. 0) go to 300 >*/ if (iflag < 0) { goto L300; } /*< 40 continue >*/ L40: /* compute the qr factorization of the jacobian. */ /*< call qrfac(m,n,fjac,ldfjac,.true.,ipvt,n,wa1,wa2,wa3) >*/ qrfac_(m, n, &fjac[fjac_offset], ldfjac, &c_true, &ipvt[1], n, &wa1[1], & wa2[1], &wa3[1]); /* on the first iteration and if mode is 1, scale according */ /* to the norms of the columns of the initial jacobian. */ /*< if (iter .ne. 1) go to 80 >*/ if (iter != 1) { goto L80; } /*< if (mode .eq. 2) go to 60 >*/ if (*mode == 2) { goto L60; } /*< do 50 j = 1, n >*/ i__1 = *n; for (j = 1; j <= i__1; ++j) { /*< diag(j) = wa2(j) >*/ diag[j] = wa2[j]; /*< if (wa2(j) .eq. zero) diag(j) = one >*/ if (wa2[j] == zero) { diag[j] = one; } /*< 50 continue >*/ /* L50: */ } /*< 60 continue >*/ L60: /* on the first iteration, calculate the norm of the scaled x */ /* and initialize the step bound delta. */ /*< do 70 j = 1, n >*/ i__1 = *n; for (j = 1; j <= i__1; ++j) { /*< wa3(j) = diag(j)*x(j) >*/ wa3[j] = diag[j] * x[j]; /*< 70 continue >*/ /* L70: */ } /*< xnorm = enorm(n,wa3) >*/ xnorm = enorm_(n, &wa3[1]); /*< delta = factor*xnorm >*/ delta = *factor * xnorm; /*< if (delta .eq. zero) delta = factor >*/ if (delta == zero) { delta = *factor; } /*< 80 continue >*/ L80: /* form (q transpose)*fvec and store the first n components in */ /* qtf. */ /*< do 90 i = 1, m >*/ i__1 = *m; for (i__ = 1; i__ <= i__1; ++i__) { /*< wa4(i) = fvec(i) >*/ wa4[i__] = fvec[i__]; /*< 90 continue >*/ /* L90: */ } /*< do 130 j = 1, n >*/ i__1 = *n; for (j = 1; j <= i__1; ++j) { /*< if (fjac(j,j) .eq. zero) go to 120 >*/ if (fjac[j + j * fjac_dim1] == zero) { goto L120; } /*< sum = zero >*/ sum = zero; /*< do 100 i = j, m >*/ i__2 = *m; for (i__ = j; i__ <= i__2; ++i__) { /*< sum = sum + fjac(i,j)*wa4(i) >*/ sum += fjac[i__ + j * fjac_dim1] * wa4[i__]; /*< 100 continue >*/ /* L100: */ } /*< temp = -sum/fjac(j,j) >*/ temp = -sum / fjac[j + j * fjac_dim1]; /*< do 110 i = j, m >*/ i__2 = *m; for (i__ = j; i__ <= i__2; ++i__) { /*< wa4(i) = wa4(i) + fjac(i,j)*temp >*/ wa4[i__] += fjac[i__ + j * fjac_dim1] * temp; /*< 110 continue >*/ /* L110: */ } /*< 120 continue >*/ L120: /*< fjac(j,j) = wa1(j) >*/ fjac[j + j * fjac_dim1] = wa1[j]; /*< qtf(j) = wa4(j) >*/ qtf[j] = wa4[j]; /*< 130 continue >*/ /* L130: */ } /* compute the norm of the scaled gradient. */ /*< gnorm = zero >*/ gnorm = zero; /*< if (fnorm .eq. zero) go to 170 >*/ if (fnorm == zero) { goto L170; } /*< do 160 j = 1, n >*/ i__1 = *n; for (j = 1; j <= i__1; ++j) { /*< l = ipvt(j) >*/ l = ipvt[j]; /*< if (wa2(l) .eq. zero) go to 150 >*/ if (wa2[l] == zero) { goto L150; } /*< sum = zero >*/ sum = zero; /*< do 140 i = 1, j >*/ i__2 = j; for (i__ = 1; i__ <= i__2; ++i__) { /*< sum = sum + fjac(i,j)*(qtf(i)/fnorm) >*/ sum += fjac[i__ + j * fjac_dim1] * (qtf[i__] / fnorm); /*< 140 continue >*/ /* L140: */ } /*< gnorm = dmax1(gnorm,dabs(sum/wa2(l))) >*/ /* Computing MAX */ d__2 = gnorm, d__3 = (d__1 = sum / wa2[l], abs(d__1)); gnorm = max(d__2,d__3); /*< 150 continue >*/ L150: /*< 160 continue >*/ /* L160: */ ; } /*< 170 continue >*/ L170: /* test for convergence of the gradient norm. */ /*< if (gnorm .le. gtol) info = 4 >*/ if (gnorm <= *gtol) { *info = 4; } /*< if (info .ne. 0) go to 300 >*/ if (*info != 0) { goto L300; } /* rescale if necessary. */ /*< if (mode .eq. 2) go to 190 >*/ if (*mode == 2) { goto L190; } /*< do 180 j = 1, n >*/ i__1 = *n; for (j = 1; j <= i__1; ++j) { /*< diag(j) = dmax1(diag(j),wa2(j)) >*/ /* Computing MAX */ d__1 = diag[j], d__2 = wa2[j]; diag[j] = max(d__1,d__2); /*< 180 continue >*/ /* L180: */ } /*< 190 continue >*/ L190: /* beginning of the inner loop. */ /*< 200 continue >*/ L200: /* determine the levenberg-marquardt parameter. */ /*< >*/ lmpar_(n, &fjac[fjac_offset], ldfjac, &ipvt[1], &diag[1], &qtf[1], &delta, &par, &wa1[1], &wa2[1], &wa3[1], &wa4[1]); /* store the direction p and x + p. calculate the norm of p. */ /*< do 210 j = 1, n >*/ i__1 = *n; for (j = 1; j <= i__1; ++j) { /*< wa1(j) = -wa1(j) >*/ wa1[j] = -wa1[j]; /*< wa2(j) = x(j) + wa1(j) >*/ wa2[j] = x[j] + wa1[j]; /*< wa3(j) = diag(j)*wa1(j) >*/ wa3[j] = diag[j] * wa1[j]; /*< 210 continue >*/ /* L210: */ } /*< pnorm = enorm(n,wa3) >*/ pnorm = enorm_(n, &wa3[1]); /* on the first iteration, adjust the initial step bound. */ /*< if (iter .eq. 1) delta = dmin1(delta,pnorm) >*/ if (iter == 1) { delta = min(delta,pnorm); } /* evaluate the function at x + p and calculate its norm. */ /*< iflag = 1 >*/ iflag = 1; /*< call fcn(m,n,wa2,wa4,fjac,ldfjac,iflag) >*/ (*fcn)(m, n, &wa2[1], &wa4[1], &fjac[fjac_offset], ldfjac, &iflag, userdata); /*< nfev = nfev + 1 >*/ ++(*nfev); /*< if (iflag .lt. 0) go to 300 >*/ if (iflag < 0) { goto L300; } /*< fnorm1 = enorm(m,wa4) >*/ fnorm1 = enorm_(m, &wa4[1]); /* compute the scaled actual reduction. */ /*< actred = -one >*/ actred = -one; /*< if (p1*fnorm1 .lt. fnorm) actred = one - (fnorm1/fnorm)**2 >*/ if (p1 * fnorm1 < fnorm) { /* Computing 2nd power */ d__1 = fnorm1 / fnorm; actred = one - d__1 * d__1; } /* compute the scaled predicted reduction and */ /* the scaled directional derivative. */ /*< do 230 j = 1, n >*/ i__1 = *n; for (j = 1; j <= i__1; ++j) { /*< wa3(j) = zero >*/ wa3[j] = zero; /*< l = ipvt(j) >*/ l = ipvt[j]; /*< temp = wa1(l) >*/ temp = wa1[l]; /*< do 220 i = 1, j >*/ i__2 = j; for (i__ = 1; i__ <= i__2; ++i__) { /*< wa3(i) = wa3(i) + fjac(i,j)*temp >*/ wa3[i__] += fjac[i__ + j * fjac_dim1] * temp; /*< 220 continue >*/ /* L220: */ } /*< 230 continue >*/ /* L230: */ } /*< temp1 = enorm(n,wa3)/fnorm >*/ temp1 = enorm_(n, &wa3[1]) / fnorm; /*< temp2 = (dsqrt(par)*pnorm)/fnorm >*/ temp2 = sqrt(par) * pnorm / fnorm; /*< prered = temp1**2 + temp2**2/p5 >*/ /* Computing 2nd power */ d__1 = temp1; /* Computing 2nd power */ d__2 = temp2; prered = d__1 * d__1 + d__2 * d__2 / p5; /*< dirder = -(temp1**2 + temp2**2) >*/ /* Computing 2nd power */ d__1 = temp1; /* Computing 2nd power */ d__2 = temp2; dirder = -(d__1 * d__1 + d__2 * d__2); /* compute the ratio of the actual to the predicted */ /* reduction. */ /*< ratio = zero >*/ ratio = zero; /*< if (prered .ne. zero) ratio = actred/prered >*/ if (prered != zero) { ratio = actred / prered; } /* update the step bound. */ /*< if (ratio .gt. p25) go to 240 >*/ if (ratio > p25) { goto L240; } /*< if (actred .ge. zero) temp = p5 >*/ if (actred >= zero) { temp = p5; } /*< >*/ if (actred < zero) { temp = p5 * dirder / (dirder + p5 * actred); } /*< if (p1*fnorm1 .ge. fnorm .or. temp .lt. p1) temp = p1 >*/ if (p1 * fnorm1 >= fnorm || temp < p1) { temp = p1; } /*< delta = temp*dmin1(delta,pnorm/p1) >*/ /* Computing MIN */ d__1 = delta, d__2 = pnorm / p1; delta = temp * min(d__1,d__2); /*< par = par/temp >*/ par /= temp; /*< go to 260 >*/ goto L260; /*< 240 continue >*/ L240: /*< if (par .ne. zero .and. ratio .lt. p75) go to 250 >*/ if (par != zero && ratio < p75) { goto L250; } /*< delta = pnorm/p5 >*/ delta = pnorm / p5; /*< par = p5*par >*/ par = p5 * par; /*< 250 continue >*/ L250: /*< 260 continue >*/ L260: /* test for successful iteration. */ /*< if (ratio .lt. p0001) go to 290 >*/ if (ratio < p0001) { goto L290; } /* successful iteration. update x, fvec, and their norms. */ /*< do 270 j = 1, n >*/ i__1 = *n; for (j = 1; j <= i__1; ++j) { /*< x(j) = wa2(j) >*/ x[j] = wa2[j]; /*< wa2(j) = diag(j)*x(j) >*/ wa2[j] = diag[j] * x[j]; /*< 270 continue >*/ /* L270: */ } /*< do 280 i = 1, m >*/ i__1 = *m; for (i__ = 1; i__ <= i__1; ++i__) { /*< fvec(i) = wa4(i) >*/ fvec[i__] = wa4[i__]; /*< 280 continue >*/ /* L280: */ } /*< xnorm = enorm(n,wa2) >*/ xnorm = enorm_(n, &wa2[1]); /*< fnorm = fnorm1 >*/ fnorm = fnorm1; /*< iter = iter + 1 >*/ ++iter; /*< 290 continue >*/ L290: /* tests for convergence. */ /*< >*/ if (abs(actred) <= *ftol && prered <= *ftol && p5 * ratio <= one) { *info = 1; } /*< if (delta .le. xtol*xnorm) info = 2 >*/ if (delta <= *xtol * xnorm) { *info = 2; } /*< >*/ if (abs(actred) <= *ftol && prered <= *ftol && p5 * ratio <= one && *info == 2) { *info = 3; } /*< if (info .ne. 0) go to 300 >*/ if (*info != 0) { goto L300; } /* tests for termination and stringent tolerances. */ /*< if (nfev .ge. maxfev) info = 5 >*/ if (*nfev >= *maxfev) { *info = 5; } /*< >*/ if (abs(actred) <= epsmch && prered <= epsmch && p5 * ratio <= one) { *info = 6; } /*< if (delta .le. epsmch*xnorm) info = 7 >*/ if (delta <= epsmch * xnorm) { *info = 7; } /*< if (gnorm .le. epsmch) info = 8 >*/ if (gnorm <= epsmch) { *info = 8; } /*< if (info .ne. 0) go to 300 >*/ if (*info != 0) { goto L300; } /* end of the inner loop. repeat if iteration unsuccessful. */ /*< if (ratio .lt. p0001) go to 200 >*/ if (ratio < p0001) { goto L200; } /* end of the outer loop. */ /*< go to 30 >*/ goto L30; /*< 300 continue >*/ L300: /* termination, either normal or user imposed. */ /*< if (iflag .lt. 0) info = iflag >*/ if (iflag < 0) { *info = iflag; } /*< iflag = 0 >*/ iflag = 0; /*< if (nprint .gt. 0) call fcn(m,n,x,fvec,fjac,ldfjac,iflag) >*/ if (*nprint > 0) { (*fcn)(m, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac, &iflag, userdata); } /*< return >*/ return 0; /* last card of subroutine lmder. */ /*< end >*/ } /* lmder_ */
/* Subroutine */ int _omc_hybrd_(S_fp fcn, integer *n, doublereal *x, doublereal * fvec, doublereal *xtol, integer *maxfev, integer *ml, integer *mu, doublereal *epsfcn, doublereal *diag, integer *mode, doublereal * factor, integer *nprint, integer *info, integer *nfev, doublereal * fjac, doublereal * fjacobian, integer *ldfjac, doublereal *r__, integer *lr, doublereal *qtf, doublereal *wa1, doublereal *wa2, doublereal *wa3, doublereal *wa4, void* userdata) { /* Initialized data */ static doublereal one = 1.; static doublereal p1 = .1; static doublereal p5 = .5; static doublereal p001 = .001; static doublereal p0001 = 1e-4; static doublereal zero = 0.; /* System generated locals */ integer fjac_dim1, fjac_offset, i__1, i__2; doublereal d__1, d__2; /* Local variables */ integer i__, j, l, jm1, iwa[1]; doublereal sum; logical sing; integer iter; doublereal temp; integer msum, iflag; doublereal delta; extern /* Subroutine */ int qrfac_(integer *, integer *, doublereal *, integer *, logical *, integer *, integer *, doublereal *, doublereal *, doublereal *); logical jeval; integer ncsuc; doublereal ratio; extern doublereal enorm_(integer *, doublereal *); doublereal fnorm; extern /* Subroutine */ int qform_(integer *, integer *, doublereal *, integer *, doublereal *), fdjac1_(S_fp, integer *, doublereal *, doublereal *, doublereal *, integer *, integer *, integer *, integer *, doublereal *, doublereal *, doublereal *, void *); doublereal pnorm, xnorm, fnorm1; extern /* Subroutine */ int r1updt_(integer *, integer *, doublereal *, integer *, doublereal *, doublereal *, doublereal *, logical *); integer nslow1, nslow2; extern /* Subroutine */ int r1mpyq_(integer *, integer *, doublereal *, integer *, doublereal *, doublereal *); integer ncfail; extern /* Subroutine */ int dogleg_(integer *, doublereal *, integer *, doublereal *, doublereal *, doublereal *, doublereal *, doublereal *, doublereal *); doublereal actred, epsmch, prered; extern doublereal dpmpar_(integer *); /* ********** */ /* subroutine hybrd */ /* the purpose of hybrd is to find a zero of a system of */ /* n nonlinear functions in n variables by a modification */ /* of the powell hybrid method. the user must provide a */ /* subroutine which calculates the functions. the jacobian is */ /* then calculated by a forward-difference approximation. */ /* the subroutine statement is */ /* subroutine hybrd(fcn,n,x,fvec,xtol,maxfev,ml,mu,epsfcn, */ /* diag,mode,factor,nprint,info,nfev,fjac,fjac, */ /* ldfjac,r,lr,qtf,wa1,wa2,wa3,wa4, userdata) */ /* where */ /* fcn is the name of the user-supplied subroutine which */ /* calculates the functions. fcn must be declared */ /* in an external statement in the user calling */ /* program, and should be written as follows. */ /* subroutine fcn(n,x,fvec,iflag) */ /* integer n,iflag */ /* double precision x(n),fvec(n) */ /* ---------- */ /* calculate the functions at x and */ /* return this vector in fvec. */ /* --------- */ /* return */ /* end */ /* the value of iflag should not be changed by fcn unless */ /* the user wants to terminate execution of hybrd. */ /* in this case set iflag to a negative integer. */ /* n is a positive integer input variable set to the number */ /* of functions and variables. */ /* x is an array of length n. on input x must contain */ /* an initial estimate of the solution vector. on output x */ /* contains the final estimate of the solution vector. */ /* fvec is an output array of length n which contains */ /* the functions evaluated at the output x. */ /* xtol is a nonnegative input variable. termination */ /* occurs when the relative error between two consecutive */ /* iterates is at most xtol. */ /* maxfev is a positive integer input variable. termination */ /* occurs when the number of calls to fcn is at least maxfev */ /* by the end of an iteration. */ /* ml is a nonnegative integer input variable which specifies */ /* the number of subdiagonals within the band of the */ /* jacobian matrix. if the jacobian is not banded, set */ /* ml to at least n - 1. */ /* mu is a nonnegative integer input variable which specifies */ /* the number of superdiagonals within the band of the */ /* jacobian matrix. if the jacobian is not banded, set */ /* mu to at least n - 1. */ /* epsfcn is an input variable used in determining a suitable */ /* step length for the forward-difference approximation. this */ /* approximation assumes that the relative errors in the */ /* functions are of the order of epsfcn. if epsfcn is less */ /* than the machine precision, it is assumed that the relative */ /* errors in the functions are of the order of the machine */ /* precision. */ /* diag is an array of length n. if mode = 1 (see */ /* below), diag is internally set. if mode = 2, diag */ /* must contain positive entries that serve as */ /* multiplicative scale factors for the variables. */ /* mode is an integer input variable. if mode = 1, the */ /* variables will be scaled internally. if mode = 2, */ /* the scaling is specified by the input diag. other */ /* values of mode are equivalent to mode = 1. */ /* factor is a positive input variable used in determining the */ /* initial step bound. this bound is set to the product of */ /* factor and the euclidean norm of diag*x if nonzero, or else */ /* to factor itself. in most cases factor should lie in the */ /* interval (.1,100.). 100. is a generally recommended value. */ /* nprint is an integer input variable that enables controlled */ /* printing of iterates if it is positive. in this case, */ /* fcn is called with iflag = 0 at the beginning of the first */ /* iteration and every nprint iterations thereafter and */ /* immediately prior to return, with x and fvec available */ /* for printing. if nprint is not positive, no special calls */ /* of fcn with iflag = 0 are made. */ /* info is an integer output variable. if the user has */ /* terminated execution, info is set to the (negative) */ /* value of iflag. see description of fcn. otherwise, */ /* info is set as follows. */ /* info = 0 improper input parameters. */ /* info = 1 relative error between two consecutive iterates */ /* is at most xtol. */ /* info = 2 number of calls to fcn has reached or exceeded */ /* maxfev. */ /* info = 3 xtol is too small. no further improvement in */ /* the approximate solution x is possible. */ /* info = 4 iteration is not making good progress, as */ /* measured by the improvement from the last */ /* five jacobian evaluations. */ /* info = 5 iteration is not making good progress, as */ /* measured by the improvement from the last */ /* ten iterations. */ /* nfev is an integer output variable set to the number of */ /* calls to fcn. */ /* fjac is an output n by n array which contains the */ /* orthogonal matrix q produced by the qr factorization */ /* of the final approximate jacobian. */ /* fjacobian is an output n by n array which contains the */ /* of the final approximate jacobian. */ /* ldfjac is a positive integer input variable not less than n */ /* which specifies the leading dimension of the array fjac. */ /* r is an output array of length lr which contains the */ /* upper triangular matrix produced by the qr factorization */ /* of the final approximate jacobian, stored rowwise. */ /* lr is a positive integer input variable not less than */ /* (n*(n+1))/2. */ /* qtf is an output array of length n which contains */ /* the vector (q transpose)*fvec. */ /* wa1, wa2, wa3, and wa4 are work arrays of length n. */ /* subprograms called */ /* user-supplied ...... fcn */ /* minpack-supplied ... dogleg,dpmpar,enorm,fdjac1, */ /* qform,qrfac,r1mpyq,r1updt */ /* fortran-supplied ... dabs,dmax1,dmin1,min0,mod */ /* argonne national laboratory. minpack project. march 1980. */ /* burton s. garbow, kenneth e. hillstrom, jorge j. more */ /* ********** */ /* Parameter adjustments */ --wa4; --wa3; --wa2; --wa1; --qtf; --diag; --fvec; --x; fjac_dim1 = *ldfjac; fjac_offset = 1 + fjac_dim1; fjac -= fjac_offset; --fjacobian; --r__; /* Function Body */ /* epsmch is the machine precision. */ epsmch = dpmpar_(&c__1); *info = 0; iflag = 0; *nfev = 0; /* check the input parameters for errors. */ if(*n <= 0 || *xtol < zero || *maxfev <= 0 || *ml < 0 || *mu < 0 || * factor <= zero || *ldfjac < *n || *lr < *n * (*n + 1) / 2) { goto L300; } if(*mode != 2) { goto L20; } i__1 = *n; for(j = 1; j <= i__1; ++j) { if(diag[j] <= zero) { goto L300; } /* L10: */ } L20: /* evaluate the function at the starting point */ /* and calculate its norm. */ iflag = 1; (*fcn)(n, &x[1], &fvec[1], &iflag, userdata); *nfev = 1; if(iflag < 0) { goto L300; } fnorm = enorm_(n, &fvec[1]); /* determine the number of calls to fcn needed to compute */ /* the jacobian matrix. */ /* Computing MIN */ i__1 = *ml + *mu + 1; msum = min(i__1,*n); /* initialize iteration counter and monitors. */ iter = 1; ncsuc = 0; ncfail = 0; nslow1 = 0; nslow2 = 0; /* beginning of the outer loop. */ L30: jeval = TRUE_; /* calculate the jacobian matrix. */ iflag = 2; fdjac1_((S_fp)fcn, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac, &iflag, ml, mu, epsfcn, &wa1[1], &wa2[1], userdata); *nfev += msum; /* store the calculate jacobain matrix */ /* added by wbraun for scaling residuals */ { int l = fjac_offset; int k = 1; for(j = 1; j <= *n; ++j){ for(i__ = 1; i__<= *n; ++i__, l++, k++){ fjacobian[k] = fjac[l]; } } } if(iflag < 0) { goto L300; } /* compute the qr factorization of the jacobian. */ qrfac_(n, n, &fjac[fjac_offset], ldfjac, &c_false, iwa, &c__1, &wa1[1], & wa2[1], &wa3[1]); /* on the first iteration and if mode is 1, scale according */ /* to the norms of the columns of the initial jacobian. */ if(iter != 1) { goto L70; } if(*mode == 2) { goto L50; } i__1 = *n; for(j = 1; j <= i__1; ++j) { diag[j] = wa2[j]; if(wa2[j] == zero) { diag[j] = one; } /* L40: */ } L50: /* on the first iteration, calculate the norm of the scaled x */ /* and initialize the step bound delta. */ i__1 = *n; for(j = 1; j <= i__1; ++j) { wa3[j] = diag[j] * x[j]; /* L60: */ } xnorm = enorm_(n, &wa3[1]); delta = *factor * xnorm; if(delta == zero) { delta = *factor; } L70: /* form (q transpose)*fvec and store in qtf. */ i__1 = *n; for(i__ = 1; i__ <= i__1; ++i__) { qtf[i__] = fvec[i__]; /* L80: */ } i__1 = *n; for(j = 1; j <= i__1; ++j) { if(fjac[j + j * fjac_dim1] == zero) { goto L110; } sum = zero; i__2 = *n; for(i__ = j; i__ <= i__2; ++i__) { sum += fjac[i__ + j * fjac_dim1] * qtf[i__]; /* L90: */ } temp = -sum / fjac[j + j * fjac_dim1]; i__2 = *n; for(i__ = j; i__ <= i__2; ++i__) { qtf[i__] += fjac[i__ + j * fjac_dim1] * temp; /* L100: */ } L110: /* L120: */ ; } /* copy the triangular factor of the qr factorization into r. */ sing = FALSE_; i__1 = *n; for(j = 1; j <= i__1; ++j) { l = j; jm1 = j - 1; if(jm1 < 1) { goto L140; } i__2 = jm1; for(i__ = 1; i__ <= i__2; ++i__) { r__[l] = fjac[i__ + j * fjac_dim1]; l = l + *n - i__; /* L130: */ } L140: r__[l] = wa1[j]; if(wa1[j] == zero) { sing = TRUE_; } /* L150: */ } /* accumulate the orthogonal factor in fjac. */ qform_(n, n, &fjac[fjac_offset], ldfjac, &wa1[1]); /* rescale if necessary. */ if(*mode == 2) { goto L170; } i__1 = *n; for(j = 1; j <= i__1; ++j) { /* Computing MAX */ d__1 = diag[j], d__2 = wa2[j]; diag[j] = max(d__1,d__2); /* L160: */ } L170: /* beginning of the inner loop. */ L180: /* if requested, call fcn to enable printing of iterates. */ if(*nprint <= 0) { goto L190; } iflag = 0; if((iter - 1) % *nprint == 0) { (*fcn)(n, &x[1], &fvec[1], &iflag, userdata); } if(iflag < 0) { goto L300; } L190: /* determine the direction p. */ dogleg_(n, &r__[1], lr, &diag[1], &qtf[1], &delta, &wa1[1], &wa2[1], &wa3[ 1]); /* store the direction p and x + p. calculate the norm of p. */ i__1 = *n; for(j = 1; j <= i__1; ++j) { wa1[j] = -wa1[j]; wa2[j] = x[j] + wa1[j]; wa3[j] = diag[j] * wa1[j]; /* L200: */ } pnorm = enorm_(n, &wa3[1]); /* on the first iteration, adjust the initial step bound. */ if(iter == 1) { delta = min(delta,pnorm); } /* evaluate the function at x + p and calculate its norm. */ iflag = 1; (*fcn)(n, &wa2[1], &wa4[1], &iflag, userdata); ++(*nfev); /* Scaling Residual vector */ /* added by wbraun */ /* { for(i__=1;i__<*n;i__++) wa4[i__] = diagres[i__] * wa4[i__]; } */ if(iflag < 0) { goto L300; } fnorm1 = enorm_(n, &wa4[1]); /* compute the scaled actual reduction. */ actred = -one; if(fnorm1 < fnorm) { /* Computing 2nd power */ d__1 = fnorm1 / fnorm; actred = one - d__1 * d__1; } /* compute the scaled predicted reduction. */ l = 1; i__1 = *n; for(i__ = 1; i__ <= i__1; ++i__) { sum = zero; i__2 = *n; for(j = i__; j <= i__2; ++j) { sum += r__[l] * wa1[j]; ++l; /* L210: */ } wa3[i__] = qtf[i__] + sum; /* L220: */ } temp = enorm_(n, &wa3[1]); prered = zero; if(temp < fnorm) { /* Computing 2nd power */ d__1 = temp / fnorm; prered = one - d__1 * d__1; } /* compute the ratio of the actual to the predicted */ /* reduction. */ ratio = zero; if(prered > zero) { ratio = actred / prered; } /* update the step bound. */ if(ratio >= p1) { goto L230; } ncsuc = 0; ++ncfail; delta = p5 * delta; goto L240; L230: ncfail = 0; ++ncsuc; if(ratio >= p5 || ncsuc > 1) { /* Computing MAX */ d__1 = delta, d__2 = pnorm / p5; delta = max(d__1,d__2); } if((d__1 = ratio - one, abs(d__1)) <= p1) { delta = pnorm / p5; } L240: /* test for successful iteration. */ if(ratio < p0001) { goto L260; } /* successful iteration. update x, fvec, and their norms. */ i__1 = *n; for(j = 1; j <= i__1; ++j) { x[j] = wa2[j]; wa2[j] = diag[j] * x[j]; fvec[j] = wa4[j]; /* L250: */ } xnorm = enorm_(n, &wa2[1]); fnorm = fnorm1; ++iter; L260: /* determine the progress of the iteration. */ ++nslow1; if(actred >= p001) { nslow1 = 0; } if(jeval) { ++nslow2; } if(actred >= p1) { nslow2 = 0; } /* test for convergence. */ if(delta <= *xtol * xnorm || fnorm == zero) { *info = 1; } if(*info != 0) { goto L300; } /* tests for termination and stringent tolerances. */ if(*nfev >= *maxfev) { *info = 2; } /* Computing MAX */ d__1 = p1 * delta; if(p1 * max(d__1,pnorm) <= epsmch * xnorm) { *info = 3; } if(nslow2 == 5) { *info = 4; } if(nslow1 == 10) { *info = 5; } if(*info != 0) { goto L300; } /* criterion for recalculating jacobian approximation */ /* by forward differences. */ if(ncfail == 2) { goto L290; } /* calculate the rank one modification to the jacobian */ /* and update qtf if necessary. */ i__1 = *n; for(j = 1; j <= i__1; ++j) { sum = zero; i__2 = *n; for(i__ = 1; i__ <= i__2; ++i__) { sum += fjac[i__ + j * fjac_dim1] * wa4[i__]; /* L270: */ } wa2[j] = (sum - wa3[j]) / pnorm; wa1[j] = diag[j] * (diag[j] * wa1[j] / pnorm); if(ratio >= p0001) { qtf[j] = sum; } /* L280: */ } /* compute the qr factorization of the updated jacobian. */ r1updt_(n, n, &r__[1], lr, &wa1[1], &wa2[1], &wa3[1], &sing); r1mpyq_(n, n, &fjac[fjac_offset], ldfjac, &wa2[1], &wa3[1]); r1mpyq_(&c__1, n, &qtf[1], &c__1, &wa2[1], &wa3[1]); /* end of the inner loop. */ jeval = FALSE_; goto L180; L290: /* end of the outer loop. */ goto L30; L300: /* termination, either normal or user imposed. */ if(iflag < 0) { *info = iflag; } iflag = 0; if(*nprint > 0) { (*fcn)(n, &x[1], &fvec[1], &iflag, userdata); } return 0; /* last card of subroutine hybrd. */ } /* _omc_hybrd_ */
/*! \fn solve non-linear system with hybrd method * * \param [in] [data] * [sysNumber] index of the corresponing non-linear system * * \author wbraun */ int solveHybrd(DATA *data, threadData_t *threadData, int sysNumber) { NONLINEAR_SYSTEM_DATA* systemData = &(data->simulationInfo->nonlinearSystemData[sysNumber]); DATA_HYBRD* solverData = (DATA_HYBRD*)systemData->solverData; /* * We are given the number of the non-linear system. * We want to look it up among all equations. */ int eqSystemNumber = systemData->equationIndex; int i, j; integer iflag = 1; double xerror, xerror_scaled; int success = 0; double local_tol = 1e-12; double initial_factor = solverData->factor; int nfunc_evals = 0; int continuous = 1; int nonContinuousCase = 0; int giveUp = 0; int retries = 0; int retries2 = 0; int retries3 = 0; int assertCalled = 0; int assertRetries = 0; int assertMessage = 0; modelica_boolean* relationsPreBackup; struct dataAndSys dataAndSysNumber = {data, threadData, sysNumber}; relationsPreBackup = (modelica_boolean*) malloc(data->modelData->nRelations*sizeof(modelica_boolean)); solverData->numberOfFunctionEvaluations = 0; /* debug output */ if(ACTIVE_STREAM(LOG_NLS_V)) { int indexes[2] = {1,eqSystemNumber}; infoStreamPrintWithEquationIndexes(LOG_NLS_V, 1, indexes, "start solving non-linear system >>%d<< at time %g", eqSystemNumber, data->localData[0]->timeValue); for(i=0; i<solverData->n; i++) { infoStreamPrint(LOG_NLS_V, 1, "%d. %s = %f", i+1, modelInfoGetEquation(&data->modelData->modelDataXml,eqSystemNumber).vars[i], systemData->nlsx[i]); infoStreamPrint(LOG_NLS_V, 0, " nominal = %f\nold = %f\nextrapolated = %f", systemData->nominal[i], systemData->nlsxOld[i], systemData->nlsxExtrapolation[i]); messageClose(LOG_NLS_V); } messageClose(LOG_NLS_V); } /* set x vector */ if(data->simulationInfo->discreteCall) memcpy(solverData->x, systemData->nlsx, solverData->n*(sizeof(double))); else memcpy(solverData->x, systemData->nlsxExtrapolation, solverData->n*(sizeof(double))); for(i=0; i<solverData->n; i++){ solverData->xScalefactors[i] = fmax(fabs(solverData->x[i]), systemData->nominal[i]); } /* start solving loop */ while(!giveUp && !success) { for(i=0; i<solverData->n; i++) solverData->xScalefactors[i] = fmax(fabs(solverData->x[i]), systemData->nominal[i]); /* debug output */ if(ACTIVE_STREAM(LOG_NLS_V)) { printVector(solverData->xScalefactors, &(solverData->n), LOG_NLS_V, "scaling factors x vector"); printVector(solverData->x, &(solverData->n), LOG_NLS_V, "Iteration variable values"); } /* Scaling x vector */ if(solverData->useXScaling) { for(i=0; i<solverData->n; i++) { solverData->x[i] = (1.0/solverData->xScalefactors[i]) * solverData->x[i]; } } /* debug output */ if(ACTIVE_STREAM(LOG_NLS_V)) { printVector(solverData->x, &solverData->n, LOG_NLS_V, "Iteration variable values (scaled)"); } /* set residual function continuous */ if(continuous) { ((DATA*)data)->simulationInfo->solveContinuous = 1; } else { ((DATA*)data)->simulationInfo->solveContinuous = 0; } giveUp = 1; /* try */ { int success = 0; #ifndef OMC_EMCC MMC_TRY_INTERNAL(simulationJumpBuffer) #endif hybrj_(wrapper_fvec_hybrj, &solverData->n, solverData->x, solverData->fvec, solverData->fjac, &solverData->ldfjac, &solverData->xtol, &solverData->maxfev, solverData->diag, &solverData->mode, &solverData->factor, &solverData->nprint, &solverData->info, &solverData->nfev, &solverData->njev, solverData->r__, &solverData->lr, solverData->qtf, solverData->wa1, solverData->wa2, solverData->wa3, solverData->wa4, (void*) &dataAndSysNumber); success = 1; if(assertCalled) { infoStreamPrint(LOG_NLS, 0, "After assertions failed, found a solution for which assertions did not fail."); /* re-scaling x vector */ for(i=0; i<solverData->n; i++){ if(solverData->useXScaling) systemData->nlsxOld[i] = solverData->x[i]*solverData->xScalefactors[i]; else systemData->nlsxOld[i] = solverData->x[i]; } } assertRetries = 0; assertCalled = 0; success = 1; #ifndef OMC_EMCC MMC_CATCH_INTERNAL(simulationJumpBuffer) #endif /* catch */ if (!success) { if (!assertMessage) { if (ACTIVE_WARNING_STREAM(LOG_STDOUT)) { if(data->simulationInfo->initial) warningStreamPrint(LOG_STDOUT, 1, "While solving non-linear system an assertion failed during initialization."); else warningStreamPrint(LOG_STDOUT, 1, "While solving non-linear system an assertion failed at time %g.", data->localData[0]->timeValue); warningStreamPrint(LOG_STDOUT, 0, "The non-linear solver tries to solve the problem that could take some time."); warningStreamPrint(LOG_STDOUT, 0, "It could help to provide better start-values for the iteration variables."); if (!ACTIVE_STREAM(LOG_NLS)) warningStreamPrint(LOG_STDOUT, 0, "For more information simulate with -lv LOG_NLS"); messageClose(LOG_STDOUT); } assertMessage = 1; } solverData->info = -1; xerror_scaled = 1; xerror = 1; assertCalled = 1; } } /* set residual function continuous */ if(continuous) { ((DATA*)data)->simulationInfo->solveContinuous = 0; } else { ((DATA*)data)->simulationInfo->solveContinuous = 1; } /* re-scaling x vector */ if(solverData->useXScaling) for(i=0; i<solverData->n; i++) solverData->x[i] = solverData->x[i]*solverData->xScalefactors[i]; /* check for proper inputs */ if(solverData->info == 0) { printErrorEqSyst(IMPROPER_INPUT, modelInfoGetEquation(&data->modelData->modelDataXml, eqSystemNumber), data->localData[0]->timeValue); } if(solverData->info != -1) { /* evaluate with discontinuities */ if(data->simulationInfo->discreteCall){ int scaling = solverData->useXScaling; int success = 0; if(scaling) solverData->useXScaling = 0; ((DATA*)data)->simulationInfo->solveContinuous = 0; /* try */ #ifndef OMC_EMCC MMC_TRY_INTERNAL(simulationJumpBuffer) #endif wrapper_fvec_hybrj(&solverData->n, solverData->x, solverData->fvec, solverData->fjac, &solverData->ldfjac, &iflag, (void*) &dataAndSysNumber); success = 1; #ifndef OMC_EMCC MMC_CATCH_INTERNAL(simulationJumpBuffer) #endif /* catch */ if (!success) { warningStreamPrint(LOG_STDOUT, 0, "Non-Linear Solver try to handle a problem with a called assert."); solverData->info = -1; xerror_scaled = 1; xerror = 1; assertCalled = 1; } if(scaling) solverData->useXScaling = 1; updateRelationsPre(data); } } if(solverData->info != -1) { /* scaling residual vector */ { int l=0; for(i=0; i<solverData->n; i++){ solverData->resScaling[i] = 1e-16; for(j=0; j<solverData->n; j++){ solverData->resScaling[i] = (fabs(solverData->fjacobian[l]) > solverData->resScaling[i]) ? fabs(solverData->fjacobian[l]) : solverData->resScaling[i]; l++; } solverData->fvecScaled[i] = solverData->fvec[i] * (1 / solverData->resScaling[i]); } /* debug output */ if(ACTIVE_STREAM(LOG_NLS_V)) { infoStreamPrint(LOG_NLS_V, 1, "scaling factors for residual vector"); for(i=0; i<solverData->n; i++) { infoStreamPrint(LOG_NLS_V, 1, "scaled residual [%d] : %.20e", i, solverData->fvecScaled[i]); infoStreamPrint(LOG_NLS_V, 0, "scaling factor [%d] : %.20e", i, solverData->resScaling[i]); messageClose(LOG_NLS_V); } messageClose(LOG_NLS_V); } /* debug output */ if(ACTIVE_STREAM(LOG_NLS_JAC)) { char buffer[4096]; infoStreamPrint(LOG_NLS_JAC, 1, "jacobian matrix [%dx%d]", (int)solverData->n, (int)solverData->n); for(i=0; i<solverData->n; i++) { buffer[0] = 0; for(j=0; j<solverData->n; j++) sprintf(buffer, "%s%10g ", buffer, solverData->fjacobian[i*solverData->n+j]); infoStreamPrint(LOG_NLS_JAC, 0, "%s", buffer); } messageClose(LOG_NLS_JAC); } /* check for error */ xerror_scaled = enorm_(&solverData->n, solverData->fvecScaled); xerror = enorm_(&solverData->n, solverData->fvec); } } /* reset non-contunuousCase */ if(nonContinuousCase && xerror > local_tol && xerror_scaled > local_tol) { memcpy(data->simulationInfo->relationsPre, relationsPreBackup, sizeof(modelica_boolean)*data->modelData->nRelations); nonContinuousCase = 0; } if(solverData->info < 4 && xerror > local_tol && xerror_scaled > local_tol) solverData->info = 4; /* solution found */ if(solverData->info == 1 || xerror <= local_tol || xerror_scaled <= local_tol) { int scaling; success = 1; nfunc_evals += solverData->nfev; if(ACTIVE_STREAM(LOG_NLS)) { int indexes[2] = {1,eqSystemNumber}; /* output solution */ infoStreamPrintWithEquationIndexes(LOG_NLS, 1, indexes, "solution for NLS %d at t=%g", eqSystemNumber, data->localData[0]->timeValue); for(i=0; i<solverData->n; ++i) { infoStreamPrint(LOG_NLS, 0, "[%d] %s = %g", i+1, modelInfoGetEquation(&data->modelData->modelDataXml,eqSystemNumber).vars[i], solverData->x[i]); } messageClose(LOG_NLS); }else if (ACTIVE_STREAM(LOG_NLS_V)){ infoStreamPrint(LOG_NLS_V, 1, "system solved"); infoStreamPrint(LOG_NLS_V, 0, "%d retries\n%d restarts", retries, retries2+retries3); messageClose(LOG_NLS_V); printStatus(data, solverData, eqSystemNumber, &nfunc_evals, &xerror, &xerror_scaled, LOG_NLS_V); } scaling = solverData->useXScaling; if(scaling) solverData->useXScaling = 0; /* take the solution */ memcpy(systemData->nlsx, solverData->x, solverData->n*(sizeof(double))); /* try */ { int success = 0; #ifndef OMC_EMCC MMC_TRY_INTERNAL(simulationJumpBuffer) #endif wrapper_fvec_hybrj(&solverData->n, solverData->x, solverData->fvec, solverData->fjac, &solverData->ldfjac, &iflag, (void*) &dataAndSysNumber); success = 1; #ifndef OMC_EMCC MMC_CATCH_INTERNAL(simulationJumpBuffer) #endif /* catch */ if (!success) { warningStreamPrint(LOG_STDOUT, 0, "Non-Linear Solver try to handle a problem with a called assert."); solverData->info = 4; xerror_scaled = 1; xerror = 1; assertCalled = 1; success = 0; giveUp = 0; } } if(scaling) solverData->useXScaling = 1; }
/*! \fn Backtracking * * forth damping heuristic: * Calculate new function h:R^n->R ; h(x) = 1/2 * ||f(x)|| ^2 * g(lambda) = h(x_old + lambda * x_increment) * find minimum of g with golden ratio method * tau = golden ratio * * compiler flag: -newton = damped_bt */ void Backtracking(double* x, int(*f)(int*, double*, double*, void*, int), double current_fvec_enorm, int* n, double* fvec, DATA_NEWTON* solverData, void* userdata) { int i,j; double enorm_new, enorm_f, lambda, a1, b1, a, b, tau, g1, g2; double tolerance = 1e-3; /* saving current function values in f_old */ memcpy(solverData->f_old, fvec, *n*sizeof(double)); for (i=0; i<*n; i++) solverData->x_new[i]=x[i]-solverData->x_increment[i]; /* calculate new function values */ (*f)(n, solverData->x_new, fvec, userdata, 1); solverData->nfev++; /* calculate new enorm */ enorm_new = enorm_(n,fvec); /* Backtracking only if full newton step is useless */ if (enorm_new >= current_fvec_enorm) { infoStreamPrint(LOG_NLS_V, 0, "Start Backtracking\n enorm_new= %f \t current_fvec_enorm=%f",enorm_new, current_fvec_enorm); /* h(x) = 1/2 * ||f(x)|| ^2 * g(lambda) = h(x_old + lambda * x_increment) * find minimum of g with golden ratio method * tau = golden ratio * */ a = 0; b = 1; tau = 0.61803398875; a1 = a + (1-tau)*(b-a); /* g1 = g(a1) = h(x_old - a1 * x_increment) = 1/2 * ||f(x_old- a1 * x_increment)||^2 */ solverData->x_new[i] = x[i]- a1 * solverData->x_increment[i]; (*f)(n, solverData->x_new, fvec, userdata, 1); solverData->nfev++; enorm_f= enorm_(n,fvec); g1 = 0.5 * enorm_f * enorm_f; b1 = a + tau * (b-a); /* g2 = g(b1) = h(x_old - b1 * x_increment) = 1/2 * ||f(x_old- b1 * x_increment)||^2 */ solverData->x_new[i] = x[i]- b1 * solverData->x_increment[i]; (*f)(n, solverData->x_new, fvec, userdata, 1); solverData->nfev++; enorm_f= enorm_(n,fvec); g2 = 0.5 * enorm_f * enorm_f; while ( (b - a) > tolerance) { if (g1<g2) { b = b1; b1 = a1; a1 = a + (1-tau)*(b-a); g2 = g1; /* g1 = g(a1) = h(x_old - a1 * x_increment) = 1/2 * ||f(x_old- a1 * x_increment)||^2 */ solverData->x_new[i] = x[i]- a1 * solverData->x_increment[i]; (*f)(n, solverData->x_new, fvec, userdata, 1); solverData->nfev++; enorm_f= enorm_(n,fvec); g1 = 0.5 * enorm_f * enorm_f; } else { a = a1; a1 = b1; b1 = a + tau * (b-a); g1 = g2; /* g2 = g(b1) = h(x_old - b1 * x_increment) = 1/2 * ||f(x_old- b1 * x_increment)||^2 */ solverData->x_new[i] = x[i]- b1 * solverData->x_increment[i]; (*f)(n, solverData->x_new, fvec, userdata, 1); solverData->nfev++; enorm_f= enorm_(n,fvec); g2 = 0.5 * enorm_f * enorm_f; } } lambda = (a+b)/2; /* print lambda */ infoStreamPrint(LOG_NLS_V, 0, "Backtracking - lambda = %e", lambda); for (i=0; i<*n; i++) solverData->x_new[i]=x[i]-lambda*solverData->x_increment[i]; /* calculate new function values */ (*f)(n, solverData->x_new, fvec, userdata, 1); solverData->nfev++; } }
/*< subroutine qrfac(m,n,a,lda,pivot,ipvt,lipvt,rdiag,acnorm,wa) >*/ /* Subroutine */ int qrfac_(integer *m, integer *n, doublereal *a, integer * lda, logical *pivot, integer *ipvt, integer *lipvt, doublereal *rdiag, doublereal *acnorm, doublereal *wa) { /* Initialized data */ static doublereal one = 1.; /* constant */ static doublereal p05 = .05; /* constant */ static doublereal zero = 0.; /* constant */ /* System generated locals */ integer a_dim1, a_offset, i__1, i__2, i__3; doublereal d__1, d__2, d__3; /* Builtin functions */ double sqrt(doublereal); /* Local variables */ integer i__, j, k, jp1; doublereal sum; integer kmax; doublereal temp; integer minmn; extern doublereal enorm_(integer *, doublereal *); doublereal epsmch; extern doublereal dpmpar_(integer *); doublereal ajnorm; (void)lipvt; /*< integer m,n,lda,lipvt >*/ /*< integer ipvt(lipvt) >*/ /*< logical pivot >*/ /*< double precision a(lda,n),rdiag(n),acnorm(n),wa(n) >*/ /* ********** */ /* subroutine qrfac */ /* this subroutine uses householder transformations with column */ /* pivoting (optional) to compute a qr factorization of the */ /* m by n matrix a. that is, qrfac determines an orthogonal */ /* matrix q, a permutation matrix p, and an upper trapezoidal */ /* matrix r with diagonal elements of nonincreasing magnitude, */ /* such that a*p = q*r. the householder transformation for */ /* column k, k = 1,2,...,min(m,n), is of the form */ /* t */ /* i - (1/u(k))*u*u */ /* where u has zeros in the first k-1 positions. the form of */ /* this transformation and the method of pivoting first */ /* appeared in the corresponding linpack subroutine. */ /* the subroutine statement is */ /* subroutine qrfac(m,n,a,lda,pivot,ipvt,lipvt,rdiag,acnorm,wa) */ /* where */ /* m is a positive integer input variable set to the number */ /* of rows of a. */ /* n is a positive integer input variable set to the number */ /* of columns of a. */ /* a is an m by n array. on input a contains the matrix for */ /* which the qr factorization is to be computed. on output */ /* the strict upper trapezoidal part of a contains the strict */ /* upper trapezoidal part of r, and the lower trapezoidal */ /* part of a contains a factored form of q (the non-trivial */ /* elements of the u vectors described above). */ /* lda is a positive integer input variable not less than m */ /* which specifies the leading dimension of the array a. */ /* pivot is a logical input variable. if pivot is set true, */ /* then column pivoting is enforced. if pivot is set false, */ /* then no column pivoting is done. */ /* ipvt is an integer output array of length lipvt. ipvt */ /* defines the permutation matrix p such that a*p = q*r. */ /* column j of p is column ipvt(j) of the identity matrix. */ /* if pivot is false, ipvt is not referenced. */ /* lipvt is a positive integer input variable. if pivot is false, */ /* then lipvt may be as small as 1. if pivot is true, then */ /* lipvt must be at least n. */ /* rdiag is an output array of length n which contains the */ /* diagonal elements of r. */ /* acnorm is an output array of length n which contains the */ /* norms of the corresponding columns of the input matrix a. */ /* if this information is not needed, then acnorm can coincide */ /* with rdiag. */ /* wa is a work array of length n. if pivot is false, then wa */ /* can coincide with rdiag. */ /* subprograms called */ /* minpack-supplied ... dpmpar,enorm */ /* fortran-supplied ... dmax1,dsqrt,min0 */ /* argonne national laboratory. minpack project. march 1980. */ /* burton s. garbow, kenneth e. hillstrom, jorge j. more */ /* ********** */ /*< integer i,j,jp1,k,kmax,minmn >*/ /*< double precision ajnorm,epsmch,one,p05,sum,temp,zero >*/ /*< double precision dpmpar,enorm >*/ /*< data one,p05,zero /1.0d0,5.0d-2,0.0d0/ >*/ /* Parameter adjustments */ --wa; --acnorm; --rdiag; a_dim1 = *lda; a_offset = 1 + a_dim1; a -= a_offset; --ipvt; /* Function Body */ /* epsmch is the machine precision. */ /*< epsmch = dpmpar(1) >*/ epsmch = dpmpar_(&c__1); /* compute the initial column norms and initialize several arrays. */ /*< do 10 j = 1, n >*/ i__1 = *n; for (j = 1; j <= i__1; ++j) { /*< acnorm(j) = enorm(m,a(1,j)) >*/ acnorm[j] = enorm_(m, &a[j * a_dim1 + 1]); /*< rdiag(j) = acnorm(j) >*/ rdiag[j] = acnorm[j]; /*< wa(j) = rdiag(j) >*/ wa[j] = rdiag[j]; /*< if (pivot) ipvt(j) = j >*/ if (*pivot) { ipvt[j] = j; } /*< 10 continue >*/ /* L10: */ } /* reduce a to r with householder transformations. */ /*< minmn = min0(m,n) >*/ minmn = min(*m,*n); /*< do 110 j = 1, minmn >*/ i__1 = minmn; for (j = 1; j <= i__1; ++j) { /*< if (.not.pivot) go to 40 >*/ if (! (*pivot)) { goto L40; } /* bring the column of largest norm into the pivot position. */ /*< kmax = j >*/ kmax = j; /*< do 20 k = j, n >*/ i__2 = *n; for (k = j; k <= i__2; ++k) { /*< if (rdiag(k) .gt. rdiag(kmax)) kmax = k >*/ if (rdiag[k] > rdiag[kmax]) { kmax = k; } /*< 20 continue >*/ /* L20: */ } /*< if (kmax .eq. j) go to 40 >*/ if (kmax == j) { goto L40; } /*< do 30 i = 1, m >*/ i__2 = *m; for (i__ = 1; i__ <= i__2; ++i__) { /*< temp = a(i,j) >*/ temp = a[i__ + j * a_dim1]; /*< a(i,j) = a(i,kmax) >*/ a[i__ + j * a_dim1] = a[i__ + kmax * a_dim1]; /*< a(i,kmax) = temp >*/ a[i__ + kmax * a_dim1] = temp; /*< 30 continue >*/ /* L30: */ } /*< rdiag(kmax) = rdiag(j) >*/ rdiag[kmax] = rdiag[j]; /*< wa(kmax) = wa(j) >*/ wa[kmax] = wa[j]; /*< k = ipvt(j) >*/ k = ipvt[j]; /*< ipvt(j) = ipvt(kmax) >*/ ipvt[j] = ipvt[kmax]; /*< ipvt(kmax) = k >*/ ipvt[kmax] = k; /*< 40 continue >*/ L40: /* compute the householder transformation to reduce the */ /* j-th column of a to a multiple of the j-th unit vector. */ /*< ajnorm = enorm(m-j+1,a(j,j)) >*/ i__2 = *m - j + 1; ajnorm = enorm_(&i__2, &a[j + j * a_dim1]); /*< if (ajnorm .eq. zero) go to 100 >*/ if (ajnorm == zero) { goto L100; } /*< if (a(j,j) .lt. zero) ajnorm = -ajnorm >*/ if (a[j + j * a_dim1] < zero) { ajnorm = -ajnorm; } /*< do 50 i = j, m >*/ i__2 = *m; for (i__ = j; i__ <= i__2; ++i__) { /*< a(i,j) = a(i,j)/ajnorm >*/ a[i__ + j * a_dim1] /= ajnorm; /*< 50 continue >*/ /* L50: */ } /*< a(j,j) = a(j,j) + one >*/ a[j + j * a_dim1] += one; /* apply the transformation to the remaining columns */ /* and update the norms. */ /*< jp1 = j + 1 >*/ jp1 = j + 1; /*< if (n .lt. jp1) go to 100 >*/ if (*n < jp1) { goto L100; } /*< do 90 k = jp1, n >*/ i__2 = *n; for (k = jp1; k <= i__2; ++k) { /*< sum = zero >*/ sum = zero; /*< do 60 i = j, m >*/ i__3 = *m; for (i__ = j; i__ <= i__3; ++i__) { /*< sum = sum + a(i,j)*a(i,k) >*/ sum += a[i__ + j * a_dim1] * a[i__ + k * a_dim1]; /*< 60 continue >*/ /* L60: */ } /*< temp = sum/a(j,j) >*/ temp = sum / a[j + j * a_dim1]; /*< do 70 i = j, m >*/ i__3 = *m; for (i__ = j; i__ <= i__3; ++i__) { /*< a(i,k) = a(i,k) - temp*a(i,j) >*/ a[i__ + k * a_dim1] -= temp * a[i__ + j * a_dim1]; /*< 70 continue >*/ /* L70: */ } /*< if (.not.pivot .or. rdiag(k) .eq. zero) go to 80 >*/ if (! (*pivot) || rdiag[k] == zero) { goto L80; } /*< temp = a(j,k)/rdiag(k) >*/ temp = a[j + k * a_dim1] / rdiag[k]; /*< rdiag(k) = rdiag(k)*dsqrt(dmax1(zero,one-temp**2)) >*/ /* Computing MAX */ /* Computing 2nd power */ d__3 = temp; d__1 = zero, d__2 = one - d__3 * d__3; rdiag[k] *= sqrt((max(d__1,d__2))); /*< if (p05*(rdiag(k)/wa(k))**2 .gt. epsmch) go to 80 >*/ /* Computing 2nd power */ d__1 = rdiag[k] / wa[k]; if (p05 * (d__1 * d__1) > epsmch) { goto L80; } /*< rdiag(k) = enorm(m-j,a(jp1,k)) >*/ i__3 = *m - j; rdiag[k] = enorm_(&i__3, &a[jp1 + k * a_dim1]); /*< wa(k) = rdiag(k) >*/ wa[k] = rdiag[k]; /*< 80 continue >*/ L80: /*< 90 continue >*/ /* L90: */ ; } /*< 100 continue >*/ L100: /*< rdiag(j) = -ajnorm >*/ rdiag[j] = -ajnorm; /*< 110 continue >*/ /* L110: */ } /*< return >*/ return 0; /* last card of subroutine qrfac. */ /*< end >*/ } /* qrfac_ */
/*! \fn solve non-linear system with newton method * * \param [in] [data] * [sysNumber] index of the corresponding non-linear system * * \author wbraun */ int solveNewton(DATA *data, threadData_t *threadData, int sysNumber) { NONLINEAR_SYSTEM_DATA* systemData = &(data->simulationInfo->nonlinearSystemData[sysNumber]); DATA_NEWTON* solverData = (DATA_NEWTON*)(systemData->solverData); int eqSystemNumber = 0; int i; double xerror = -1, xerror_scaled = -1; int success = 0; int nfunc_evals = 0; int continuous = 1; double local_tol = solverData->ftol; int giveUp = 0; int retries = 0; int retries2 = 0; int nonContinuousCase = 0; modelica_boolean *relationsPreBackup = NULL; int casualTearingSet = data->simulationInfo->nonlinearSystemData[sysNumber].strictTearingFunctionCall != NULL; DATA_USER* userdata = (DATA_USER*)malloc(sizeof(DATA_USER)); assert(userdata != NULL); userdata->data = (void*)data; userdata->threadData = threadData; userdata->sysNumber = sysNumber; /* * We are given the number of the non-linear system. * We want to look it up among all equations. */ eqSystemNumber = systemData->equationIndex; local_tol = solverData->ftol; relationsPreBackup = (modelica_boolean*) malloc(data->modelData->nRelations*sizeof(modelica_boolean)); solverData->nfev = 0; /* try to calculate jacobian only once at the beginning of the iteration */ solverData->calculate_jacobian = 0; // Initialize lambda variable if (data->simulationInfo->nonlinearSystemData[sysNumber].homotopySupport) { solverData->x[solverData->n] = 1.0; solverData->x_new[solverData->n] = 1.0; } else { solverData->x[solverData->n] = 0.0; solverData->x_new[solverData->n] = 0.0; } /* debug output */ if(ACTIVE_STREAM(LOG_NLS_V)) { int indexes[2] = {1,eqSystemNumber}; infoStreamPrintWithEquationIndexes(LOG_NLS_V, 1, indexes, "Start solving Non-Linear System %d at time %g with Newton Solver", eqSystemNumber, data->localData[0]->timeValue); for(i = 0; i < solverData->n; i++) { infoStreamPrint(LOG_NLS_V, 1, "x[%d] = %.15e", i, data->simulationInfo->discreteCall ? systemData->nlsx[i] : systemData->nlsxExtrapolation[i]); infoStreamPrint(LOG_NLS_V, 0, "nominal = %g +++ nlsx = %g +++ old = %g +++ extrapolated = %g", systemData->nominal[i], systemData->nlsx[i], systemData->nlsxOld[i], systemData->nlsxExtrapolation[i]); messageClose(LOG_NLS_V); } messageClose(LOG_NLS_V); } /* set x vector */ if(data->simulationInfo->discreteCall) { memcpy(solverData->x, systemData->nlsx, solverData->n*(sizeof(double))); } else { memcpy(solverData->x, systemData->nlsxExtrapolation, solverData->n*(sizeof(double))); } /* start solving loop */ while(!giveUp && !success) { giveUp = 1; solverData->newtonStrategy = data->simulationInfo->newtonStrategy; _omc_newton(wrapper_fvec_newton, solverData, (void*)userdata); /* check for proper inputs */ if(solverData->info == 0) printErrorEqSyst(IMPROPER_INPUT, modelInfoGetEquation(&data->modelData->modelDataXml,eqSystemNumber), data->localData[0]->timeValue); /* reset non-contunuousCase */ if(nonContinuousCase && xerror > local_tol && xerror_scaled > local_tol) { memcpy(data->simulationInfo->relationsPre, relationsPreBackup, sizeof(modelica_boolean)*data->modelData->nRelations); nonContinuousCase = 0; } /* check for error */ xerror_scaled = enorm_(&solverData->n, solverData->fvecScaled); xerror = enorm_(&solverData->n, solverData->fvec); /* solution found */ if((xerror <= local_tol || xerror_scaled <= local_tol) && solverData->info > 0) { success = 1; nfunc_evals += solverData->nfev; if(ACTIVE_STREAM(LOG_NLS_V)) { infoStreamPrint(LOG_NLS_V, 0, "*** System solved ***\n%d restarts", retries); infoStreamPrint(LOG_NLS_V, 0, "nfunc = %d +++ error = %.15e +++ error_scaled = %.15e", nfunc_evals, xerror, xerror_scaled); for(i = 0; i < solverData->n; i++) infoStreamPrint(LOG_NLS_V, 0, "x[%d] = %.15e\n\tresidual = %e", i, solverData->x[i], solverData->fvec[i]); } /* take the solution */ memcpy(systemData->nlsx, solverData->x, solverData->n*(sizeof(double))); /* Then try with old values (instead of extrapolating )*/ } // If this is the casual tearing set (only exists for dynamic tearing), break after first try else if(retries < 1 && casualTearingSet) { giveUp = 1; infoStreamPrint(LOG_NLS_V, 0, "### No Solution for the casual tearing set at the first try! ###"); } else if(retries < 1) { memcpy(solverData->x, systemData->nlsxOld, solverData->n*(sizeof(double))); retries++; giveUp = 0; nfunc_evals += solverData->nfev; infoStreamPrint(LOG_NLS_V, 0, " - iteration making no progress:\t try old values."); /* try to vary the initial values */ /* evaluate jacobian in every step now */ solverData->calculate_jacobian = 1; } else if(retries < 2) { for(i = 0; i < solverData->n; i++) solverData->x[i] += systemData->nominal[i] * 0.01; retries++; giveUp = 0; nfunc_evals += solverData->nfev; infoStreamPrint(LOG_NLS_V, 0, " - iteration making no progress:\t vary solution point by 1%%."); /* try to vary the initial values */ } else if(retries < 3) { for(i = 0; i < solverData->n; i++) solverData->x[i] = systemData->nominal[i]; retries++; giveUp = 0; nfunc_evals += solverData->nfev; infoStreamPrint(LOG_NLS_V, 0, " - iteration making no progress:\t try nominal values as initial solution."); } else if(retries < 4 && data->simulationInfo->discreteCall) { /* try to solve non-continuous * work-a-round: since other wise some model does * stuck in event iteration. e.g.: Modelica.Mechanics.Rotational.Examples.HeatLosses */ memcpy(solverData->x, systemData->nlsxOld, solverData->n*(sizeof(double))); retries++; /* try to solve a discontinuous system */ continuous = 0; nonContinuousCase = 1; memcpy(relationsPreBackup, data->simulationInfo->relationsPre, sizeof(modelica_boolean)*data->modelData->nRelations); giveUp = 0; nfunc_evals += solverData->nfev; infoStreamPrint(LOG_NLS_V, 0, " - iteration making no progress:\t try to solve a discontinuous system."); } else if(retries2 < 4) { memcpy(solverData->x, systemData->nlsxOld, solverData->n*(sizeof(double))); /* reduce tolarance */ local_tol = local_tol*10; retries = 0; retries2++; giveUp = 0; nfunc_evals += solverData->nfev; infoStreamPrint(LOG_NLS_V, 0, " - iteration making no progress:\t reduce the tolerance slightly to %e.", local_tol); } else { printErrorEqSyst(ERROR_AT_TIME, modelInfoGetEquation(&data->modelData->modelDataXml,eqSystemNumber), data->localData[0]->timeValue); if(ACTIVE_STREAM(LOG_NLS_V)) { infoStreamPrint(LOG_NLS_V, 0, "### No Solution! ###\n after %d restarts", retries); infoStreamPrint(LOG_NLS_V, 0, "nfunc = %d +++ error = %.15e +++ error_scaled = %.15e", nfunc_evals, xerror, xerror_scaled); if(ACTIVE_STREAM(LOG_NLS_V)) for(i = 0; i < solverData->n; i++) infoStreamPrint(LOG_NLS_V, 0, "x[%d] = %.15e\n\tresidual = %e", i, solverData->x[i], solverData->fvec[i]); } } } if(ACTIVE_STREAM(LOG_NLS_V)) messageClose(LOG_NLS_V); free(relationsPreBackup); /* write statistics */ systemData->numberOfFEval = solverData->numberOfFunctionEvaluations; systemData->numberOfIterations = solverData->numberOfIterations; return success; }
/* Subroutine */ int dogleg_(integer *n, doublereal *r__, integer *lr, doublereal *diag, doublereal *qtb, doublereal *delta, doublereal *x, doublereal *wa1, doublereal *wa2) { /* Initialized data */ static doublereal one = 1.; static doublereal zero = 0.; /* System generated locals */ integer i__1, i__2; doublereal d__1, d__2, d__3, d__4; /* Builtin functions */ double sqrt(doublereal); /* Local variables */ static integer i__, j, k, l, jj, jp1; static doublereal sum, temp, alpha, bnorm; extern doublereal enorm_(integer *, doublereal *); static doublereal gnorm, qnorm, epsmch; extern doublereal dpmpar_(integer *); static doublereal sgnorm; /* ********** */ /* subroutine dogleg */ /* given an m by n matrix a, an n by n nonsingular diagonal */ /* matrix d, an m-vector b, and a positive number delta, the */ /* problem is to determine the convex combination x of the */ /* gauss-newton and scaled gradient directions that minimizes */ /* (a*x - b) in the least squares sense, subject to the */ /* restriction that the euclidean norm of d*x be at most delta. */ /* this subroutine completes the solution of the problem */ /* if it is provided with the necessary information from the */ /* qr factorization of a. that is, if a = q*r, where q has */ /* orthogonal columns and r is an upper triangular matrix, */ /* then dogleg expects the full upper triangle of r and */ /* the first n components of (q transpose)*b. */ /* the subroutine statement is */ /* subroutine dogleg(n,r,lr,diag,qtb,delta,x,wa1,wa2) */ /* where */ /* n is a positive integer input variable set to the order of r. */ /* r is an input array of length lr which must contain the upper */ /* triangular matrix r stored by rows. */ /* lr is a positive integer input variable not less than */ /* (n*(n+1))/2. */ /* diag is an input array of length n which must contain the */ /* diagonal elements of the matrix d. */ /* qtb is an input array of length n which must contain the first */ /* n elements of the vector (q transpose)*b. */ /* delta is a positive input variable which specifies an upper */ /* bound on the euclidean norm of d*x. */ /* x is an output array of length n which contains the desired */ /* convex combination of the gauss-newton direction and the */ /* scaled gradient direction. */ /* wa1 and wa2 are work arrays of length n. */ /* subprograms called */ /* minpack-supplied ... dpmpar,enorm */ /* fortran-supplied ... dabs,dmax1,dmin1,dsqrt */ /* argonne national laboratory. minpack project. march 1980. */ /* burton s. garbow, kenneth e. hillstrom, jorge j. more */ /* ********** */ /* Parameter adjustments */ --wa2; --wa1; --x; --qtb; --diag; --r__; /* Function Body */ /* epsmch is the machine precision. */ epsmch = dpmpar_(&c__1); /* first, calculate the gauss-newton direction. */ jj = *n * (*n + 1) / 2 + 1; i__1 = *n; for (k = 1; k <= i__1; ++k) { j = *n - k + 1; jp1 = j + 1; jj -= k; l = jj + 1; sum = zero; if (*n < jp1) { goto L20; } i__2 = *n; for (i__ = jp1; i__ <= i__2; ++i__) { sum += r__[l] * x[i__]; ++l; /* L10: */ } L20: temp = r__[jj]; if (temp != zero) { goto L40; } l = j; i__2 = j; for (i__ = 1; i__ <= i__2; ++i__) { /* Computing MAX */ d__2 = temp, d__3 = (d__1 = r__[l], abs(d__1)); temp = max(d__2,d__3); l = l + *n - i__; /* L30: */ } temp = epsmch * temp; if (temp == zero) { temp = epsmch; } L40: x[j] = (qtb[j] - sum) / temp; /* L50: */ } /* test whether the gauss-newton direction is acceptable. */ i__1 = *n; for (j = 1; j <= i__1; ++j) { wa1[j] = zero; wa2[j] = diag[j] * x[j]; /* L60: */ } qnorm = enorm_(n, &wa2[1]); if (qnorm <= *delta) { goto L140; } /* the gauss-newton direction is not acceptable. */ /* next, calculate the scaled gradient direction. */ l = 1; i__1 = *n; for (j = 1; j <= i__1; ++j) { temp = qtb[j]; i__2 = *n; for (i__ = j; i__ <= i__2; ++i__) { wa1[i__] += r__[l] * temp; ++l; /* L70: */ } wa1[j] /= diag[j]; /* L80: */ } /* calculate the norm of the scaled gradient and test for */ /* the special case in which the scaled gradient is zero. */ gnorm = enorm_(n, &wa1[1]); sgnorm = zero; alpha = *delta / qnorm; if (gnorm == zero) { goto L120; } /* calculate the point along the scaled gradient */ /* at which the quadratic is minimized. */ i__1 = *n; for (j = 1; j <= i__1; ++j) { wa1[j] = wa1[j] / gnorm / diag[j]; /* L90: */ } l = 1; i__1 = *n; for (j = 1; j <= i__1; ++j) { sum = zero; i__2 = *n; for (i__ = j; i__ <= i__2; ++i__) { sum += r__[l] * wa1[i__]; ++l; /* L100: */ } wa2[j] = sum; /* L110: */ } temp = enorm_(n, &wa2[1]); sgnorm = gnorm / temp / temp; /* test whether the scaled gradient direction is acceptable. */ alpha = zero; if (sgnorm >= *delta) { goto L120; } /* the scaled gradient direction is not acceptable. */ /* finally, calculate the point along the dogleg */ /* at which the quadratic is minimized. */ bnorm = enorm_(n, &qtb[1]); temp = bnorm / gnorm * (bnorm / qnorm) * (sgnorm / *delta); /* Computing 2nd power */ d__1 = sgnorm / *delta; /* Computing 2nd power */ d__2 = temp - *delta / qnorm; /* Computing 2nd power */ d__3 = *delta / qnorm; /* Computing 2nd power */ d__4 = sgnorm / *delta; temp = temp - *delta / qnorm * (d__1 * d__1) + sqrt(d__2 * d__2 + (one - d__3 * d__3) * (one - d__4 * d__4)); /* Computing 2nd power */ d__1 = sgnorm / *delta; alpha = *delta / qnorm * (one - d__1 * d__1) / temp; L120: /* form appropriate convex combination of the gauss-newton */ /* direction and the scaled gradient direction. */ temp = (one - alpha) * min(sgnorm,*delta); i__1 = *n; for (j = 1; j <= i__1; ++j) { x[j] = temp * wa1[j] + alpha * x[j]; /* L130: */ } L140: return 0; /* last card of subroutine dogleg. */ } /* dogleg_ */