' program demo_nle.bas April 30, 2017

' this program demonstrates the procedure for calling
' subroutine nlbrent which solves an unconstrained
' system of non-linear equations using brent's method.

' nlbrent requires a subroutine called userfunc which
' returns the function values of a user-defined system
' of non-linear equations.

' this program demonstrates the use of nlbrent to solve
' the system of two non-linear equations which define the
' relationship between geocentric and geodetic coordinates.

' (c + altitude) * cos(lat) - rmag * cos(decl) = 0

' (s + altitude) * sin(lat) - rmag * sin(decl) = 0

'''''''''''''''''''''''''''''''''''''''''''''''''''

option default float

option base 1

const dtr = pi / 180.0 ' degrees to radians
const rtd = 180.0 / pi ' radians to degrees
const req = 6378.14 ' equatorial earth radius
const flat = 1.0 / 298.257.0 ' earth flattening factor
const rpolar = req * (1.0 - flat) ' polar earth radius

dim rmag, decl

' define number of variables

nvar% = 2

' dimension solution vector

dim x(nvar%), fvec(nvar%)

do
print " "
print "please input the geocentric declination (degrees)"
print "(-90 degrees <= declination <= 90 degrees)"
input decl_deg
loop until (abs(decl_deg) >= 0.0 and abs(decl_deg) <= 90.0)

decl = decl_deg * dtr

x(1) = decl

do
print " "
print "please input the geocentric radius (kilometers)"
input rmag
loop until (rmag > rpolar)

x(2) = rmag - rpolar

do
print " "
print "please input the maximum number of iterations"
print "( a value of between 20 and 50 is recommended )"
input maxiter%
loop until (maxiter% > 0)

do
print " "
print "please input the convergence criteria"
print "( a value of 1.0e-8 is recommended )"
input tol
loop until (tol > 0.0)

' solve the system of nonlinear equations

nlbrent(nvar%, x(), fvec(), tol, maxiter%, info%, nfev%)

print " "
print "program demo_nle - solution of a non-linear system"
print "=================================================="
print " "

select case info%

case 1, 2, 3

print " "
print "geocentric declination "; str$(decl_deg, 0, 6); " degrees"
print " "
print "geocentric radius "; str$(rmag, 0, 6); " kilometers"

print " "
print "geodetic latitude "; str$(x(1) * rtd, 0, 6); " degrees"
print " "
print "geodetic altitude "; str$(x(2), 0, 6); " kilometers"
print " "

for i% = 1 to nvar%

print " "

print "value of residual #"; i%; " "; str$(fvec(i%), 0, -16)

next i%

print " "
print "number of iterations "; nfev%
print " "
print "convergence criterion "; tol
print " "

case 4

print
print "maximum number of iterations exceeded!"

case 5

print " "
print "approximate jacobian matrix is singular!"

case 6

print " "
print "iteration is not making good progress!"

case 7

print " "
print "iteration is diverging!"

case 8

print " "
print "iteration is converging, but tol is too small,"
print "or the convergence is very slow due to a jacobian"
print "singular near the output x or due to badly scaled"
print "variables!"

end select

end

'''''''''''''''''''''''''''''''''''''''''''''''''''''
'''''''''''''''''''''''''''''''''''''''''''''''''''''

sub nlbrent(n%, x(), fvec(), tol, maxfev, info%, nfev%)

' brent's solution of a non-linear system subroutine

' input

' n% = number of equations
' x() = initial guess for solution vector
' tol = convergence tolerance
' maxfev = maximum number of function evaluations allowed

' output

' x() = solution vector
' fvec() = residual vector
' info% = information flag
' 1 = all residuals are at most tol in magnitude
' 2 = relative error between two sucessive iterates
' is at most tol
' 3 = conditions for both info% = 1 and info% = 2 hold
' 4 = number of function evaluations >= maxfev
' 5 = approximate jacobian matrix is singular
' 6 = iteration is not making good progress
' 7 = iteration is diverging
' 8 = iteration is converging, but tol is too small,
' or the convergence is very slow due to a jacobian
' singular near the output x or due to badly scaled
' variables
' nfev% = number of actual function evaluations

' note: requires sub userfunc

'''''''''''''''''''''''''''''

local sigma(n%), wa1(n%), wa2(n%), q(n%, n%)

' initialization

epsmch = 2.2e-16

p05 = 0.05

scale = 10.0

ftol = tol
xtol = tol

info% = 0
nfev% = 0
nfcall% = 0

nier6% = -1
nier7% = -1
nier8% = 0

fxnorm = 0.0
difit = 0.0
xnorm = 0.0

for i% = 1 to n%

if (abs(x(i%)) > xnorm) then xnorm = abs(x(i%))

next i%

eps = sqr(epsmch)

delta = scale * xnorm

if (xnorm = 0.0) then delta = scale

' estimate number of iterative refinements

emax = 0.0

for i% = 1 to n%

temp = log(i% + 1.0) / (n% + 2.0 * i% + 1.0)

if (temp < emax) then exit for

mopt% = i%

emax = temp

next i%

' principal iteration loop

do

nsing% = n%

fxnorm1 = fxnorm

difit1 = difit

fxnorm = 0.0

' determine divided difference step

h = eps * xnorm

if (h = 0.0) then h = eps

for j% = 1 to n%

for i% = 1 to n%

q(i%, j%) = 0.0

next i%

q(j%, j%) = h

wa1(j%) = x(j%)

next j%

