|||||||||||I|| USOO5435503A

United States Patent 19 11) Patent Number: 5,435,503

Johnson, Jr. et al. 45 Date of Patent: Jul. 25, 1995
54 REAL TIME MISSILE GUIDANCE SYSTEM Engineers, 1961, McGraw-Hill, Inc., New York, N.Y.,
75 Inventors: Malvin G. Johnson, Jr., Grand pp. 344-372, 471-480, 784-793.
Prairie; Glenn T. Haskins, Arlington, Robert R. Bless, "Time-Domain Finite Elements in
both of Tex.; Dewey H. Hodges, Optimal Control With Application to Launch-Vehicle
Dunwoody, Ga. Guidance,” NASA Contract Report 4376, National
Aeronautics and Space Administration, 1991.
73 Assignee: Loral Vought Systems Corp., Grand Primary Examiner-Ian J. Lobo
Prairie, Tex. Attorney, Agent, or Firm-Arnold, White & Durkee
21 Appl. No.: 113,636 57 ABSTRACT
22 Filed: Aug. 27, 1993 A weak Hamiltonian finite element method is used for
51 Int. Cl........................... F41G 7/00; F42B 15/01 iterative computation of missile guidance acceleration
(52) ... 244/3.15: 364/424.01 commands for maximizing a missile's terminal velocity
58 Field of Search ............................. 244/3.11, 3.15; while satisfying control authority limits and terminal
364/423, 424.01 attitude constraints. The guidance acceleration com
56 References Cited mands include commands for controlling the angle of
attack (a) and the bank angle (db) of the missile. The
U.S. PATENT DOCUMENTS angle of attack (a) and bank angle (db) are related to a set
5,253,823 10/1993 Lawrence .......................... 244/3.15 of virtual control variables selected to avoid conver
5,259,569 11/1993 Waymeyer et al. ............... 244/3.22 gence problems when the angle of attack is approxi
mately zero. The preferred control variables are 62 and
OTHER PUBLICATIONS A3 such that (32=cos(btano, and 33=sindbtand. Iterative
Hodges et al., “Weak Hamiltonian Finite Element convergence is facilitated when control inequality con
Method for Optimal Control Problems,” Journal of straint parameters are reached by adjusting iterative
Guidance Control, and Dynamics, vol. 14, No. 1, Jan solutions between iterations toward satisfaction of the
.-Feb. 1991, American Institute Aeronautics and Astro constraints. An approximation to an optimal trajectory
nautics, pp. 148-156. is calculated at each guidance cycle during missile flight
Bless et al., “Finite Element Solution of Optimal Con using data which are revised during each guidance
trol Problems with Inequality Constraints,” Proceed cycle. The revised data include current position data for
ings of the 1990 American Control Conference, San
the target and the current position for the missile. The
revised data are taken from the most reliable source
Diego, California, May 23-25, 1990, vol.1, American currently available, such as on-board target-seeking
Automatic Control Conference, pp. 242-247. radar when the target-seeking radar is locked onto the
CRC Handbook of Mathematical Tables, 1980, Chemical target, uplink data from ground or airborne tracking
Rubber Company Press Inc., Boca Raton, Florida, pp. radar when an uplink is operational, or inertial guidance
43-44. data. Extracted from the optimal trajectory is an opti
McGraw-Hill Encyclopedia of Science & Technology, vol. mal acceleration command for optimally controlling the
8, 1992, McGraw-Hill, Inc., New York, N.Y., pp. angle of attack and bank angle of the missile.
307-310.
Korn et al., Mathematical Handbook for Scientists and 33 Claims, 7 Drawing Sheets
45
MULTIPLEX SYNCHRONIZATION INTERRUPT
UPLINK DATA BUS AND CLOCK ANO RESET
RECEIVER CONTROLLER GENERATOR CONTROL

RANDOM 4.
ACCESS
MEMORY
ALGNMENT AUTOPLOT 49
FUNCTON FUNCION READ 42
ONLY
NAVIGATION GUIDANCE MEMORY
FUNCTION FUNCTION 51 43
FLOATING-PONT
COPROCESSOR

RADAR INERTIA ATTTUDE AERODYNAMC

SEEKER MEASUREMENT CONTROL MANEUVERNG
UNIT SYSTEM SYSTEM
22 23 24
U.S. Patent July 25, 1995 Sheet 1 of 7 5,435,503

26

23--- - -
22- - -
21 - - -

FIG 1
U.S. Patent July 25, 1995 Sheet 2 of 7 5,435,503
U.S. Patent July 25, 1995 Sheet 6 of 7 5,435,503

WHOA
GUIDANCE
(A)
61 69
YES c
E (x)
CONVERGED
NYES
-x-x-xfc NO calculate
ADGN-g(x)
64
NC -- NC- RETURN
K -- 1 7O
YES
65
TRANSFORM MISSILE 76
VELOCITY AND POSITION, NO
AND TARGET INTERCEPT NC NC-1
ATTTUDE AND POSITION, ADG-ADG
TO CONSISTENT UNITS
67
66 REDEFNE 71 RETURN

RESZE YES X AND UPDATE JACOBAN

JACOBAN MATRIX MATRIX
SIZE 72
NVERT UACOBAN
AND SOLVE:
68 AXN-J'E (x)
COMPUTE
NC
E(x) 73
-- xc - AX

(a) 74
75
FIG 8 CHECK CONTROL BOUNDARIES
AND CONSTRAN SOLUTION
U.S. Patent July 25, 1995 Sheet 7 of 7 5,435,503

ADJUST
FOR MTS FIG 9
81 82

<> NO

84 85
YES NO MOVE U OFF
BOUNDARY |

SET u =O AND
CALCULATE K

RETURN )
88
YES YES MOVE U ON
BOUNDARY
89 9O
CALCULATE K SET K=O AND
CALCULATE L

92 (Kz O RETURN )
.AND. Liz O) YES
.O.R. (K=O AND.
40) YES
94
SET K=O AND
CALCULATE Ll

SET u =O AND
CALCULATE K

RETURN )
 5,435,503
1. 2
The Euler-Lagrange equations are obtained by set
REAL TIME MISSILE GUIDANCE SYSTEM ting the “first variation' of the integral in Equation 4 to
zero. The first variation is found by varying each of the
BACKGROUND OF THE INVENTION functions hi(x) by an amount eiu(x) where ei is an arbi
A. Field of the Invention trary constant independent of X and h(x), and ui(x)
This invention relates to missile guidance, and more denotes any arbitrary function of X which is indepen
particularly to real-time computation of missile guid dent of ei. Then the integral I-8I is defined by:
ance commands for optimal guidance of the missile.
B. Description of the Related Art O tf (Equation 6)
1. General Theoretical Art I - 6 =? Whi(t) + eui(t), ..., hi(t) + eru;(t), ...,idt.
o
a. Calculus of Variations
The basic objective of the calculus of variations is to Upon solving for 6I to obtain a power series expan
find the unknown function which will locate an extre
mum for a definite integral. Ordinary calculus locates sion of ÖI in terms of powers of ei, the “first variation
the extremum for a function of one or more variables.
15 is the coefficient of the term for ei to the first power.
For example: It is still necessary, for the problems of the type in
optimal control, to solve a variational problem with
==Withrz,x,y,u) auxiliary conditions. Now consider that the functionsh;
(Equation 1)
are not independent but restricted by some auxiliary
is a velocity function. This function varies with the 20 conditions. Initially we considered velocity as a func
positions x,y,z, the thrust (thr) and the control variable tion of several variables x, y, z, thr and u. In solving the
u. To find the extremum of the velocity, the first varia definite integral we allowed the integral of velocity to
tion of the velocity equation is used: be several functions of one variable t. Now we will
consider a system composed of several functions of
25 several variables. The auxiliary system functions will be
(Equation 2)
aV portrayed as fi, with the variables as xi and t, creating
82 82 -- aathr
V
8thr a8.V 8.u. the set of equations:
Following it has been shown that the aforementioned f(xii) = 0, (i = 1,mi = 1,n), (Equation 7)
variational relation simplifies to the following set of
equations: with Vh;(x,t) as before. The auxiliary condition on the
integrand creates a problem when the system functions
a y (Equation 3) are not known to be independent. As an expletive, if the
8.
35
System functions were independent, the independent
equations could be expressed under separate integrals as
follows:

tf (Equation 3)
t = ? f(x,t)dt, (i = 1,m) and
o
V
thr

V
al
45
The variables which solve these equations define an From this set of equations would follow m-1 indepen
extremun for velocity. dent Euler-Lagrange equations. However, since it is not
As noted earlier, it is the extremum of a definite inte known that the functions are independent, knowledge
gral which is formally in the domain of the calculus of of the other relationships is required. Using a Lagrange
variations. Using another function for velocity provides 50 multiplier, the previously assumed independent equa
the example: tions may be expressed as dependent equations under
the same integral as:
tf (Equation 4)
Is ? Vh1(t), h2(t), . . . hi'(t), h(t), ...,tdt. tf (Equation 9)
o 55 8 st ? (ÖV -- A1&f -- . . . An 8f)di = 0.
to
In lieu of an extremum for a function, an extremun
for a definite integral is found. And in lieu of finding the A solution set utilizing the Euler-Lagrange equations
appropriate variables, the appropriate functions (h) are can be shown to be:
found. It has been shown that the necessary and suffi
cient condition for the solution to this type of extremum (Equation 10)
problem can be the solution of the Euler-Lagrange 2 - - d. a y
Al
af
-- . . . -- An
afn = 0.
equations: axi di axi oxi axi

(Equation 5) 65 An alternative methodology to the Euler-Lagrange

ld?-a-
di ah; J
technique for solution to the variational problem with
auxiliary conditions is the Finite Element Method. This
alternative methodology is a key element in making
 5,435,503
3 4.
optimal missile guidance "viably eligible' for real-time a function that defines an inequality constraint. For a
processing. missile, the control variables include the angular accel
b. Finite Element Method (FEM) eration of the missile, up to certain maximum values.
Given the integral: For a solid-fueled rocket, the thrust generally is not
controlled, although the thrust may be controlled for a
tf (Equation 11) liquid fueled missile, up to a certain maximum value.
? o f(x,t)dt. b. Weak Hamiltonian Optimization Method (WHOA)
For solving the missile control problem, the dynamic
The time intervals can be broken up into N elements O state of the missile is represented by E state variables
with a normalized elemental time definition of: x(t) satisfying E "state equations' that are the first
order differential equations:
it - it (Equation 12)
7, = , = , (i = 1, N-1). dxi
dt = f(x1, x2, ..., xn, u1, u2, . . . , ur)
(Equation 14)
15
As an example, the aforementioned equation using a
trapezoidal integration scheme becomes: The n state equations are specific formulations of the
accessory conditions in equation (3) above, in which the
N-1 Af
X
(Equation 13) time t has been selected as the independent variable.
i-1 -- If(xi+1, TH) - f(xi, ti), 20 The state variables x(t) typically include the missile's
coordinates (x, y, z), angular orientation, linear veloc
and are of a similar nature to the forthcoming equations ity, angular velocity, and mass. The functions ui(t),
used in the weak Hamitonian method of optimal missile u2(t), . . . , u(t) are the control variables. The control
guidance. The finite discretization appears similar to problem can be expressed as the problem of finding the
trapezoidal integration because of the selection of the 25 control variables that maximize or minimize the definite
shape functions. integral
2. Application Specific Art
a. Missile Guidance (Equation 15)
Missiles are rocket-motor-boosted vehicles that are .., xn, ui, u2, . . . . urdt.
designed to fly pilotless from a launch position to a 30
desired target position. Various techniques have been
used to guide a missile depending upon the launch and In general, the final time t? will be an unknown.
target positions and the operational characteristics of Real-time control of the missile requires a numerical
the missile. method of solving the equations subject to suitable
A first class of techniques uses an external processor 35 boundary conditions. Due to limited data processing
for performing computations to derive guidance com capability when a numerical solution is computed in the
mands for a missile. The external processor is located on missile, a finite-element procedure is used in which the
the ground or in an airborne launch vehicle. The guid control variables are iteratively computed for a time
ance commands are transmitted from the external pro t--At based on conditions existing at a time t. The com
cessor to the missile via a radio frequency or laser putation should give a finite approximation of an opti
uplink communication path. In response to the transmit
ted guidance commands, onboard instruments guide the mal solution. Additionally, the computation should be
“unconditionally stable.”
missile.
A second class of techniques uses computations that propertiesA computational technique having these desirable
are performed onboard the missile. Inputs for such on 45 is proposed in Hodges et al., "Weak Hamilto
board computations may come from instruments car nian Finite Element Method for Optimal Control Prob
ried onboard the missile itself or from signals transmit lems,’ Journal of Guidance Control, and Dynamics, Vol.
ted to the missile from external sources. 14, No. 1, January-February 1991, American Institute
Generally, instruments carried in a missile for per of Aeronautics and Astronautics, pp. 148-156,incor
forming onboard guidance include inertial devices that 50 porated herein by reference; and Bless et al., “Finite
sense body accelerations and rotational rates with re Element Solution of Optimal Control Problems with
spect to three orthogonal axes. These inertial measure Inequality Constraints,” Proceedings of the 1990 Amer
ments provide position, velocity, and attitude informa ican Control Conference, San Diego, Calif., May 23–25,
tion to a programmed digital processor unit that com 1990, Volume 1, American Automatic Control Confer
putes the guidance commands for the missile. 55 ence, pp. 242-247, incorporated herein by reference.
Optimal guidance of a missile involves guiding the The weak Hamiltonian method is a combination of
missile to optimize a preselected performance function optimal control theory and finite element discretization
(J) subject to certain constraints. with weakly coupled boundary conditions. Permitting a
The real-time control of the missile involves the addi
tional problem of determining control values as a func finite nance
element solution to the first variation, the perfor
function (J) is formulated as:
tion of time in order to guide the missile along a trajec
tory that will optimize the performance function J sub
ject to certain inequality constraints on the control J = (Equation 16)
variables. An inequality constraint is a constraint that is
an inequality. In other words, an inequality constraint is 65 ? to (L + A'(f - i) + p."(G + Kdt + 1 o + are: -3).
a prescribed condition that one quantity is less than, less
than or equal to, greater than, or greater than or equal
to another quantity. An inequality constraint function is where:
 5,435,503
5 6
L is an integrand component of the performance fied integration which looks like a trapezoidal integra
function; tion scheme.
A is a matrix of unknown Lagrangian multiplier func Equation 17 is solved by performing finite element
tions for adjoining the system state equations; discretization. The integral in Equation 17 is broken
T denotes the matrix transpose operation; into N integrals, with each of the N integrals being an
f is a matrix of system state equations; integral over one of the N finite elements, respectively
k is a matrix of the state rates with respect to time; from t=toto t=t?. Constant shape functions are used for
L is a matrix of unknown Lagrangian multiplier func u, 6u, X, A, u, and K, and linear shape functions are used
tions for adjoining the control constraints; for Öx, 8A, 8pu, and 6K. The integral over each element
G is a matrix of control inequality constraint func O can therefore be evaluated by inspection, and the result
tions G(x,u,t) for control functions u(t) such that ing integral in Equation 17 becomes a summation over
Gs0; N elements. Solutions are found for only discrete mid
K is a matrix of slack variables such that G--k2=0; point values for the controls u(t). The boundaries be
d=db-i-uT where tween the finite elements are referred to as nodes, which
d is a performance function of the states and time, 15 occur at t=ti. When there are no state discontinuities,
defined only at the initial and final times to and t?, the nodal values of the shape functions in the neighbor
1 is a matrix of unknown discrete Lagrangian multi ing elements cancel each other in the summation. When
pliers defined only at the initial and final times to and tr there are state discontinuities, the nodal values of the
for adjoining boundary condition constraints; shape functions in the neighboring elements do not
is a matrix of boundary condition constraints in 20
necessarily cancel each other in the summation. This
posed at the initial and final times to and t?, problem is conveniently solved by treating the state
c is a matrix of unknown discrete Lagrangian discontinuity as a pair of boundary conditions at the
boundary condition multipliers for natural cou node, one boundary condition for the integral or sum
pling of finite element values to finite element 25
mation for a time less than but approaching it in the
nodes of the states at the initial and final times to limit, and the other boundary condition for the integral
and t?, and or summation greater than but approaching ti in the
i is a matrix offinite element nodes of the states at the limit. In other words, the time line and the performance
initial and final times to and t?, such that integral in Equation 9 is broken into two different “pha
X to is the limit of x(t) as t-to; 30
ses' by the state discontinuity, and a respective discrete
xtris the limit of x(t) as t->t?, Lagrangian multiplier is introduced for adjoining each
itois X (to); and of the two boundary conditions into the performance
itfis x(t). function (J).
As described in Hodges et al. and Bless et al. cited The Summation over N elements becomes a system of
above, the first variation is taken of the performance 35 simultaneous algebraic equations. In general, 2n of the
function (J) of Equation 16. The first variation has an x 4n endpoint values for the states and corresponding
term, which is then integrated by parts. The boundary Lagrangian multipliers (&a, i?, a, i) must be specified.
conditions are placed in weak form, and the first varia The initial conditions (3) are known in accordance
tion is set to zero, giving the following solution for a with physical constraints. Also, f can be specified in
minimum or maximum to the performance index J of 40 terms of other unknowns, because the admissible varia
Equation 6: tions of the states must be continuous at the initial and
final times:
(Equation 17)
? (six 45 (Ax - A) o = 0
tf (Equation 18)

sI(4)-(+)-(+)- a
A =-
add
o
(Equation 9)

-(+)-(+)-(+)-cis 50 A.
f=-
d (Equation 20)

Therefore, the system of algebraic equations includes as

55 many equations as unknowns, and it can be solved for
the control variables.
Normally, the system of equations can be solved by
Tril - adb - -1. T al
it 7. + v t + "ck + GI tf --- expressing the Jacobian explicitly and using a Newton
Raphson solution procedure. The Newton-Raphson
solution procedure is described, for example, on pages
ov'l- 8xff -- 8x. + 8Afif - 8Aoi = 0 43-44 of the CRC Handbook of Mathematical Tables,
1980, Chemical Rubber Company Press Inc., Boca Ra
Equation 17 is the governing equation for the weak ton, Fla. Let the system of algebraic equations be desig
Hamiltonian method with control constraints. An im nated as ECX)=0, where X is a vector of unknowns.
portant advantage of the weak Hamiltonian method is 65 The Newton-Raphson solution procedure is an iterative
that time derivatives of the states and co-states do not method that starts with a trial solution XO), and obtains
appear in Equation 17. This facilitates the simplistic successive approximations Xi+1) by solving the simul
choice for the shape functions which leads to the simpli taneous linear equations
 5,435,503
