ex1.c

multipleshooting to find the roots of the non-linear functions

#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include "math_util.h"
/*

  Optimal control of a second order system.
  
  find u(t) to minimimize 
  J = integral(from t=0 to t=2) { (u*u)/2 } dt
  
  subject to 
  
     x_dot = y
     y_dot = u
     
     x(0) = 1,     y(0) = 1
     x(2) = 0,     y(2) = 0
  
*/
main()
{
   FILE *output_file;
   int n, m, ms, i, j,ierr;
   long double tend, **yout, **zout, *time, eps;

   void fun( long double *, long double *, long double *,
             long double * );
   void dfun( long double *, long double *, long double **,
              long double **, long double **, 
              long double ** );
   void bc( long double *, long double *, long double * );
   void dbc( long double *, long double *, long double **,
             long double ** );
   int mshoot_dae( long double **, long double **, 
                   long double *, int, int, int, 
                   long double, 
                   void(*)( long double *, long double *, 
                            long double *, long double * ), 
                   void(*)( long double *, long double *, 
                            long double **, long double **, 
                            long double **, long double ** ), 
                   void(*)( long double *, long double *, 
                            long double * ), 
                   void(*)( long double *, long double *,
                            long double **, long double ** ) );
   

   n = 4;
   m = 1;
   ms = 21;
   time = a1d_allo_dbl( ms );
   yout = a2d_allo_dbl( ms, n );
   zout = a2d_allo_dbl( ms, m );
   eps = 1.0e-4;
   tend = 2.0;
   
   if( time == NULL || yout == NULL || zout == NULL ){
      printf("Out of memory in main\n");
   }
   
   output_file = fopen("ex1.dat","w");
   if( output_file == NULL ) {
      printf("ERROR unable to open output file\n");
      exit(1);
   }
   
   mat_null( yout, ms, n );
   
   for( i = 0; i < ms; i++ ) {
      time[i] = i*tend/(ms-1);
      zout[i][0] = 0.0;
      printf("%Le %Le %Le %Le %Le %Le\n",time[i],
         yout[i][0],yout[i][1],yout[i][2],yout[i][3],
         zout[i][0]);
   }
    
   ierr = mshoot_dae( yout, zout, time, n, m, ms, eps,
                      fun, dfun, bc, dbc );
   
   for( i = 0; i < ms; i++ ) {
      fprintf(output_file,"%Le\t",time[i]);
      for( j = 0; j < n; j++ )
         fprintf(output_file,"%Le\t",yout[i][j]);
      for( j = 0; j < m; j++ )
         fprintf(output_file,"%Le\t",zout[i][j]);
      fprintf(output_file,"\n");
   }    
   fclose( output_file );    
}
void bc( y_o, y_f, r )
long double *y_o, *y_f, *r;
{
   r[0] = y_o[0]-1.0;
   r[1] = y_o[1]-1.0;
   r[2] = y_f[0];
   r[3] = y_f[1];
}
void dbc( y_o, y_f, drdy_o, drdy_f )
long double *y_o, *y_f, **drdy_o, **drdy_f;
{

   mat_null( drdy_o, 4, 4 );
   mat_null( drdy_f, 4, 4 );

   drdy_o[0][0] = 1.0;
   drdy_o[1][1] = 1.0;
   drdy_f[2][0] = 1.0;
   drdy_f[3][1] = 1.0;
}
void fun( y, z, f, g )
long double *y, *z, *f, *g;
{
   f[0] = y[1];
   f[1] = z[0];
   f[2] = 0.0;
   f[3] = -y[2];
   g[0] = y[3]+z[0];
}
void dfun( y, z, fy, fz, gy, gz )
long double *y, *z, **fy, **fz, **gy, **gz;
{
   
   mat_null( fy, 4, 4 ); /* fy(n,n) */
   mat_null( fz, 4, 1 ); /* fz(n,m) */
   mat_null( gy, 1, 4 ); /* gy(m,n) */
   mat_null( gz, 1, 1 ); /* gz(m,m) */
   
   fy[0][1] = 1.0;
   fy[3][2] = -1.0;
   
   fz[1][0] = 1.0;
   
   gy[0][3] = 1.0;
   
   gz[0][0] = 1.0;
}