' perform subiteration

for k% = 1 to n%

userfunc(wa1(), fvec())

fky = fvec(k%)

nfcall% = nfcall% + 1

nfev% = nfcall% / n%

if (abs(fky) > fxnorm) then fxnorm = abs(fky)

for j% = k% to n%

for i% = 1 to n%

wa2(i%) = wa1(i%) + q(i%, j%)

next i%

userfunc(wa2(), fvec())

fkz = fvec(k%)

nfcall% = nfcall% + 1

nfev% = nfcall% / n%

sigma(j%) = fkz - fky

next j%

fvec(k%) = fky

eta = 0.0

for i% = k% to n%

if (abs(sigma(i%)) > eta) then eta = abs(sigma(i%))

next i%

if (eta <> 0.0) then

nsing% = nsing% - 1

sknorm = 0.0

for i% = k% to n%

sigma(i%) = sigma(i%) / eta

sknorm = sknorm + sigma(i%) ^ 2

next i%

sknorm = sqr(sknorm)

if (sigma(k%) < 0.0) then sknorm = -sknorm

sigma(k%) = sigma(k%) + sknorm

for i% = 1 to n%

wa2(i%) = 0.0

next i%

for j% = k% to n%

temp = sigma(j%)

for i% = 1 to n%

wa2(i%) = wa2(i%) + temp * q(i%, j%)

next i%

next j%

for j% = k% to n%

temp = sigma(j%) / (sknorm * sigma(k%))

for i% = 1 to n%

q(i%, j%) = q(i%, j%) - temp * wa2(i%)

next i%

next j%

sigma(k%) = sknorm * eta

temp = fky / sigma(k%)

if (h * abs(temp) > delta) then temp = sgn(temp) * (delta / h)

for i% = 1 to n%

wa1(i%) = wa1(i%) + temp * q(i%, k%)

next i%

end if

next k%

xnorm = 0.0

difit = 0.0

for i% = 1 to n%

if (abs(wa1(i%)) > xnorm) then xnorm = abs(wa1(i%))

if (abs(x(i%) - wa1(i%)) > difit) then difit = abs(x(i%) - wa1(i%))

x(i%) = wa1(i%)

next i%

if ((scale * xnorm) > delta) then delta = scale * xnorm

if ((fxnorm < fxnorm1) and (difit < difit1) and (nsing% = 0)) then iconv = 1

nier6% = nier6% + 1
nier7% = nier7% + 1
nier8% = nier8% + 1

if (iconv = 1) then nier6% = 0

if ((fxnorm < fxnorm1) or (difit < difit1)) then nier7% = 0

if (difit > (eps * xnorm)) then nier8% = 0

' convergence tests

if (fxnorm <= ftol) then info% = 1

if ((difit <= (xtol * xnorm)) and (conv = 1)) then info% = 2

if ((fxnorm <= ftol) and (info% = 2)) then info% = 3

if (info% <> 0) then exit do

' termination tests

if (nfev% >= maxfev) then info% = 4

if (nsing% = n%) then info% = 5

if (nier6% = 5) then info% = 6

if (nier7% = 3) then info% = 7

if (nier8% = 4) then info% = 8

if (info% <> 0) then exit do

if ((iconv = 0) or (difit > p05 * xnorm) or (mopt% = 1)) then

' null

else

' perform iterative refinement

for m = 2 to mopt%

fxnorm1 = fxnorm

fxnorm = 0.0

for k% = 1 to n%

userfunc(wa1(), fvec())

fky = fvec(k%)

nfcall% = nfcall% + 1

nfev% = nfcall% / n%

if (abs(fky) > fxnorm) then fxnorm = abs(fky)

if (fxnorm >= fxnorm1) then

fxnorm = fxnorm1

exit for

end if

temp = fky / sigma(k%)

for i% = 1 to n%

wa1(i%) = wa1(i%) + temp * q(i%, k%)

next i%

next k%

xnorm = 0.0

difit = 0.0

for i% = 1 to n%

if (abs(wa1(i%)) > xnorm) then xnorm = abs(wa1(i%))

if (abs(x(i%) - wa1(i%)) > difit) then difit = abs(x(i%) - wa1(i%))

x(i%) = wa1(i%)

next i%

' stopping criteria for iterative improvement

if (fxnorm <= ftol) then info% = 1

if (difit <= (xtol * xnorm)) then info% = 2

if ((fxnorm <= ftol) and (info% = 2)) then info% = 3

if ((nfev% >= maxfev) and (info% = 0)) then info% = 4

next m

end if

loop

end sub

''''''''''''''''''''''
''''''''''''''''''''''

sub userfunc (x(), fx())

' user-defined system of non-linear equations subroutine

' (c + altitude) * cos(lat) - rmag * cos(decl) = 0

' (s + altitude) * sin(lat) - rmag * sin(decl) = 0

'''''''''''''''''''''''''''''''''''''''''''''''''''

' input

' x() = function argument vector

' output

' fx() = array of function values evaluated at x

'''''''''''''''''''''''''''''''''''''''''''''''''

local xlat, altitude, c, s

xlat = x(1)

altitude = x(2)

c = req / sqr(1.0 - (2.0 * flat - flat ^ 2) * sin(xlat) ^ 2)

s = c * (1.0 - flat) ^ 2

fx(1) = (c + altitude) * cos(xlat) - rmag * cos(decl)

fx(2) = (s + altitude) * sin(xlat) - rmag * sin(decl)

end sub