8
angle (d) as virtual control variables. The transforma
(Equation 21) tion between the control variables (32 and 63, and the
E(x) + $ ilot" - X) = 0 for i = 1 to n angle of attack (a) and the bank angle (db), avoids con
vergence difficulties when specifying an angle of attack
(a) in the vicinity of zero.
Equation 21 can be written as a matrix equation In a preferred embodiment, the control variables are
computed in accordance with a weak Hamiltonian
E(Xil)+(J (Xi+1)-Xil)=0 (Equation 22) method of optimizing the missile trajectory, and the
control variables are selected to obtain a formulation
where J is the so-called Jacobian matrix having the 10 that avoids negative eigenvalues, when the angle of
elements that are the partial derivatives attack is near zero in a partial derivative of the "Hamil
tonian' matrix, corresponding to the performance func
aE tion (J), with respect to the control variables. The pre
aXk ferred set of control variables includes 62 and 63 such
15
that (32=cos(btana and 33=sindbtana. Because both g2
evaluated at Xk=Xkil. Equation 22 can be solved by and 63 are proportional to tana, it is also desirable to
inverting the Jacobian matrix, to obtain the iterative define an intermediate variable g=tana, and to express
equation: the aerodynamic tables, which are conventionally a
Xi+1=Xi-V-E(xii) function of alpha and Mach number, as a function of G
(Equation 23) 20 and Mach number.
A sparse matrix solver can be used as coded in Duff, In accordance with another aspect of the invention,
I.S., Harwell Subroutine Library, Computer Sciences the guidance commands are computed by an iterative
and Systems Division, Harwell Laboratory, Oxford Newton-Raphson procedure, and convergence is facili
shire, England, February 1988, Chapter M. To solve for 25 tated when control inequality constraint parameters are
the solution at the first element, an initial guess can be reached by modifying iterative solutions between itera
made based on the initial conditions. An initial guess for tions so that the iterative solutions satisfy the inequality
successive elements can be based upon the solutions for constraints while facilitating the movement of the con
a number of previous elements. In general, the iterations trols on to and off of the control boundaries. Moreover,
of the Newton-Raphson procedure converge rapidly, so 30 when it is known that the solution must fall within cer
that a large number of elements can be solved with a tain limits, and when it is known that certain parameters
very efficient run-time on a computer. have either positive or negative values, corresponding
Successful flight of a missile to its target and the solution constraints can be defined for the purpose of
missile's arrival at the target with a desired attitude and accelerating convergence with this technique. A solu
velocity depends upon the nature of the guidance pro 35 tion constraint, for example, can be imposed on the time
cedure and its mechanization. Current guidance proce of flight in order to accelerate convergence by guaran
dures and their mechanizations have failed to achieve teeing positive time steps of at least a certain magnitude.
the potential success suggested by optimal guidance and In accordance with a further aspect of the invention,
control theory. This results primarily from approxima an optimal trajectory is calculated at each computa
tions and mathematical truncations imposed in order to tional cycle during missile flight using data that are
enable completion of the guidance computational task revised during each computational cycle. The revised
in an acceptable time frame. data include current position data for the target and the
The weak Hamiltonian finite element method used current position for the missile. Airborne initialization
for the basis of this invention failed to robustly con of each computational cycle is permitted to accommo
verge upon an optimal solution when put into practice, 45 date externally derived and uplinked data. The revised
as have all of the published schemes for real time opti data are taken from the most reliable source currently
mal guidance. In particular the method as published has available, such as on-board target-seeking radar when
been subject to convergence problems when the control the target-seeking radar is locked onto the target, uplink
variables enter certain ranges. When the control values data from ground or airborne tracking radar when an
either enter around zero or close to a control boundary, 50 uplink is operational, or inertial guidance data.
the published method fails to converge upon a solution. BRIEF DESCRIPTION OF THE DRAWENGS
SUMMARY OF THE INVENTION Other objects and advantages of the invention will
This invention is a real-time missile guidance system become apparent upon reading the following detailed
and method that periodically computes guidance con 55 description and upon reference to the drawings, in
trol commands. The guidance control commands gener which:
ate a field of extrema for maximizing the missile's termi FIG. 1 is a plan view of a tactical missile incorporat
nal velocity while satisfying control authority limits and ing the present invention;
terminal attitude constraints. The guidance control FIG. 2 is a schematic diagram illustrating the use of
commands include commands for controlling the angle the tactical missile of FIG. 1 for air defense;
of attack (a) and the bank angle (db) of the missile. FIG. 3 is a block diagram of a guidance processor and
In accordance with a first aspect of the invention, at its interconnection to various systems in the tactical
least two control variables, 62 and 33, are selected missile of FIG. 1.
which specify the angle of attack (a) and the bank angle FIG. 4 is a schematic diagram of the guidance system
(d), but which have definite and limited values when 65 for the tactical missile of FIG. 1, including a dynamic
specifying an angle-of-attack (a) of approximately zero. model of the missile;
The selection of g2 and g3 as virtual control variables is FIG. 5 is a time line illustrating the finite elements
made in lieu of selecting the angle of attack (a) and bank and nodes used in the derivation of the system of alge
 5,435,503
10
braic equations for optimizing a performance function the seeker 27 has a limited range and limited field-of
(J) in accordance with the weak Hamiltonian finite-ele view, successful acquisition by the seeker can be facili
ment method; tated by initially guiding the missile in response to
FIG. 6 is a schematic diagram illustrating body axes tracking data transmitted from the ground radar 21 or
of the missile, a relative wind vector, and the angle of 5 the airborne radar 32 before acquisition by the seeker.
attack; The tracking data are transmitted via laser or radio
FIG. 7 is a schematic diagram illustrating the bank along an uplink path 33.
angle of the missile; The tactical missile 20 should have a maximum veloc
FIG. 8 is a flowchart illustrating the iterative control ity at the time of interception, and should have a prede
procedure of the present invention for generating guid O termined attitude at interception in order to maximize
ance acceleration commands in real time during flight damage to the ballistic missile 30. The predetermined
of the missile; and attitude at interception is specified by a desired crossing
FIG. 9 is a flow chart of a procedure for accelerating angle 6CR between the longitudinal axis 35 of the tacti
convergence when control inequality constraints are cal missile 20 and the longitudinal axis 36 of the ballistic
reached by modifying iterative solutions between itera 15 missile. The tactical missile 20 should impact the ballis
tions when the iterative solutions fall outside of the tic missile 30, for example, at a desired aim point 34 near
inequality constraint boundaries. the warhead of the ballistic missile. This desired aim
While the invention will be described in connection point 34 is generally different from the radar centroid 35
with a certain preferred embodiment, it is not intended as viewed by the seeker 27. In general, the field-of-view
to limit the invention to this particular embodiment. On 20 of the seeker 27 is directed along a line-of-sight that is
the contrary, it is intended for the invention to include the same as the velocity 38 of the tactical missile 20
all modifications, alternatives, and equivalent arrange relative to the tactical missile 30.
ments as may be included within the scope of the ap The present invention enables guidance of the missile
pended claims. to a terminal geometry condition where the crossing
25 angle 8CR can be specified with respect to two axes,
DETAILED DESCRIPTION OF THE while maximizing the missile's terminal velocity. For
PREFERRED EMBODIMENT targets, such as airplanes, that are not approximately
Turning now to the drawings, there is shown in FIG. symmetrical about their longitudinal axis, the crossing
1 a plan view of a tactical missile generally designated angle 6CR could be specified by two values, such as a
20. Various internal components of the missile 20 are 30 total angle component 6CR giving the magnitude of the
shown in phantom lines. For controlling the missile, the total angle between the between the longitudinal axis 35
missile has a guidance processing unit 21 (GPU), which of the tactical missile 20 and the longitudinal axis of the
is a digital computer 21 that will be further described target, and an orientation component 0CR specifying
below with reference to FIG. 2. For determining the the crossing angle in a plane perpendicular to the longi
position and angular orientation of the missile, the mis 35 tudinal axis of the target and with respect to a lateral
sile has an inertial measurement unit 22 (IMU) including axis of the target. For a target such as an airplane, for
a respective gyro or angular rate sensor for sensing example, the lateral axis of the target could be assumed
angular rotation of the missile about each of the three to be a vertical axis with respect to the Earth.
orthogonal (x,y,z) axes of the missile as shown in FIG. Turning now to FIG. 3, there is shown a block dia
7, and a respective accelerometer for sensing linear gram of the guidance processing unit 21 and its inter
acceleration of the missile along each of the (x,y,z) axes connection to other components and systems of the
of the missile. For adjusting the attitude or angular tactical missile (20 in FIG. 1). The guidance processing
orientation of the missile, the missile has an attitude unit 21 is a high-speed digital processor having an inter
control system 23 (ACS) and an aerodynamic maneu nal floating-point math unit. It operates with external
vering system 24 (AMS). The aerodynamic maneuver 45 random access memory 41, external read-only memory
ing system 24 adjusts the angular orientation of fins 25 42, an external floating-point coprocessor 43, interrupt
at the tail end of the missile, which will adjust the atti and reset control circuitry 44, synchronization and
tude of the missile when the missile is at low altitude. clock generator circuitry 45, and a multiplex bus con
The attitude control system 23 includes lateral-thrusting troller 46. The multiplex bus controller controls a multi
rocket motors near the nose 26 of the missile, which can 50 plex bus (MUX) for parallel input/output of the guid
adjust the attitude of the missile even at high altitude. ance processing unit. External interfaces to other mis
For seeking the target, the missile has a seeker 27 built sile subsystems are accomplished through both discrete
into the nose of the missile. Depending on the desired signal interconnects and via the multiplex data bus. The
target and the size of the missile 20, the seeker 27 may multiplex bus interfaces the guidance processing unit to
use radar, laser, infra-red, or optical imaging techniques 55 the radar seeker 27, the inertial measurement unit 22,
for seeking a target. the aerodynamic maneuvering system 24, and an uplink
FIG. 2 illustrates the use of the tactical missile 20 for data receiver 47. The guidance processing unit 21 is
seeking and intercepting a target 30, such as a ballistic interfaced to the attitude control system 23 via a serial
missile. The ballistic missile 30 is initially detected and link 48.
tracked by ground radar 31 or airborne radar 32. When The guidance processing unit 21 is programmed to
the tactical missile 20 is launched, it is programmed perform a number of time-shared functions. These func
with the coordinates of a predicted point of intercep tions include an alignment function 48, an autopilot
tion. To intercept the ballistic missile 30, however, it is function 49, a navigation function 50, and a guidance
necessary for the tactical missile 20 to be guided in real function 51. The alignment function 48 involves sensing
time so that the trajectory of the tactical missile inter 65 and controlling the attitude and roll of the missile 20, as
sects the trajectory of the ballistic missile. The seeker 27 will be further described below with reference to FIG.
of the tactical missile 20 must eventually acquire or lock 4. The autopilot function 49 maintains the velocity vec
onto the ballistic missile to ensure intersection. Because tor of the missile 20 along a specified direction by issu
 5,435,503
11. 12
ing angular acceleration commands to the alignment The missile's dynamic and aerodynamic response 52 to
function. The navigation function 50 keeps track of the the reactions of the attitude control system 23 or the
current position of the missile 20 and changes the speci aerodynamic maneuvering system 24 result in the mis
fied heading of the missile along a specified trajectory. sile flying to a new position with an altered attitude and
The guidance function 50 calculates in real time an velocity. Linear accelerometers (not shown) and gyros
optimal trajectory to a target based on data available (not shown) sensing angular rates of change in the iner
during flight of the missile. tial measurement unit 22 provide measurement data of
Turning now to FIG. 4, there is shown a schematic
diagram that more particularly illustrates the relation of thefor
missile's new state and enable feedback information
comparison with desired values and the initiation of
the guidance function 51 with the other functions and O the next computational cycle in the missile GPU.
systems in the tactical missile. The guidance function 51
is responsive to time as indicated by the synchronization controlThe guidance function 51 is restricted by at least one
and clock generator circuits 45. The guidance function limit or inequality constraint. For aerodynamic
51 is also responsive to a position error signal as indi stability, for example, the "angle of attack (a)' of the
cated by the radar seeker 27 when the seeker 27 has 15 missile must not exceed a predetermined value, such as
acquired a definite lock on the target, or as indicated by 20 degrees. In the system of FIG. 4, the guidance func
the difference between the target position, as indicated tion does not receive as an input the actual angle of
by tracking data received by the uplink data receiver 47, attack, nor does the guidance function directly specify
and the position of the tactical missile, as maintained by as an output what the angle of attack (a) should be.
the navigation function. The target position at desired 20 Instead, the guidance function 51 has a mathematical
intercept and the desired missile attitude at intercept are model of the aerodynamic characteristics of the missile.
either pre-computed prior to launch and downloaded to The guidance function can therefore determine, for any
the missile's guidance processing unit prior to launch or potential trajectory, whether the angle of attack (a) will
provided in-flight via an uplink message received by the exceed its control limit. Consequently, the guidance
uplink data receiver 47. 25 function will not produce acceleration commands that
The navigation function 50 is initially programmed would require the angle of attack (a) to exceed its con
with the position of the launch site, and after launch, the trol limit.
navigation function updates the initial position by inte The guidance function 51 periodically computes
gration of linear acceleration signals from the inertial guidance
measurement unit 22. The navigation function may 30 mand foracceleration each of the
commands. An acceleration con
three missile body axes is com
adjust this position computation with tracking data
received by the uplink data receiver 47, to coincide puted during each computational cycle. During each
with the position of the missile as determined by the cycle, a solution vector X is computed iteratively by the
ground or airborne tracking radar (31 and 32 in FIG. 2). Newton-Raphson solution procedure of Equation 23
The guidance function is responsive to an attitude 35 above. The solution vector X represents a complete
error signal that represents the difference between a trajectory from the present position of the missile to the
desired terminal attitude at target interception, and the predicted position of the target at the time of intercep
missile attitude as indicated by the alignment function. tion of the target by the missile.
The missile attitude is conveniently represented by two Preferably the system of equations ECX) in Equation
attitude angles (y, ). The first attitude angle y, for 23 above are algebraic equations result from a discreti
example, represents a vertical angle from a horizontal zation of Equation 17, above, that computes the integral
inertial plane to the missile's velocity vector. The sec in Equation 17 by a Finite Element Method. This sys
ond attitude angle , for example, represents a horizon tem of algebraic equations will be listed below, but to
tal angle measured from North to the projection of the understand the listing, it is necessary to first understand
missile's velocity vector on the horizontal plane. 45 the notation employed in the equations. The equations
The guidance function 51 is responsive to other per will use the Einstein summation representation, wherein
formance criteria. The performance criteria may spec each permutation of the indices represents one equation
ify that the guidance function 51 is to maximize velocity in the system. The equations are written to represent a
at the point of interception with the target. system for any number of nodes, and any number of
The guidance function 51 computes guidance accel 50 phases, or groups, of consecutive nodes.
eration commands in order to approximate an optimal As shown in FIG. 5, the time interval from an initial
trajectory. The acceleration commands, for example,
specify linear acceleration in g’s along each of body ments At1 to AtN. time
time to to a final tris broken into N discrete ele
The endpoints of the elements repre
axes (b1, b2, b3) of the missile, as defined in FIG. 6
below. The autopilot 49 is responsive to these linear 55 sent nodes atto-1 to tN+1=t?. The value of a parameter
(i) at a node will be represented as (x) The value of a
acceleration commands, and specifies changes to the parameter (x) at a mid-point between a pair of neighbor
bank angle (b) and angle of attack (a) of the missile.
The bank angle (d) and angle of attack (a) are further ing nodes will be represented as (x). Moreover, the
discussed below with reference to FIGS. 6 and 7. The parameter will be indexed as follows:
alignment function 48 operates the attitude control sys
tem 23 or the aerodynamic maneuvering system 24 to X
A.
Represents the finite-element node
change the bank angle (b) and angle of attack (a) of the phase state condition for parameter x for a given
missile in accordance with the commands from the node state, phase and node.
autopilot 49. The actual change in attitude of the missile x Represents the mid-point condition for
is sensed by the inertial measurement unit 22 and indi 65 phase state parameter x for a given state, phase,
element and element.
cated to the alignment function, so that the alignment
function may use feedback control to compensate for
the missile's dynamic and aerodynamic response 52. Examples:
 5,435,503
13 14
-continued
Represents the 3rd state in phase 2
for the 2nd node.
5 With the above notation in mind, the preferred sys
Represents the 2nd state in phase 3 tem of algebraic equations is:
for the ist element.

(Equation 24-1)
fk + j k - -A- f= 0; j = 1, number of phases (nph)
1 1 1; k = 1, number of states (ns)
(Equation 24-2)
f* - -A-fk + jk - --fc = 0. i = 1, if of elements in
i i i + 1 i - l; a phase - 1 (ne(j) - 1)
(Equation 24-3)

(Equation 24-4)

(Equation 24-5)
2 d

i -- 1 jk jk i -- 1 jk j
i -- 1 i -- 1 i -- i --

- - - T M
(Equation 24-6)
frk -- A2 d t +-22a. f^k + F- = 0
ne(i) jk jk nei) jk j jk
ne(j) ne(j) ne(j) ne(i) 2
aL3. -- 9au : 2G p"2-n
= 0; i -
= 1, if element in

jm jm j J ; a phase (ne(i)) (Equation 24-7)

i i

af af. agT 2 - (Equation 24-8)

- + -i- + -i- i = 0; i = 1.2

G+ K2 = 0 (Equation 24-9)
j j
i i

6+ k2 = 0 (Equation 24-10)
j j
i i

2pi K=0 (Equation 24-11)

 5,435,503
15 16
-continued
22 k=0 (Equation 24-12)
j j
i i

A. ad T
ak -
(Equation 24-13)
f^k + aik -- v a3k T
1 j J
1

A ad ak (Equation 24-14)
jk + i + v i =
- T -

2 j J
2 2

f =0 (Equation 24-15)
In accordance with an aspect of the present inven- -continued
tion, the control variables (up) include a set of control d = arctan(A3/A2)
variables functionally related to the angle of attack (a)
and bank angle (d) in such a way that convergence 25 The inverse transformation is:
problems are avoided when the angle of attack is ap
proximately zero. When the angle of attack (a) and (Equation 27)
bank angle (d) are used as two of the control variables g = tana - N p, g?
(uk), there are convergence problems in the iterative
solution procedure. These convergence problems arise 30 A = Acosdb
A3 = gsind
when the angle of attack (a) is approximately zero, db = arccos(32/3) = arcsin(33/13)
because the bank angle (d) may assume any value when
angle of attack (a) is zero. To solve these convergence It is also convenient to define an intermediate vari
problems, at least two control variables are used that are able (3 such that:
related to the angle of attack (a) and the bank angle (g) 35
by a predefined mathematical transformation of vari As2 = cosdbtana. (Equation 25)
ables such that the two control variables assume appro As3 = sindstana.
priate values for typical flight characteristics of the
missile. Moreover, because the guidance function does The control variables are used in the state equations
not directly sense the actual values of the angle of attack 40 of the missile, and in particular are related to forces in
(a) or the bank angle (3), nor does the guidance func the equations of motion for the missile. The forces along
tion directly control the angle of attack (a) or the bank the missile body axes are given by:
angle (3), the two control variables related to the angle
of attack (a) and the bank angle (3) are more precisely (Equation 28)
referred to as "virtual' control variables. 45
Thrust - Drag
The angle of attack is shown in FIG. 6 in relation to Fody = Lift
the body axes (b1b2, b3) of the tactical missile 20. The O
b body axis is the longitudinal axis of the missile 20.
The angle of attack (a) is defined as the total angle
between the longitudinal axis (b1) of the missile and the 50 It is convenient to define a 3x3 transformation matrix
relative wind vector rw. The relative wind vector is the Cwb) between the wind frame and the missile body
velocity of the wind relative to the missile. Therefore, frame, and a 3x3 transformation matrix Cw between
when the velocity of the missile with respect to an the wind frame and the local inertial frame. Then the
inertial frame greatly exceeds the velocity of the wind forces originating in the inertial frame are due to the
with respect to an inertial frame, the relative wind vec 55 mass (m) of the missile and the acceleration (g) due to
tor will become approximately anti-parallel to the ve gravity:
locity vector of the missile, and the angle of attack (a)
will become relatively small. (Equation 29)
The bank angle (b) is shown in FIG. 7. The bank 0.
angle (dB) is the angle of the relative wind vector rw Fravity = 0
with respect to the -b1 body axis of the missile 20. Ing
The preferred set of control variables are 32 and g3
such that:
The forces in the wind frame are therefore given by:
(Equation 26) 65
a = arctan \ 622 + 63? Find = (Cwb) Fody + (Cwl) Fravity (Equation 30)
 5,435,503
17 18
The matrix Cwb) is a function f(a,100) of the angle of i=1,2,3, is defined to be fixed in W, and we write the
attack (a) and the bank angle (db) according to: associated direction cosine matrix
CwB=f(a,d) (Equation 31) Cij= widi (34-3)
Equation 24-7 and 24-8 above, however, can be satisfied in terms of Rodrigues parameters so that
with a negative eigenvalue for the optimality condition
when a st0. The eigenvalues of the optimality condition
are eigenvalues of the matrix (34-4)
- AT -- A.T
O
C=
(-4 97), 44 9T3 - 6
i = 1, # controls (Equation 32)
a Hk j = 1, # controls
1 + i.1. 979
auiaui
k = 1, it states
5
where CCT-CTC=I, the 3x3 identity matrix, and
where; H = Alf -- L -- 2To
34-5
81 O -63 62 (34-5)
A negative eigenvalue indicates that the solution con
verges upon a stationary point other than the desired e = 62 6- 63 O -6
extremum. Another formulation that has a negative 83 -82 (91 O
eigenvalue is Cwb) = f(a1, a2), where a1 is a “vertical'
angle of attack, and a2 is a “horizontal' angle of attack. Since there are three 6 and we only need two parame
Negative eigenvalues are avoided by the "tangent vari ters, we will need to constrain 6. We choose to do so
able' formulation such that:
with a nonholonomic constraint on the angular velocity
25 below.
(Ecuation 53
g2 = g22 + 332 Equating expressions for V from Eqs. (34-1) and
CWB = f(g,62,83) where: £3 = cosdbtance (34-2), and using (34-3), we obtain three kinematical
A3 = sindbtan cc
equations of the form
30 i=uwai (34-6)
A set of equations governing the motion of the missile or, in matrix form
20 is needed for trajectory optimization. It is assumed i=uCTel (34-7)
that the missile 20, represented as a rigid body M, is
moving in an inertial frame A subject to inertial, aero 35
where
dynamic, and body forces. The inertial properties of M
are idealized to be those of a particle M* of mass m. The (34-8)
gravity force at M is mgk, where k is a unit vector
directed vertically downward. The geometry of M is
axisymmetric, and the aerodynamic forces acting at M*
can be characterized in terms of a frame B in which the
unit vector b1 is fixed, parallel to the axis of M, and in
which the direction of V, the velocity of M* in A, is Denoting the angular velocity of W in A by cy, we let
known. Thus, the aerodynamic forces consist of an axial
force (T-DOb1 and a lift force Lin, where n is normal to 45
b1 and parallel to the plane which contains b1 and V. (34-9)
The quantities D and L are known functions of v=v) c)
and a, the angle between V and b1; T is a known func 2.
tion of time. (In the degenerate case for which V is c3
parallel to b1, L=0. Note that the frame B, in which b1 50
is fixed, is not a body-fixed frame. The missile is free to
spin about its axis in B.) Initial conditions affect only the where co1=a).Wi. Based on Eq. (26-4), one can show
initial position and velocity of M in A. that
First, we define a point 0 and a dextral triad ai, for
is 1,2,3, all fixed in A. Letting p denote the position 55 (34-10)
vector of M* relative to 0 and x=p-ai, we can form the
velocity of M in A as
V=k1a1+:2a2-isa3 (34-1)
Denoting the unit vector tangent to the path of M* as 60
w1 we can also write the velocity as the inverse of which is given by
(34-11)

One could characterize the direction of V in terms of 65

only two parameters, but we choose instead to intro
(1 + + 6 + +67)
-- a-- A.T

duce an intermediate frame Wassociated with the path We need the acceleration of M*, which can be writ
of M* and in which V is fixed. A dextral triad wi, for ten as the inertial time derivative of uw so that
 19
