Improve initial guesses for GBODE (#14050)

phannebohm · bernhardbachmann · web-flow · commit ef17c1b3623e · 2025-06-28T15:59:00.000+02:00
Also check for NaN in xloc and generated calls

Co-authored-by: bernhardbachmann &lt;bernhardbachmann@users.noreply.github.com&gt;
diff --git a/OMCompiler/SimulationRuntime/c/simulation/solver/gbode_nls.c b/OMCompiler/SimulationRuntime/c/simulation/solver/gbode_nls.c
@@ -502,17 +502,26 @@ NLS_SOLVER_STATUS solveNLS_gb(DATA *data, threadData_t *threadData, NONLINEAR_SY
       infoStreamPrint(OMC_LOG_GBODE_NLS, 0, "GBODE: Solution of NLS failedat time %g. Try with updated Jacobian.", gbData->time);
       set_kinsol_parameters(kin_mem, newtonMaxSteps, SUNFALSE, 1, newtonTol);
       solved = solveNLS(data, threadData, nlsData);
-      if (!solved) {
-        infoStreamPrint(OMC_LOG_STDOUT, 0, "GBODE: Solution of NLS failed at time %g, Try with less accuracy.", gbData->time);
-        set_kinsol_parameters(kin_mem, newtonMaxSteps, SUNFALSE, 1, 10*newtonTol);
-        solved = solveNLS(data, threadData, nlsData);
-      }
+    }
+    if (!solved) {
+      infoStreamPrint(OMC_LOG_GBODE_NLS, 0, "GBODE: Solution of NLS failed at time %g, Try with old start value.", gbData->time);
+      memcpy(nlsData->nlsx, nlsData->nlsxOld,  nlsData->size*sizeof(modelica_real));
+      set_kinsol_parameters(kin_mem, newtonMaxSteps, SUNFALSE, 1, newtonTol);
+      solved = solveNLS(data, threadData, nlsData);
+    }
+    if (!solved) {
+      infoStreamPrint(OMC_LOG_STDOUT, 0, "GBODE: Solution of NLS failed at time %g, Try with less accuracy.", gbData->time);
+      set_kinsol_parameters(kin_mem, newtonMaxSteps, SUNFALSE, 1, 10*newtonTol);
+      solved = solveNLS(data, threadData, nlsData);
     }
     if (OMC_ACTIVE_STREAM(OMC_LOG_GBODE_NLS)) get_kinsol_statistics(kin_mem);
   } else {
     solved = solveNLS(data, threadData, nlsData);
   }
 
