CONSTRAINT SET TRACKING: OPTIMAL CONTROL PROBLEM
				
by

Paul J. Scott

Introduction

This notebook performs the numerical calculations proposed in a paper with same title.  References below refer to this paper.  This is a shooting method in which one varies the inital values of the costate until the constraint set is reached.

Part Zero

This function performs one Runge Kutta step for vector right hand side function df  for initial vector y0 and step size dt.

\(rkStep[\ \ y0_, \ dt_]\  := 
	
      Module[{\ k1, \ k2, \ k3, \ k4\ }, 
		
        k1\  = \ dt\ N[\ df[\ y0\ ]\ \ ]; 
		
        k2\  = \ dt\ N[\ df[\ y0\  + \ k1/2]\ ]; 
		
        k3\  = \ dt\ N[\ df[\ y0\  + \ k2/2\ ]\ ]; 
		
        k4\  = \ dt\ N[\ df[\ y0\  + \ k3]\ ]; 
		
        y0\  + \ \((k1\  + \ 2\ k2\  + \ 2\ k3\  + \ k4)\)/6
	]\)

This function performs a series of fixed step size Runge Kutta steps to generate a trajectory.

\(\(rk[\ y0_List, \ {t1_, \ dt_}]\  := 
	
      NestList[\ rkStep[\ \ #, \ N[dt]]&, \ N[y0], \ Round[N[t1/dt]]\ ]\ 

    \)\)

BOTH FUNCTIONS BASED ON A PACKAGE BY ROMAN E. MAEDER IN MathSource.

Part One

We set up equations using the same notation as in the paper.

Declaring Variables

Number of y state variables (called n).

n  =  2

Declaration of y state variables.

yy  =  Table[ (Subscript[y, i]) ,  { i ,  1 ,  n } ]

Declaration of y- costate variables (called λ).

λλ  =  
      Table[  (Subscript[λ, i])  ,  { i , 1 ,  Length[ yy ] }  ]

Number of z state variables (called m).

m  =  2

Declaration of z-state variables.

zz  =  Table[  (Subscript[z, i])  ,  { i , 1 , m }  ]

Corresponding numerical variables.

nzz  =  Table[  (Subscript[nz, i])  ,  { i , 1 , m }  ]

Initial z-state variables at start of trajectory

initialz  =  Table[  (Subscript[iz, i])  ,  { i , 1 , m }  ]

Declaration of initial z-state variables. Used for initial approximation in solving (4.6).

ninitialz  =  Table[  (Subscript[niz, i])  ,  { i , 1 , m }  ]

Transform variable \(TraditionalForm\`\(gg\&^\)\). See paper (4.6).

\(gg\&^\  = \ Table[\ \ \(g\_i\)\&^\ \ , \ {\ i\ , 1\ , m\ }\ \ ]\)

Corresponding numerical variables.

\(ngg\&^\  = \ Table[\ \ \(ng\_i\)\&^\ \ , \ {\ i\ , 1\ , m\ }\ \ ]\)

Variable to hold rhs of DE for transformed variable \(TraditionalForm\`\(gg\&^\)\). See paper (4.8).

\(dgg\&^\  = \ Table[\ \ \(dg\_i\)\&^\ \ , \ {\ i\ , 1\ , m\ }\ \ ]\)

Corresponding numerical variables.

\(ndgg\&^\  = \ Table[\ \ \(ndg\_i\)\&^\ \ , \ {\ i\ , 1\ , m\ }\ \ ]\)

Declaration of z-costate variables (called ξ).

ξξ  =  
      Table[  (Subscript[ξ, i])  ,  { i , 1 , Length[ zz ] }  ]

Declaration of error function e. See paper (1.4)

ee  =  Table[  (Subscript[e, i])  ,  { i , 1 ,  Length[ zz ] }  ]

Corresponding numerical variable.

nee  =  
      Table[  (Subscript[ne, i])  ,  { i , 1 ,  Length[ zz ] }  ]

Declaration for a matrix occuring frequently ( transpose of inverse of DzG). See (4.2), (4.3), (4.4), and (4.8).

\(TinDzG\  = \ 
      Table[\ Tin[i, j]\ , \ {i, 1, Length[zz]}

        	\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ , \ {j, 1, Length[zz]}\ ]\)

Numerical form for matrix occuring frequently. See (4.2), (4.3), (4.4), and (4.8).

\(nTinDzG\  = \ 
      Table[\ nTin[i, j]\ , \ {i, 1, Length[zz]}

        	\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ , 
        \ {j, 1, Length[zz]}\ ]\)

Defining the differential equation and constraint

Definition of rhs of ODE   F.  See paper (1.1)

Each component of F is defined.

(Subscript[f, 1])   =   -(Subscript[y, 1])   +   (Subscript[z, 1])

(Subscript[f, 2])   =   -(Subscript[y, 2])   +   (Subscript[z, 2])

The components are listed in vector F.

F  =  Table[  (Subscript[f, i])  ,  { i , 1 , Length[ yy ] }  ]

The Jacobian Matrix (∂_y) F

DyF   =   
      Transpose[  
        Table[ (∂_((Subscript[y, i]))) F ,  {i, 1, Length[ yy ]} ]  ]

MatrixForm[ DyF ]

The Jacobian Matrix (∂_z) F

DzF   =   
      Transpose[  
        Table[ (∂_((Subscript[z, i]))) F ,  {i, 1, Length[ zz ]} ]  ]

MatrixForm[ DzF ]

Definition of constraint function G.  See paper (1.2)

Each component of g is defined.

(Subscript[sg, 1])  =   
      (Subscript[y, 1])^3   +   (Subscript[y, 2])^2   -  ((Subscript[z, 1])^3   +   (Subscript[z, 2])^2 )    + 
          (Subscript[y, 1])  ((Subscript[z, 2])^2  -  4 )

(Subscript[sg, 2])  =     (Subscript[y, 1])   -   (Subscript[z, 1])

The components are listed in a vector G.

G  =  Table[  (Subscript[sg, i])  ,  { i , 1 , Length[ zz ] }  ]

The Jacobian Matrix (∂_y) G

DyG   =   
      Transpose[  
        Table[ (∂_((Subscript[y, i]))) G ,  {i, 1, Length[ yy ]} ]  ]

MatrixForm[ DyG ]

The Jacobian Matrix (∂_z) G

DzG   =   
      Transpose[  
        Table[ (∂_((Subscript[z, i]))) G ,  {i, 1, Length[ zz ]} ]  ]

MatrixForm[ DzG ]

Setting up equations to solve for z.  See (4.6)

\(ls\  = \ {\ Thread[\ G\  - \ gg\&^\ ]\ }\)

in  =  Thread[  { zz ,  initialz }  ]

Do[ ls  =  Append[ ls ,  in[[i]]   ] , 
       {i, 1, Length[in]} ]

ls

Setting up the right hand side of state costate equations

"stcst"  consists  of  the  rhs of  all  differential  equations.  Its  first component 1 is for integrating the time equation  (∂_t) t  =  1.  The order of all the components is t,y, \(TraditionalForm\`\(g\&^\)\), λ, ξ.

First we include the rhs of (1.1) and (1.3).

\(stcst\  = \ Flatten[\ {\ 1\ , \ F\ \ , \ dgg\&^\ }\ ]\)

Next we compute the y-derivative of the error function, "Dyee" using (4.3) and (4.2).

Dyee  =  
      TinDzG . 
        Transpose[ 
          Table[ (∂_((Subscript[y, i])))( Transpose[DzG]) . ee , 
             {i, 1, Length[yy]} ]  ]

Next we compute the y-derivative of the Hamiltonian using (3.6).

DyH  =  2 ee . Dyee   +   λλ . DyF

Next we append the rhs of the λ-costate DE. See paper (3.2). Note minus sign.

stcst  =  Flatten[ { stcst ,  -DyH } ]

Next we compute the z-derivative of the error function, "Dzee" using (4.4) and (4.2).

Dzee  =  
      TinDzG . 
        Transpose[ 
          Table[ (∂_((Subscript[z, i])))( Transpose[DzG]) . ee , 
             {i, 1, Length[zz]} ]  ]

Next we compute the z-derivative of the Hamiltonian using (3.7).

DzH  =  2 ee . Dzee   +   λλ . DzF

Next we append the rhs of the ξ-costate DE. See paper (3.3). Note minus sign.

stcst  =  Flatten[ { stcst ,  -DzH } ]

Part Two

Vector Input For Runge Kutta Step

Vector variables and substitutions.

"st" will be the vector input to rkStep.  The order of the components of "st" is: t,y,\(TraditionalForm\`\(g\&^\)\),λ, ξ.

st   =   
      Table[ s[ i ] ,  {i, 1 ,  1  +  n  +  m  +  n  +  m } ]

"st0" will store initial values set up later.

st0   =   
      Table[  int ,  {i, 1 ,  1  +  n  +  m  +  n  +  m } ]

In all the equations developed in Part One we must insert the correct components of "st".  To do this we make the following lists of substitutions.

ysub  =  
      Table[ yy[[i]]  ->  st[[ 1  +  i ]] , 
         {i, 1, Length[ yy ] } ]

zsub  =  
      Table[ zz[[i]]  ->  nzz[[ i ]] , 
         {i, 1, Length[ zz ] } ]

\(gsub\&^\  = \ 
      Table[\ gg\&^[\([i]\)]\  -> \ st[\([\ 1\  + n\  + \ i\ ]\)]\ , 
        \ {i, 1, Length[zz]}\ ]\)

\(dgsub\&^\  = \ 
      Table[\ dgg\&^[\([i]\)]\  -> \ ndgg\&^[\([\ i\ ]\)]\ , 
        \ {i, 1, Length[zz]}\ ]\)

\(λsub\  = \ 
      Table[\ λλ[\([i]\)]\  -> \ 
          st[\([\ 1\ \  + n\  + m\  + \ i\ ]\)]\ , \ 

        		\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ {i, 1, 
          Length[\ yy\ ]}
          \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ ]\)

\(ξsub\  = \ 
      Table[\ ξξ[\([i]\)]\  -> \ 
          st[\([\ 1\ \  + \ n\  + m\  + n\  + \ i\ ]\)]\ , 

        		\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ {i, 1, 
          Length[zz]}
          \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ ]\)

\(esub\  = \ 
      Table[\ ee[\([i]\)]\  -> \ nee[\([\ i\ ]\)]\ , 

        		\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ {i, 1, 
          Length[zz]}
          \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ ]\)

\(Tsub\  = \ 
      Table[\ TinDzG[\([i, j]\)]\  -> \ nTinDzG[\([i, j]\)]\ , 

        		\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ {i, 1, 
          Length[zz]}, \ {j, 1, Length[zz]}\ ]\)

The following, "ssub", is a master list of substitutions.

\(ssub1\  = \ 
      Flatten[\ Union[\ ysub\ \ , \ λsub\ , \ ξsub\ \ , \ 
          gsub\&^\ \ , \ dgsub\&^\ , \ esub\ \ , \ Tsub\ ]]\)

ssub  =  Union[ ssub1 ,  zsub  ]

Objects needed in vector calculations

In the following we insert the components of "st" into the objects set up in Part One.  These objects have names starting with "n".  These objects will be used in a function,  called "df", for numerically computing the rhs of the DE's during the rkStep.

nls  =  ls  //.  ssub1

nDzG  =  DzG   //.  ssub

nξξ  =  ξξ  //.  ssub

nstcst  =  stcst  //.  ssub

Part Three

The following module, called "df", numerically computes the right hand side of all DE's.  It uses the numerical variables set up in Part Two.  The module begins with computing the K-T multipliers and control.

\(\(\ df[\ inst_\ ]\ \  := \ 
      \(Module[\ {\ }\ , {

             (*\ The\ next\ command\ solves\ \((4.6)\)\ for\ \*"""
                \(z \*"" "\)*) 
		
            nzz\  = \ 
              zz\  /. \ 
                Apply[\ FindRoot\ , \ 
                  \((\((\ nls\  /. \ 

                          							\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 
                          Thread[\ st\  -> \ inst\ \ ]\ )\)\ \  /. 

                      							\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 
                      Thread[\ initialz\  -> \ ninitialz\ ]\ \ \ )
                    \)\ \ \ \ \ \ ]\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ ; 


            						
	
             (*\ The\ next\ command\ sets\ new\ initial\ values

              				
              for\ the\ FindRoot
                \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ *) 
		
            Do[\ niz\_i\  = \ nzz[\([i]\)]\ , \ {i, 1, Length[zz]}\ ]
              \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ ; 

            		Do[\ nz\_i\  = \ nzz[\([i]\)]\ , \ {i, 1, Length[zz]}\ ]
              \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ ; 
			
			 (*\ The\ next\ command\ finds\ the\ matrix\ occuring\ 

              				\ \ 
              in\ many\ 
                \(equations.
                  \)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ *) 
		nTinDzG\  = \ 
              Transpose[\ 
					\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 
                Inverse[\ nDzG\  /. \ Thread[\ st\  -> \ inst\ ]\ ]]
                \ \ \ \ \ \ \ \ \ \ \ \ \ ; 
					
            Do[\ nTin[i, j]\  = \ nTinDzG[\([i, j]\)]\ , 

              						\ \ \ \ \ {i, 1, Length[zz]}, {j, 1, Length[zz]}
                \ \ ]\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ ; 
					

            					 (*\ The\ next\ command\ finds\ the\ error\ function

              						\ \ \*"""
              e \*"" using (4.2)                               "*) 

            				nee\  = \ 
              \(-\ nTinDzG\ .
                    \((\ nξξ\  /. \ Thread[\ st\  -> \ inst\ ])\)\)\ /
                \ 2\ \ \ \ \ \ ; 
					
            Do[\ ne\_i\  = \ nee[\([i]\)]\ , \ {i, 1, Length[zz]}\ ]
              \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ ; 

            					
					
             (*\ The\ next\ command\ finds\ the\ rhs\ of\ the\ g\&^ - DE

              						\ \ 
              using\ \((4.8)\)\ and\ 
                \(\((4.2)\).
                  \)\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ *) 

            				Do[\ \(ndg\_i\)\&^\  = \ nee[\([i]\)]\ , 
              \ {i, 1, Length[zz]}\ ]
              \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ ; 
			

            						\ 
            nstcst\  /. \ 
              Thread[\ st\  -> \ inst\ ]
                \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ }]\)[\([1]\)]
\)\)

Part Four

Set initial values for y-state and \(TraditionalForm\`\(g\&^\)\)-state.

initialy  =  {  -1 ,  -2 }

The following two commands are needs to give an initial approximation for FindRoot in the "df" module aaat the first attempts.

(Subscript[niz, 1 ])  =  -1

(Subscript[niz, 2])  =  2

(*^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^*)

\(initialg\&^\  = \ {\ \ 0\ , \ 0\ }\)

Set the initial time equal zero

st0[[1]]  =  0

Put the initial values of y and z into the initial data vector "st0".

Do[  st0[[1 + i]]  =  initialy[[i]] , 
       {i, 1, Length[ yy ]}  ]

\(Do[\ st0[\([1 + Length[\ yy\ ] + i]\)]\  = \ initialg\&^[\([i]\)]\ , 

      	\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ {i, 1, Length[\ zz\ ]}\ \ ]\)

st0

Part Five

Set initial values of costate. Run Runge-Kutta solver. View graphs of output.

Set initial costate values

Adjust initial costate values until constraint set is approached.

initialλ  =  { 0 ,  0  }

initialξ  =  { 0 ,  0 }

Put the initial values of λ and ξ into the initial data vector "st0".

\(Do[\ \ 
      st0[\([1 + Length[\ yy\ ] + Length[\ zz\ ] + i]\)]\  = \ 
        initialλ[\([i]\)]\ , 

      	\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ {i, 1, Length[\ yy\ ]}\ \ ]\)

\(Do[\ 
      st0[\([1 + Length[\ yy\ ] + Length[\ zz\ ] + Length[\ yy\ ] + i]\)]\  = 
        \ 
	\ \ \ \ \ \ \ initialξ[\([i]\)]\ , 
      \ \ {i, 1, Length[\ zz\ ]}
        \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ ]\)

st0

"tend" is the end time for solving the trajectories.  You might want to change this sometimes.

tend   =   1.

"dt" is the step size used in the Runge-Kutta step.

ninitialz

dt  =  .05

The following is a set to hold the z-state trajectory

nDzG  /.  Thread[ st  ->  st0 ]

(Subscript[nz, 2])

df[ st0 ]

nzz

st0

Run Runge-Kutta Solver

Solution is kept in table "sol".  Since this is a big table I have ended the command with a ";" so the table is not printed.  Remove the ";" to see the table.

Reset these for the beginning of the trajectory

(Subscript[niz, 1 ])  =  -1

(Subscript[niz, 2])  =  2

(*^^^^^^^^^^^^^^^^^^^^^^^^^^^^*)

sol  =  rk[ st0,  { tend,  dt }]   ;

\(\(sol
\)\)

Solving for the z-trajectory

Reset the initial values of "z" at beginning of trajectory.

ninitialz  =  { -1 ,  2 }

(*^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^*)

The following is a set that will hold the z-trajectory

zset  =  { }

\(\(\ Do[
      \ {\ \ nzz\  = \ 
          zz\  /. Apply[\ FindRoot\ , \ 
              \((ls\  /. \ \ \ 
                    Thread[\ \ 
									\ \ 
                      Flatten[
                          \ {\ t\ , \ yy\ , \ gg\&^\ , \ 
                            λλ\ , \ ξξ\ }\ ]\  -> 

                        									\ \ \ sol[\([i]\)]\ ])\)\  /. \ 
                Thread[\ initialz\  -> \ ninitialz\ ]\ ]\ \ \ ; 


        \ \ \ \ \ \ \ \ \ \ \ \ \ \ 
        ninitialz\  = \ nzz\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ ; 


        \ \ \ \ \ \ \ \ \ \ 
        zset\  = \ 
          Append[\ \ \ zset\ , \ 
				\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 
            Flatten[\ {\ \(sol[\([i]\)]\)[\([1]\)]\ , \ nzz\ }\ \ ]\ \ ]
            \ \ \ \ \ \ \ ; \ \ \ \ }\ \ , 

      \ \ \ \ \ \ \ \ \ \ \ \ {i, 1, Length[sol]}
        \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ ]\)\)

zset

Set up interpolating functions for graphics.

s  = 
      Flatten[ sol[[  Range[1, Length[sol] ] ,  Range[1, 1]  ]]]
         ;

\(Do[\ 
      gy[i]\  = 
        Interpolation[\ \ 
          Transpose[{\ s\ , \ 
              Flatten[\ 
						\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 
                sol[\([\ \ Range[1, Length[sol]\ ]\ , 

                    								\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ Range[1 + i, 1 + i]\ \ ]
                    \)]]\ }]\ ]\ \ \ , 

      	\ \ \ \ \ {i, 1, Length[\ yy\ ]}
        \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ ]\)

\(Do[\ 
      ggg\&^[i]\  = 
        Interpolation[\ \ 
          Transpose[{\ s\ , \ 
              Flatten[\ 
						\ \ \ \ \ \ \ \ \ \ \ \ \ 
                sol[\([\ \ Range[1, Length[sol]\ ]\ \ , 

                    										\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 
                    Range[1 + Length[yy] + i, 
                      1 + 
										\ \ \ \ \ \ \ \ \ \ \ \ \ \ 	
                        Length[yy] + i]\ \ ]\)]]\ }]\ ]\ \ \ , 

      	\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ {i, 1, Length[\ zz\ ]}
        \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ ]\)

\(Do[\ 
      gλ[i]\  = 
        Interpolation[\ \ 
          Transpose[{\ s\ , \ 
              Flatten[\ 
						\ \ \ \ \ \ \ \ \ \ \ \ \ 
                sol[\([\ \ Range[1, Length[sol]\ ]\ \ , 

                    										\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 
                    Range[1 + Length[yy] + Length[zz] + i, 
                      1 + 
										\ \ \ \ \ \ \ \ \ \ \ \ \ \ 	
                        Length[yy] + Length[zz] + i]\ \ ]\)]]\ }]\ ]\ \ \ , 

      	\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ {i, 1, Length[\ yy\ ]}
        \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ ]\)

\(Do[\ 
      gξ[i]\  = 
        Interpolation[\ \ 
          Transpose[{\ s\ , \ 
              Flatten[\ 
						\ \ \ \ \ \ \ \ \ \ \ \ \ 
                sol[\([\ \ Range[1, Length[sol]\ ]\ \ , 

                    										\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 
                    Range[1 + Length[yy] + Length[zz] + Length[yy] + i, 
                      1 + 
										\ \ \ \ \ \ \ \ \ \ \ \ \ \ 	
                        Length[yy] + Length[zz] + Length[yy] + i]\ \ ]\)]]\ }]
            \ ]\ \ \ , 

      	\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ {i, 1, Length[\ zz\ ]}
        \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ ]\)

We do the same thing with "zset" to set up graphics for the z-trajectory.

ss  = 
      Flatten[ zset[[  Range[1, Length[zset] ] ,  Range[1, 1]  ]]]
         ;

\(Do[\ 
      gz[i]\  = 
        Interpolation[\ \ 
          Transpose[{\ ss\ , \ 
              Flatten[\ 
						\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 
                zset[\([\ \ Range[1, Length[zset]\ ]\ , 

                    								\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ Range[1 + i, 1 + i]\ \ ]
                    \)]]\ }]\ ]\ \ \ , 

      	\ \ \ \ \ {i, 1, Length[\ zz\ ]}
        \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ ]\)

Plots of the components of y-trajectory

\(\(\ Do[\ \ 
      Plot[\ Evaluate[\ \(gy[i]\)[t]\ ]\ , \ {\ t, 0, tend\ }\ , 

        		\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ {\ 
          AxesOrigin\  -> \ {0, 0}\ , 

          			\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 
          PlotLabel\  -> \ i\ \ \ }\ ]\ \ \ \ \ \ \ \ \ , 
      \ {i, 1, Length[yy]}\ \ ]\ \ \ \ \)\)

Plots of the components of z-trajectory

\(\(\ Do[\ \ 
      Plot[\ Evaluate[\ \(gz[i]\)[t]\ ]\ , \ {\ t, 0, tend\ }\ , 

        		\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ {\ 
          AxesOrigin\  -> \ {0, 0}\ , 

          			\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 
          PlotLabel\  -> \ i\ \ \ }\ ]\ \ \ \ \ \ \ \ \ , 
      \ {i, 1, Length[zz]}\ \ ]\ \ \ \ \)\)

Plots of the components of the constraint G

\(\(\ Do[\ \ 
      Plot[\ Evaluate[\ \(ggg\&^[i]\)[t]\ ]\ , \ {\ t, 0, tend\ }\ , 

        		\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ {\ 
          AxesOrigin\  -> \ {0, 0}\ , 

          			\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 
          PlotLabel\  -> \ i\ \ \ }\ ]\ \ \ \ \ \ \ \ \ , 
      \ {i, 1, Length[zz]}\ \ ]\ \ \ \ \)\)

Click the "il" to go back to initial costate conditions.

il

Compute some numerical values

tset  =  tend

\({\ Table[\ \(gy[i]\)[t]\ , \ {i, 1, Length[yy]}]\ , 
	\ 
        Table[\ \(ggg\&^[i]\)[t]\ , \ {i, 1, Length[zz]}]\ , 
	\ 
        Table[\ \(gλ[i]\)[t]\ , \ {i, 1, Length[yy]}]\ , 
	\ 
        Table[\ \(gξ[i]\)[t]\ , \ {i, 1, Length[zz]}]\ \ \ \ \ \ \ }\  /. 
      \ t\  -> \ tset\)

\(\(\ Do[\ \ 
      Plot[\ Evaluate[\ \(gλ[i]\)[t]\ ]\ , \ {\ t, 0, tend\ }\ , 

        		\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ {\ 
          AxesOrigin\  -> \ {0, 0}\ , 

          			\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 
          PlotLabel\  -> \ i\ \ \ }\ ]\ \ \ \ \ \ \ \ \ , 
      \ {i, 1, Length[zz]}\ \ ]\ \ \ \ \)\)

\(\(\ Do[\ \ 
      Plot[\ Evaluate[\ \(gξ[i]\)[t]\ ]\ , \ {\ t, 0, tend\ }\ , 

        		\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ {\ 
          AxesOrigin\  -> \ {0, 0}\ , 

          			\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ 
          PlotLabel\  -> \ i\ \ \ }\ ]\ \ \ \ \ \ \ \ \ , 
      \ {i, 1, Length[zz]}\ \ ]\ \ \ \ \)\)