5,435,503
20
This concludes the definition of all kinematical quan
A = w + vo X w. (34.12) tities needed to express the equations of motion.
The equations of motion can be written in accordance
with Newton's second law
Finally, we need to define the relationship between B F=ma (35-1)
and A to define the aerodynamic forces. We will actu
ally work in terms of relating B to W, but first W must where, if k is taken as parallel to as, the active force
be well defined. We will set col=0, which non resultant acting at M is given by
holonomically constrains the set of coordinates 6. This 10
way the orientation of frame W is governed only by the
path of M*, and only the orientation of b1 and the lift Introducing the column matrix of force components in
force need to be defined relative to W. the W system
As with pointing the velocity vector in A, we only 15
need two parameters to accomplish this: the angle of (35-3)
attack a and the "bank” angle d to orient the plane in W Fl
which contains the missile axis. Let B be tentatively F - F2
oriented so that bi coincides with wi for i=1,2,3. Now, F3
to get to the final orientation, we rotate Babout b1 by an 20
angle db. Next, rotate B about bi by an angle a. This
brings b1 into alignment with the axis of M during flight, where Fis [Link] so that
and the lift is assumed to act along b2. Thus, (34-13)
F = mgCl3+ (IP) - LE=- (35-4)
25 \ 2 \ , , e.
b coso sino. 0 1 O O w
b2 = -sina cosa 0 || 0 cosd sind w2 F2 = mgC23 + (T - DB2 -- - E
b3 O O 1 || 0 -sind cosd '3 le: e\ 62
cosa cosdbsina sindbsina. w
30 F, - mgcs + 2g; + -6\ E =
162
- -sina cosqbcosa sindbcosa. w2.
O -sing costs w3 and taking the dot product of Eq. (35-1) with will, and
making use of Eqs. (34-12) and (35-4), one obtains three
35 equations of motion of the form
Now, to avoid convergence difficulties created by the
vanishing of a in which db then becomes undefined, we
introduce two variables (32 and 33 such that
A2=cos distan a 40
-nuo2=F3 (35-5)
f33=sin d tan a (34-14)
Eqs. (35-5b) and (35-5c), along with the definition of c)
so that and the constraint this col=0,
45
g2=622-632=tana. (34-15)
o) = - 1F (35-6)
Thus,
The system equations can now be written as a system
(34-16) 50 of seven first-order ordinary differential equations. The
b1 three equations embodied in Eq. (34-7) are already in a
b2 = matrix form. Eq. (35-5a) is one scalar equation. Finally,
b3 Eqs. (34-11) and (35-6) can be written as one matrix
equation. Thus, the system equations are
55
- -- - - - i: = uCTel (36-1)
\ , i.e. \ , , e. \ 1 e2 w = - e, TF
-B B2 A3 w2.
\ 1- 2 e\ 1 2 e\ 1 - 2 '3 60
O -£83
As
f32
B 8 --- 1-a-, - AAT
= (1 + +G++ dor) if
When combined with the expressions for the force and where Fi is given by Eqs. (35-4). Control variables g2
with the () operator notation introduced below, an 65 and g3 can be used to determine the trajectory of M*.
orientation description such as embodied in Eq. (34-16) Eqs. (35-4) appear to be quite complex. The reason
should be free of convergence difficulties when a and g for the complexity is, at least in part, related to the fact
vanish. that L and D are functions of a while (3 appears explic
 5,435,503
21 22
itly in the coefficients of L and D, as well as that of T. in the vicinity of g=0 every time there is differentiation
To circumvent this, L and D can be expressed in terms with respect to one of the control variables.
of 6 instead. All explicit 6 dependence can be grouped The first derivative of any even function of 6 will be
into implicit functions which can be splined or approxi odd, but its first derivative divided by 6 will be even.
mated as a series. One way to do this is to introduce Motivated by this observation, the following notation is
introduced: ()*=()'/g such that ()' = a()/ag. Now,
D=HucA
L= Lucy (37-1)
O
o' =--Or:
a- C" =--- r" - a
a-C," =- (37-7)
where
and so forth for higher derivatives
pSA (37-2)
p =- (37-8)
15 o** = -(2-
g = -l-
g - Cg -
and where CN and CA are considered functions of £3 and
Mach number. Thus, the force components become
(37-3) C."kk ---(C)a----- ( Cag )
F.1 = ngul3
C3 -- T uv°CA |LucNf3 20
- - - see - E
\ , , e. N a e.
C."... -- - A ---- i?( ga )
F2 = mgC23 +
2 se mon T(32 uu’CA62 --
anawaaaaaa- uu’CNf32
---

N le: \ , , e. 8\ 62 25 This way, if any of these functions, say Q, is represented

as a power series
F = mgC33 +
TS3 uu’CA63 uv'CNf33
se - - -Ass + -itse
\ , , e. e\ 1 - 2 c X. -
ai-2 (37-9)
O I=0,2,4,... Qig
Now, upon substitution of the following even functions 30
of 6 one can show that

Q= l (37-4) * X [Link]-2 (37-10)

Q i=2,4,6,... igf8
35
C = -2 Q**kak = i=4,6,3,... i(i
- - 2
- 2)Q 8i-4
e.
C.n --\ S 2- Once these are calculated, we then can find
Q =BQ* Q'-629**--Q* (37-11)
which can be conveniently represented as splines, Eq.
(29-3) simplifies to We note that if the expansion of Q is limited to cubic
order about the origin, say a cubic spline, then
45 Q*=2O2 and Q*=0.
(37-5)
Of course, this operator does not change the results of
finding derivatives. It just removes the myriad of inde
terminate forms. Also, this operator greatly simplifies
In construction of the nonlinear algebraic equations 50
the expressions for the second derivatives. For example,
and the associated Jacobian, we will ultimately need to
form first and second derivatives of the force compo
nents with respect to the control variables B2 and (33. 82F (37-12)
Such partial derivatives of any function of £3 must be i = To" + Bio" -
found from the chain rule, which involves derivatives 55
of the function with respect to g times one of the fol uu? (C + 2C+ C*g2 + 332 (C** + 4C, + C** {3)
lowing partial derivatives:
and

6 - Pg -
ag2
(37-6) 82F (37-13)
60 se2
which will have the effect of introducing indeterminate
forms
which are well behaved in the limit of small (3. In fact,
65 Fi and all their first and second derivatives with respect
to £32 and 63 are also well behaved, as has been demon
strated by Mathematica 5 computations. For 6=0 it can
be shown that
 5,435,503
23 24
time to the end-of-phase, taph; and the time to the end
a?Fy (37-14) of-flight, tigo. An initial solution vector XFCC is down
-
afésaf3n . = 0 8, n, and yy =
n, and = 22 an
and 3 loaded into the guidance processing unit (21 in FIG. 3)
from a fire control computer (not shown) external to the
while missile. ECX) is the vector used in Equations 21 to 23
above and which contains the residuals from the equa
a2F 37.15
(37.15) tions when the equations are evaluated with the vector
af320A3 p=0 Xji.
The variable NC is the cycle counter. NC is initial
and 10 ized to zero, and NC is incremented every time that the
(37-16)
WHOA optimal guidance routine is called. The variable
'k' is a Newton-Raphson iteration counter, which is
E;
agile-0 ag3 8-0 reinitialized before the Newton-Raphson iteration
scheme is called each cycle. The variable "kna, is the
15
where the fact that Q(0)= -1 is used. maximum value number of Newton-Raphson iterations
If H is the Hamiltonian, and u is the column matrix allowed for convergence. ADG is the acceleration
which contains the control variables (32 and 33, then command vector for the missile. The acceleration com
Hu for this problem becomes a diagonal matrix in the mand vector ADG is the output from the guidance
limit as 6 approaches zero. Both diagonal elements are 2 procedure.
the same and are given by In the first step 61 of FIG. 8, execution branches to
step 62 if NC is zero. In step 62, the solution vector X11
(Huu)11 g=0 = (37-17) is initialized with the solution vector XFCC down
My 2
loaded from the fire control computer. If NC is not
zero, then step 63 is performed, which loads the initial
(Hui).2b-0 = -- (T + w(C.'(0) + 2C,"(0)} 25 solution vector X1NC with the last best solution vector
It is therefore evident that Hu will be positive definite vector for thisthecycle
X1 NC1 from last cycle. Once the initial solution
is obtained in steps 63 or 64, the
in the vicinity of a = 6=0 if A (0. execution continues in step 64.
A specific solution depends on the mass, thrust, gravi
tational, atmospheric, and aerodynamic data. In the 30 In step 64, the cycle counter NC is incremented, and
following data, for example, the thrust is presumed to the iteration counter 'k' is initialized to 1. In step 65, the
have discontinuities at Stage1-time seconds and input units
boundary conditions are transformed to consistent
used by the program. These input boundary condi
Stage2 time seconds after launch. Therefore, it is con
venient to divide the performance integral J into three tions include the missile position and velocity vector as
phases; a first phase from t=0 seconds to 35 well as the desired target intercept position and attitude.
t=Stage1-time seconds; a second phase from In step 66, execution branches to step 67 if it is desirable
t=Stagel-time seconds to t=13 seconds; and a third to re-size the Jacobian. An increase in the number of
phase from t=13 seconds to the time of intercept with elements, for example, increases the precision of the
the target. solution, but also increases the computation time needed
40 for computing a solution and obtaining convergence.
The computation time is dependent on the particular
MASS = F(time): phase of flight. Therefore, it is generally desirable to
weight time
re-size the Jacobian if a flight phase transition occurs. In
300 kg 0.0 sec. step 67, the solution vector [Link] is redefined based
273 .09 X Stage2-time 45
240 .18 X Stage2-time upon the re-sized Jacobian matrix. In step 68, the com
207 .27 X Stage2 time putation of the equation residuals ECXi NC) is done in
200
180
.29 X Stage2-time
4 X Stage2-time
vector form using the proposed solution vector Xk.N.C.
170 .55 X Stage2 time This is the initial step in the Newton-Raphson iteration
160 .69 X Stage2-time 50 loop including the following steps 68 to 75. During each
150 .83 X Stage2 time iteration 'k', the Newton-Raphson iteration loop com
143 .95 X Stage2-time putes a new solution Xk-1NC-1 as a predetermined
140 Stage2 time
solution function S(XkNC) of the last solution XkNC.
THRUST = F(time,ambient condition): The computation of the new solution, however, in
Thrusto = Stage1 thrust Newtons, for: 0-Stagel-time
Thrusto = Stage2 thrust Newtons, for: Stagel-time-Stage2-time 55 volves steps 71, 72 and 73 in addition to the initial step
Aexit = 024 meters squared 68.
Thrust = Thrusto - (ambient pressure) X Aexit Once the equation residuals are defined, they are
validated against convergence criteria, in step 69. The
Turning now to FIG. 8, there is shown a top-level convergence criteria, for example, are specified by a
flowchart of the guidance function (introduced as box 60 vector of predefined error limits, and if any element of
51 in FIG. 4). This flowchart includes a number of the vector of equation residuals ECXkNC) has a magni
variable names and symbols. Xi denotes the solution tude that exceeds it corresponding error limit, then the
vector for the jth cycle and the ith Newton-Raphson solution vector XkWC fails the convergence criteria.
iteration. The solution vector Xi is composed of values Otherwise, if each element of the vector of equation
for the states, x; the costates, A; the Lagrangian multipli- 65 residuals ECXkNC) has a magnitude that is less than its
ers u for the control constraint equations; the slack corresponding error limit, then the solution vector
variables K for the control constraint equations; the XNC passes the convergence criteria, and in step 77, the
controls, u; the boundary condition multipliers v; the acceleration command requisite for optimal flight
 5,435,503
25 26
(AGDWC) is computed as a predetermined function dure is performed on each respective set of K, G, u, and
g(XNC) of the converged solution vector [Link]. u values.
If the convergence criteria are not met, then in step In a first step 81 of FIG.9, execution branches to step
70, the Newton-Raphson iteration counter k is com 82 when the computed time to the end of flight (tgok) is
pared againstkmax. This step is used to control the com not greater than zero. In step 82, the time to the end of
putation time used by the procedure. If k is greater than flight tgok for the present iteration 'k' is set equal to the
knac, then an acceleration command vector is computed time to the end of flight for the previous iteration k-1.
either based upon the last best acceleration command When the computed time to the end of flight tgok is
vector or by using a conventional proportional naviga found to be greater than zero in step 81, or after step 82,
tion scheme, depending upon the time-off-light. This is O execution continues in step 83.
done in step 76. In step 83, execution branches depending on the logi
If the Newton-Raphson iteration counter k does not cal value of the proposition “ud0. AND. K=0'. When
exceed the limit "kna, execution continues from step 70 this proposition is true, then execution branches to step
to step 71. In step 71, the Jacobian matrix is updated by 84. Execution branches in step 84 depending on
re-evaluating all of the non-zero elements of the Jaco 5 whether G is greater than or equal to zero. When G is
bian matrix J for the previously-computed solution not greater than or equal to zero, an inequality con
vector XNC. In step 72 the Jacobian matrix is inverted, straint is violated, and execution branches from step 84
and the inverted Jacobian matrix is multiplied by the to step 85. In step 85, the value of the control (u) is
previously-computed solution vector Xk NC in order to moved off of the constraint boundary. The degree to
compute the next delta value AX on the solution vector which the new value of the control (u) differs from its
X. Therefore, in step 73, the new solution vector limit at the control boundary is not critical, and the new
Xk-1NC is computed by subtracting the delta value AX value may be set to a value that differs from its limit by
from the previously-computed solution vector XNCIn an amount proportional to the value of u. Then, in step
step 74 the Newton-Raphson counter, 'k', is incre 86, the discrete Lagrangian multiplier u is set to zero,
mented by one. Then, in step 75, the control constraint 25 and K is calculated based on the value of the control (u)
equations are checked using the control values (u) in the and for u = 0, or example, such that K2=-G(u). Then
new solution vector Xk-1NC. This step is essential to execution returns.
free up the controls (u) to move on and off of control When G is greater than or equal to zero, none of the
boundaries. The Newton-Raphson iteration scheme is constraints are violated. However, in this case, execu
resumed again in step 68, until either the convergence 30 tion branches from step 84 to step 86, because conver
criteria are passed in step 69 or the maximum number of gence can be accelerated in this case by setting u = 0
iterations kna is exceeded in step 70. and calculating K for u =0, for example, such that
Turning now to FIG.9, there is shown a flowchart of K2=-G(u).
a procedure for checking whether control limits are When it is not true that “u0. AND. K=0”, then
exceeded in each iterative solution XkWC, and adjusting 35 execution branches from step 83 to step 87. Execution
the iterative solution when the controls move on or off branches from step 87 depending on the logical value of
a boundary or when a new time solution exceeds the the proposition “KCO. AND. u =0'. When this propo
bounds of realizability. The virtual angle of attack (a), sition is true, execution branches to step 88. In step 88,
for example, should be limited to less than a predeter execution branches depending on whether G is greater
mined value such as 20. This could be done by impos than or equal to zero. When G is found to be less than
ing a constraint involving g2 and S3 such as zero, then execution branches to step 89. In step 89, Kis
tan2(20)-322-g32=G. The limit of 20 represents a calculated from the values of the other parameters, for
constraint boundary for the angle of attack. If the example as K2=-G(u), and execution returns. When
boundary is exceeded, then the constraint can be en G is found in step 88 to be greater than or equal to zero,
forced by either setting the specified value for the angle 45 then execution branches to step 90. In step 90, the con
of attack to 20 or some value less than 20. Sometimes trol (u) is moved on the constraint boundary. Next, in
the constraint is enforced by setting the specified value step 91, K is set to zero and u is calculated for the value
for the angle of attack (a) to the limit of 20, because the of the control (u) and for K=0, for example, by apply
limit value might be optimum. This is a case where a ing Equation 24-7 above. Then execution returns.
control is set on the constraint boundary, in order to SO When step 87 finds that “K(0. AND. L=0” is not
enforce the constraint. Sometimes the constraint is en true, then execution branches to step 92. In step 92,
forced by setting the specified value for the angle of execution branches depending on the logical value of
attack (a) to a value less than 20. In this case, the con the proposition "(K-7.0. AND. uz-0) .OR. (K=0
trol is said to be moved off of the boundary. The proce .AND. u = 0)'. If this proposition is false, then execu
dure of FIG. 9 may also check for solution constraints 55 tion returns. Otherwise, execution branches to step 93.
imposed for the express purpose of manifesting realiza In step 93, execution branches depending on whether
bility thereby accelerating realizable convergence. For G20. When G is greater than or equal to zero, then
example, when it is known that the time step At must be execution branches to step 94. In step 94, K is set to
greater than a certain predetermined amount, then a Zero, u is calculated for K=0, for example by applying
solution constraint can be imposed on At to accelerate Equation 24-7 above, and execution returns. When G is
convergence. less than zero, then execution branches to step 95. In
In general, the procedure in FIG. 9 enforces the con step 95, u is set equal to zero, and K is calculated for
straints k20, u'K=0, GSO, to20 and pu20 in the K=0, for example such that K2=-G(u). Then execu
iterative solution Xi NC. Although a true solution to the tion returns.
system of algebraic equations being solved satisfies the 65 Additional features and advantages of the present
constraints, an iterative solution does not necessarily invention are illustrated in the program listed below in
satisfy the constraints because an iterative solution is Appendix I to the specification. The entry point of the
only a guess at the true solution. Moreover, the proce program is a subroutine Whoa flight. The subroutine
 27
5,435,503
28
Whoa flight receives, as an input parameter, a speci a subroutine Eval-cc to check control boundaries and
fied time after launch of the missile. The specified time constrain the solution, and calls a subroutine
is used to determine the thrust of the rocket motor and Evaluate-convergence to check the new solution. The
the weight of the missile, because the thrust and weight : subroutine Newton returns when a converged solution .
are assumed to be predetermined functions of time. The has been found, or after a predetermined number of
subroutine WHOA-flight also receives, as input pa iterations.
rameters, the present vehicle velocity, vehicle position, The subroutine Eval cc calls either a subroutine
and the anticipated final azimuth, final elevation, and Check ccl. to check a single control constraint equa
final position. The subroutine Whoa flight returns a tion, or a subroutine Check-cc2 to check two control
parameter body-guidance cmdC3), which specifies the 10 constraint equations. The subroutine Check-ccl calls a
acceleration in g’s, along each of the three body axes subroutine Eval-bindry cnd1 to evaluate the single
(b1, b2, b3), that the missile should have to be guided boundary condition, and the subroutine Check-cc.2
along an optimal trajectory. calls a subroutine Eval-bindry cn2-to evaluate two
The subroutine Whoa flight first calls a subroutine boundary conditions. These subroutines are specific
Whoal init flight that sets up what needs to be initial 15 examples of programming for the flowchart of FIG. 9,
ized for computing the Hamiltonian in flight. Then the as described above.
subroutine Whoa-flight calls a subroutine Whoal opti A large portion of the program in Appendix I is de
mize to compute an optimal trajectory. If a converged voted to subroutines called by the subroutine Errjac
solution is obtained, or if the time is less than 0.001 and used to fill the Jacobian matrix. A low-level routine
seconds since launch, then the subroutine Whoa flight 20 Get f -g-eqn der, for example, computes the deriva
calls a subroutine Inert-guidance to compute a new tives for the elements of the Jacobian matrix, and also
guidance command. Otherwise, the previous guidance computes the derivative terms appearing in Equation
command is used. In any case, the subroutine Whoal-f- 17.
light is finished. In view of the above, there has been described a weak
The first time that the subroutine Whoa init flight 25 Hamiltonian finite element method for real-time itera
is called, it obtains an initial solution by calling a subrou tive computation of missile guidance acceleration con
tine Initialize-I-O(sol), which creates aerodynamic mands that define and optimal solution for maximizing
data tables by calling a subroutine Bcuaero. Otherwise, the missile's terminal velocity while satisfying control
the subroutine Whoa init-flight uses the present ve authority limits and terminal attitude constraints. The
hicular states as boundary conditions. The subroutine 30 guidance acceleration commands include commands for
Whoa init flight then calls a subroutine Find matrix controlling the angle of attack (a) and the bank angle
size to determine the number of elements needed, and (d) of the missile. In the virtual model of the missile
thus determine the size of the Jacobian matrix. If the contained in the guidance procedure, control variables
size needs to be changed, then the subroutine (32 and B2 are utilized in lieu of a and d to avoid conver
Whoa init flight calls a subroutine Gen 35 gence problems when the angle of attack is approxi
whoa init-param in order to alter the solution vector mately zero. This transformation avoids negative eigen
to accommodate a new matrix size. Finally, the subrou values in the Hamiltonian corresponding to the perfor
tine Whoa init flight determines an initial estimate for mance function (J). Iterative convergence is permitted
the time-of-flight (tgo). when control inequality constraint parameters are
The subroutine Whoal-optimize first determines the reached by modifying iterative solutions between itera
number of non-zero elements in the Jacobian matrix. tions so that the iterative solutions satisfy the inequality
This information is used by low-level matrix manipula constraints. This facilitates the movement of the con
tion routines (e.g., Factor matrix, Invert-matrix) that trols on and off of constraint boundaries. Moreover,
are from the Harwell Subroutine Library, as cited solution constraints can be imposed for the purpose of
above. The subroutine Whoa optimize then calls a 45 accelerating convergence by using this technique.
subroutine Newton that performs the Newton-Raphson Preferably, an optimal trajectory is calculated at each
iterations. The subroutine Whoal-optimize is then fin guidance cycle during missile flight using data that are
ished. revised during each guidance cycle. The revised data
The subroutine Newton calls a subroutine Errjac to include current position data for the target and the
compute the error of a solution, to fill in the Jacobian 50 current position for the missile. The revised data are
matrix, and to invert the Jacobian matrix, which is actu taken from the most reliable source currently available,
ally done by the Invert matrix routine from the Har such as on-board target-seeking radar when the target
well Subroutine Library. The subroutine Newton com seeking radar is locked onto the target, uplink data from
putes a new solution from the previous solution and the ground or airborne tracking radar when an uplink is
inverted Jacobian matrix. The subroutine Newton calls 55 operational, or inertial guidance data.
APPENDIX I

WEAK HAMILTONIAN REAL-TIME GUIDANCE PROGRAM

Subroutine Whoa flight ( Tine
Vehicle velocity,
Vehicle position
final azimuth
final elevation, final position
S Body guidance CIEld)
 29
5,435,503 30
tinplicit None
Include ' - . . include wifc all ré. inc'
C Irguts:
real * 8 title title Since launch in seconds
real 8 vehicle Velocity (3) ft/s
real * 8 vehicle position (3) ft
real 8 final azililuth ! final angle constraint
degrees
real 8 final elevation final elevation constraint
! degrees
real 8 final position (3) final position constraint
ft)
C Oliputs :
real 8 Body guidance CEld (3) | g 's
C for the beta angles.
c Local Variables

rear 8 sol (835)

real * 8 state (Inx X), control (IXu)
S, old body CInd (3), beta 2, beta 3
S, inin guid range/600.0/ -
Integer i, j, k, l,
nuII phases, nuIt points in traj, nuIn iterations
S, creep count, Ilax Creep/20/, iteration total (3)
Logical converged/. false. /, factor Inatrix
real 8 crossing constraint/1. Od0/, inittgo
c call the initialization algorithin which will set up what needs
O
C ce initialized for COInputing the hamiltonian in flight.
10 call whoa init flight ( Time
S, Vehicle Velocity, Vehicle position
S, final [Link]
S, final elevation, final position M
S, converged

S, sol, factor Inatrix)

converged = . false. ! initialize every title
c corpute the optitial trajectory
call whoa optitize ( factor Ithatrix, sol
f State, COIntrol
t Converged )
C using the trajectory pull out the appropriate guidance command
if (converged. Or. tille. lt. 0.001) then compute a new
guidance cornianand
call Inert guid (State, control, title
 5,435,503
31 32
S, body guidance Cr(d. )
do i = 1, 3
old body CInd(i) = body guidance CInd(i)
endido

else use old guidance cominand on non converged solution

do i = 1, 3
body guidance CInd (i) = old body CInd (i)
endido
endilf

return
end

Subroutine Inert guid (State, control, tine, body guid CInd)

Implicit None
Include ' - . . include wifc alliré.inc
Include ' - . . includel wifc thrustré.inc
Include ' - . . include Wifc bcu ré.inc
C insultS
real * 8 state (mx X) Vehicle optiInal states at the nodes
real 8 control (Inx u) Vehicle optimal States at the
real * 8 tille ! midpoints time since launch in
Seconds
C OutputS
real 8 body guid cind (3)
c local variables

integer nid, i, j, ierr

real * 8 rho, p, tellip, a, Illach, Cl, cd, alpha
S, thrust, drag, lift, throt, throst, denon, c (3, 3), d (3, 3)
S, calpha, Salpha, Cphi, Sphi, acc body (3), acc Wind (3)
S, total Inagnitude, control Inagnitude, que, beta 2, beta 3
S, beta, Vel, Cll, cd l, dragg, liftg b, gW (3)
S, acc wind from body (3), Cr1, Sr1, roll1, grav inv
S, body guidance CIEld (3), beta1, alpha1, Sal, call, sh1, cb1
S, durin&
real 8 beta ra, Inachra, beta 2 r4, beta 3ra, beta Imax/5.5/
S, inach Itax/6.0/

C et atmos situation w
grav inv = 1. OdO/grav
CALL ATMOS62 METRIC (-state (3), RHO, P, TEMP, A)
C Get Mach Number
vel = state (4)
MACH = vel/A
C Get Lift And Drag Coefficient
 5,435,503
35 36
, c clear out the COIn hands
did i = 1, 3
body guid CInd (i) = 0 . OdC)
inert guid CInd (i) = 0 . Od. O
acc wind from body (i) = 0. Od0
endido
c define the wind to inertial matrix
denon = ( State (5) * state (5)
S + State (6) * state (6)
S + state (7) * state (7)
S ) /4 - 0 + 1. Odo
c (1,1) = (state (5) * state (5)
S - State (6) * state (6)
S - state (7) * state (7)
S ) /4 - O + 1. OdO
c (2,2) = (state (6) * state (6)
S - State (5) * state (5)
S - State (7) * state (7)
S ) /4. O + 1. Odo
c (3, 3) = (state (7) * state (7)
S - State (6) * state (6)
S - State (5) * state (5)
S ) / 4.0 + 1. OdO
c (1,2) = (state (5) * state (6) /2.0) - State (7)
c (1,3) = (state (5) * state (7) /2.0) + State (6)
c (2, 1) = (state (5) * state (6) /2.0) + state (7)
c (2, 3) = (state (6) * state (7) /2.0) - State (5)
c (3,1) = (state (5) * state (7) /2.0) - State (6)
c (3,2) = (state (6) * state (7) / 2. O) + State (5)
define the gravity acceleration Vector in the Wind frame
gW (1) = c (3, 1)
gw. (2) = c (3, 2)
gW (3) = c (3, 3)
C

C define. Euler Roll Illatrix

Cr1 = cos (vehicle attitude (1))
Sr1 = sin (vehicle attitude (1))
d (1,1) = O. OdO Zeroed COInmland in x axis
d (1,2) = O. OdO
d (1,3) = O. Odo
d (2, 1) = O. OdO
d (2,2) = Cr1
d (2,3) = -sr1.
d (3, 1) = O. Od)
d (3,2) = Sr1
d (3, 3) = Cr1
Transform the Wind acceleration to the body frame
acc wind (1) = 0. 0d 0
call MA A3 led B33T dot C3 (body guid CIRd, d, acc Wind)
 5,435,503
37 38
call MA A3 leg B3 dot C (body guid CIld,
body guid CInd, gravinV)
call MA A3 leg B3 dot C (body guid CInd.,
body guid CInd, body Cind Sf)

return
end

Subroutine Find Imatrix Size (time, sol

p
Switch phase 32, Switch phase 21
r d2 phase 1, drop cc eq
r drop time eg, resize, factor thatrix)
Implicit None
Include ' - . Inciudel wifc all ré. inc'

reall* 8 time, Sol (*)

OutputS
logical factor matrix
logical Switch phase 32, Switch phase 21, d2 phase 1
S, drop title eq, resize, drop cc ed
locals

logical stage ii/. false. /, stage iii/. false. /

S, first divide/. true. /, second divide/. true. /
S, dropped title/. faise. /
S, dropped capphi/. false. /
real * 8 last X accel
check for title to switch

if ( (nph. e.g. 1). and. (sol (iorder).[Link] go_1st divide)

S ..and. ( . not. first divide) ) then
d2 phase 1 = . true.
factor Ithatrix F . . true.
resize = . . true.
first divide = . true.
elseif ( (nph. eq. 1) . and. (Sol (iOrder).lt. tine go 2nd divide)
S . . and. (...not. second divide) ) then
d2 phase 1 = . true.
factor thatrix F . . true.
second divide = . true.
resize = . true.
elseif (title. ge. drop cc. and. Set drop cc
S ..and. ( . not. dropped capphi) ) then
drop cc eq = . true.
resize = . . true.
factor matrix F . . true.
dropped capphi = . . true.
endilf
return
end

Subroutine whoa init flight ( Tilne

S, vehicle Velocity, vehicle position
 5,435,503
39 40
S, final azimuth
S, final elevation, final position
S, Converged
S, Sol, factor matrix)
Implicit None
Include ' - . Include Wifc all ré.inc
Olt S :
real 8 time ! time since launch in seconds
real* 8 vehicle velocity (3) ft/s
real 8 vehicle position (3) ft
real * 8 final azimuth final angle constraint -
! degrees
real 8 final elevation ! final elevation constraint
! degrees -
real * 8 final position (3) ! final position constraint
(ft)
logical converged indicates goodness of last
Sol.

O OutS :
real-8 sol (1) ! initialization parameters
logical factor. Inatrix ! initially true
c Local variables

Double Precision angle (3), attitude init (3)

integer Status/0/, good/0/, i
Logical initial/. true. / true the Very first time to
! Simulate
logical first/. true. W essentially what the fols would
load.
logical Switch phase 3 2/. false. /, switch phase 2 1/. false. /
S,
d2 phase 1/. false. /, drop time eq/. false. /, resize/. false. /
S, drop cc eq/. false. /
On the very first call go get the natelist and the name of the
initial file which We Will use for the first initialization
routine's solution and the convergence criteria. This would
crdinarily be supplied by the fire control collaputer at launch.
Time since launch F title Set time Since launch in Innass
! for table look-ups
if (initial) then first time read the initialization files
Call Initialize IO (Sol)
factor Inatrix F . true.
initial = . false.
else use the present Vehicular states as boundary
Conditions.
First find the phase we're in which we are located
XO1 vehicle position (1) *ft to Ineters
XO2 vehicle position (2) *ft to meters
x03 Vehicle position (3) *ft to Ineters
call MA Aeq Tag B3 (X04, Vehicle Velocity)
 5,435,503
41 42
x04 = x04 * ft to Leters
Call Init bc attitude (attitude init
S, Vehicle VeloCity, status)
XO 5 attitude init (1) -
x06 attitude init (2)
XO7 attitude init (3)
f_c11 = dcos (final elevation" deg_to_rad)
S *dcos (final azimuth.* deg to rad)
f c12 = dicos (final elevation * deg to rad)
S * disin (final azimuth * deg to rad)
f c13 = - disin (final elevation * deg to rad)
xf1 . . final position (1) *ft to iteters
xf2 = final position (2) *ft to Ineters
xf3 = final position (3) *ft to ineters
Cali Find tatrix Size (time, Sol
S, Switch phase 3 2, Switch phase 21
S, d2 phase 1, drop cc eq
S, drop title eq, resize, factor Inatrix)
c now alter the solution vector to accornodate a new matrix size
if (resize) Call Gen whoa init paran (Switch phase 3 2
Switch phase 21

: f
resize = . false.
c set the time; everytine
d2 phase 1
drop cc eq
drop title eq, Sol)

if (converged. and . first. and. Sol (iOrder) . . le. 4.0) then

xtine start (1) = Sol (iOrder) + title TOF estimate with
1 15 S. togo
first = . false.
else
if (sol (iorder) . lt. 3.0) then crly for a couple seconds
sol (iorder) = xtile start (1) - time s
endilf
endilf
99.99 return
end

Subroutine Gen whoa init parain (Switch phase 32

Switch phase 21, d2 phase 1
f dircip cc eq, drop tille eq, Sol)
iItplicit none
Include ' -- include Wifc all rô.inc
c Inputs
logical
switch phase 32, Switch phase 21, d2 phase 1, drop title eq
S, drop CC eq
c Inputs/Outputs
 5,435,503
43 44
real * 8 sol (*)
[Link] Variables
logical dummy logical
real 8 t1nx (40), t2nX (40) taxiituin equations this can
hai CS e
per element
Integer i, out offset, inp offset, old iOrder, old n, old netot
S, old nph, old Xne 1,
nbc.32/22/, nbc21/13/, nibc/8/, nthc/5/
S,
old n s, old n, old intot, j, k, nbc4/12/, nbc5/13/, ns2/7/
S, Old Inp
old iorder = iOrder
old nbc = Inbc
old netot = netot
old nph = nph
old Xne1 = Xne (1)
Old Ins S
old np = np
old. In
old intot = intot
if (switch phase 3 2. Or. Switch phase 21) then
nph=nph-1 ! set number of new phases
do i = 1, nph
write (26, ( ' 'xtime start shifted at: ' ' , g14.5) ' )
S time Since launch
xtine start (i) = xtine start (i+1) new phase start
time (S)
endido
do i = 1, nph
Xne (i) = xne (i+1) ! shift for new elements
endido
endilf

if (switch phase 3-2) then reset the number of boundary

! conditions
Inbc = InbC32
elseif (Switch phase 2 1) then
nbc = nbc21.
elseif (drop time eq) then
ns = ns2
Inbc = nbc4
elseif (drop cc eq) then
nbc = nbCS
np F 0 ! dropping control constriant equations
elseif (d2 phase 1) then
Xne (1) = xne (1)/2
endilf

n = ns + inp
intot = 2* n + In
NETOT = 0 v .
DO 5 = 1, NPH - -
NETOT = NETOT + XNE (I) count the number of elements
5 CONTINUE
IORDER = NTOT* (NETOT + 2*NPH) + NBC + NPH
 5,435,503
45 46
This shifts everything Straight from old phase 2
to new phase 1.
if (switch phase 32) then
inp offset = (xne (1) +2) *ntot start of next phase
do i = 1, iorder - nbc-nph -
sol (i) = sol (i+inp offset)
endido

out offset = iOrder-nbc-nph

Start of the new loc
inp offset old iOrder- old nbc-old nph
start of old + skip old bc
do i = 1, nib ! do the State bcs, for the initial
sol (out offset + i) = sol (inp offset + i)
endido
inp offset = inp offset + i - 1 + ns skip the first
internal node bc's
out offset = out offset + i - 1
do i = 1, (nph-1) * Ins ! do the internal node bc
sol (out offset + i) = Soli (inp offset + i)
endido

out offset out offset + i-1

inp offset inp offset + i-1
did i = 1, Intoc ! do the state bcs, 5 is for the end
sol (out offset + i) = sol (inp offset + i)
endido

inp offset inp offset + i-1

out offset out offset + i-1
sol (out offset + 1) = soil (inp offset + 2) : Inove 2nd phs
bc title to 1st
sol (out offset + 2) Sol (inp offset + 4)
Inove 2nd phase
time to 1st
sol (out offset + 3) = Sol (inp offset + 5)
! Inove 3rd phase
time to 2nd
switch phase 32 = . false.
elseif (switch phase 2.1) then -
! shift the nuinber of states 8 to 7.
inp offset = (xne (1) +2) *ntot start of next phase
do i = 1, iorder - noc-nph
sol (i) = sol (i+ inp Offset)
endido

out offset = iorder-nbc-nph Start of the new bc

inp offset = old iOrder- old nbc-old nph ! Strt of
old + skip old bc
do i = 1, nibc ! do the state bcs, init, drop time
 BEST AVAILABLE COPY
5,435,503
47 48
sol (out offset + i) = Sol (inp offset + i)
endido
inp offset = inp offset + i-1 + ns ! skip the first
internal node bc's
out offset = out offset + i -1
do i = 1, nitbC do the state bc's, 5 is for the end
sol (out offset + i) = sol (inp offset + i)
endido
inp offset = inp offset + i - 1
out offset = out offset + i -1
sol (out offset + 1) = Sol (inp offset + 3)
! Ilove 2nd phs title to 1st
switch phase 21 F - false. the last time this is used
elseif (drop time eq) then
! shift the number of states 8 to 7.
do i = 1, Xne (1) +2
do j = 1, 2
do k = 1, InS
sol ( intot * (i-1) + (j - 1) * n + k ) =
sol ( (i-1) * old intot + (j-1) * old n + k)
endido
do k = 1, inp
sol ( intot * (i-1) + (j - 1) *n + ns + k ) =
sol ( (i-1) * old intot + (j - 1) * old n + old ins+k)
endido
endido
did j = 1, In, 4.
sol ( intot * (i-1) + n* 2 + j ) =
sol ( (i-1) * old intot + old n*2 + j )
endido
enddo

out offset icrder-nbc-nph Start of the new bc

inp offset old iOrder-clid nbc-old nph strt of
! old + skp old bc
do i = 1, nib C - 1 ! do the state bcs, init, drop time
sol (out offset + i) = Sol (inp offset + i)
endido
inp offset = inp offset + i-1 + 1 skip the first
internal node bc's
out offset = out offset + i - 1
do i = 1, Intbc ! do the State bcs, 5 is for the end
sol (out offset + i) = sol (inpoffset + i)
endco
inp offset inp_offset + i - 1
out offset out offset + i-1
sol (out offset + 1) = Sol (inp offset + 1) move 2nd phs
time to 1st

drop time ea = . false. the last time this is used

ulilit = ulimit r1 change the litit
elseif (drop cc eq) then drop the cd equations
 ES AVAILABLE COPY
5,435,503
48 - SO
do i = 1, netot-2nph
do i = 1, n.
sol (ntot * (i-1)+j) = sol (old intot* (i-1)+j)
sol (ntot (i-1)+n+j) = sol (old intot* (i-1)+old n+j)
endido
do ij = 1, IL.
sol (ntot* (i-1)+2*n-j) = soil (old intot* (i-1)+2* old n+j)
endido
endido
do i = 1, nbc
sol (iorder-nbc-t-il) =sol (old icroer - old nbc--i)
endido
drop coed = . false. the last time this is used
elseif (d2 phase 1) then
! We're doing a divide phase 1 by 2.
c. * * Dividing the number of eleInents in phase 1, in stage 3 by 2
d2 phase 1 = . false. last title this is used
if (old Xne1 . eq. 4) then
! shift from 4 to 2 elements in phase 1
do i = 1 , intot
sol (Intot + i ) = (sol (nitot + i)
S + sol (Intot*2 + i) )/2
sol (ntot 2 + i ) (sol (ntot * 3 + i)
S + sol (ntot * 4 + i) ) / 2
sol (ntot *3 + i ) = Sol (Intot * (old xne1+1) + i)
endido
else shift from 2 to 1 eleitent in phase 1
! stage iii. Only.
do i = 1 , intot
sol (Intot + i ) = (sol (ntot + i)
S + soli (intot *2 + i) ) / 2
sol (ntot 2 + i ) = sol (ntot * (old xne1+1) + i)
endido
endilf
do i = 1 , nbc + nph shift the bc & time down
soil (intot (netot-i-2) + i ) = sol (ntot (old netot+2) + i)
endido
if (np. ed. 2)
S call check cc2 (title Since launch, Sol, dummy logical)
new Controls
if (np. eq. 1)
S call check cc1 (time Since launch, Sol, duriny logical)
IleW controls
endilf
returI
end

Subroutine Init bc attitude ( attitude init

S, Vehicle Velocity, status)
Implicit None
Include ' - . include wifc all ré.inc
c inputs
Real 8 vehicle velocity (3) : inertial vehicle velocity
 5,435,503
51 52
ft/s
C OutputS
Real'8 attitude init (3) either Rodriquez parameters
C cr FP unit vector
Integer Status
C Locals
Integer i
Double Precision Maximum value, Trans Inertial VEL (3, 3)
S, Inert level Inag, Trans VEL Inertial (3, 3)
Double Precision C11, C12, C13, C21, C22, c23, c31, c32, c33, denon
S,
angle rad (3), ca1, Ca2, Ca3, Sal, Sa2, Sa3, duril2, durin3
logical use euler/false. /, use two init_bc/. false. /
Inert level mag= sqrt (vehicle velocity (1) * * 2
S + vehiclevelocity (2) * * 2 )
Angle rad (1) [Link]
Angle rad (2)
Angle rad (3)
datan (-vehicle velocity (3)/Inert level mag)
dacos ( vehicle Velocity (1) / Inert level mag)
ca3 = cos(angle rad (3) )
sa3 = sin (angle rad (3) )
ca2 = cos(angle rad (2) )
sa2 = sin (angle rad (2) )
ca1 = cos(angle rad(1))
sa1 = sin (angle rad(1))
c13 = ca3 * sa2 * ca1 + Sai * sa1
C31 = - Sa2
c23 = sai * sa2 * ca.1 - ca3 * sa1
c32 = ca2 * sa1
C12 = ca3 * sa2 * Sa1 - Sal * ca.1
c21 = ca2 * sa3
C11 = Ca2 * cas
C22 = Sas * Sa2 * Sa1 + Ca3 * ca.1
C33 = Ca2 * ca.1 -
denon = (1+c11+ c22+c33)
attitude init (1) = 2 * (c.32-c23) /denom
attitude init (2) 2 * (c13-c31) / denon
attitude init (3) 2 * (C21- c.12) /denon
Return
end

Subroutine Initialize IO (Sol)

Inplicit None
Include ' (- - includewfcall ré.inc
Include ' - . . includelwfc thrust ré.inc
Include ' - . . include) wifc bou ré. inc'
C Cutputs
real 8 sol (1)
Integer Status , i.
c local variables
 5,435,503
53 54
real * 8. 28/0. OdO/
integer ierr
natelist/bc and constants/ aexit, grav, Sa, thrust1, thrust2, a1
S, a2, ulimit, x01, x02, X03, X04, XO5, X06, X07, X08, Xf1, Xf2, Xf3
S., f c11, f_c12, f_c13, In, nbc., np, nph, ns, Xtille, Xne
S, ccerr, cceXp, CInerr, ulimit r1, range O2, thrust title out
namelist/controls/file name in, file name out
IClass fillie in, thrust file in

: SaV Illatrix, Sav Illatrix time

Conver, use Spline thrust
newton Scale factor, Max nurt of iterations
execute frequency
-

print derivatives
save first solution
cap cc, final conver Crit
print derivatives Start time, pn Kn, pnks
print derivatives end tille,
final convertine
time go 1st divide, title go 2nd divide
set drop title, drop time
write initial derivatives
f Set drop cc, drop cc, CCV8, ccV9
S
cdy CInd Sf, use spline thrust, use spline mass
Data Conver/5* 0.0001/ avg. eqn. Convergence Criteria
Sf ulimit r1/0. 09/ new uliinit set at 'drop tine'
SW newton scale factor/835* 1. Od0/
init. newton scale factor
Sr title go 1st divide /10. Odo/
S time go 2nd divide /5. 0d 0/, nptr/1/
S Inax nurn of iterations/5*4/
Illax number of iterations
Sf pn kn/0. 1557/, pn ks/1.8/ between the error vector
Calculation before the
iteration file saves the
! difference in the error vector
calculation of the Itax error.

This routine tries to Open the necessary files to costruct

the initialization necessary for WHOA.

Open (unit=13, file="whoa flight ré. In Inl", status = 'old'

S , forts forIQatted' , err=9004)
read (13, nml=controls, err=9007)
read (13, nrnl=bc and constants, err=9008)
close (13)

do i = 1, nph read in the initial phase times.

Xtitle Start (i) = xtime (i)
endido
c DEFINE THE initial SIZE OF THE SYSTEM
 5,435,503
54 56
N = NS+NP
NTOT = 2 * N+M
NETOT = O
DO 5 I=1, NPH
NETOT = NETOT + xNE (I)
5 CONTINUE

IORDER = NTOT * (NETOT + 2*NPH) + NBC + NPH

epen (unit=10, file=file name in, status = 'old', form= formatted

S, err=9001)
print *, ' order = ' , iorder
do i = 1, iorder
read (10, *, err=9006) sol (i)
endido
close (10)
c define range o2 , the initial range to target squared.
c this assumes the first three States are x, y, and Z. inertially,
i = iorder-nbc-nph-ntot
range oz = sol (i+1) * * 2 + sol (i+2) * * 2 + sol (i+3) * * 2
CALL BCUAERO create aero tables
C READ IN THE MASS TIME HISTORY

OPEN (15, FILE=mass file in, STATUS= 'old', err=9009)

READ (15, *) mass in
DO 11 i = 1, Ilass in
1. READ (15, *, err=9010) Itass time thl (i), mass thl (i)
close (15)
terminal Inass Itass thl (mass n)
c initialize the Inass Spline data
call chSpl (mass time thl, Inass tol, Inassa, Inass b, mass c
S, ITass. In , 0, 0, 28, 28, ierr)

c READ IN THE thrust TIME HISTORY

OPEN (15, FILE=thrust file in, STATUS= 'old', err=9011)
gth s
READ (15, *) thr n
DO 1.2 L = 1, thrn
2 READ (15, *, err=9012) thr time thil (i), thrthl (i)
close (15)
c initialize the thrust Spline data
call chSpl (thr time thil, thr thi, thra, thr_b, thr_c
S, thr_n, 0, 0, z8, 28, ierr)
goto. 9000
9001 write (*, *) Error opening input file name '
goto 9000
9004 write (*, *) error opening whoa init [Link]
goto 9 000
 5,435,503
57 58
9 OO6 write (*, *) error reading input initialization paratheters
goto 9000
9 007 write (*, *) error reading controls namelist
goto 9000
9 OO8 write (*, *) 'error reading bc, and initial state namelist
goto 9000
9 OO9 write (*, *) error opening file: ' , Ilass file in
goto 9000
9010 write (*, *) error reading fille: ' , mass file in
goto 9000 -
9011 write (*, *) error opening file: ' , thrust file in
goto 9000
9012 write (*, *) error reading file: ' , thrust file in
goto 9000
9 OOO continue
relir
end

C File Name: ATMOS62 METRIC

C Last Update: 10 April 1992
C
SUBROUTINE ATMOS62 METRIC ( ALT , RHO , PRESS , T , VSOUND )
C
Cze r * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * x x x x x x x * : x x x * x x x . . . . . . . . . . . . . .
C CALCULATES DIMENSIONAL ATMOSPHERIC QUANTITIES.
C

C INPUTS -

C ALT = ALTITUDE, METERS

C
e

C OUTPUTS -

C RHO = DENSITY, KG / METER CUBED

C PRESS = PRESSURE, PASCALS
C TK = TEMPERATURE, DEGREES KELVIN
m

C VSOUND = SPEED OF SOUND METERS / SECOND

is tie

Yale k .

C
r x x * x k is x * x x . x sex k k k k l k k k x x x * * * * x x * x x x x x x k k k k k + k is k . . . . . . . . . . . . . . . . .
se se

C
IMPLICIT REAL * 8 (A-H, O-Z)
DIMENSION H (8), TEMP (8), DELT (8), DTDH (8)
THIS IS THE 1962 U. S. STANDARD ATMOSPHERE DEFINITION
H, HKM ARE IN GEOPOTENTIAL KELOMETERS. ALT SHOULD BE
GEOPOTENTIAL METERS.
TEMP IS IN DEGREES. K.
C=G*MO/R G=9.80665 M/SEC** 2, MO=28.9644 R=8.31432 (METRIC)
DTDH IS DEGREES K/KM
 5,435,503
59 60
DATA C /34 . 1632/
DATA H /O. , 11. , 20. , 32. , 47. , 52. , 61. , 79. W
DATA DELT
1. , .223361,.0540328, .00856663, .001094.55, .000582289,
OOO 179718, . OOOO 10241/
DATA DTDH /- 6.5, O. , 1 . , 2.8, 0 . , -2. , -4. , 0./
DATA R /287. 04/
DATA SLPRES /101325.0/ - --
DATA TEMP /288. 15, 216. 65,216. 65, 228. 65,270. 65,270. 65,
252. 65, 180 - 65/
DATA VSCON /20. 04645/
HKM = ALT * 0.001
DO 10 J-2, 8
IF (HKM.L.E. H. (J) ) GO TO 20
= -- .
10 CONTINUE
20 DH=HKM-H (I)

IF (DTDH (I). EQ. O. O.) THEN

TK = TEMP (I)
DELTEMP = -C * DH / TEMP (I)
IF (DELTEMP.L.T. - 675. O) DEL = O. O
IF (DELTEMP. GE.-675. O) DEL =DELT (I) *EXP (DELTEMP)
ELSE s
TK = TEMP (I) + DTDH (I) * DH
DEL = DELT (I) * (TEMP (I) /TK) * * (C/DTDH (I))
END IF

DELTA=DEL
PRESS = SIPRES * DELTA
VSOUND = WSCON * SQRT (TK)
RHO = PRESS / (R* TK)
RETURN
END
C
SUBROUTINE BCUAERO .
IMPLICIT DOUBLE PRECISION (A-H, O-Z)
C ROUTINE TO CALCULATE GRIDPOINT DERVATIVES USING FORWARD &
C BACKWARD PERTURBATONS OR ONE-DIRECTIONAL PERTURABTION IF
C GRIDPOINT IS ON THE EDGE.
C SHOULD BE FIRST EXECUTED IN THE MAIN PROGRAM BEFORE ANY
C CALCULATION IS DONE.
Include ' -- includelwfc bcu ré.inc
OPEN (1, FILE= aero. ini', STATUS= 'OLD ' )
DO 5 = 1, 1
READ (1, *) J1, K1
READ (1, *) (X1A (I,J), J =1, J1) MACH NO.
READ (1, *) (X2A (I, K), K=1, K1) ALPHA
DO 3 J = 1, J1.
DO 3 L = 1, 12 originally cl and cd
3 READ. ( 1, *) (YA (I, L, J, K), K=1, K1)
C YA=YA (STAGE, COEFF, MACH, ALPA)
N MACH (I) =J1 ! Illach
N ALPHA (I) =K1 ! alpha
5 CONTINUE
CLOSE (UNIT=1, STATUS= "KEEP')
 3EST AVAILABLE COPY
5,435,503
67 68
SO CONTINUE
RETURN
END

SUBROUTINE CBSPL (X, Y, A, B, C, N, IBEG, IEND, ALPHA,

BETA, IERR)
C- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
C CUBIC SPLINE INTERPOLATION
C
IMPLICT DOUBLE PRECISION (A-h, O-Z)
REAL * 8 X (N), Y (N), A(N), B (N), C (N)
C
IF (N - LT. 3) GO TO 200
C A TRIDIAGONAL LINEAR SYSTEM FOR THE UNKNOWN SLOPES S (I) OF
C F AT X (I), I=1, . . . , N, IS GENERATED AND THEN SOLVED BY GAUSS
C ELIMINATION, WITH S (I) ENDING UP IN A (I) FOR ALL I. A, B, C
C ARE USED INITIALLY FOR WORK SPACES.
C
DO 1.0 M = 2 N
B (M) = X (M) - X (M-1)
IF (B (M) . L.E. O. O) GO TO 210
C (M) = (Y (M) - Y (M-1))/B (M)
10 CONTINUE
IERR = 0
C
C CONSTRUCT THE FIRST EQUATION FROM THE BOUNDARY CONDITION,
OF
C THE FORM
C
C C (1) *S (1) + B (1) *S (2) = A (1)
C
IF (IBEG - 1) 20, 30, 40
C
C NO CONDITION AT LEFT END.
C
20 C (1) = B (3)
B (1) = X(3) - X (1)
A (1) = ((B (2) + 2. O* B (1)) * B (3) * C (2) + B (2) * B (2) * C (3))/B (1)
GO TO 5 O
C
C SLOPE PRESCRIBED AT LEFT END.
C
30 C (1) = 1.0
B (1) = 0. 0 s
A (1) = ALPHA
GO TO 50
C
C SECOND DERIVATIVE PRESCRIBEDAT LEFT END.
C
40 C (1) 2.. O
B (1) 1... O w
A (1) 3. O* C (2) - 0. 5* ALPHA* B (2)
FOR THE INTERIOR. KNOTS, GENERATE THE CORRESPONDING
EQUATIONS AND CARRY OUT THE FORWARD PASS OF GAUSS
ELIMINATION, AFTER WHICH THE M-TH EQUATION READS
C (M) *S (M) + B (M) *S (M+1) = A (M).
 BEST AWALABLE COPY
5,435,503
68 70
5 O NM1 = N - 1
DO 51 M = 2, NM1
T = -B (M-1) / C (M-1)
A(M) = Tik A (M-1) + 3.0 * (B (M) * C (M+1) + B (M+1)*C (M))
C (M) = TK B (M-1) + 2. O* (B (M) + B (M-1)) .
51 CONTINUE

IF THE SLOPE AT THE RIGHT END IS GIVEN THEN SET A (N) TO

THE SLOPE AND GO TO BACK SUBSTITUTION. OTHERWISE, CONSTRUCT
THE LAST EQUATION FROM THE SECOND BOUNDARY CONDITION, OF
THE FORM R*S (N-1) + C (N) *S (N) = A(N)
IF (IEND - 1) 60, 8O, 90
60 IF (N - EQ. 3 ...AND. IBEG - EQ. 0) GO TO 70
NO CONDITION AT THE RIGHT END. EITHER N . . GE. 4 OR
THERE IS A CONDITION AT THE LEFT END.

R = X(N) - X (N-2)
DEL = (Y (NM1) - Y (N-2))/B(NM1)
A (N) = ( (B (N) + 2. O*R) * B (NM1) * C (N) + B (N) * B (N)* DEL)/R
C (N) = B (NM1)
GO TO OO

NO CONDITIONS AT THE END POINTS AND N = 3. IN THIS CASE,

THE SECOND BOUNDARY CONDITION DOES NOT PROVIDE US WITH A
NEW EQUATION. FOR CONVENIENCE, WE USE THE FOLLOWING...
70 A(N) 2. O*C (N)
C (N) 1... O
R = 1 -O
GO TO 100
SLOPE PRESCRIBED AT RIGHT END.
8O A (N) = BETA
GO TO 110

SECOND DERIVATIVE PRESCRIBED AT RIGHT END.

9 O A (N) 3. O* C (N) + O. 5* BETA * B (N)
C (N) 2. O
R = 1 .. O

COMPLETE FORWARD PASS OF GAUSS ELIMINATION.

100 T = -R/C (NM1)
A (N) = (T*A (NM1) + A(N)) / (T*B (NM1) + C (N) )
CARRY OUT BACK SUBSTITUTION.
1 O DO 1.20 E = 1, NM1
J = N - I
A (J) = (A (J) - B (J) * A (J-1)) / C (J)
120 CONTINUE

GENERATE THE CUBIC COEFFICIENTS B (I) AND C (I).

DO 130 I = 1, NM1
H = B (I+1)
 5,435,503
71 72
DEL = (Y (I+1) - Y (I))/H
T = A (I) + A (I+1) - 2. O*DEL
B (I) = (DEL - A (I) - T)/H
C (I) = (T/H)/H
13 O CONTINUE
RETURN
C
C ERROR RETURN
C
200 ERR = 1
RETURN
20 ERR = 2
RETURN
END
C
SUBROUTINE SEVAL (X, Y, A, B, C, N, X, YI, YPI, YPPI)
C
C ABSTRACT
C
C SEVAL EVALUATES A CUBC SPLINE AND ITS FIRST AND SECOND
C DERVATIVES AT THE ABSCSSAS IN XI. T S ASSUMED THAT
C THE COEFFICIENTS OF THE POLYNOMIALS WHICH FORM THE
SPLINE
C ARE PROVIDED.
C
C DESCRIPTION OF ARGUMENTS
C
C --INPUT--
C
C x - ARRAY OF THE FIRST N ABSCISSAS (IN INCREASING
ORDER)
THAT DEFINE THE SPLINE.
Y - ARRAY OF THE FIRST N ORDINATES THAT DEFINE THE
PT NE
A, B, C ARRAYS THAT CONTAIN THE COEFFICIENTS OF THE
OLYNOMEALS --

WHICH FORM THE SPLINE. IF I = 1,...,N THEN THE

PLINE
HAS THE WALUE -
Y(I) + A (I) *Dx + B (I) * DX** 2 + c (I) *DX* *3
FOR X (I) . LE. XX . LE. X (I+1). HERE DX = XX-X (I).
N - THE NUMBER OF POLYNOMIALS THAT DEFINE THE SPLINE.
THE ARRAYS X, Y, A, B, C MUST BE DIMENSIONED AT
LEAST N. N MUST BE GREATER THAN OR EQUAL TO 1.
XI - THE ABSCISSA OR ARRAY OF ABSCISSAS (IN ARBITRARY
ORDER) AT WHICH THE SPLINE IS TO BE EVALUATED
--OUTPUT--

YI - ARRAY OF VALUES OF THE SPLINE (ORDINATES) AT XI.

YPI - ARRAY OF VALUES OF THE FIRST DERIVATIVE OF SPLINE
AT X.
YPPI- ARRAY OF VALUES OF SECOND DERIVATIVES OF SPLINE
AT X.

IMPLICIT DOUBLE PRECISION (A-h, O-Z)

REAL * 8 X(N), Y (N), A(N), B (N), C (N), XI, YI, YPI, YPPI
C
XX = XI
IF (XX - LT. X (1)) GO TO 90
IF (XX . . GE. X (N)) GO TO 80
 5,435,503
73 74
IL = 1
IR = N
C
C BSECTION SEARCH
C
10 I = (IL + IR)/2
IF (I . EQ. IL) GO TO 100
IF (XX - X (I)) 20, 100, 30
20 IR =
GO TO 10
3O L =
GO TO O

LINEAR FORWARD SEARCH

50 IF (XX . L.T. X (I-1)) GO TO 10C

= I + 1
GO TO 50
C
C XX IS GREATER THAN X (N) OR LESS THAN X (1)
C
8O = N
GO TO 100
9O L = 1
C
C EVAUATION
C .
100 DX = XX - X (I)
YI = Y(I) + DX* (A (I) + DX* (B (I) + DX* C (I)))
BI = B (I) + B (I)
CI = 3. O* C (I) .
YPI = A (I) + DX* (BI + DX*CI)
YPPI = BI + DX* (CI + CI)
return
END

C k k k k is k ar ki ke k k k l k k k k k k k k k k k k k k ki ke k is k are k ke k see le ree r is k k is rare k . . . . . .

Subroutine whoa optimize (factor Inatrix, soil

S, State, control
S, converged )
IMPLICIT DOUBLE PRECISION (A-H, O-Z)
Include ' -- includewfc prob order ré.inc
Include ' -- includelwfc. Inass rö...inc
Include ' -- includewfo bou ré.inc
Include ' - . . include wifc sol ré.inc
Include ' -- includewfc controls ré.inc
c inputs
Real * 8 Sol (*) initialization parameters
Logical factor Inatrix should the matrix be inverted ?
C Outputs
real'8 state (Inx X) nodal values
(state/cc. silk, init/final, phase)
real 8 control (I(Xu) Inidpoint values
(state/cc. silk, element, phase)
logical converged did we neet the average
 5,435,503
75 76
Convergence criteria ?

c local variable definitions

C NZ. . . . . . NUMBER OF NONZERO ENTRIES IN JACOBAN

logical stop nri/. false. /

! Stop newton raphson, the error is too big.
C DEFINE THE NUMBER OF NONZEROES IN THE JACOBAN FOR USE BY
ARWEL

if (stop_nri) return
if (factor matrix) then

S + NP* (NS+M+2) + 2*NP)

IJIN2 = NS* (XNE (1) + 1) + 2*NS* (NETOT-xNE (1) +NPH-1)
IJEN = 2*NS + Inns
IJBC = 2*NS* (1+NBC) + M* (NS+N+M) +
S NP* (NS+M+2) + 2*NP
JPS = 2 *NS*NPH*NBC + NPHk. NBC
IJTI = (NPH-1) * (2* (2*NS+M) +NBC) + 2*NS+M+NBC
NZ1 = (IJST+IJEN+IJBC) *NPH + IJIN1 + IJPS+IJTI

endilf

CALL NEWTON (NZ1, NZ2, NZ, factor thatrix

S, SOL, nurt iterations, converged, stop nri)
do i = 1, Ins
state (i) = Sol (i)
endido
do i = 1, In
control (i) = sol (ntot-In--i)
endido
retlin
END

SUBROUTINE BCTERM (KSW, IRTOT, IJTOT, PN, PHIX, PHIXX,

& PSIX, PSIXX, A, B, IRN, CN)
IMPLICIT DOUBLE PRECISION (A-H, O-Z)
Include ' [-..include) wifc_prob_order ré.inc."
Include ' -- includel wifc tass ré.inc
Include ' -- includewfc. Sol ré.inc
INTEGER PN, IRN (*), ICN (*), tEN
DOUBLE PRECISION PHIX (mx bc), PHIXX (mx bc, mix bo),
& PSIX (IX bc, Illk nu), PSIXX (Inx bo., Inx bo, Inx nu)
DOUBLE PRECISION X (Inx X), U (IX u), F (inx eqn, 0:Inx ux, O:Inx ux)
S, L (0 : Inx ux, 0:Ink ux)
DOUBLE PRECISION A (*), B (*)
DOUBLE PRECISION CAPHIX (mx bc), CAPHXX (Inx bo, mx bc)
 5,435,503
77 78
IF (KSW .EQ. 0) then
CALL INDEX4 (IRTOT, IJTOT, PN, IRN, ICN)
else
Do 17 I=1, NS
X (I) =XHAT (I, 2, PN)
17 CONTINUE
DO 18 = 1, M
U (I) =UHAT (I, 2, PN)
18 CONTINUE
ten = 0 .
do i = 1, pn
ten = ten + xne (i) +2
endido
CALL GET F. G EQN DER (PN, ten, X, U, F, L)
endilf

IF (KSW .EQ. 1) THEN

CCCCCCCCCCCCC BEGIN ERROR VECTOR FILL IN CCCCCCCCCCCCCCC
I=RTOT

C DEFINE CAPHIX = D (PHI) /DX + NUT*D (PSI) /DX

C WHERE 'X' REPRESENTS ALL OF THE STATES AT ALL OF THE PHASE
C BOUNDARIES

DO 5 J= 1, 2*NS*NPH
SUM= 0 . OD+OO
Do 10 I=1, NBC
SUM=SUM+PSIX (J, ) * GNU (I)
10 CONTINUE
CAPHIX (J) =PHIX (J) +SUM
5 CONTINUE

COSTATE BOUNDARY CONDITIONS AT START AND END OF PHASE

J=2*NS* (PN-1)
DO 15 = 1, NS --
B (IL--I) = -CAPHIX (J--I) -LHAT (I, 1, PN)
B (IL--NS+I) = -CAPHIX (J.--NS+I)+LHAT (I, 2, PN)
15 CONTINUE

IL=IL-2 *NS

C OPTIMALITY CONDITION (DH/DU == 0) AT END OF PHASE

DO 25 =1, M
SUM = 0. OD+OO
DO 20 K=1, N
IF (K - L.E. NS) THEN
SUM = SUM + F (K, NS+I, 0) *LHAT (K, 2, PN)
ESE
SUM = SUM + vocu (F (K, NS+I, 0), XHAT (K, 2, PN), LHAT (K, 2, PN) )
ENDF m
2O CONTINUE
B (IL--I) = -L (NS+1, 0) - SUM
25 CONTINUE
=L--M.
 5,435,503
79 80
C CONTROL CONSTRAINT (G + K2 == 0) AT END OF PHASE
DO 35 I=1, NP
B (IL--I) = -vccInu (F (NS+I, 0, 0), XHAT (ins+i, 2, PN),
LHAT (ins+i, 2, PN) )
35 CONTINUE
IL=--NP

C MULTIPLIER CONDITION (MU K == 0) AT END OF PHASE

DO 45 I=1, NP
B (IL-I) = -vcck (F (NS+I, 0, 0), XHAT (Ins+i, 2, PN),
m LHAT (ins+i, 2, PN) )
45 CONTINUE
IL = IL + NP
IRTOT = IL

CCCCCCCCCCCCCC END ERROR VECTOR FILL IN CCCCCCCCCCCCCCC

ENDIF

IF (KSW .EQ. 2) THEN .

CCCCCCCCCCCCCC BEGIN JACOBLAN FILL IN CCCCCCCCCCCCCCC

C DEFINE CAPHXX=D^2 (PHI) /DX^2 + NU*T* D^2 (PSI)/DX^2

DO 37 K=1,2 *NS*NPH
DO 38 J = 1, 2*NS*NPH
SUM= 0 . OD+00
DO 40 I=1, NBC
SUM=SUM+PSIXX (K, J, I) * GNU (I)
40 CONTINUE
CAPHXX (K, J) =PHIXX (K, J) +SUM
38 CONTINUE
37 CONTINUE

C TAKE DERIVATIVE OF COSTATE BOUNDARY CONDITIONS

c W. R. T. STATES :

IPTST= 2 *NS* (PN-1)

INJ-'O
C W.R.T. COSTATES:
DO 52 I=1, NS.
A (IJ--I) = 1. OD+00
A (IJ--I+NS) = -1. OD+00
52 CONTINUE
IJ-IJ-2*NS
C W. R. T. DISCRETE MULTIPLERS NU:
 5,435,503
83 84
S LHAT (k, 2, PN) )
ENDIF
100 CONTINUE
A (IJ--INJ) = L (NS+I, NS+J) + SUM
1.05 CONTINUE
O CONTINUE
IJ = IJ-MSQ
C DERIVATIVES OF CONTROL CONSTRAINTS
C W. R.T. STATES X:

INJ = 0
DO 1.25 = 1, NP
DO 1.20 J-1, NS
INJ = INJ-1
a (ijt-inj) =puVCCIllu (f(ns+i, j, 0),xhat (ins+i, 2, pn)
S, lhat (nS+i, 2, pn))
120 CONTINUE
125 CONTINUE
J = IJ--NPNS

C W. R.T. SLACK VARIABLES (K) :

DO 135 = 1, NP
A (IJ--) = w
pkvccInu (F (NS+I, 0, 0), XHAT (ins+i, 2, PN), LHAT (ins+i, 2, PN) )
135 CONTINUE
J = J--NP

C W. R.T. multipliers (Itill) :

DO 136 I= 1, NP
A (IJ--) =
p:RuvccInu (F (NS+I, 0, 0), XHAT (ins+i, 2, PN), LHAT (ins+i, 2, PN))
36 CONTINUE .
IJ = J--NP

C W. R.T. CONTROLS (U) :

INJ = 0
DO 15 O I-1, NP
DO 40 J-1, M
INJ = INJ + 1

A140
(IJ-FINJ)=puvccmu. (F (NS+I, ins+j, 0), XHAT (ins+i, 2, PN), LHAT (ins+i, 2, PN))
CONTINUE
150 CONTINUE
IJ = IJ+NPM

C DERIVATIVES OF MULTIPLIER CONDITIONS

C W. R. T. MULTIPLIERS AND SLACK WARIABLES
DO 250 = 1, NP
A (IJ-i-I) =
pkvcck (F (NS+I, 0, 0), XHAT (ins+i, 2, PN), LHAT (Ins+i, 2, PN) )
A (IJ--NP+I)=pmuvcck (F (NS+I, 0, 0), XHAT (ins+i, 2, PN), LHAT (ins+i, 2, PN))
250 CONTINUE
IJ = J-2*NP
IJTOT = IJ
 5,435,503
85 86
CCCCCCCCCCCCCCCCCCCCCC END JACOBAN FILL N CCCCCCCCCCCCCCC
ENDIF

RETURN
END
SUBROUTINE INTER (KSW, IRTOT, IJTOT, IJTIM1, IJTIM2, PN EN
S DELT, A, B, IRN, ICN)
IMPLICIT DOUBLE PRECISION (A-H, O-Z)
Include ' - . include wifc proborder rö.inc
Include ' -- includelwfc soil ré.inc
INTEGER PN, EN, ten w
DOUBLE PRECISION X (IX X), U (IX u), F (Inx eqn, 0:Inx ux, 0:Inx ux)
S, L (0:Inx ux, O:Inx ux)
DIMENSION A (*), B (*), IRN (*), ICN (*)

C THIS routine LOADS THE PART OF THE EQUATIONS THAT DEPENDS ON

"suantities AT THE MIDPOINTS OF THE (EN)TH ELEMENT OF THE (PN) TH
its, INCLUDES TWO SETS OF STATE EQUATIONS, COSTATE EQUATIONS,
c OPTIMALITY CONDITIONS, AND CONTROL CONSTRAINTS.
& ALL JACOBAN ELEMENTS ARE IN THE COLUMN CORRESPONDING TO THE
BARRED
C QUANTITIES IN ELEMENT NUMBER (EN)
C KSW = 1 ==X FILL IN IND CES OF JACOBAN ELEMENTS
C KSW = 2 ==> FILL IN PARTS OF EQUATIONS
C KSW = 3 ==X FILL IN COMPLETE JACOBAN ELEMENTS

IF (KSW .EQ. O) THEN

CALL INDEX2 (IRTOT, IJTOT, IJTIM1, IJTIM2, PN, IRN, ICN)
ESE
DO 7 I = 1, NS
X (I) = XBAR (I, EN, PN)
7 CONTINUE
DO 8 = 1, M.
U (I) = UBAR (I, EN, PN)
8 CONTINUE
ten = en +1
do i = 1, pn-1
ten = ten + xne (i) + 2
endido
CALL GET F. G EQN DER (PN, ten, X, U, F, L)
ENDIF

IF (KSW .E.O. 1) THEN

CCCCCCCC BEGIN ERROR VECTOR FILL IN CCCCCCCCCCCCCCCCCCCCCC
L=IRTOT - NTOT
K=IRTOT

C=DELT/2. OD+00
C COMPUTE RIGHT HAND SIDE:
 5,435,503
87 88
C TWO SETS OF STATE EQUATIONS
DO 10 I=1, NS
B (IL--I) = B (IL--I) - XBAR(I, EN, PN) + C*F (I, O, O.)
B (IK+I) = B (IK+I) + XBAR (I, EN, PN) + C*F (I, 0, 0) .
10 CONTINUE
L=I--NS
K=TK+NS

C TWO SETS OF COSTATE EQUATIONS

DO 25 = 1, NS.
SUM = 0 . OD+00
DO 20 K=1, N.
IF (K.L.E. NS) THEN
SUM = SUM + F (K, I, O) *LBAR (K, EN, PN)
ELSE
surn = sum + VCCX (F (K, I, 0), Xbar (K, en, PN),
S Lbar (K eIn, PN) )
ENDIF
2O CONTINUE

B (ILI) B (IL-i-I) + LBAR (I, EN, PN) + C* (L (I, 0) +SUM)

B (IK+I) B (IK+I) - LBAR (I, EN, PN) + C* (L (I, 0) +SUM)
25 CONTINUE
IK-K--NS

C OPTIMALITY CONDITION FOR (EN) TH ELEMENT

DO 45 =1, M
SUM = 0. OD+00
DO 40 K=1, N
IF (K .L.E. NS) THEN
SUM = SUM + F (K, NS+I, O) *LBAR (K, EN, PN)
ELSE
SUM=SUM+ vocu (F (K, NS+I, O.), Xbar (K, en, PN), Lbar (K, en, PN) )
ENDIF
40 CONTINUE
B (IK+I) = -1. O* (L (NS+I, 0) + SUM)
45 CONTINUE
K=K--M

C CONTROL CONSTRAINT FOR (EN)TH ELEMENT

DO 55 = 1, NP

B (IK+I)=-vccmu. (F (NS+I, 0, 0), Xbar (ins+i, en, PN), Lbar (ins+i, en, PN))
55 CONTINUE
K=K--NP

C CONTROL CONSTRAINT MULTIPLIER CONDITION FOR (EN)TH ELEMENT

DO 65 I=1, NP
B (IK+I) = -vcck (F (NS+I, 0, 0), Xbar (ins+i, en, PN),
S Lbar (ins+i, en, PN) )
65 CONTINUE
 5,435,503
89 90
IK = IK+NP
RTO =IRTOT+NTOT

ccCCCCCCC END ERROR VECTOR FILL IN CCCCCCCCCCCCCCCCCCCCCC

ENDIF

IF (KSW . EQ. 2) THEN

CCCCCCCC FILL JACOBIAN CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC
J=JTOT

C=DELT/2. OD+00
CN=2. OD+00 * xNE (PN)
TAKE DERIVATIVES OF STATE EQUATIONS FOR THIS ELEMENT (IJR)
AND THE PREVIOUS (IJ)
(OR, FOR EN = 1, THE EQUATIONS AT THE START OF THE PHASE)
IR = TOTAL NUMBER OF JACOBIAN ELEMENTS IN BLOCK FOR PREVIOUS
ELEMENT

DERIVATIVE W. R.T. STATES (XBAR) IN EN 'TH ELEMENT

INJ = 0
DO 75 I= 1, NS
DO 70 J-1, NS
NJ = INJ -- 1
IF (I . EQ. J.) THEN
A (IJ--INJ) = 1. OD+00 - C*F (I, J, O)
A (IJR-NJ) -1. OD+00 - C*F (I, J, O)
ETSE
A (IJ--INJ) = -C*F (I, J, O)
A (IJR+INJ) = A (IJ--INJ)
ENDF
70 CONTINUE
75 CONTINUE

IJ = IJ--NSSQ
JR = IJ--IR

DERVATIVE W. R. T. CONTROL (UBAR) IN EN 'TH ELEMENT

NJ - O
DO 85 = 1, NS
DO 80 J=1, M .
INJ = INJ + 1
A (IJ--INJ) = -C*F (I, NS+J, O)
A (IJR+INJ) = A (IJ--INJ)
 5,435,503
95 96
ENDIF
78 CONTINUE
A (IJTIM2+I) =A (IJTIM2+I) - (L (I, O) +SUM) /CN
A (IJTIM2+NS+I) = -A (IJTIM2+I)

77 CONTINUE
JTIM2=IJTIM2+2 *NS
ENDIF

C TAKE DERVATIVE OF OPTIMALITY CONDITION JUST FOR THIS

c ELEMENT (IJR)
INJ = 0
JNJ = 0
DO 150 = 1, M
C W. R.T. STATES (XBAR) FOR EN'TH ELEMENT
DO 140 J=1, NS
JNJ - JNJ-1
SUM = 0. OD+00
DO 135 K-1, IN
IF (K . LE, NS) THEN
SUM = SUM + F (K, NS+I,J) *LBAR (K, EN, PN)
ELSE
Sun-SuttpuVccu (f(k,ns+i, j), xbar (k, en, pn),
S lbar (k, en, pn))
ENDIF :
135 CONTINUE
A (IJR+JNJ) L (NS+I,J) +SUM
14 O CONTINUE

C W. R. T. COSTATES (LBAR) IN THE EN 'TH ELEMENT

DO 145 J-1, N
NJ = INJ --
IF (J .L.E. NS) THEN
A (IJR+NSM--INJ) = F (J, NS+E, O)
ELSE
A (IJR+NSM+INJ) =pInuvccu (F (j, NS+I, O), Xbar (j, en PN),
S Lbar (j, en, PN) )
ENDIF . . .
145 CONTINUE
15 O CONTINUE
IJR = IJR-NV--NSM

C W.R.T. CONTROL (UBAR) IN THE EN'TH ELEMENT

INJ = 0
DO 1.80 = 1, M.
DO 1.75 J=1, M
INJ = INJ - 1
SUM = 0 . OD+OO
DO 17 O K=1, N.
IF (K.L.E. NS) THEN
SUM = SUM + F (K, NS+I, NS+J) *LBAR (K, EN, PN)
ELSE
 5,435,503
97 98
SUM=SUM+puvccu. (F (k, NS+I, NS+j), Xbar (k, en, PN),
S Lbar (k, en, PN) )
ENDIF
17 O CONTINUE
A (IJR+INJ) = L (NS+I, NS+J) + SUM
175 CONTINUE
18O CONTINUE
IJR = IJR--MSQ
C TAKE DERIVATIVE OF CONTROL CONSTRAINT EQUATION:
C W. R.T. STATES (XBAR) IN THE ENTH ELEMENT
NJ = 0
DO 200 = 1, NP
DO 19 O J = 1, NS
INJ = INJ-1
A (IJR+INJ) = F (NS+L, J, O)
19 O CONTINUE
2O O CONTINUE
IJR = IJR+NPNS
C W. R. T. SLACK VARIABLES (K) IN THE EN 'TH EiEMENT
DO 21 O = 1, NP
A (IJR+I) = pkvccrinu (F (NS+I, 0, 0), Xbar (ins+i, en, PN),
S Lbar (ins+i, en, PN) )
210 CONTINUE
JR = IJR--NP

C W. R. T. cc Inultipliers (mu) IN THE ENTH ELEMENT

DO 211 = 1, NP
A (IJR+I)=pInuvccInu (F (NS+I, 0, 0), Xbar (ins+i, en, PN),
4. Lbar (ins+i, en, PN) )
211 CONTINUE
JR = JR+NP

C W. R.T. CONTROLS (U) IN THE ENTH ELEMENT

NJ = 0
DO 225 I=1, NP
DO 220 JF1, M
INJ = INJ + 1 -
A (IJR+INJ) =puvccInu (F (NS+I, ins+j, 0), Xbar (ins+i, en, PN)
S, Lbar (ins+i, en, PN) )
220 CONTINUE
225 CONTINUE
JR - JR-NPM

C TAKE DERVATIVE OF MULTIPLIER CONDITION:

C W. R.T. MULTIPLIER (MU), THEN SLACK VARIABLE
C IN THE EN 'TH ELEMENT

DO 235 I=1 NP
A (IJR+I) =pmuVCck (F (NS+I, 0, 0), Xbar (ins+i en PN),
S Lbar (ns--i, en, PN) )
A (IJR+NP+I) = pkvcck (F (NS+I, O, C), Xbar (ins+i en, PN),
S Lbar (ins+i, en , PN) )
235 CONTINUE
 5,435,503
99 100
JR = JR+2+NP
JTOT = JR

CCCCCCCCCCC END JACOBAN FILL IN CCCCCCCCCCCCCCC

ENDIF

RETURN
END

SUBROUTINE START (KSW, IRTOT, IJTOT, PN, A, B, IRN, ICN)

IMPLICIT DOUBLE PRECISION (A-H, O-Z)
Include (-, . . includelwfc prob order ré.inc
Include ' -- includel wifc controls re...inc
Include ' -- includel wifc sol rô.inc
Include ' -- include) wifc Inass rö..inc
INTEGER PN, EN, ten
DOUBLE PRECISION X (IX x), U (Inku), F (Inx eqn, 0:Inxux, 0:Inx ux)
S, L (0 : IX uX, 0: Inx ux)
DIMENSION A (*), B (*), IRN (*), ICN (*)

THIS ROUTINE LOADS THE PART OF THE EQUATIONS FOR THE FIRST
ELEMENT IN A PHASE THAT DEPENDS ON THE QUANTITIES AT THE
NODES, I. E. HATTED QUANTITIES w

ALL, DERIVATIVES ARE WITH RESPECT TO THESE HATTED QUANTITIES

KSW = 0 ==X FILL IN INDTCES OF JACOBAN ELEMENTS
KSW 1 ==> FILL IN RIGHT HAND SIDES OF EQUATIONS
KSW 2 == X FILL IN ACTUAL. JACOBIAN ELEMENTS

IF (KSW .EQ. O) THEN

CALL INDEX1 (IRTOT, IJTOT, IRN, ICN)
ELSE
C USE WALUES AT THE BEGINNING OF PHASE IN ERROR WECTOR AND
C JACOBIAN

EN=1
DO 7 I = 1, NS
X (I) = XHAT (I, EN, PN)
7 CONTINUE
DO 8 E = i, M
U (I) = UHAT (I, EN, PN)
8 CONTINUE
ten = 0
do i = 1, pn-1
ten = ten +Xne (i) +2
endido
ten = ten -- e.
CALL GET F. G. EQN DER (PN, ten, X, U, F, L)
ENDIF

IF (KSW .EQ. 1) THEN

CCCCCCCCCC BEGIN ERROR VECTOR FILL IN CCCCCCCCCCCCCCC
IL=IRTOT
 5,435,503
101 102
C STATE AND COSTATE EQUATIONS
DO 10 = 1, NS
B (IL--I) = XHAT (I, 1, PN)
B (IL--NS+I) = -LHAT (I, 1, PN)
O CONTINUE -
L=IL--2*NS

C OPTIMALITY CONDITION (DH/DU == 0)

DO 25 = 1, M
SUM = 0 . OD+OO
DO 20 K=1, N
IF (K. L.E. NS) THEN
SUM = SUM + F (K, NS+I, O) *LHAT (K, 1, PN)
ELSE
SUM = SUM + voicu (F (K, NS+I, O), Xhat (K, 1, PN),
Lhat (K, i., PN) )
ENDF
2O CONTINUE
B (IL--I) = -L (NS+I, 0) - SUM
25 CONTINUE
=--M

C CONTROL CONSTRAINT (G + K^2 == 0)

DO 35 = 1, NP
B (IL--I) = -vccInu (F (NS+I, 0, 0), Xhat (ins+i, 1, PN)
35 CONTINUE
IL-L-NP

C MULTIPLIER CONDITION (MU K == 0)

DO 45 I=1, NP
B (IL-i-I) = -vcck (F (NS+I, 0, 0), Xhat (ins+i, 1, PN)
S Lnat (ins+i, 1, PN) )
45 CONTINUE
L = L - NP
IRTOT = IL

CCCCCCCCCCCCC END ERROR VECTOR FILL IN CCCCCCCCCCCCCCC

ENDIF

IF (KSW EQ. 2) THEN

cCCCCCCCCCCCC BEGIN JACOBIAN FILL IN CCCCCCCCCCCCCCC

NPNS = NP *NS
DERIVATIVE OF STATE AND COSTATE EQUATIONS W. R.T. X AND
AMBDA

DO 55 =1, NS
 5,435,503
103 104
A (IJ--I) = -1. OD+00
A (IJ--NS+I) = 1. OD+00
55 CONTINUE

C DERIVATIVE OF OPTIMALITY CONDITION, . . .

W. R. T. STATES (XHAT)
INJ = 0
DO 75 I=1, M
DO 65 J-1, NS
NJ = INJ-1
SUM = 0 . OD+00
DO 60 K=1, N
IF (K.L.E. NS) THEN
SUM = SUM + F (K, NS+I, J) *LHAT (K, 1, PN)
ELSE
SUM = SUM + puvccu (F (K, NS+I, J), Xhat (K, 1, PN),
Lhat (K, 1, PN) )
ENDIF
60 CONTINUE
A (IJ--INJ) = L (NS+I, J) + SUM
65 CONTINUE
75 CONTINUE
J = EJ - NSM

W. R. T. COSTATES (LAMBDA. HAT) AND MULTIPLIER (MU HAT)

INJ = 0
DO 90 = 1, M.
DO 80 J-1, N
INJ = INJ + 1
IF (J .L.E. NS) THEN
A (IJ--INJ) = F (J, NS+I, O)
is ELSE
A (IJ--INJ) =pinuVccu (F (j, NS+1, 0), Xhat (j, 1, PN),
Lhat (, 1, PN) )
ENDIF
80 CONTINUE
9O CONTINUE
IJ = IJ--NM

W. R.T. CONTROL (U HAT)

INJ =. 0
DO 110 I=1, M.
DO 105 J = 1, M
INJ = INJ + 1
SUM = 0 . OD+0.0
DO 100 K=1 N
IF (K . LE. NS) THEN
SUM = SUM + F (K, NS+I, NS+J) *LHAT (K, 1., PN)
ELSE
SUM = SUM+ puvccu (F (k, NS+I, NS+j), Xhat (k, 1, PN),
Lhat (k, 1, PN) )
ENDIF
100 CONTINUE
 5,435,503
105 106
A (IJ--INJ) = L (NS+I, NS+J) + SUM
105 CONTINUE
110 CONTINUE
IJ = IJ--MSQ
C DERVATIVES OF CONTROL CONSTRAINTS
C W. R.T. STATES X HAT
NJ = 0
DO 1.25 I= 1, NP
DO 1.20 J-1, NS
INJ = INJ--1
A (IJ--NJ) = puVCCInu (F (NS+I, J, 0), Xhat (ins+i, 1, PN)
120
S, CONTINUE
Lhat (Ins--i 1, PN) )
125 CONTINUE
J = IJ--NPNS

C W. R. T. SLACK WARIABLES (K HAT)

DO 135 = 1, NP
A (IJ-i-I) =pkVccinu (F (NS+, 0, 0), Xhat (ins+i, 1, PN),
S Lhat (ins+i, 1, PN) )
35 CONTINUE
J = IJ--NP

C W. R. T. Multiplier VARIABLES (MU HAT)

DO 137 = 1, NP
A (IJ-i-I) =pictuVCCIllu (F (NS+I, 0, 0), Xhat (ins+i, 1, PN),
S Lhat (Ins+i, 1, PN) )
37 CONTINUE
IJ = J--NP

C W. R. T. CONTROLS (U HAT)
N = 0
DO 150 = 1, NP
DO 14 O J = 1, M
INJ = INJ + 1 .
A (IJ--INJ) =puVccitlu (F (NS+I, Is+j, 0), Xhat (ins+i, 1, PN)
S, Linat (Ins--i, 1, PN) )
14 O CONTINUE
50 CONTINUE
J = IJ--NPM

C. DERVATIVES OF MULTIPLIER CONDITIONS

C W. R. T. MULTIPLIERS (MU HAT) AND SLACK VARIABLES (K HAT)
DO 250 I=1, NP
A (IJ-i-I) =pkvcck (F (NS+I, 0, 0), Xhat (ins+i, 1, PN),
S Lhat (ins+i, 1, PN) )
A (IJ--NP+1) =pinuvcck (F (NS+, 0, 0), Xhat (ins+i, 1, PN),
Lhat (ins+i, 1, PN) )
250 CONTINUE
IJ = J-2*NP
IJTOT = IJ
CCCCCCCCCCCCCCC END JACOBIAN FILL IN CCCCCCCCCCCCCCCCCCCCCCC
 5,435,503
107 108
ENDIF
RETURN
END
SUBROUTINE INDEX1 (IRTOT, IJTOT, IRN, CN)
IMPLICIT DOUBLE PRECISION (A-H, O-Z)
Include ' - . . includel wifc proborder ré.inc
INTEGER IRN (*), ICN (*)
C THIS ROUTINE MARKS THE PLACE IN THE JACOBAN OF THE WARIOUS
C PARTIAL DERIVATIVE TERMS OF THE EQUATIONS FOR THE FIRST ELEMENT
C IN A PHASE (HATTED QUANTITIES ONLY)

II, RTO
IJ JTOT

C DERIVATIVE OF STATE AND COSTATE EQUATIONS. W. R.T. X AND

C LAMBDA

DO 5 = 1, NS
IRN (IJ--I) IL--
ICN (IJ--I) L+
IRN (IJ--NS+I) L+NS+.
ICN (IJ--NS+I) IL--N--
5 CONTINUE
IJ = J + 2*NS

C DERIVATIVE OF OPTIMALITY CONDITION EQUATION

C W. R. T. STATES (X) :
INJ = 0
DO 35 = 1, M
DO 20 J-1, NS
INJ = INJ-1
IRN (IJ--INJ) = IL+2*NS+I
ICN (IJ--INJ) = IL-J
20 CONTINUE
35 CONTINUE
IJ-IJ--NSM

C W. R. T. COSTATES (LAMBDA) AND MULTIPLIER (MU):

NJ = 0
DO 3 O =1, M.
DO 25 J-1, N
NJ = INJ + 1
IRN (IJ--INJ) =IL-2*NS+I
ICN (IJ-INJ) = IL+N+J
25 CONTINUE
30 CONTINUE
IJ = J--NM

C W. R. T. CONTROL (U)
 5,435,503 r
111 112
=IJ-2*NP
IJTOT = J
IRTOT =- IL+2*N+M
RETURN
END

SUBROUTINE INDEX2 (IRTOT, IJTOT, IJTIM1, IJTIM2, PN, IRN, ICN)

IMPLICIT DOUBLE PRECISION (A-H, O-Z)
Include ' -- includewfc proborder ré.inc
INTEGER IRN (*), ICN (*), PN
THIS ROUTINE MARKS THE PLACE IN THE JACOBIAN OF
PARTIAL DERIVATIVES WITH RESPECT TO BARRED QUANTITIES.
THE EQUATIONS INCLUDE TWO SETS OF STATE EQUATIONS AND COSTATE
EQUATIONS, AND ONE SET OF OPTIMALITY CONDITIONS AND CONTROL
CONSTRAINTS PER CALL. THIS ROUTINE IS CALLED ONCE FOR EACH
ELEMENT OF EACH PHASE

THE POINTERS ERTOT AND JTOT MARK WHICH BLOCKS YOURE FILLING

C DERIVATIVES OF STATE EQUATIONS

IL = IRTOT
J = JTOT
IR = NSSQ+NSM+NS* (NS+N+M)
JR = IJ-R

C W. R.T. STATES (X) :

NJ = 0
DO 75 I= 1, NS
DO 70 J-1, NS
NJ = INJ +
IRN (IJ--NJ) = IL-NTOT-I
ICN (IJ--INJ) = IL-J
IRN (IJR+INJ) = IL-I
ICN (IJR+INJ) = IL-J
7O CONTINUE
75 CONTINUE
IJ = IJ--NSSO
JR = IJ-IR

C W. R.T. CONTROL (U)

N.J. s. O
DO 85 = 1, NS
DO 80 J-1, M .
NJ = INJ -- i.
IRN (IJ--INJ) = IL-NTOT+I
 5,435,503
119 120

220 CONTINUE
225 CONTINUE
JR = IJR-NPM

C DERIVATIVE OF MULTIPLIER CONDITION

C W. R.T. MULTIPLIER (MU), THEN SLACK VARIABLE (K)

235 CONTINUE

IJR = IJR-2*NP
IJTOT IJR
RTOT IRTOT-i-NTOT

RETURN
END

SUBROUTINE INDEX4 (IRTOT, IJTOT, PN, IRN, ICN)

THIS ROUTINE MARKS THE PLACE IN THE JACOBIAN FOR PARTIAL

DERIVATIVES WITH RESPECT TO THE HATTED QUANTITIES. THE
EQUATIONS OF INTEREST HERE ARE THE COSTATE BOUNDARY
CONDITIONS. ALSO INCLUDED ARE THE CONTROL CONSTRAINTS,
OPTIMALITY CONDITIONS AND THE MULTIPLIER CONDITIONS AT
THE END OF EACH PHASE.

IMPLICIT DOUBLE PRECISION (A-H, O-Z)

Include (- includelwfc prob order ré.inc
INTEGER IRN (*), ICN (*), PN

J=JTOT
L=IRTOT

NSM NS*M
NM = N* M.
MSQ MkM
NPM NP kM
NPNS = NP *NS

C DERIVATIVE OF COSTATE BOUNDARY CONDITIONS

C W. R. T. STATES

NJ = 0
DO 44, K=1, NPH
IF (K . EQ. 1) THEN
PTST=O
ELSE
IPTST=IPTEND-NTOT
ENDIF
IPTEND=IPTST+NTOT* (xNE (K) +1)
IF (K . EQ. PN) THEN
KEEPS=IPTST
KEEPE=IPTEND
 5,435,503
123 124
IRN (IJ--INJ) =IL+2*NS+I
ICN (IJ--INJ) =IL-2*N+J
45 CONTINUE
50 CONTINUE
IJ =IJ--MSO
C DERIVATIVES OF CONTROL CONSTRAINTS :
C W. R. T. STATES X
INJ = 0
DO 65 I=1, NP
DO 60 J-1, NS
NJ - NJ-1.

ICN (IJ--INJ) =IL+J

60. CONTINUE
65 CONTINUE
IJ = IJ--NPNS

C W. R. T. SLACK WARIABLES (K)

75 CONTINUE
J=IJ--NP

c W. R. T. Multipliers (Mul)

76 CONTINUE
J=J--NP

C W. R.T. CONTROLS (U)

INJ = 0
DO 85 I=1, NP
DO 80 J-1, M
INJ = INJ + 1
IRN (IJ--INJ) =IL-2 *NS+M+
ICN (IJ--INJ) =IL+2 *N+J
80 CONTINUE
85 CONTINUE
J = IJ--NPM

C DERIVATIVES OF MULTIPLIER CONDITIONS

C W. R. T. MULTIPLIERS AND SLACK VARIABLES
 5,435,503
125 126
RETURN
END

Function gegn (u1, u2, X,Y,Z)

Itplicit None
Include ' - ...include wifc proborder ré.inc
Include ' -- include Wifc nancOIt ré.inc
Real 8 g eqn, u1, u2, x, y, z
g eqn = u1 * u1 - ulimit *
S ( (abs (range o2* (1+CInerr) -xx-yy-z*z))/range o2) * *ccexp
return
end

Function VCcu (pg., k, Inui)

IIIplicit None
Include ' - . . includelwfc proborder ré.inc
Include ' - . . include Wifc controls ré.inc
Real * 8 pg., k, Illu, VCCul
if (ccv8) then
vccu = 2* pg. (Inu Itiu-dabs (Illu) )
elseif (ccv9) then
vccu = abs. (Inu) *pg
else
vccu = Illu Inupg
endilf
return
end

Function puVCCu (ppg, k, Inui)

Implicit None
Include ' -- includelwfc proborder ré.inc
Include ' -- include wifc controls ré.inc
Real 8 ppg, k, Itu, puVCCu
if (ccV8) then
puVccu = 2* ppg (Illu Inu-dabs (inu) )
elseif (ccV9) then
puVccu. = abs. (Itu) ppg
else
puVccu = ppg Inu Inu.
endilf
return
end

Function pluVCCul (pg., k, Inui)

Inplicit None
Include ' -- includelwfc prob order ré.inc W
Include ' -- includel wifc controls ré.inc
Real 8 pg., k, Clu, pluVccu.
if (ccv8) then
pIlluvccu = 2*pg * (2* Inu-1)
elseif (ccV9) then
OIlluVCC F pg
else
pinuvccu = 2* Inupg
endif
rer
end

Function vccx (pg., k, Inu)

 5,435,503
127 128
Implicit None
Include ' -- includelwfc proborder ré.inc
Include ' -- include) wifc controls ré.inc
Real * 8 pg., k, Inu, VCCX
if (ccv8) then
vccx = 2*pg* (Itumu +dabs (mu) )
elseif (ccV9) then
Vccx = abs (Tu) *pg
else
vcCx = Inu Inu *pg
endif
return
end

Function puvccx (ppg, k, mu)

Implicit None
Include ' -- include) wifc proborder ré.inc
Include ' - . . include wifc controls ré.inc
Real 8 ppg, k, Itu, puVCCX
if (ccv8) then
puvccx = 2* ppg (Inu. "Inu +dabs (Inu))
elseif (ccV9) then
puvccx = abs (Inu) ppg
else
puVCCX ppg *Inunu
endilf
return
end

Function pituVCCX (pg., k, Inui)

Implicit None
Include ' -- includewfc proborder ré.inc
Include (- - includel wifc controls ré.inc
Real * 8 pg., k, Itu, pluVCCX
if (ccv8) then
pmuvccx = 2*pg (2*Inu-1)
elseif (ccV9) then
pInuvccx = pg
else
pInuvccx = 2* Inupg .
endilf
retual
end

Function VCCInu (g, k, Illu)

Implicit None
Include ' -- include wifc proborder ré.inc
Include ' -- include wifc nailcon ré.inc
Include ' -- includewfc controls ré.inc
Real * 8 g., k, Inu, vccmu.
if (ccv8) then
vccInu = (2*g+k*k+dabs (k)) * (2*mu+1)
elseif (ccv9) then
vccInu = g +k k +k ccerr
else
vccInu = g-i-kirk
endilf
retlr).
end

Function puVCCIllu (pg., k, Illu)

 5,435,503
129 130
II uplicit None
Include ' - . . includel Wifc prob order ré.inc
Include ' - . includelwfc controls ré.inc
Include E-. include wifc namcort ré.inc
Real * 8 pg., k, Illu, puVCCIllu
if (ccv8) then
puvccInu = 2*pg (2*Inu--1)
elseif (ccV9) then
puVCCIllu = pg
else
plVCCIIll F pg
endilf
return
end

Function pkVccInu (g, k, Illu)

Inplicit None
Include ' -- include wifc prob order ré.inc
Include ' -- include wifc controls ré.inc
Include ' - includewfc nancoIn ré.inc
Real * 8 g, k, Illu, pkVCCIllu
if (ccv8) then
pkvccInu = (2*k+1) * (2*Inu-I-1)
elseif (ccV9) then
pkvccInu = 2* k + ccerr
else
pkvccrinu = 2*k
endilf
return
end

Function pluvCCInu (g, k, Illu)

Itplicit None
Include ' -- include wifc prob order ré.inc
Include ' -- includewfc controls ré.inc
Include ' (- - include wifc naticcit ré.inc
Real 8 g., k, Inu, pInuVCCIllu
if (ccv8) then
pInuvccIthu = 2 * (2* g +k*k+dabs (k))
elseif (ccV9) then
pInuvccInu = 0
else
prinu VCCInu. O
endilf
return
end

Function viccK (g, k, Inu.)

Inplicit None
Include (- - includewfc prob order ré.inc
Include ' -- includel wifc controls ré.inc
Include - . . include wifc namcot ré.inc
Real 8 g, k, Inu, VCCk
if (ccv8) then
vcck = (2*k-1) * (Inu Inu +dabs (Illu) )
elseif (ccV9) then
vcck = abs (IEnu) *k--ccerr
else
Vcc k = Inuk
endlif
return
 5,435,503
131 132
end

Function pkVcck (g, k, Ill)

Implicit None
Include ' - . . includelwfc prob order ré.inc
Include ' - . . includel wifc controls ré.inc
Include ' - . . include) wifc nancoln ré.inc
Real * 8 g., k, Inu, pkVcck
if (ccv8) then
pkvcck = 2* (Inu. "Inu +dabs (mu) )
elseif (cCV9) then
pkVcck = Inu.
else
pkVcck = Inu.
endilf
return
end
Function pluvcck (g, k, mu)
Implicit None
Include ' - . . include Wific prob order rö.inc
Include ' -- include wifc controls ré.inc
Include ' -- include wifc naucoli ré.inc
Real 8 g., k, Itu, piau Vcck . "
if (ccv8) then
pmuvcck = (2*Inu-1) * (2*k-1)
elseif (ccV9) then
pInuvcck = k + ccerr
else
pInuvcck = k
endif.
return
end

Function filt (temp, shifti)

real 8 teip, g filter, filt
integer shift i
integer* 2 ifilter (4), shift, iishift, filter
equivalence (g filter, ifilter)
shift = shifti
gfilter tellip
filter ifilter (4)
filter = ii-shift (filter, Shift)
filter = ii shift (filter, -shift)
ifilter (4) = filter -
filt = gfilter
return
end

subroutine check cc 1 (time, Sol, refactor matrix)

implicit none
Include ' - . include wifc prob order ré.inc
Include ' -- include wifc nancontré.inc
Include ' -- include wifc sol ré. inc'
Include ' -- includewfc controls ré.inc
Routine to evaluate a single control costraint equation
 5,435,503
133 134
double precision Sol (*), k off blin/0. 0d O/, k off brux
k on bV/0. 0d 0/, Itu off bV/O. 0d O/
highest Inu Value/O. 0d 0/, time
Inu delta exceeding bindry/-1. O/
k delta leaving bndry/0.001/
percent leaving bindry/0. 005/
Illu alpha Slope/ -5.0/
alpha percent error/O. 0001/
k percent error/0.0002/
Inu percent error/0.003/
State (Itax X), costate (mx x),
control (IX u), Sulln, x,y
f(Illx eqn, 0:Inx ux, 0:Inkux),
l (0 : Inx ux, 0:Illxux)
last refactor title, Inu upper error,
Illulower error
real * 8 ki (Inx cc) , ko (Itax cc), Inu. O (Inx cc),
Inui (I?ix CC), ratio, 2
u i (inx CC), u_O (Ink-co), g isol, g_coup, tot u
g_i (Inx CC), glo (Itx-cc), g eqn, u Inax, u, filt
Xi, yi, Zi
integer i,j,k, error trig val (nx cc), phase, shift
integer lW sl, up Sl, up Illu, lWitu, alpha i.
S littlit trig Val (Inx cc) , i.1
logical refactor thatrix, limit trig (IX cc), error trig (IX cc)
S, u ride (Illx cc), click (IDX cc), clic Inu (IX Co.),
S refactor (ITX cc)

c Initialize
ul IIlax disqrt (ulirclit)
k. On bV - ccerr / 2. O
k off blin O. 0d.O -ccerr*1000. O
highest Inu value = 0. 0d 0
Inul lower error = u Inax*Inu percent error
Inu upper error = u Inax*Inu percent error
c do for each unit (ie. elements and phase nodes)
do i = 1, netot-2*nph
xi = soil (nitot * (i-1) + 1)
yi = sol (ntot (i-1) + 2)
Zi = Sol (Intot (i-1) + 3)
k off bmx = disqrt (dabs (g eqin (0. OdO, O. 0d 0, xi, yi, zi) ))

check for errors before resetting the cd's

do j = 1, Ill filtering out bit twiddling
u i (j) = sol (ntot* (i-1)+2*n-j)
u o (j) = filt (uli (j), 0) drop lower 4 bits
ui (j) = u o (j)
 5,435,503
135 136
k_i(j) = sol (ntot (i-1)+ns+j)
Inui (j) = sol (ntot * (i-1)+n+ns+j)
ko (j) = filt (ki (j), 0) drop lower 4 bits 8
muo (j) = filt (Inui (j), 0) drop lower 4 bits
ki (j) = k O (j)
Illu i (j) = Itu o (j)
= u o (1)
y = u O (2)
gi (j) = g eqn (u, y, xi, yi, Zi)
i go (j) = g i (j)
enddo
if (dabs (g_i(1)).lt. ulimit alpha percent error ) then
go (1) = g eqn (u o (1), u o (2), Xi, yi, Zi)
endilf
if (ki (1) .gt. k off bmx
S . . and .ki (1). lt.
S k off bmx* (1+k percent error)) then
ko (1) = k off bmx
endilf
if (muli (1) . gt. highest Inu Value
S . . and. Ituli (1). lt. Inu upper error) then
Inu O (1) = Inu i (1)
endilf

Begin evaluating the control constraints which have come in

and been compensated for errors. The values used are the
"xxo" values. The "xx i" values are strictly for record. .
Call eval bndry cind1 (percent leaving bindry, u max
F Xi, Yi, zi, CInerr, cceXp, range o2
t K off bmn, k off bmx, k on bv, Inu off bv
7 mu-O (1), k-O (1), g_o (1), u_o (1), u o (2)
clc-k (1), clic Inu (1), u ride (1), refactor (1))
Okay the bndary conditions have been evaluated and the proper
Values have been set for "u". Now, I have to check to see if I
need to calculate Values for Inu or k and do so.
if (refactor (1)) refactor matrix = . true.
if (clic Inu (1)) then
do k = 1, ns
state (k) = sol (ntot * (i-1)+k)
costate (k) = sol (Intot * (i-1)+n+k)
endido

control (1) = u o (1)

control (2) = u o (2)
if (i.e. Xne (1) +2) then w
phase = 1
elseif (i.e. Xne (1) +xne (2) +4) then
phase = 2
elseif ([Link] (1)+xne (2) +xne (3) +6) then
phase = 3
 5,435,503
137 138
endilf - - - - - -- - - - -

call get f g eqn der (phase, i, State, control, f, l)

suit = 0. Odi O

do k = 1,ns .
surn = sum + f(k,ns+j, 0) * costate (k)
endido
endido

if ( dabs (2* (u_o (1) +u o (2))).lt. 1.0e-5) then

added for single cd proble in
suit = 1. Odo
else
surn = disqrt (dabs (surn/ (2* (u o (1) +u o (2) )))) .
endilf
Inu o (1) = - dabs (Sun)
endif Illul's have been appropriately set, now do k's.
if (click (1)) ko (1) disqrt (dabs (go ( 1) ))
if (click (2)) ko (2) disgrt (dabs (go (2)))
c now write the appropriate values in the suggested solution and
c save a file
do j = 1, np
sol (ntot* (i-1)+ns+j) = ko (j)
sol (ntot (i-1)+n+ns+j) = Inu o (j)
endido
do j = 1, II
sol (ntot (i-1)+n+n+j) = u o (j)
endido
endido
return
end

Function ratio (u1, u2, u Inod, u inax)

IIIlplicit None
real * 8 u1, u2, u todi, u tax, ratio, di
d = disqrt (u1* u1+u2* u2)
if (d. . gt. 1e-15) then
ratio = u Inodu Inax/d
else
ratio = 0. Od0
endif
return 4.
end

Subroutine eval bindry cnd1 (percent leaving bindry, u_max

S, Xi, yi, Zi, citierr, cceXp, range o2
S, k-off briln, k off bill.x, k on bv, Illu off bv.
S, Illu, k, g, ul, y
S, click, clic Illu, uride, refactor )
implicit none
real * 8 ul Illax, Illu, k, g, u, percent leaving bindry,
S y, Z, ratio, geq n, tu
 5,435,503
139 140
S, Xi, yi, Zi, CInerr, CCeXp, range O2
S, koff bran, k off bmx, k on bV, Inu off bv
logical click, Clc Inu, uride, refactor, limit trig
integer limit trig Val
c initialize the output Values
Click = . false.
clic Inu. = . false.
u ride = . false.
refactor = . false.
limit trig = . false.
if ([Link] off [Link]. k. eq. k_on_bv) then
if ([Link]. 0) then Inu : on, u : on the bindry
u ride = . true. -
till = dSign (ratio (u, y, u, u, Inax), u)
y = [Link] (ratio (u, y, y, u Inax), y)
a tell
C = 0. OCO
elseif (g. ed. O. 0d 0) then Inui: on, u : on the bndry
u ride = . true.
else
if (g eqn (u, y, Xi, yi, Zi) . gt. 0.050) then believe u
click = . true.
l = Inu off bV
refactor = . . true.
else ! believe Itu
if (Illul. ge. 0. Od0) then
clic Inu. F . . trlle.
endilf
k = 0 . OdO
u ride F . . true.
tl = disign (ratio (u, y, u, u, Inax), u)
y = dSign (ratio (u, y, y, u Inax),y)
ll = tu
g = 0 . OdC)
endilf
endilf w
elseif ([Link]. Inu off bv. and. k. eq.k on bV) then
if (g. lt. O) then leaving the bindry
= Inu off bV
click = . true.
refactor = . true.
else
Click = true.
refactor = . . true.
9 = - percent leaving bndry
till F disign (ratio (u, y, u, u, Inax) * ,
S (1-percent leaving bindry), u)
y = disign (ratio (u, y, y, u tax) *
S (1 percent leaving bindry), y)
l = tu
= Inu off bV
endilf
elseif (Inu. eq. Itu off bV. and. [Link]. k off bran) then
Inu: off k: off
if (g. lit. 0) then l k and u agree to stay off the bndry
refactor = . false.
elseif (g. ed. 0) then k: Off, ll: On , believing k.
 5,435,503
141 142
Clc k F - trille.
9 = - percent ileaving bindry
tu = [Link] (ratio (u, y, u, u, Inax) *
S (1-percent leaving bindry), u)
y = [Link] (ratio (u, y, y, u Inax) *
S (1-percent leaving bndry), y)
tul
else
if (dabs (g) . ge.k) then believing ul
clic Illul = . true.
u ride = . true.
refactor - . . true.
9 = 0 . OdO
k = k on bV
tu = [Link] (ratio (u, y, u, u, Inax), u)
y dSign (ratio (u, Y, y, lu Inax) , y)
F
else believing k
= Inu off bV
click of . rule.
tu = dSign (ratio (u, y, u, u, Inax) *
S (1-percent leaving bndry), u)
y = [Link] (ratio (u, y, y, u Inax) *
S (1-percent leaving bindry), y)
ll = till
9 F g eqn (u, Y,Xi, yi, Zi)
endilf
endif.
elseif (flu. eq. Itu off bV. and. k. lt. k off bran) then
Inul: Off, k: on
if (g. ge. 0) then
clic Illu F . . true.
u ride . trille.
refactor of . trille.
tl. = [Link] (ratio (u, y, u, u, Itax), u)
y F disign (ratio (u, y, y, u tax), y)
* = till
9 = 0. Od. O
k = k on bV
else
Click = . trille.
= Inu off bV
endilf
elseif ([Link]. k. On bV. and . ( (k. lt. k off bInn ) . Or.
S ([Link].k off bmx) ) ) then
k is out of good bounds, check u
if (g. ge. O) then Illove on bindry
k F k. On bV
u ride F . . trille. - -
Ul F disign (ratio (u, y, u, u, Inax), u)
y = [Link] (ratio (u, y, y, u Inax) , y)
F tell
if (Inu. ge. Inu off bV) clic Inu. = . true.
if (Inu. ed. Inu off bv) refactor = . true.
9 = 0 . Od. O
else move off bndry
Click = . true.
if (Inu. eq. Inu off bV) refactor = . false.
= Itu off bV
endilf
 5,435,503
143 144
elseif ( ([Link].k off blin. and. [Link]. Inu off bV) . Or.
S (k. eq.k on bv. and. Iu. eq. Illu off bV) ) then
if (g. ge. 0) then ! Inoving on bindry
clic Illu = . true.
u ride = . trule.
refactor = . . true.
k = k On bV
9 = 0 . Od0
till = [Link] (ratio (ll, y, u, u, Inax), u)
y = [Link] (ratio (u, y, y, u Inax), y)
l = till
else leaving bindry
click F . . trille.
refactor = . . true.
= Inu off bV
endilf
endilf
retulrn
end

subroutine eval cc (tille, Sol, refactor matrix)

iIllplicit none
Include ' -- includel wifc proborder ré.inc
real * 8 Sol (*), time
logical refactor thatrix
if (np. eq. 1) then
call check cc1 (Time, Sol, refactor matrix)
elseif (np. ed. 2) then
call check cc2 (Title, sol, refactor matrix)
endilf
return
end

subroutine check cc2 (title, Sol, refactor Imatrix)

implicit none
Include ' -- include) wifc proborder ré.inc
Include ' - . include wifc nancon ré.inc
Include ' - . . include wifc. Sol rô. inc'
Include ' -- includewfc controls ré.inc
double precision sol (*), k off brun/0. Odo/, k off brix
k on bV/0. 0d0/, Inu off bv/0. Odo/, time
Inu delta exceeding bndry/- 1.0/
k delta leaving bndry/0.001/
percent leaving bndry/0.005/
Inu alpha Slope/ -5.0/
alpha percenterror/0.0001/
k percent error/O. 0002/
Inu percent error /. 003/
State (Inx X), costate (IRX x),
Control (Inx u), Sun, x,y
f(Inx eqn, 0:IX ux, 0:Iux lux),
l (0 : Inx ux, 0:Inx ux)
last refactor time, Inu upper error,
 5,435,503
145 146
S Inul lower error -
real * 8 ki (nx cc) , ko (Inx cc) , Illu O (Illx cc), Inu i (Inx cc)
f ui (Illx cc) , u_o (nx cc), g isol, g_coup, tot u
f g-i (Illx cc), g o (InX cc), g eqn, Sqrt two, u. Ilax, u, filt
f cal k, call 1Illu bndry, Cal 2Itu bindry
f ck 1 Inu bindry, cK2IIlu bindry
Xi, yi, Zi
integer i, j, k, error trig Val (InX cc), phase, shift
integer lW Sl, up Sl, up Inu, iW Illu, alpha i,
S limit trig Val (IX cc) , i.1
logical refactor Flatrix, limit trig (IX cc), error trig (inx cc)
uride (nx cc), clic k (inx cc), clic Inu. (Itax cc),
refactor (IXcc)
ck k au
C
c Initialize
c Define Functions
calk (u, xi, yi, zi) = (-1. Od0+disgrt (1. Od0
S 4 * g eqn (u, u, Xi, yi, Zi) ))/2.
ck k (u, xi, yi, Zi) = (1 . 0d O. ge. 4 * g eqn (u, u, Xi, yi, zi) )
ck_1mu_bindry (sum) 1.
ck 2Inu bindry (Sun) 1.
cal 1Itu bindry (Sun) - disqrt (abs (suit) / (2*u Inax))
cal 2Inubndry (Sun) - disqrt (abs (suit) / (4*u Inax) )
k. On bV = -cCerr/2. O
koff brinn = 0. Od0 - CCerr - 10 OO . . O
sqrt two = disqrt (2.0 d0)

c do for each unit (i.e. elements and phase nodes)

do i = 1, netot--2*nph
xi = sol (ntot" (i-1)+1)
yi = sol (ntot (i-1)+2)
Zi = sol (Intot * (i-1)+3)
koff bitx cal_k(0. Odo, xi, Yi, zi)
disqrt (dabs (g eqn (0. 0d O, O. Od0, Xi, yi, zi) ))
u Inax
mu lower error = u Inax*Inu percent error
Inu upper error = u Inax*Inu percent error
C
c check for errors before resetting the co’s
C
do j = 1, In filtering out bit twiddling
ui (j) = sol (ntot* (i-1)+2*n-j)
u o (j) = filt (uli (j), 0) drop lower 4 bits
lui (j) = u O (j)
endido
do j = 1, np
ki (j) = sol (ntot (i-1)+ns+j)
Inu i (j) = sol (ntot (i-1)+n+ns+j)
 5,435,503
147 148
ko (j) = filt (ki (j), 0) ! drop lower 4 bits
Inu o (j) = filt (muli (j), 0) drop lower 4 bits
ki (j) = k o (j)
Inui (j) = Illu o (j)
= u o (j)
gi (j) = g eqn (u, u, Xi, Yi, Zi) -
go (j) = g i (j)
endido

do j = 1, Inp . .
if (dabs (g_i (j)).lt. ulimit"alpha percent error ) then
u_o (j) = dSign (u Inax, u_i(j))
go (j) = 0
endif
if (Inui () . gt. Itu off bV
S . . and. Inui (j).lt. Ithu upper error) then
Inu o (j) = -Inui (j)
endilf
endido

Begin evaluating the control constraints which have come in

and been compensated for errors. The values used are the
"xxo" values.
The "xx i" values are strictly for record.
do i = 1, np
call eval bndry Cnd2 (percent leaving bndry, u. Inax
Xi, yi, Zi, CInerr, cceXp, range O2
k off brun, k off binx, k on bV, Illu off bV
Inu o (j) , ko (j), g o (j), u o (j)
click (j), clic Inu (j), uride (j), refactor (j))
endido

Okay the bndary conditions have been evaluated and the proper
values have been set for "u". Now, I have to check to see if I
need to calculate values for Itu or k and do so.

if (refactor (1) . or refactor (2)) refactor matrix = . true.

if (clic Inu (1). Or - clic Inu (2)) then
do k = 1, Ins
state (k) = sol (Intot (i-1)+k)
costate (k) = sol (ntot * (i-1)+n+k)
endido

control (1) = u o (1)

control (2) = u O (2)
if ([Link] (1) +2) then
phase = 1
elseif ([Link]. Xne (1) +Xne (2) +4) then
phase = 2
elseif (i.e. Xne (1) +Xne (2) +Xne (3) +6) then
phase = 3
 5,435,503
149 150
endilf
call get f_g eqn der (phase, i, State, control, f, l)
suIt = 0. Odo
do j = 1, Ill
do k = 1, IS
suit = suit + f(k,ns-Fj, 0) * costate (k)
endido
endido
sum = -dabs (sun) .
if ( (clic Illu (1). and. Clc Inul (2)). Or.
S (clic Inu (1) . and u ride (2)). or .
S (clic Inu (2) . . and u ride (1)) ) then
if (ck 2nubndry (suit) . ge. O. 0d 0) then
Inu o (1) = call 2Illu bndry (Sun)
mulo (2) = Inu O (1)
else we're in SOIne sort of trouble
Inu o (1) = -2. Odo
mu o (2) = -2. 0d0
endilf

elseif (clic mu (1) ) then

Inu o (1) = -2. Odo
if (ck 1mu bndry (sun) - ge. 0-0 d0) Inu. O (1) =
S cal 1Inubndry (Sun)
elseif (clic Inu (2)) then
Inu o (2) = -2. Od0
if (ck 1nu bndry (Surt) . ge. O. 0d 0) Inu. O (2) =
S Cal [Link] bindry (Sun)
endilf
endif Inu's have been appropriately Set, now do k's.
if (click (1) . and ck k (u O (1), Xi, yi, Zi) )
S KO (1) = calk (u. O (1), Xi, yi, Zi)
if (click (2) . and ck k (u. O (2), Xi, yi, Zi) )
S. k O (2) = calk (u. O (2), Xi, yi , zi)
c now write the appropriate values in the Suggested solution and
c save a file
do j = i, Ilp
sol (ntot * (i-1)+ns+j) = ko (j)
sol (ntot (i-1)+n+ns+j) = Inu o (j)
endido
do j = 1, In
sol (ntot* (i-1)+n+n+j) = u o (j)
endido

enddo
return w
end

subroutine eval bindry Cnd2 (percent leaving bndry, unax

Xi, yi, Zi, CInerr, cceXp, range O2
f k off binn, k-off binx, k on bV, Inu off bv.
Inul, k, g, u
f click, clic Inu, uride, refactor)
inplicit none
 5,435,503
151 152
real*8 u_max, mu, k, g, u, percent leaving bindry
S, koff blin, koff bmx, k on bV, Illu off bV
S, Xi, yi, Zi, CInerr, cceXp, range O2
logical click, clic Inu, uride, refactor, limit trig
integer limit trig Val
c initialize the output Values
click = . false.
clic Inu. = . false.
u ride = . false.
refactor = . false.

if (Inu. lt. Itu off bv. and . k. eq. kon bV) then
if ([Link]. 0) then ! Illul: On , , k: On, u : On
staying on the bndry
u ride = . true.
l = disign (u. Itax, u)
g = 0 . OdO
elseif (g. ed. 0. OdO) then ! Illu: On , , k: On, u : on
! staying on the bndry
u ride F . . trille.
else Illul: On , , k: On , u : On
if ( (u Inax - dabs (u)) /u. [Link]. 0.40) then
leaving bindry
Click = . trule.
U = Inu off bV
refactor = . . true.
else ! staying on bndry
Clc Inu. = . trille.
u ride = . true.
l = [Link] (u Inax, u)
9 = 0. OdO
endilf
endilf
elseif ([Link]. Inu off bV. and. k. ed. kon bv) then
! Inu: off, k: on the bndry
if (g. St. 0) then
l = Inu off bv.
click = . true.
refactor = . . true.
else ! Illul: Off, k: On, u : on; leaving bndry
Click = trlle.
refactor = . . true.
9 = - percent leaving bndry
l
disign (u max* (1-dabs (Inu) *percent leaving bndry), u)
if (dabs (u). gt. l. lax) u =
S -
disign (u Inax* (1-percent leaving bindry), u)
Ill = Inu off bV
endilf
elseif ([Link] off [Link]. k_of_bmn) then
! Illu: off, k: off
if (g. lt. 0) then ! u: off staying off bndry
refactor = . false.
elseif (g. eq. 0) then u : on, staying off
click F . . trille.
9 = - percent leaving bndry
 5,435,503
153 154
l = [Link] (u Inax* (1-percent leaving bindry), u)
else ul: On
if (dabs (g) . ge.k) then Illoving on bindry
Clic Illu = . true.
u ride = . true.
refactor = . . true.
C = 0 . Od. O
k = k on bV
l = disign (u, Illax, u)
else ! staying off bndry
= Inu off bV
Click = . trile.
l = disign (u Inax* (1-percent leaving bndry), u)
endilf
endilf
elseif (Illu . . ed. Inu off [Link]. [Link].k off bmn) then
Illu: off, k: on
if (g. ge. 0) then ul: O ; InoVing on bindry
Clc Inu. . . true.
u ride . . trie.
refactor . . true.
l disign (u tax, ll)
g O. Od. O
k k on bV
else u : off ; staying off bndry
clic k . . true.
I Inu off bV
endif
elseif ([Link]. k on bV. and. ( (k. lt. k Off bran ) . . O
S ([Link]. k off brux) ) ) then l k is out of good bounds,
check ul
if (g. ge. 0) then Ilove on bindry
k = k on bV v
u ride = . trlle.
= disign (u, Itax, u)
if (Inu. ge. Illu off bV) clic Inu. = . true.
if (Inu. eq. Inu off bV) refactor = . true.
g = 0 . OdC)
else Inove off bndry
Click = . true.
if (mu. ed. Inu off bV) refactor = . false.
IU = Inu off bV -
endilf
elseif ( ([Link]. k off brun. and. [Link]. Inu off bv) .or.
S (k. eq.k on bV. and. Inu. ed. Itu off bV) ) then
if (g. ge. O) then Inoving on bridry
clic Itill = . trille.
u ride = . true.
refactor = . . trule.
k = k on bV
9 = 0. Odi O
l = [Link] (u Inax, u)
else leaving bindry
Click = true.
refactor = . . true.
I = Inu off bV
endilf ..
endilf
return
end
 5,435,503
155 156
SUBROUTINE ENDPH (KSW, IRTOT, IJTOT, PN, delt, A, B, IRN, ICN)
IMPLICIT DOUBLE PRECISION (A-H, O-Z)
Include ' - . . include) wifc proborder ré.inc
Include ' -- includelwfc soil ré.inc
INTEGER PN, ten
DOUBLE PRECISION X (IX x), U (Itx u), F (IX eqn, 0:Inx ux, 0:mx ux)
S, I (0 : Tax uX, 0:Itxux)
DIMENSION A (*), B (*), IRN (*), ICN (*)

C THIS ROUTINE LOADS THE PART OF THE EQUATIONS FOR THE LAST
EEMENT
C IN A PHASE THAT DEPENDS ON THE QUANTITIES AT THE
C NODES, I. E. HATTED QUANTITIES
C
C ALL DERIVATIVES ARE WITH RESPECT TO THESE HATTED QUANTITIES
C
C KSW = 1 ==X FILL IN INDICES OF JACOBIAN ELEMENTS
C KSW = 2 ==X FILL IN RIGHT HAND SIDES OF EQUATIONS
C KSW = 3 ==X FILL IN ACTUAL, JACOBAN ELEMENTS

IF (KSW .EQ. O) CALL INDEX3 (IRTOT, IJTOT, IRN, ICN)

IF (KSW .EQ. 0) then

CALL INDEX3 (IRTOT, IJTOT, IRN, ICN)
ELSEIF (kisw . ed. 2) then ! Only need this for the Jacobian
C = Delt/2. OdO
DO 7 = 1, NS
X (I) = xhat (I, 2, PN)
7 CONTINUE -
DO 8 = 1, M.
U (I) = Uhat (I, 2, PN)
8 CONTINUE
ten = 0
do i = 1, pn
ten = ten + Xne (i) + 2
endido
CALL GET F. G EQN DER (PN, ten, X, U, F, L)
ENDIF

IF (KSW .E.O. 1) THEN

CCCCCCCCCCCCCC BEGIN ERROR VECTOR FILL IN CCCCCCCCCCCCCCC
IL=IRTOT - NTOT
C STATE AND COSTATE EQUATIONS
DO 5 I=1, NS
B (IL--I) = B (IL-I) - XHAT (I, 2, PN)
B (IL--NS+I) = B (IL--NS+I) + LHAT (I, 2, PN)
5 CONTINUE

CCCCCCCCCCCCCCC END ERROR VECTOR FILL IN CCCCCCCCCCCCCCC

 5,435,503
157 158
ENDF -
IF (KSW .EQ. 2) THEN
CCCCCCCCCCCCCCC BEGIN JACOBAN FILL IN CCCCCCCCCCCCCCCCCCCC
C DERIVATIVE OF STATE AND COSTATE EQUATIONS. W. R.T. X AND
AMBDA
J = IJTOT

DO 15 I=1, NS
A (IJ--I) = 1. OD+00
A (IJ--NS-I) = -1. OD+00
15 CONTINUE
IJ = IJ + 2*NS

C Derivative of the node state wrt control D^xhat/Du .

INJ = 0
DO 85 I= 1, NS
DO 80 J-1, M.
NJ - NJ --
A (IJ--INJ) = -C*F (I, NS+J, O).
80 CONTINUE -
85 CONTINUE
IJ-IJ--NS*M
C
JTOT = IJ

CCCCCCCCCCCCCCCC END JACOBIAN FILL IN CCCCCCCCCCCCCCCCCCCC

ENDF

RETURN
END
C
SUBROUTINE ERRJAC (sol, KSW, factor, NZ1 NZ2, NZ
S, b, good vector)
IMPLICIT DOUBLE PRECISION (A-H, O-Z)
Include ' - . . includelwfc prob order ré.inc
Include ' -- include wifc controls re...inc
Include ' - . . include wifc soiré.inc
Include ' - includel wifc mass rô.inc
Include ' - . . includel wife harwell ré.inc
C THIS ROUTINE FILLS THE INDICES, THE ERROR VECTOR, OR THE
C JACOBAN DEPENDING ON KSW. IIT ALSO SOLVES THE LINEARIZED
C SYSTEM IF NEEDED
C inputS :
real * 8 sol (*) ! contains the guessed solution vector
integer KSW 0: . GET INDICES
1. GET ERROR VECTOR (B)
2: GET JACOBAN (A)
logical factor decides whether or not to factor the
1 thatrix
 5,435,503
159 160
integer nz1 integer nonzero counter
integer in Z2 integer nonzero counter
integer. In Z. integer total non-zero counter
c Outputs :
C
logical good Vector
real * 8 b (*) the error Vector from the Iatrix
c local
integer livect (80000) row index
integer jvect (80000) Column index
DOUBLE PRECISION PHIX (Inx bo), PHIXX (inx bo, Ink bc),
& PSIX (Inx bc, Incx nu), PSIXX (IX bc, Inx bo., Inx nu)
S, PHIT (Inx nph), PSI (Inx nu), PSIT (mx nph, inx nu)
DOUBLE PRECISION A (200000), W (8325), tenpa (60000), temp b (835)
S, uu/1. OdO/
INTEGER -
IRN (80000), ICN (200000), IKEEP (41625), IW (66600), iflag/0/
S, icn/200000/, lirn/80000/
C
good vector = . false.
initialize harwell results indicator.
C BREAK THE SOLUTION VECTOR INTO WARIABLES
C THIS IS THE ORDER OF THE COLUMNS IN THE JACOBAN

IF (KSW .NE. O) THEN

CALL XLAMU (Sol)
ENDIF

c ZERO ERROR VECTOR AND JACOBIAN IF APPROPRIATE

IF (KSW .EQ. 1) THEN
DO 65 = 1, ORDER
B (I) = 0. OD+00
65 CONTINUE
ENDIF

IF (KSW .EQ. 2) THEN

DO 70 I=1, NZ
A (I) = 0. OD+OO
70 CONTINUE
ENDIF

C OBTAIN PHI, PSI AND THEIR DERIVATIVES W. R.T. TIME AND THE
C STATES

IF (KSW .NE. O.) THEN

CALL GET BND EQN DER (PHIT, PHIX, PHIXX, PSI, PSIT, PSIX, PSIXX)
ENDIF

C IRTOT S A RUNNING TOTAL OF THE ROW NUMBERS

C IJTOT IS A RUNNING TOTAL OF THE JACOBIAN ELEMENTS
C IJTIME IS A RUNNING TOTAL OF CERTAIN JACOBAN ELEMENTS
IRTOT = O
IJTOT = O
JTIM = NZ
JTIM2 = NZ2
 5,435,503
161 162
FILL IN THE INDICES, ERROR VECTOR, OR JACOBIAN
IRN (I) : HOLDS ROW INDEX FOR I. TH JACOBIAN ELEMENT B (I)
ICN (I) : HOLDS COLUMN INDEX FOR ITH JACOBIAN ELEMENT B (I)
DO 100 KPH=1, NPH
IF (KPH EQ. 1.) THEN
DELT=TTIL (1) /XNE (1)
ELSE
DELT= (TTIL (KPH) -TTIL (KPH-1))/xNE (KPH)
ENDIF

C EQUATIONS AT THE START OF A PHASE:

CALL START (KSW, IRTOT, IJTOT, KPH, A, B, IRN, ICN)
C INTERNAL EQUATIONS IN A PHASE
DO 200 K=1, xNE (KPH)
CALL INTER (KSW, IRTOT, IJTOT, IJTIM1, IJTIM2, KPH,
& K, DELT, A, B, IRN, ICN)
200 CONTINUE

C UPDATE COUNTERS FOR THE TIME DERIVATIVES

JTIM1 = IJTM1 + NS
JTIM2 JTM2 + NS

IF (KPH . GT. 1.) THEN

JTIM = IJTM1 + NS
JTIM2 = JTIM2 + NS
ENDIF

C EQUATIONS AT THE END OF A PHASE

CALL ENDPH (KSW, IRTOT, IJTOT, KPH, delt, A, B, IRN, ICN)
C BOUNDARY CONDITIONS ON COSTATES, AND OTHER CONDITIONS AT THE
C END OF A PHASE

CALL BCTERM (KSW, IRTOT, IJTOT, KPH, PHIX PHIXX.

& PSIX, PSIXX, A, B, IRN, ICN)
1 OO CONTINUE

C PRESCRIBED BOUNDARY CONDITIONS, PSI

CALL PSITER (KSW, IRTOT, IJTOT, PSI, PSIX, PSIT, A, B, IRN, ICN)
C CONTINUITY OF HAMILTONIAN REQUIREMENTS FOR THE UNKNOWN TIMES
CALL TIMTER (KSW, IRTOT, IJTOT, PHIT, PSIT, A, B, IRN, ICN)
IF ([Link].0) THEN
DO 85 =1, NZ
IVECT (I) = IRN (I)
JVECT (I) = ICN (I)
85 CONTINUE
ENDF

C SOLVE THE SYSTEM IF REQUIRED

 5,435,503
163 164
88 IF (KSW .EQ. 2) THEN

IF ([Link]. lt. 0) THEN

DO 75 I=1, NZ
IRN (I) = IVECT (I)
ICN (I) = JVECT (I)
temp a (i) = a (i)
75 CONTINUE

CALL factor Ithatrix

(IORDER, NZ, A., LICN, IRN, LIRN, ICN, UU, IKEEP
S, IW, W, IFLAG)
if (iflag. lt. 0) then
good vector . . false.
return
endilf
CALL invert Inatrix (IORDER, A, LICN, ICN, IKEEP, B, W, 1)
factor = . . false.
ELSE

DO = 1, NZ
temp a (i) = a (i)
endido
CALL
factor matrix a (IORDER, NZ, A., LICN, IVECT, JVECT, ICN, IKEEP
S, W, W, IFLAG)
if (iflag. lt. 0) then
do i = 1, inz
a (i) = tellipa (i)
endido
goto 88
endif
CALL invert Imatrix (IORDER, A, LICN, ICN, IKEEP, B, W, 1)
ENDIF for the invert matrix question
ENDIF ! if we want to get the correction vector
good vector = . true.
RETURN
END

function hari (PN, EN, X, U, [Link])

THIS SUBROUTINE CALCULATES THE HAMILTONIAN, 'HAM'
X, LAM, U ARE ASSUMED TO HAVE ALREADY BEEN TAKEN FROM HAT OR
BAR VECTORS BEFORE BEING INPUT.
IMPLICIT DOUBLE PRECISION (A-H, O-Z)
Include ' - . includelwfc proborder ré.inc
Include ' -- includewfc. Sol ré.inc
INTEGER PN, en
 5,435,503
16S 166
DOUBLE PRECISION X (mx x), U(IRX u), F (mx eqn, 0:Inx ux, O:mxux)
S, I (0 : Itax ux, 0: Ink ux), haill, la?t. (Inx x)

CALL GET F. G EQN DER (PN, en, X, U, F, L)

SUM= 0 . OID--00
DO 10 = 1, NS
if ([Link]. Ins) then
SUM=SUM + F (I, O, O.) * LAM (I)
else
SUM=SUM + voccu (F (, 0, 0), x (i), LAM (I) )
endilf
O CONTINUE
HAM=L (0, 0) +SUM
RETURN
END

SUBROUTINE INDEX3 (IRTOT, IJTOT, IRN, ICN)

Include ' - . . include wifc prob order ré.inc
INTEGER IRN (*), ICN (*)
THIS SUBROTINE MARKS THE PLACE IN THE JACOBIAN OF THE WARIOUS
PARTIAL DERIVATIVE TERMS WITH RESPECT TO HATTED QUANTITIES FOR
THE EQUATIONS FOR THE LAST ELEMENT IN A PHASE
J IJTOT
II. IRTOT

DERIVATIVE OF STATE AND COSTATE EQUATIONS. W. R. T. X AND

LAMBDA

DO 5 T=1, NS
IRN (IJ--I) IL-NTOT-I
ICN (IJ--I) -- -

IRN (IJ--NS+I) L-NTOT--NS--

ICN (IJ--NS+I) IL--N+I
5 CONTINUE

INJ = 0
DO 85 = 1, NS
DO 80 J-1, M
INJ = INJ + 1
IRN (IJ--INJ) = IL-NTOT+I
ICN (IJ--INJ) = IL-2*N+J
80 CONTINUE
85 CONTINUE
J=J--NS*M
IJTOT =J

RETURN
END
 5,435,503
167 168
SUBROUTINE INDEX5 (IRTOT, IJTOT, IRN, ICN)
THES ROUTINE MARKS THE PLACE IN THE JACOBAN FOR PARTIA
DERIVATIVES OF THE PRESCRIBED BOUNDARY CONDITIONS, PSI, WITH
RESPECT TO THE WALUES OF THE STATES AT THE BEGINNING AND END
OF EACH PHASE

REMINDER: NBC == NUMBER OF PRESCRIBED BOUNDARY CONDITIONS

IMPLICIT DOUBLE PRECISION (A-H, O-Z)

Include ' - . . include wifc prob order ré.inc
INTEGER IRN (*), ICN (*)

IL=IRTOT
Js IJTOT

DO 20 = 1, NPH
IF (I . EQ. 1) THEN
PTST=O
ELSE
PTST=PTEND+NTOT
ENDIF w
IPTEND=IPTST+NTOT* (xNE (I) +1)
NJ = 0
DO 3 O J = 1, NBC
DO 40 K=1, NS
INJ = INJ
IRN (IJ--INJ) =IL+J
ICN (IJ+INJ) =IPTST+K
IRN (IJ--INJ--NS) =IL-J
ICN (IJ--NJ--NS) =IPTEND+K
40 CONTINUE
J=J-2 nS-1
NJ = 0
IRN (IJ) =IL--J
ICN (IJ) =IORDER-NPH+I
3O CONTINUE
2O CONTINUE
IJTOT =J
IRTOT=IL+NBC
RETURN
END

SUBROUTINE INDEX6 (IRTOT, IJTOT, IRN, ICN)

THIS ROUTINE MARKS THE PLACE IN THE JACOBAN FOR DERVATIVES
WITH RESPECT TO QUANTITIES AT THE BEGINNING AND END OF EACH
PHASE THE EQUATIONS OF INTEREST ARE THE JUMP CONDITIONS ON
THE HAMILTONIAN AT THE PHASE BOUNDARES AND THE END POINT
CONDITION ON THE HAMILTONIAN
IMPLICIT DOUBLE PRECISION (A-H, O-Z)
Include ' -- includelwfc prob order ré.inc
INTEGER IRN (*), ICN (*)

JJTOT
 5,435,503
171 172
J = IJ--NBC
NPTR = NPTR-NTOT

ESE

NPTR=ORDER-NPH-NBC-NTOT
I= ORDER

c TAKE DERIVATIVES OF HMINUS + D (CAP PSI)/DT = 0,

C ASSUMING THAT PHL AND PSI ARE LINEAR FUNCTIONS OF TIME

c DERIVATIVES W. R.T. STATES AT FINA. NODE (X HAT)

DO 27 J-1, NS
IRN (IJ--J) =IL
ICN (IJ--J) =NPTR+J
27 CONTINUE

C W. R.T. COSTATES AT FINAT, NODE (LAMBDA HAT)

IJ-J.--NS
DO 37 J-1, NS
IRN (IJ--J) =IL
ICN (IJ--J) =NPTR+N+J
37 CONTINUE

C W. R.T. CONTROLS AT FINAL NODE (U HAT)

IJIJ--NS
DO 47 J-1, M
IRN (IJ--J) =IL
ICN (IJ--J) =NPTR+2*N+J
47 CONTINUE
C W. R.T. NU
IJ-IJ-M
DO 57 J-1, NBC
IRN (IJ--J) =IL
ICN (IJ--J) =NPTR--NTOT+J
57 CONTINUE
J-J.--NBC
ENDIF
2O CONTINUE
JTOT=IJ
IRTOT = IL-NPH

RETURN
END

SUBROUTINE NEWTON (NZ1, NZ2, NZ, parent refactor

S, SOL, totcInt, converged, stop nri)
C THIS ROUTINE DOES A RESTRICTED STE NEWTON METHOD

IMPLICIT DOUBLE PRECISION (A-H, O-Z)

Include ' -- includewfc proborder ré.inc
Include ' - . includel wifc mass ré.inc
Include ' -- includel wifc controls ré.inc
Include ' - . . includel wifc naticci ré.inc
C inputs
 5,435,503
173 174
integer nZ1, in Z2, n.2 nurricer of nonzeroes in the Inatrix
logical parent refactor
c inputs/outputs
diInension Sol (*) ! initial and final solution
C OutputS
integer totCInt
number of iterations redrired for Convergence
logical converged did we converge 2
logical Stop nri
! stop the newton raphson iteration we're lost
c locals

real * 8 err avg., erraVg best, err Inax, err Illax best
logical child refactor , factor
logical dum f/. false. /, good inversion
DMENSION correction vector (835), sol best (835), b (835)
INTEGER trijidx/0/, cycle/0/
S, index jacobian/0/, get error/1/,
S, fill invert jacobian/2/
S, get avg err/1/, get tax err/2/
S, locelle err Illax, loc Var err Inax, loc err Inax
c Initialize
converged = . false.
cycle = cycle + 1
totCint = 0

if (parent refactor) then

CALL ERRJAC (Sol, index jacobian, durin f, NZ1, NZ2, NZ, b, dum f)
factor = . . true.
parent refactor = . false.
endilf

if (cap cc. and. ([Link]. O)) then

call eval cc (Tille Since launch, Sol, child refactor)
if (child refactor) then
child refactor = . false.
factor = . true.
endilf
endilf

CALL ERRJAC (Sol, get error, durif, NZ1, NZ2, NZ, b, dun f)
call evaluate error (get a Vg err, i, b, erraVg best, i, i, ii)
call evaluate error (get Inax err, i , b, err Inax best
S, locelle err Inax, loc Var err tax, loc err Inax)
err avg err avg best
err Tax err Inax best
110 continue
CATI ERRJAC (sol, fill invert jacobian, factor, NZ1, NZ2, NZ
 5,435,503
175 176
S, b, good inversion)
DO = 1, IORDER
correction vector (i) = b (i)
SOL best (I) =SOL (I) save what right be the best guess
endido - -

20 totCint = totCint + 1 . 'total iteration counter

if (good inversion) then
do i = 1, iorder
SOL (I) =SOL (I) +newton scale factor (i) * correction vector (i)
endido
endilf

if (cap cc. and. ([Link]. O)) then

call eval cc (Tilne since launch, Sol, child refactor)
if (child refactor) then
child refactor = . false.
factor F . . true.
endilf
endif

CALL ERRJAC (sol, get error, durin f, NZ1, NZ2, NZ, b, durin f)
call evaluate convergence (totcInt, err Inax best
trijidx, Correction Vector, b
Sol, Sol best, factor
p int converg eval)
goto (130, 110, 120, 140, 150) int converg eval
130 converged = . true.
RETURN

140 stop nri = . true. Stop newton raphson iteration

exceeded Inax error
50 return return without converging
END

Subroutine Evaluate error (eval request, variable request

S, loc b, error, err locelle
S, err loc. Var, error loc)
IIuplicit None
Include ' -- includelwfc prob order ré.inc
c inputs
Integer eval request
1=aVg, 2=Inax, 3-var, 4=bc st, 5= bc tillne
Integer variable request
! variable index when eval request = 3
real* 8 loc b (*)
C OltputS
real * 8 error error Value
integer err loc ele, err loc Var, error loc
location indexes
 5,435,503
177 178
C local
integer i
error = 0.0
goto (100, 200, 300, 400, 500) eval request
1 OO DO =1, IORDER find average error
error = error--DABS (loc b (I) ) surn new error
endido
error = error/dfloat (iOrder)
relir

2OO DO = 1, ORDER find Itax error

if (dabs (error).lt. dabs (loc b (ii) )) then
error = loc b (i) find inax error
error loc = i
endilf
endido
goto 900 find the location cit the error index
3OO do i = 1, iorder find the largest variable requested
if ( Itod (i-1, intot) +1. ed. Variable [Link].
S int (float (i) / float (nitot) +1). [Link]-i-2*nph. and.
S abs (error). lt. dabs (loc b (ii) ) ) then
error = loc b (i)
error loc = i
endilf
endido
goto 908
DO = (netot 4-2*nph) *Intot, IORDER-2^nph-1
! find tax oc state error
if (abs (error). lt. dabs (loc b (ii) )) then
error = loc b (i) find tax error
error loc = i
endilf
endidio
goto 900 find the location of the error index
5 OO DO I= (IORDER-2*nph-1), IORDER find Inax oc title error
if (abs (error). lt. dabs (loc b (i) )) then
error = loc b (i) find sax error
error loc = i
endilf
endido
goto 900 find the location of the error index
9 OO err loc Var Inod (error loc-, nitot) +1
err loc ele int (float (error loc) / float (intot) ) + 1
retur
end

Subroutine Evaluate Convergence (totCnt, err best

trijid X, correction vector, b
Sol, Sol best, refactor Imatrix
int converg eval)
Implicit None
Include ' - . . includel Wifc prob order ré.inc
Include ' - . . include Wifc. Ilass ré. inc'
Include ' - . . include wifc controls ré.inc
 5,435,503
179 180
inputs
integer totcnt, trijidx
logical refactor Imatrix
real 8 b (*), sol (*), Sol best (*), correction vector (*)
C I/o
real * 8 err best
C Output
integer int converg eval
local
Integer int durn, iccC/0/, err flu ele
S, loc ele err Illax, locvar err Inax
S, loc err lax, i,j
S, get avg err/1/, get Inax err/2/
real * 8 err avg., err Inax, Inaxiiiu II error/500000/
logical converged last Cycle/. false. /
call evaluate error (get avg err, i, b
S, erraVg, i, i , i.)
call evaluate error (get Inax err, i, b
S, err Illax, loc ele err Inax, loc Var err Inax
S, loc err tax)

if (sol (iorder). lt. final Conver time

S ..and. (nph. eg. 1)) conver (1) = final conver crit
if (dabs (err Inax) .gt. Inaximum exror) then
int converg eval = 4
! stop the newton raphson process
elseif (dabs (err tax) .lt. conver (nph)) then
int converg eval F. 1 ! Sol is good enough leave it
do i = 1, iorder reset the newton Scale factor since
! We converge
newton scale factor (i) = 1. 0d.O
endido
converged last cycle = . true.
elseif (dabs (err Illax) . le. dabs (err best)) then
getting better
do i = 1, iOrder
if (converged last cycle
S . and. (newton scale factor (i). le. 0.25)) then
newton Scale factor (i) = newton scale factor (i) * 4.0
elseif (newton Scale factor (i) . lt. 1. Od0) then
newton scale factor (i) = newton scale factor (i) * 2.0
endilf
endido
err best = err Inax
int converg eval = 2
recalculate a new correction vector
converged last cycle = . . true.
else diverging Scale factor needs to be reduced
int converg eval = 2
! recalculate a new correction vector
 5,435,503
181 182
converged last cycle = . false.
endif

if ( ([Link]. Illax nuII of iterations (nph))

S ..and. (int converg [Link]. 1)
S ..and. (int converg [Link]. 4)) int converg evai = 5
return
end

SUBROUTINE PSITER (KSW, IRTOT, IJTOT, PSI, PSIX, PSIT, A, B, IRN, ICN)
THIS ROUTINE CAICULATES THE ERROR VECTOR OR JACOBAN ELEMENTS
CORRESPONDING TO THE PRESCRIBED BOUNDARY CONDITIONS
IMPLICIT DOUBLE PRECISION (A-H, O-Z)
Include ' - . . include wifc prob order ré.inc
Include ' - . . include wifc Solre...inc
DOUBLE PRECISION
PS (Inx nu), PSIX (IX bc, IX nu), PSIT (six riph, Inx nu)
DIMENSION A (*), B (*)
INTEGER IRN (*) , ICN (*)
IF (KSW . EQ. O) CALL INDEX5 (IRTOT, IJTOT, IRN, ICN)
IF (KSW .EQ. 1) THEN
CCCCCCCCCCCCCCCCC BEGIN ERROR VECTOR FILL IN CCCCCCCCCCCCCCC
IL = IRTOT

DO 10 = 1, NBC
B (IL--I) = - PSI (I)
10 CONTINUE

RTOT IL + NBC
ccCCCCCCCCCCCCCCC END ERROR VECTOR FILL IN CCCCCCCCCCCCCCCCC
ENDIF

IF (KSW .EQ. 2) THEN

CCCCCCCCCCCCCCCCC BEGIN JACOBAN FL IN CCCCCCCCCCCCCCCCCCCCCC

J=JTOT

DO 20 = 1, NPH
IPSIST= 2*NS* (I-1)
NJ-0
DO 30 J-1, NBC
IDO 40 K=1, NS
INJ = INJ--
A (IJ--INJ) =PSIX (IPSIST+K, J)
A (IJ--INJ--NS) =PSIX (IPSIS.--K--NS, J)
40 CONTINUE
 5,435,503
183 184
J=IJ-2* IS-1
NJ = 0
A (IJ) =PSIT (I,J)
CONTINUE
2O CONTINUE

JTOT=IJ

CCCCCCCCCCCC END JACOBIAN FILL IN CCCCCCCCCCCCCCCCCCCCCC

ENDF

RETURN
END

SUBROUTINE TIMTER (KSW, IRTOT, IJTOT, PHIT, PSIT, A, B, IRN, ICN)

THIS ROUTINE FILLS IN THE ERROR VECTOR OR JACOBIAN ELEMENTS
FOR THE EQUATIONS DESCRIBING THE JUMPS IN THE HAMILTONIAN
BETWEEN PHASES AND THE FINAL BOUNDARY CONDITION ON THE
HAMILTONIAN

IMPLICIT DOUBLE PRECISION (A-H, O-Z)

Include ' -- include Wifc proborder ré.inc
Include ' -- includel wifc Sol ré.inc
DOUBLE PRECISION X (IX x), U (Inx u), LAM (Ex. x),
& L (0 : Inx lux, 0:Inx lux), F (IX eqn, 0:IX ux, 0:Inx lux)
DOUBLE PRECISION PHIT (mx_nph), PSIT (Inx nph, Inx nu)
S, CAPHIT (mx nu)
DIMENSION A (*), B (*)
INTEGER IRN (*), ICN (*), EN

IF (KSW .EQ. O). CALL INDEX6 (IRTOT, IJTOT, IRN, ICN)

IF (KSW .E.O. 1) THEN
CCCCCCCCCCCCC BEGIN ERROR VECTOR FIL IN CCCCCCCCCCCCCCCCCCC
ILeIRTOT

C DEFINE CAPHIT = D (PHI)/DT + NUT * D (PSI)/DT

DO 5 J-1, NPH
SUM= 0 . OD-OO
DO 7 Is 1, NBC
SUM=SUM+PSIT (J, I) * GNU (I)
CONTINUE
CAPHIT (J) =PHIT (J) +SUM
CONTINUE
en = 0
DO 10 I=1, NPH
IF (I .NE. NPH) THEN
We AT PHASE BOUNDARIES, H(t+)-H(t-) - CAPHIT = O
DO 11 J = 1, N
X (J) = XHAT (J, 2, I)