- change handling for numerical issue for hessian

git-svn-id: https://openmodelica.org/svn/OpenModelica/trunk@18655 f25d12d1-65f4-0310-ae8a-bbce733d8d8e
OpenModelica · Jan 16, 2014 · f6c36c9 · f6c36c9
1 parent 0c00097
commit f6c36c9
Showing 1 changed file with 16 additions and 5 deletions.
diff --git a/SimulationRuntime/c/optimization/lagrangeFun/ipopt_hessian.c b/SimulationRuntime/c/optimization/lagrangeFun/ipopt_hessian.c
@@ -125,8 +125,16 @@ Bool ipopt_h(int n, double *v, Bool new_x, double obj_factor, int m, double *lam
            num_hessian(x, iData->time[p], iData, ll,iData->lagrange,mayer_yes,obj_factor);
            ll += iData->nx;
          }else{
-           for(i = 0; i< iData->nx; ++i)
-             iData->sh[i] = iData->d1[4]*( iData->invd1_4*ll[i+2*iData->nx] + ll[i] - ll[i + iData->nx]);
+           for(i = 0; i< iData->nx; ++i){
+             if(ll[i] != ll[i + iData->nx]){
+               if(iData->invd1_4*ll[i+2*iData->nx] != ll[i + iData->nx])
+                 iData->sh[i] = iData->d1[4]*(ll[i] + (iData->invd1_4*ll[i+2*iData->nx] - ll[i + iData->nx]));
+               else
+                 iData->sh[i] = iData->d1[4]*ll[i];
+             }else{
+               iData->sh[i] = ll[i+2*iData->nx];
+             }
+           }
            num_hessian(x, iData->time[p], iData, iData->sh ,iData->lagrange,mayer_yes,obj_factor);
          }
 
@@ -183,9 +191,12 @@ static int sumLagrange(IPOPT_DATA_ *iData, double * erg,int ii, int i, int j, in
   for(l = 0; l<iData->nx; ++l)
     sum += iData->H[l][i][j];
 
-  if(iData->lagrange)
-    sum += iData->br[p-1]*iData->oH[i][j];
-
+  if(iData->lagrange){
+    if(ii)
+      sum += iData->br[p-1]*iData->oH[i][j];
+    else
+      sum += iData->bl[p]*iData->oH[i][j];
+  }
   sum = iData->dt[ii]*sum;
   if(mayer_yes)
     sum += iData->mH[i][j];