+  if (solved)
+    infoStreamPrint(OMC_LOG_GBODE_NLS_V, 0, "GBODE: NLS solved.");
+
   if (OMC_ACTIVE_STREAM(OMC_LOG_GBODE_NLS)) {
     cpu_time_used = rt_ext_tp_tock(&clock);
     infoStreamPrint(OMC_LOG_GBODE_NLS, 0, "Time needed for solving the NLS:  %20.16g", cpu_time_used);
@@ -551,9 +560,13 @@ void residual_MS(RESIDUAL_USERDATA* userData, const double *xloc, double *res, c
   const int nStages = gbData->tableau->nStages;
 
   // Set states
+  for (i = 0; i < nStates; i++)
+    assertStreamPrint(threadData, !isnan(xloc[i]), "residual_MS: xloc is NAN");
   memcpy(sData->realVars, xloc, nStates*sizeof(modelica_real));
   // Evaluate right hand side of ODE
   gbode_fODE(data, threadData, &(gbData->stats.nCallsODE));
+  for (i = 0; i < nStates; i++)
+    assertStreamPrint(threadData, !isnan(fODE[i]), "residual_MS: fODE is NAN");
 
   // Evaluate residuals
   for (i = 0; i < nStates; i++) {
@@ -596,6 +609,7 @@ void residual_MS_MR(RESIDUAL_USERDATA* userData, const double *xloc, double *res
   // Set fast states
   // ph: are slow states interpolated and set correctly?
   for (ii = 0; ii < nFastStates; ii++) {
+    assertStreamPrint(threadData, !isnan(xloc[ii]), "residual_MS_MR: xloc is NAN");
     i = gbfData->fastStatesIdx[ii];
     sData->realVars[i] = xloc[ii];
   }
@@ -605,9 +619,10 @@ void residual_MS_MR(RESIDUAL_USERDATA* userData, const double *xloc, double *res
   // Evaluate residuals
   for (ii = 0; ii < nFastStates; ii++) {
     i = gbfData->fastStatesIdx[ii];
+    assertStreamPrint(threadData, !isnan(fODE[i]), "residual_MS_MR: fODE is NAN");
     res[ii] = gbfData->res_const[i]
               - xloc[ii] * gbfData->tableau->c[nStages-1]
-              + fODE[ i] * gbfData->tableau->b[nStages-1] * gbfData->stepSize;
+              + fODE[i]  * gbfData->tableau->b[nStages-1] * gbfData->stepSize;
   }
 }
 
@@ -639,15 +654,19 @@ void residual_DIRK(RESIDUAL_USERDATA* userData, const double *xloc, double *res,
   const int nStates = data->modelData->nStates;
   const int nStages = gbData->tableau->nStages;
   const int stage_  = gbData->act_stage;
+  const modelica_real fac = gbData->stepSize * gbData->tableau->A[stage_ * nStages + stage_];
 
   // Set states
+  for (i = 0; i < nStates; i++)
+    assertStreamPrint(threadData, !isnan(xloc[i]), "residual_DIRK: xloc is NAN");
   memcpy(sData->realVars, xloc, nStates*sizeof(double));
   // Evaluate right hand side of ODE
   gbode_fODE(data, threadData, &(gbData->stats.nCallsODE));
 
   // Evaluate residuals
   for (i = 0; i < nStates; i++) {
-    res[i] = gbData->res_const[i] - xloc[i] + gbData->stepSize * gbData->tableau->A[stage_ * nStages + stage_] * fODE[i];
+    assertStreamPrint(threadData, !isnan(fODE[i]), "residual_DIRK: fODE is NAN");
+    res[i] = gbData->res_const[i] - xloc[i] + fac * fODE[i];
   }
 
   if (OMC_ACTIVE_STREAM(OMC_LOG_GBODE_NLS)) {
@@ -694,6 +713,7 @@ void residual_DIRK_MR(RESIDUAL_USERDATA* userData, const double *xloc, double *r
   // Set fast states
   // ph: are slow states interpolated and set correctly?
   for (ii = 0; ii < nFastStates; ii++) {
+    assertStreamPrint(threadData, !isnan(xloc[ii]), "residual_DIRK_MR: xloc is NAN");
     i = gbfData->fastStatesIdx[ii];
     sData->realVars[i] = xloc[ii];
   }
@@ -703,6 +723,7 @@ void residual_DIRK_MR(RESIDUAL_USERDATA* userData, const double *xloc, double *r
   // Evaluate residuals
   for (ii = 0; ii < nFastStates; ii++) {
     i = gbfData->fastStatesIdx[ii];
+    assertStreamPrint(threadData, !isnan(fODE[i]), "residual_DIRK_MR: fODE is NAN");
     res[ii] = gbfData->res_const[i] - xloc[ii] + fac * fODE[i];
   }
 }
@@ -735,13 +756,18 @@ void residual_IRK(RESIDUAL_USERDATA* userData, const double *xloc, double *res,
   const int nStates = data->modelData->nStates;
   int stage, stage_;
 
+  for (i = 0; i < nStages*nStates; i++)
+    assertStreamPrint(threadData, !isnan(xloc[i]), "residual_IRK: xloc is NAN");
+
   // Update the derivatives for current estimate of the states
   for (stage_ = 0; stage_ < nStages; stage_++) {
     /* Evaluate ODE for each stage_ */
     if (!gbData->tableau->isKLeftAvailable || stage_ > 0) {
       sData->timeValue = gbData->time + gbData->tableau->c[stage_] * gbData->stepSize;
       memcpy(sData->realVars, xloc + stage_ * nStates, nStates*sizeof(double));
       gbode_fODE(data, threadData, &(gbData->stats.nCallsODE));
+      for (i = 0; i < nStates; i++)
+        assertStreamPrint(threadData, !isnan(fODE[i]), "residual_IRK: fODE is NAN");
       memcpy(gbData->k + stage_ * nStates, fODE, nStates*sizeof(double));
     } else {
       // memcpy(sData->realVars, gbData->yLeft, nStates*sizeof(double));
@@ -801,6 +827,7 @@ int jacobian_SR_column(DATA* data, threadData_t *threadData, JACOBIAN *jacobian,
   }
 
   for (i = 0; i < jacobian->sizeCols; i++) {
+    assertStreamPrint(threadData, !isnan(jacobian_ODE->resultVars[i]), "jacobian_SR_column: jacobian_ODE is NAN");
     jacobian->resultVars[i] = fac * jacobian_ODE->resultVars[i] - jacobian->seedVars[i];
   }
 
@@ -854,6 +881,7 @@ int jacobian_MR_column(DATA* data, threadData_t *threadData, JACOBIAN *jacobian,
 
   for (ii = 0; ii < nFastStates; ii++) {
     i = gbData->fastStatesIdx[ii];
+    assertStreamPrint(threadData, !isnan(jacobian_ODE->resultVars[i]), "jacobian_MR_column: jacobian_ODE is NAN");
     jacobian->resultVars[ii] = fac * jacobian_ODE->resultVars[i] - jacobian->seedVars[ii];
   }
 
@@ -909,6 +937,8 @@ int jacobian_IRK_column(DATA* data, threadData_t *threadData, JACOBIAN *jacobian
 
   // call jacobian_ODE with the mapped seedVars
   data->callback->functionJacA_column(data, threadData, jacobian_ODE, NULL);
+  for (i = 0; i < nStates; i++)
+    assertStreamPrint(threadData, !isnan(jacobian_ODE->resultVars[i]), "jacobian_SR_column: jacobian_ODE is NAN");
 
   /* Update resultVars array for corresponding jacobian->seedVars*/
   for (stage = 0; stage < nStages; stage++) {
diff --git a/OMCompiler/SimulationRuntime/c/simulation/solver/gbode_step.c b/OMCompiler/SimulationRuntime/c/simulation/solver/gbode_step.c
@@ -292,8 +292,14 @@ int expl_diag_impl_RK(DATA* data, threadData_t* threadData, SOLVER_INFO* solverI
 
       // Set start vector
       memcpy(nlsData->nlsx,    gbData->yOld, nStates*sizeof(modelica_real));
-      memcpy(nlsData->nlsxOld, gbData->yOld, nStates*sizeof(modelica_real));
-      extrapolation_gb(gbData, nlsData->nlsxExtrapolation, gbData->time + gbData->tableau->c[stage_] * gbData->stepSize);
+      addSmultVec_gb(nlsData->nlsxOld, gbData->yv, gbData->kv, gbData->time + gbData->tableau->c[stage_] * gbData->stepSize - gbData->tv[0], nStates);
+
+      if (stage>1) {
+        extrapolation_hermite_gb(nlsData->nlsxExtrapolation, gbData->nStates, gbData->time + gbData->tableau->c[stage_-2] * gbData->stepSize, gbData->x + (stage_-2) * nStates, gbData->k + (stage_-2) * nStates,
+                             gbData->time + gbData->tableau->c[stage_-1] * gbData->stepSize, gbData->x + (stage_-1) * nStates, gbData->k + (stage_-1) * nStates, gbData->time + gbData->tableau->c[stage_] * gbData->stepSize);
+      } else {
+        extrapolation_gb(gbData, nlsData->nlsxExtrapolation, gbData->time + gbData->tableau->c[stage_] * gbData->stepSize);
+      }
 
       solved = solveNLS_gb(data, threadData, nlsData, gbData);
 
@@ -315,6 +321,7 @@ int expl_diag_impl_RK(DATA* data, threadData_t* threadData, SOLVER_INFO* solverI
     // copy last calculation of fODE, which should coincide with k[i], here, it yields stage == stage_
     memcpy(gbData->k + stage_ * nStates, fODE, nStates*sizeof(double));
   }
+  infoStreamPrint(OMC_LOG_GBODE_NLS_V, 0, "GBODE: all stages done.");
 
   // Apply RK-scheme for determining the approximations at (gbData->time + gbData->stepSize)
   // y       = yold+h*sum(b[stage_]  * k[stage_], stage_=1..nStages);
diff --git a/OMCompiler/SimulationRuntime/c/simulation/solver/gbode_util.c b/OMCompiler/SimulationRuntime/c/simulation/solver/gbode_util.c
@@ -62,7 +62,7 @@ void addSmultVec_gbf(double* a, double* b, double *c, double s, int nIdx, int* i
 }
 
 /**
- * @brief Scalar multiplication and vector addition a = b + s*c for selected indices.
+ * @brief Scalar multiplication and vector addition a = b + s*c.
  *
  * Determines the scalar multiplication of an vector and adds the result
  * to another vector.
@@ -386,6 +386,24 @@ void extrapolation_gbf(DATA_GBODE* gbData, double* nlsxExtrapolation, double tim
   }
 }
 
+/**
+ * @brief Extrapolation for all states.
+ *
+ * Using interpolation method specified in gbData->interpolation.
+ *
+ * @param gbData              Generic ODE solver data.
+ * @param nlsxExtrapolation   On output contains function values at extrapolation point time.
+ * @param time                Extrapolation time.
+ */
+void extrapolation_hermite_gb(double* nlsxExtrapolation, int nStates, double t0, double *x0, double* k0, double t1, double *x1, double* k1, double time)
+{
+  gb_interpolation(GB_INTERPOL_HERMITE,
+                   t0, x0,  k0,
+                   t1, x1,  k1,
+                   time, nlsxExtrapolation,
+                   nStates, NULL, nStates, NULL, NULL, NULL);
+}
+
 /**
  * @brief Extrapolation for all states.
  *
diff --git a/OMCompiler/SimulationRuntime/c/simulation/solver/gbode_util.h b/OMCompiler/SimulationRuntime/c/simulation/solver/gbode_util.h
@@ -55,6 +55,7 @@ void gb_interpolation(enum GB_INTERPOL_METHOD interpolMethod, double ta, double*
                         int nIdx, int* idx, int nStates, BUTCHER_TABLEAU* tableau, double* x, double *k);
 double error_interpolation_gb(DATA_GBODE* gbData, int nIdx, int* idx, double tol);
 void extrapolation_gb(DATA_GBODE* gbData, double* nlsxExtrapolation, double time);
+void extrapolation_hermite_gb(double* nlsxExtrapolation, int nStates, double t0, double *x0, double* k0, double t1, double *x1, double* k1, double time);
 void extrapolation_gbf(DATA_GBODE* gbData, double* nlsxExtrapolation, double time);
 
 // Copy only specific values referenced by an index vector
