sqp_2.c

an modern SQP method to make the original matrix more concise

/*
	This function implements the algorithm sqp_2 as described in Chapter 7.

	sqp_2 solves the following minimization problem:

    Find, x_0,...,x_{n-1}, that minimizes f(x_0,...,x_{n-1})

    subject to   g_j(x_0,...,x_{n-1}) = 0,  j = 0,1,...,m-1
                  
                 g_j(x_0,...,x_{n-1}) <= 0, j = m,...,m+p-1
                  
    with continuously differentiable functions f and g_j, j=0,...,m+p-1
 
 	The function declaration for sqp_2 is as follows:
 	
 		void sqp_2( fun, x, n, m, p, tol )
 	
 	(1) fun:
 	
 	Here, fun is a pointer to a user supplied function that returns the value of 
 	f(x_0,...,x_{n-1}) and g_j(x_0,...,x_{n-1}).  The declaration for fun must be
 	as follows:
 	
 		void fun( f, g, x )
 		long double *f, *g, *x;
 		{
 		   ...
 		   *f = ...
 		   g[0] = ..,
 		   ...
 		   g[m-1] = ...
 		   ...
 		   g[m+p-1] = ...
 		}
 	
 	(2) x:
 	
 	Here, x is a pointer to the n dimensional vector of unknown parameters.
 	On input x contains an initial guess.
 	On completion of the algorithm x contains the desired solution.
 	
 	(3) n:
 	Here, n is an integer that indicates the size of the vector x.
 	
 	(4) m:
 	Here, m is an integer that indicates the number of equality constraints.

 	(5) p:
 	Here, p is an integer that indicates the number of inequality constraints.

 	(6) tol:
 	Here, tol is a positive real number that indicates the desired solution error. 	

	During the solution of the problem the function prints the iteration number,
	the value of the function to be minimized, the norm of the constraint vector,
	the norm of the change in x from the last iteration, and the norm of the change
	in the Lagrangian function. 	
*/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "math_util.h"
#define MAX_ITT_SQP 200
#define dmin(x,y) (((x)>(y))?(y):(x))
#define dmax(x,y) (((x)<(y))?(y):(x))
long double *x0, *x1, *f0, *f1, *g0, *g1, *df0, *df1, **dg0, **dg1, 
            *lam_1, **Lxx;
long double *pen, *h_bx, *dL, alpha, ndL=0.0;
int *act, n_act, itt;
void get_storage( int, int, int );
void free_storage( int, int, int );
void sqp_fd_grad( void(*)(), long double *, long double *, long double *, 
                  long double *, long double **, int, int, int );
void find_active( int, int, int );
void vec_incr( long double *, long double *, long double *, long double, int );
void cal_h( int, int, int );
void vec_muls( long double *, long double *, long double, int );
long double sqp_line_search( void(*)(), 
		long double *, long double *, long double *, 
		long double *, long double *, long double *,
		long double *, long double *, int, int, int, long double );
void update_Lxx( int, int, int );
void form_dL( int, int, int );

void sqp_2( fun, x, n, m, p, tol )
void (*fun)( long double *, long double *, long double * );
long double *x, tol;
int n, m, p;
{
   int i;
   long double dfn = 0.0, dgn = 0.0, nh = 0.0, adf = 0.0, dx = 0.0, norm_g = 0.0;
   
   if( tol <= 0.0) tol = 1.0e-4;
   if( check_data( n, m, p ) == 1 ) return;
   
   get_storage( n, m, p );vec_null( pen, m+p );vec_null( lam_1, m+p );
   
   vec_copy( x0, x, n );
   
   (*fun)( f0, g0, x0 );sqp_fd_grad( fun, f0, g0, x0, df0, dg0, n, m, p );

   norm_g = enorm( g0, m );
   
   mat_null( Lxx, n, n );for(i = 0; i < n; i++ ) Lxx[i][i] = 1.0;

   for( itt = 0; itt < MAX_ITT_SQP; itt++ ) {
      printf("itt = %d, f = %8.3Le, |g| = %8.3Le, |dx| = %8.3Le, |dL| = %8.3Le\n",itt,f0[0],norm_g,dx,ndL);
/*
   Step 1 - Find the active constraints
*/
      find_active( n, m, p );
/*
   Step 2 - Calculate (1) The search direction, h,  
            (2) The gradient of the Lagrangian, and
            (3) The penalty parameter for the line search
*/
      cal_h( n, m, p );
      
      form_dL( n, m, p );

      ndL=enorm(dL,n+m+p);if( ndL <= tol ) {printf("\nnorm( dL ) = %8.3Le\n",ndL);vec_copy(x,x0,n);free_storage(n,m,p);return;}
      
      if( itt == 0 ){dfn=enorm(df0,n);for(i=0;i<m+p;i++) pen[i]=dfn/dmax(MACH_EPS,enorm(dg0[i],m+p));}

      if( itt > 0 ) {for( i = 0; i < m+p; i++ ) {pen[i] = dmax(fabs(lam_1[i]),0.5*(fabs(lam_1[i])+pen[i]));}}
/*
   Step 3 - Find alpha
*/
      alpha = sqp_line_search( fun, f0, g0, x0, f1, g1, x1, h_bx, pen, n, m, p, tol ); 
/*
   Step 4 - Check for convergence
*/
      norm_g = enorm( g1, m );      
      dx = 0.0;for(i=0;i<n;i++) dx+=(x1[i]-x0[i])*(x1[i]-x0[i]);dx=sqrt(dx);      
      /*if( dx <= tol ){printf("\nnorm(x1-x0) = %8.3Le\n",dx);vec_copy(x,x1,n);free_storage(n,m,p);return;}*/
      /*adf = fabs(f1[0]-f0[0]);if( adf <= tol ){printf("|f1-f0|=%Le\n",adf);vec_copy(x,x1,n);free_storage(n,m,p);return;}*/     
/*
   Step 5 - Update the Hessian of the Lagrangian, Lxx
*/
      sqp_fd_grad( fun, f1, g1, x1, df1, dg1, n, m, p );
      
      update_Lxx( n, m, p );            
      
/*
   Step 6 - Use the current solution as a starting guess for the next iteration
*/
      f0[0]=f1[0];
      vec_copy(x0,x1,n);
      vec_copy(g0,g1,(m+p));
      vec_copy(df0,df1,n);
      mat_copy(dg0,dg1,(m+p),n); 
   }
   printf("\nToo many iterations\n");printf("itt = %d, f = %8.3Le, |g| = %8.3Le, |dx| = %8.3Le, |dL| = %8.3Le\n",itt,f0[0],norm_g,dx,ndL);vec_copy(x,x1,n);free_storage(n,m,p);return;
}
void form_dL( n, m, p )
int n, m, p;
{
   int i, j;

   for( i = 0; i <n; i++ ) {
     dL[i] = df0[i];
     for( j = 0; j < m; j++ ) {
        dL[i] += lam_1[j]*dg0[j][i];
        dL[j+n] = g0[j];
     }
     for( j = 0; j < p; j++ ) {
        dL[n+m+j] = dmax(0.0,g0[m+j]);
        if( act[j] == 1 )
           dL[i] += lam_1[j+m]*dg0[j+m][i];
     }
   }
}
void update_Lxx( n, m, p )
int n, m, p;
{
   long double *Lx0, *Lx1, *d;
   long double gd, dHd, *Hd, *gamma, theta, theta1;

   int i, j;
   
   Lx0 = a1d_allo_dbl( n );Lx1 = a1d_allo_dbl( n );d = a1d_allo_dbl( n );
   Hd = a1d_allo_dbl( n );
   
   for( i = 0; i < n; i++ ) d[i] = x1[i]-x0[i];
   												/* Form Lx0, Lx1 */
   for( i = 0; i <n; i++ ) {
     Lx0[i] = df0[i];Lx1[i] = df1[i];
     for( j = 0; j < m; j++ ) {
        Lx0[i] += lam_1[j]*dg0[j][i];Lx1[i] += lam_1[j]*dg1[j][i];
        }
     for( j = 0; j < p; j++ )
        if( act[j] == 1 ) {
           Lx0[i] += lam_1[j+m]*dg0[j+m][i];Lx1[i] += lam_1[j+m]*dg1[j+m][i];
        }
   }
/**/
   gamma = Lx1;
   for( i = 0; i < n; i++ )
      gamma[i] -= Lx0[i];
      
   gd = 0.0;
   dHd = 0.0;
   for( i = 0; i < n; i++ ) {
      gd += gamma[i]*d[i];          /* gamma^T d */
      Hd[i] = 0.0;
      for( j = 0; j < n; j++ )
         Hd[i] += Lxx[i][j]*d[j];   /* Hd */
      dHd += d[i]*Hd[i];            /* d^T Hd */
   }
   if( gd < 0.2*dHd ){
      theta = 0.8*dHd/(dHd-gd);theta1=1.0-theta;
      for(i=0;i<n;i++){
         gamma[i] = theta*gamma[i];
         for(j=0;j<n;j++)
            gamma[i]+=theta1*Lxx[i][j]*d[j];
      }
      gd = 0.0;for(i=0;i<n;i++) gd+=gamma[i]*d[i];
   }
   for( i = 0; i < n; i++ )
      for( j = 0; j < n; j++ ){
         Lxx[i][j] +=  gamma[i]*gamma[j]/gd;
         Lxx[i][j] -=  Hd[i]*Hd[j]/dHd;
      }
   a1d_free_dbl( Lx0 );
   a1d_free_dbl( Lx1 );
   a1d_free_dbl( d );
   a1d_free_dbl( Hd );      
}
void cal_h( n, m, p )
int n, m, p;
{
   long double **big_a, *big_b, *big_x;
   int *pivot;
   int i, j, k;
   big_a = a2d_allo_dbl( n+m+n_act, n+m+n_act );
   big_b = a1d_allo_dbl( n+m+n_act );
   big_x = a1d_allo_dbl( n+m+n_act );
   pivot = a1d_allo_int( n+m+n_act );
   mat_null( big_a, n+m+n_act, n+m+n_act );
   vec_null( big_b, n+m+n_act );
   vec_null( big_x, n+m+n_act );
   for( i = 0; i < n; i++ ) {
      big_b[i] = -df0[i];
      for( j = 0; j < n; j++ )
         big_a[i][j] = Lxx[i][j];
      for( j = n; j < n+m; j++ ) {
         big_a[i][j] = dg0[j-n][i];
         big_a[j][i] = big_a[i][j];
         big_b[j] = -g0[j-n];
      }
      k = 0;
      for( j = 0; j < p; j++ )
         if( act[j] == 1 ) {
            big_a[i][n+m+k] = dg0[j+m][i];
            big_a[n+m+k][i] = big_a[i][n+m+k];
            big_b[n+m+k] = -g0[j+m];
            k++;
         }
   }
   k = factor1( big_a, n+m+n_act, pivot );
   if( k == 0 ) {
   		printf("Error: Hessian of Lagrangian is singular\n");
   		vec_null(h_bx,n);vec_null(lam_1,m+p);
   		a2d_free_dbl( big_a, n+m+n_act );
   		a1d_free_dbl( big_b );
   		a1d_free_dbl( big_x );
   		a1d_free_int( pivot );  
		return;
   }
   solve1( big_a, pivot, big_b, n+m+n_act, big_x);
   for( i = 0; i < n; i++ )
      h_bx[i] = big_x[i];
   for( i = 0; i < m; i++ )
      lam_1[i] = big_x[i+n];
   k = 0;
   for( i = 0; i < p; i++ ) {
      lam_1[i+m] = 0.0;
      if( act[i] == 1 ) {
         lam_1[i+m] = big_x[m+n+k];
         k++;
      }
   }
   a2d_free_dbl( big_a, n+m+n_act );
   a1d_free_dbl( big_b );
   a1d_free_dbl( big_x );
   a1d_free_int( pivot );  
}
int check_data( n, m, p )
int n, m, p;
{
   if( n <= 0 ) {printf("Error: number of variables <= 0\n");return(1);}
   if( m < 0 )  {printf("Error: number of equality constraints < 0\n");return(1);}
   if( p < 0 )  {printf("Error: number of inequality constraints < 0\n");return(1);}
   if( m >= n ) {printf("Error: number of equality constraints > number of variables\n");return(1);}
   return( 0 );
}
void find_active( n, m, p )
int n, m, p;
{
   int i;
   n_act = 0;
   for( i = 0; i < p; i++ ) { /* make sure no more than n-1 constraints are active */      
      if( (g0[i+m] >= 0.0 || lam_1[i+m] > 0.0) && (n_act+m) < n ) {n_act++; act[i]=1;}
      else act[i]=0;
   }
}
void get_storage( n, m, p )
int n, m, p;
{
   f0 = a1d_allo_dbl( 1 );
   f1 = a1d_allo_dbl( 1 );
   x0 = a1d_allo_dbl( n );
   x1 = a1d_allo_dbl( n );
   df0 = a1d_allo_dbl( n );
   df1 = a1d_allo_dbl( n );
   g0 = a1d_allo_dbl( m+p );
   g1 = a1d_allo_dbl( m+p );
   pen = a1d_allo_dbl( m+p );
   h_bx = a1d_allo_dbl( n );
   lam_1 = a1d_allo_dbl( m+p );
   dL = a1d_allo_dbl( n+m+p );
   act = a1d_allo_int( p );
   Lxx = a2d_allo_dbl( n, n );
   dg0 = a2d_allo_dbl( m+p, n );
   dg1 = a2d_allo_dbl( m+p, n );
}
void free_storage( n, m, p )
int n, m, p;
{
   a1d_free_dbl( x0 );
   a1d_free_dbl( x1 );
   a1d_free_dbl( f0 );
   a1d_free_dbl( f1 );
   a1d_free_dbl( df0 );
   a1d_free_dbl( df1 );
   a1d_free_dbl( g0 );
   a1d_free_dbl( g1 );
   a1d_free_dbl( pen );
   a1d_free_dbl( h_bx );
   a1d_free_dbl( lam_1 );
   a1d_free_dbl( dL );
   a1d_free_int( act );
   a2d_free_dbl( Lxx, n );
   a2d_free_dbl( dg0, m+p );
   a2d_free_dbl( dg1, m+p );
}
void vec_incr( x1, x0, h, alpha, n )
long double *x1, *x0, *h, alpha;
{
   int i;
   for( i = 0; i < n; i++ )
      x1[i] = x0[i] + alpha*h[i];
}
/* 
Compute the gradient using finite difference 
*/
void sqp_fd_grad( fun, fs, gs, xs, dfs, dgs, n, m, p )
void (*fun)( long double *, long double *, long double * );
int n, m, p;
long double *fs, *gs, *xs, *dfs, **dgs;
{
   int i, j;
   long double fe[1], *xe, *ge, h;
   void ufun_grad( long double *, long double **, long double * );

   xe = a1d_allo_dbl( n );
   ge = a1d_allo_dbl( (m+p) );
      
   for( i = 0; i < n; i++ ) {
      vec_copy( xe, xs, n );
      if( fabs((double)(FD_TOL*xs[i])) > MACH_EPS )
         h = fabs(FD_TOL*xs[i]);
      else
         h = FD_TOL;
      xe[i] += h;
      (*fun)( fe, ge, xe ); 
      dfs[i]  = (fe[0]-fs[0])/h;
      for( j = 0; j < m+p; j++ )
         dgs[j][i] = (ge[j]-gs[j])/h;
   }
   a1d_free_dbl( xe );a1d_free_dbl( ge );   
}
#define MAX_ITT_3 10
long double sqp_line_search( fun, f0, g0, x0, f1, g1, x1, h, pen, n, m, p, eps )
void (*fun)( long double *, long double *, long double * );
long double *f0, *g0, *x0,  *h, *pen, eps, *f1, *g1, *x1;
int n, m, p;
{
   long double alpha, test0, test, z, *ga, *xa, fa;
   int i, ils;
   
   ga = a1d_allo_dbl( m+p );
   xa = a1d_allo_dbl( n );
   alpha = 1.0;
   test0 = f0[0];
   for( i = 0; i < m; i++ )
      test0 += fabs(g0[i])*pen[i];
   for( i = 0; i < p; i++ ) {
      z = dmax(0.0,g0[i+m]);
      test0 += z*pen[i+m];
   }
   for(ils = 0; ils < MAX_ITT_3; ils++ ) {
      vec_incr( xa, x0, h, alpha, n );
      (*fun)( &fa, ga, xa );
      test = fa;
      for( i = 0; i < m; i++ )
         test += fabs(ga[i])*pen[i];
      for( i = 0; i < p; i++ ) {
         z = dmax(0.0,ga[i+m]);
         test += z*pen[i+m];
      }
      if( test < test0 ) {f1[0]=fa;vec_copy(x1,xa,n);vec_copy(g1,ga,m+p);a1d_free_dbl(ga); a1d_free_dbl(xa); return(alpha); }
      alpha /= 3.0;
   }
   f1[0]=fa;vec_copy(x1,xa,n);vec_copy(g1,ga,m+p);a1d_free_dbl(ga); a1d_free_dbl(xa); return(alpha);
}