AN EMBEDDED FUNCTION TOOL FOR MODELING AND SIMULATING ESTIMATION PROBLEMS IN AEROSPACE ENGINEERING

Transcription

1 AAS -8 AN EMBEDDED FUNCTION TOOL FOR MODELING AND SIMULATING ESTIMATION PROBLEMS IN AEROSPACE ENGINEERING INTRODUCTION D. Todd Griffith *, James D. Turner, and John L. Junkins An automatic differentiation-based embedded function tool, OCEA (Object Oriented Coordinate Embedding Method), is presented for solving common estimation problems in Aerospace Engineering. The orbit determination and ballistic projectile parameter estimation problems have been chosen as examples. OCEA is extremely useful for computing n th order partial derivatives of scalar, vector, matrix, and higher dimension tensor functions for these applications. Both applications consider algorithm performance and robustness issues associated with applying high order generalizations of the classical firstorder optimization and estimation algorithms. OCEA-based tools are expected to have broad applicability for Aerospace problems in particular and engineering problems in general. An automatic differentiation-based embedded function tool is presented for solving some common estimation problems in Aerospace Engineering. The two common Aerospace Engineering problems include orbit determination and aircraft parameter estimation. The embedded tool, OCEA (Object Oriented Coordinate Embedding Method), has broad potential for solving engineering design and optimization problems. - 6 OCEA is extremely useful for computing nth order partial derivatives of scalar, vector, matrix, and higher dimension tensor functions. A considerable advantage is found for applications requiring partial derivative calculations (e.g. gradient, Jacobian, Hessian, state transition matrices). Hidden operator overloading tools completely free the analyst from the time consuming and error prone tasks of deriving, coding, and validating analytical partial derivative models. The user merely needs to define the embedded functions for the problem, using the standard programming language tools. The partial derivatives are automatically computed and evaluated. The user can select one through fourth order partial derivative models. Extensive use of operator overloading provides a greatly simplified modeling development environment, because vector, matrix, and tensor * Graduate Research Assistant, Department of Aerospace Engineering, Texas A&M University, College Station, TX 778-, griffith@tamu.edu, Student member AAS and AIAA. Adjunct Faculty, Department of Aerospace Engineering, Texas A&M University, and President Amdyn Systems, White GA 8. George Eppright Chair, Distinguished Professor, Department of Aerospace Engineering, Texas A&M University, College Station, TX 778-, junkins@tamu.edu, AAS and AIAA fellow. Copyright (c) by the authors. Permission to publish granted to The American Astronautical Society.

2 equations can be expressed and manipulated in a form that closely resembles the way an analyst derives the results by hand. The first problem is a ballistic projectile identification problem which involves estimation of pitch and yaw angles. Computation of the required Jacobian for the first order method requires considerable attention for a problem with many unknown parameters. Computation of the Hessian, and higher order Hessians would typically not be attempted; however, these higher order methods are quickly and easily implemented in OCEA. At the heart of this problem lies the desire to estimate parameters for nonlinear systems. The ability to implement higher order methods is very promising for expediting convergence rates for any system, especially nonlinear systems, without the overhead of computing partial derivatives by hand or using mathematical software packages. Orbit determination is the second problem. The objective here is to determine the orbit of a spacecraft from range and line-of-sight measurements. The goal of orbit determination is to determine the initial conditions of the orbit and obtain parameter estimates for quantities such as drag and perturbing accelerations. The procedure here is no different than the well known Gaussian Differential Correction method; however, by using OCEA, considerable time is saved by the analyst because the partial derivatives required in the calculation of the state transition matrices need not be derived and coded by hand. The analyst simply specifies the dynamical model (an embedded function) and the measurement model (an embedded function) and proceeds with the simulation. Tedious Calculus and Algebra is not required. The analyst can focus his time on the algorithm itself. Advanced optimization algorithms are considered for first- through fourth-order generalized state transition matrix algorithms. Both applications consider algorithm performance and robustness issues associated with applying high order generalizations of the classical first-order optimization and estimation algorithms. OCEA-based tools are expected to have broad applicability for Aerospace problems in particular and engineering problems in general. OVERVIEW OF OCEA AND AUTOMATIC DIFFERENTIATION The computational tool used in this paper is the OCEA (Object Oriented Coordinate Embedding Method) extension for FORTRAN9 (F9). The OCEA package is an object-oriented equation manipulation package. OCEA defines embedded variables that represent abstract data types. OCEA replaces each scalar variable in the problem with a differential n-tuple consisting of the following variables for a second-order OCEA method: f f f f () : =

3 where and denote first- and second-order gradient tensors w.r.t. a user-defined set of independent variables. The introduction of the abstract differential n-tuple allows the computer to continue to manipulate each scalar variable as a conventional scalar variable, even though the first- and higher-order partial derivative are attached to the scalar variable in a hidden way. The individual objects are extracted using structure constructor types (%) as follows: f = f % E, f = f % V, and f = f % T. The automatic computation of the partial derivatives is achieved by operator-overloading methodologies that redefine the intrinsic mathematical operators and functions using the rules of calculus. For example, addition and multiplication are redefined as follows. : a + b = a + b a + b a + b () ( ) ( ) a * b : = a* b i a* b j i a * b () Additional operations for the standard mathematical library functions, such as trigonometric and exponential functions, are redefined to account for the known rules of differentiation. In essence, this approach pre-codes, once and for all, all of the partial derivatives required for any problem. At compile time, and without user intervention, the OCEA-based approach links the subroutines and functions required for evaluating the system and partial derivative models. REVERSION OF SERIES SOLUTION In this section, we present the reversion of series solution, which was previously reported by Turner. 5,6 This solution provides the correction to be applied to the current guess of the unknown parameters in the estimation problem, where g(x) = defines the necessary condition for the root of the equation. In order to develop the reversion of series solution, the necessary condition is defined as a root solving problem with the following parameter embedding problem : G( x( s), s) = g( x( s)) sg( x guess ) = () where s is a scalar embedding parameter, x guess is the starting guess, and x = x(s). The reversion of series solution is given by dx d x d x d x x xguess + δ = xguess (5) ds! ds! ds! ds g g g g

4 The differential rates appearing in Eq. (5) are obtained repeatedly differentiating G(x(s),s) w.r.t. s. The developments leading to computing these rates are presented in Reference 5 and are repeated here for completeness. The first through fourth order terms are given by dx ds s= = ( G) h( x ) guess d x dx dx = ( G) G ds ds s s= ds = s= dx dx dx d x dx G + G + ds s= ds s= ds s= ds ds s= s= = ( G) ds s= dx d x G ds s= ds s= d x dx dx dx dx G + ds s= ds s= ds s= ds s= d x dx dx G + ds s ds s ds = = s= d x dx d x dx = ( G) G + (6) ds ds s s ds ds = = s= s= dx dx d x d x dx G + G + ds s= ds s= ds ds ds s= s= s= d x d x dx d x G + G ds ds ds s= s= s= ds s= The gradient terms ( G, G, and so on) are understood to be taken with respect to the unknown parameters to be estimated. Equations (5) and (6) is used to update the state estimate. These gradient terms, or better yet sensitivities, are explained in greater detail in the estimation algorithm section. Weight Matrix Issues We note here that some special attention must be given to weighting observations for higher order corrections. For uncorrelated measurements, the optimal choice for

5 weighting is wi σ = i where σ i is the variance of the i th measurement. This weighting approach results in a diagonal weight matrix, W, which can be factored by the Cholesky T T Decomposition as W = LL. Thus, for the optimal choice, L = L = diag(/ σ i ). The simplest way to implement weighted observations is to pre-multiply the observations and the measurement model predictions, which in this work includes the evaluations of the T measurement model and it s first and higher order partial derivatives, by L. NONLINEAR LEAST SQUARES Review of Nonlinear Least Squares It is a fact of life that most estimation problems are nonlinear. A description of the Nonlinear Least Squares algorithm can be found in many books on estimation 7. In summary, given a set of observations or measurements and a model for these measurements, the task is to estimate a set of measurement model parameters which best fit the observations. For a nonlinear problem, an iterative solution procedure must be employed. First, a starting guess for the unknown model parameters is supplied, and one iteration of the algorithm produces a correction to the starting guess. This process repeats until the estimate for the model parameters has converged. Of course, the algorithm is considered to have failed if the residual errors increase during the iteration process or the residual error remains essentially unchanged for many iterations of the algorithm. Typically, one begins in finding the Least Squares estimate ˆx which minimizes the following cost function J T = ( y h( xˆ )) W ( y h( xˆ )) (7) where ỹ is the vector of measurements, h( x ˆ) is the measurement model, and W is an assumed weighting matrix. Taking the gradient of Eq. (7) w.r.t. the unknown state ˆx results in the necessary condition for minimizing the cost function, leading to: x h T W ~ = ( y h( xˆ )) which is equivalent to the necessary condition of Eq. (). Linearization of the h measurement model produces h( xˆ ˆ k + ) = h( xk ) + xk, which when substituted into x the necessary condition produces the well known normal equations. xˆ ( ) y (8) T T k = H WH H W k xˆ k 5

6 where H h = x xˆ k and y = y h( xˆ ). The normal equations represent exactly the first k k equation given in Eq. (6), which is the first order correction term. In the remainder of this paper, the higher order solutions given in Eq. (6) will be used to compute the updated state as x ˆ = x ˆ + x ˆ (9) k+ k k The automatic differentiation capability of OCEA is well suited for computing the sensitivities for the Nonlinear Least Squares algorithm. These sensitivities are the first through fourth order partial derivatives of the measurement model with respect to the unknown model parameters. The benefit for the analyst is quite significant with respect to the time saved in computing these sensitivities by hand or by symbolic manipulation. As well, the analyst is freed from validating and hard coding these partial derivative expressions. This capability is particularly advantageous when there are a large number of model parameters to be estimated. For example, given a measurement model with n unknown model parameters, n partials are required for a first order correction model. When going to higher order, the number of partial derivatives to be computed explodes to n o where o is the order of the correction model. In addition, the benefit of having the capability to change the measurement model without recomputing or revalidating the sensitivities cannot be overstated. An example is presented in the following section. Ballistic Projectile Identification Example As an example, consider the orientation an aerodynamically and inertially symmetric projectile. Along the trajectory, measurements are taken of the pitch and yaw angles. The following model is assumed for the pitch and yaw angles, respectively λ λ θ ( t, x) = h ( t, x) = k e cos( ω t + δ ) + k e cos( ω t + δ ) t t λ + k e cos( ω t + δ ) + k t () λ λ ψ ( t, x) = h ( t, x) = k e sin( ω t + δ ) + k e sin( ω t + δ ) t t λ + k e sin( ω t + δ ) + k t 5 () where = ( k, k, k, k, k, λ, λ, λ, ω, ω, ω, δ, δ, δ ) x are the unknown 5 constant model parameters. Therefore, first through fourth order sensitivities would require,,, and partial derivatives to be computed and validated. Future 6

7 research will dramatically reduce the number of higher-order partial derivatives required, by exploiting the symmetry and sparsity of the gradient tensor operators. The Nonlinear Least Squares algorithm described above can be coded once and for all. In order to solve the problem at hand, only the measurement model and starting guess must be specified. For this aircraft identification problem, the FORTRAN 9 subroutine containing the measurement model given in Eqs. (7) and (8) is shown Appendix A. An important remark to be made about the measurement model subroutine given in Appendix A is that the analyst can invoke automatic differentiation by standard FORTRAN programming. A USE statement (USE EB_HANDLING) is included in order to invoke the automatic differentiation tool. The embedded variables (the model parameters) and embedded functions (the measurement model) are defined as embedded objects and coded using standard FORTRAN arithmetic operators. The output of this subroutine is the measurement model, and the gradient and higher order partials, evaluated at the current state and time. The automatic differentiation capability makes higher order computational methods readily available. Results for norms of the measurement residuals for first and second order solutions for the aircraft identification example are shown in Table. Table. Cost for Ballistic Projectile Identification Problem Iteration First order Second order.65e+.65e+.788e+.676e+.59e+.e+.65e+.9e E-.9E E E- The results of Table show that for an initial guess of more than 5% error, the first order method converges in 7 iterations, while the second order method shows rapid convergence in 5 iterations. GAUSSIAN LEAST SQUARES DIFFERENTIAL CORRECTION Higher-order Generalizations of GLSDC The problem of determining the orbits of the heavenly bodies has been studied in great detail for many hundreds of years. Just over years ago, Gauss devised a method for solving this problem which bears his name, Gaussian Least Squares 7

8 Differential Correction or GLSDC. The essence of this method involves estimating the position and velocity at some time, usually the initial time in which the first measurement was viewed. Along with a model for the dynamics of the body of interest, the complete orbit can be reconstructed with the estimated position and velocity at some time. In addition, uncertain model parameters can be estimated. These parameters include force model parameters such as drag, solar radiation pressure, and gravitational constants. One aim of this paper is to present higher order solution methods within the GLSDC framework. In addition, the necessary higher order derivatives are shown to be computed automatically using automatic differentiation. As was the case for the Nonlinear Least Squares algorithm, the result is a general computation tool, which can be coded once and for all, in this case for validating dynamical models by a set of measurements of the state of the system at various times. The following sections present the GLSDC algorithm. The first order GLSDC algorithm 7 is a standard topic in many textbooks on estimation; however, it will be presented again in order to proceed logically to the second and higher order GLSDC algorithms. Simply put, the distinction between a Nonlinear Least Squares problem and a GLSDC problem is that in the latter case the measurements are chosen to fit a dynamical model as opposed to a set of algebraic equations. Both the dynamical model and the measurement model are nonlinear functions of the state and force model parameters. The dynamical model and measurement models are written in general in Eqs. () and (), respectively. x ( t) = f ( t, x( t)); x = x( t ) () y = h( x ( t)) () Here, the state vector x contains the position and velocity states as well as any force model parameters in the model. A nonlinear estimation problem is defined because the dynamics and measurement models are nonlinear functions of the state and force model parameters. Therefore, an iteration procedure identical to that of Nonlinear Least Squares is utilized as is done in Eq. (9). Unlike the Nonlinear Least Squares problem, GLSDC is an explicit function of the unknown parameters initial position and velocity parameters. Therefore, the sensitivity calculations require state transition matrix calculations. First, we will begin with the necessary sensitivity calculations. For the time being, we focus on the first and second order differential corrections. The reversion of series provides the following corrections for first and second order, respectively, which follow from the first two equations of Eq. (6). x = ( h) y () x o 8

9 x = ( h) y ( h) i( h) i( h) yi ( h) y (5) x x x x x o o o o o It can be seen that first and second order sensitivities are needed in Eq. (5), and are computed as follows: h h x( t) x h = = = Φ( t, t) o xh x( t ) x( t) x( t ) (6) h = x x x o o o h h x( t) x( t) h x( t) = i i + i x( t) x( t ) x( t ) x( t) x( t ) = xhiφ t tiφ t t + xhiφ t t t (, ) (, ) (,, ) (7) where the symbol Φ ( t, t) represents a first-order state transition matrix and Φ( t, t, t) denotes a second-order state transition matrix. These sensitivities are written here in vector/matrix form for simplicity. A more thorough interpretation of how to carry out the implied multiplications in the first through fourth order sensitivity calculations is given in Appendix B in indicial notation. Now we develop the necessary state transition matrix differential equations. For first through fourth order generalizations, we must introduce a number of state transition matrices. In this paper we adopt the following notation for first through fourth order state transition matrices: x( t) First order: Φ ( t, t) = x( t ) Second order: Third order: Fourth order: x( t) = x( t) x( t) = x( t) x( t) = x( t) Φ ( t, t, t ) Φ ( t, t, t, t ) Φ ( t, t, t, t, t ) The state transition matrices must be computed at each instant in time by solving differential equations associated with each. These state transition matrices are developed in the following by beginning with the integral form of Eq. (). 9

10 t = o + to x( t) x( t ) f ( τ, x ( τ )) dτ (8) We then differentiate Eq. (8) with respect to the initial state to compute t x( t) f ( τ, x( τ )) x( τ ) Φ ( t, to) = = I + dτ x( to ) ( to) t x x o (9) The first order state transition matrix differential equation is obtained upon time differentiation of Eq. (9). ( t, ( t)) Φ f x ( t, to) = Φ( t, to ) () x Upon differentiating Eqn. (9) once again with respect to the initial state, and then time differentiating this expression, we arrive at the following second order state transition matrix differential equation. ( t, ( t)) ( t, ( t)) Φ f x f x ( t, to, to) = Φ ( t, to, to) + Φ( t, to ) Φ( t, to) ( t) i i i () x x( t) Again, it is obvious that the state transition matrix differential equations can be extended to higher order by continuing along the path described above. Developments in indicial notation are given for first through fourth order state transition matrix differential equations in Appendix C. The utility of automatic differentiation is even more profound for the GLSDC algorithm. Here, the measurement model must be differentiated in order to compute the sensitivities, and the dynamical equations must be differentiated in order to solve the state transition matrix differential equations. The GLSDC algorithm can be summarized as follows:. Given measurements, ỹ, and an intial guess for the state, ˆx.. Integrate state transition differential equations along with dynamical equations until the next measurement is available: Eqs. () and (), and ().. Compute sensitivity for each measurement time: Eqs. (6) and (7).. Once final measurement time is reached, compute differential correction: x ˆ k. 5. Update state: x ˆ = x ˆ + x ˆ k+ k k 6. Check convergence.

11 7. If not converged, then repeat steps If converged, then done Orbit Determination Example In order to demonstrate higher order corrections for the GLSDC algorithm, we consider as an example the planar motion of projectile in a constant gravity field. We also consider a quadratic drag model of the form fdrag = p V V where p is the drag constant, V = [ x x ] is the velocity vector, and V is the magnitude of velocity. The equations of motion in first order form are thus given by x x x x x = x px = V x g px V p () Range and line of sight measurements are taken along the projectile s trajectory. The measurement model is given by r x + x h = = () θ tan ( x / x ) OCEA automatically computes the partial derivatives of Eq. () which are required for integrating the state transition matrix differential equations given in Appendix C, and also computes the partial derivatives of Eq. () required for the sensitivity calculations shown in Appendix B. Therefore, a generalized estimation tool can be coded once and for all since partial derivatives need not be hand coded for each dynamics model or measurement model. For a particular problem, the analyst can simply change these models and the required sensitivities are automatically computed. The objective here is to estimate the initial state whose true value is given by x = m 5m m / s m / s. It is assumed that the standard deviation of [ ] the range measurement and the line of sight measurement is meters and. rad, respectively. For these simulations, the projectile is observed for a total of seconds at second intervals. Results are shown for two cases: I) without drag and II) with drag. When drag is present, the equations of motion are nonlinear as seen in Eq. (); however, without drag this is a linear system. It can be observed from Appendix C that for a linear system, second and higher order state transition matrices are zero for all time; however, first order state transition matrices are not. Therefore, second and higher order

12 sensitivities are not zero since they are a function of the first order state transition matrices as can be seen in Appendix B. In each of the cases, we are primarily interested in evaluating rate of convergence and domain of convergence, or put another way, we want to know how fast the algorithms converge and from how poor of a guess they will converge. Case I: Zero Drag Trajectory Here we consider the case when the drag parameter p is removed from Eq. () and we estimate only the initial position and velocity. Since the measurements are a function of only position (no velocity dependence), we expect to have a better guess for the initial position than the velocity. For this reason and practical issues dealing with the large number of possible guesses, we simulate the first and second order algorithms with an initial position guess in % error of the truth and varied initial velocity guess error. The results for the number of iterations required for convergence for the first and second order GLSDC algorithms are given in Table. The stopping criterion used for these simulations is 6 digit consistency of the measurement residual error. Table. Convergence Study for Case I Initial guess First Order Iteration Count Second Order Iteration Count.9 x true,. ẋ true 5.9 x true,.9 ẋ true 5.9 x true,.8 ẋ true 5.9 x true,.7 ẋ true 5.9 x true,.6 ẋ true 5.9 x true,.5 ẋ true 6.9 x true,. ẋ true x true,. ẋ true x true,. ẋ true x true,. ẋ true x true,. ẋ true 8 6 The results of Table show that for a large practical range of poor guesses in the initial velocity the second order algorithm converges in - fewer iterations. The state convergence history for an initial velocity guess of. ẋ true is given in Table for the first order algorithm and in Table for the second order algorithm.

13 Table. First Order Algorithm State History Results Iteration X Z Ẋ Ż Cost Table. Second Order Algorithm State History Results Iteration X Z Ẋ Ż Cost Case II: Drag Trajectory Now we look at the case of estimating five states including initial position and velocity, and the drag parameter. The results of Table 5 show that the first-order algorithm converges faster than the second-order algorithm with the exception of starting guesses very close to the truth. For Case II, the trajectory is very sensitive to the estimate of the drag parameter on the first iteration. The first-order algorithm consistently produces a better drag parameter estimate on the first iteration, which results in faster convergence. One would expect that a second-order algorithm would show rapid convergence near the solution. The results here indicate that algorithm performance is problem dependent. Second-order algorithms are well known to have some potential difficulties related to reduction in sensitivity and diminished domain of convergence. Whereas first-order algorithms are typically insensitive to the starting guess, second-order algorithms can have a diminished domain of convergence because some starting guesses outside the

14 region of convergence have second-order sensitivity (or curvature) which has the wrong sign. Simply put, if the initial guess lies where the curvature is wrong, the predicted corrections to the state are not necessarily in the correct direction. Convergence depends upon the ability to extrapolate from one state to another state with a reduction in the performance index, which does not happen when the curvature has the wrong sign. It is anticipated that simulation of third and fourth-order generalizations of the GLSDC algorithm will provide additional insight. With the second-order method, one term is added to the correction (extrapolation) and there is no guarantee that curvature conditions are satisfied for any choice of initial guess. When state is corrected by including third and fourth-order terms as well, one would expect an improvement in the prediction of the state for the next iteration in the correct direction over the addition of only one additional second-order term..9 x true Table 5. Convergence Study for Case II Initial guess First Order Iteration Count Count,. ẋ true, p =.95*ptrue 5 Second Order Iteration.9 x true,.9 ẋ true, p =.95*ptrue x true,.8 ẋ true, p =.95*ptrue x true,.7 ẋ true, p =.95*ptrue x true,.6 ẋ true, p =.95*ptrue x true,.5 ẋ true, p =.95*ptrue 6 8 Tables 6 and 7 show results for one particular optimistic initial guess in which all states guesses are in 5% error of the truth. The results show that the second-order algorithm produces a better estimate of the drag parameter on the first iteration and converges in one less iteration.

15 Table 6. First Order State Time History Iteration X Z Ẋ Ż p Cost Table 7. Second Order State Time History Iteration X Z Ẋ Ż p Cost

16 CONCLUSION Higher order generalizations of the commonly used Nonlinear Least Squares and Gaussian Least Squares Differential Correction estimation algorithms have been presented in this paper. The automatic differentiation tool, OCEA, was utilized to compute the partial derivatives required in these algorithms. The automatic differentiation capability permits coding these algorithms once and for all. New problems are solved by simply changing the appropriate dynamical and measurement models for the problem. An example for each case was presented. An improvement in convergence was found for the ballistic projectile identification problem with the second order algorithm. Some of the difficulties associated with these higher order generalizations, such as domain of convergence and reduction in sensitivity, were addressed with an orbit determination example. Overall, these higher-order generalizations offer new algorithms which show promise for improving convergence. An important contribution of this paper is the development of the differential equations for higher-order state transition matrices. This development is essential in computing sensitivities for the higher-order GLSDC estimation algorithms. The higherorder state transition matrices will prove useful in higher-order methods for propagation of uncertainty or covariance. Future work includes simulating third and fourth-order estimation algorithms, as well as higher-order methods for propagation of uncertainty. REFERENCES. J. D. Turner, "Quaternion-Based Partial Derivative And State Transition Matrix Calculations For Design Optimization," Paper Presented To th AIAA Aerospace Sciences Meeting And Exhibit, Reno, Nevada, -7 Jan.. J. D. Turner, "Object Oriented Coordinate Embedding Algorithm For Automatically Generating The Jacobian And Hessian Partials Of Nonlinear Vector Functions," Invention Disclosure, University Of Iowa, May.. J. D. Turner, "The Application Of Clifford Algebras For Computing The Sensitivity Partial Derivatives Of Linked Mechanical Systems," Invited Paper Presented To Mini-Symposium: Nonlinear Dynamics And Control, USNCTAM: Fourteenth U.S. National Congress Of Theoretical And Applied Mechanics, Blacksburg, Virginia, USA, June -8,.. J. D. Turner, "Automated Generation Of High-Order Partial Derivative Models, To Appear, AIAA Journal, August. 5. J. D. Turner, "Generalized Gradient Search And Newton's Methods For Multilinear Algebra Root-Solving And Optimization Applications," Invited Paper No. AAS - 6, To Appear In The Proceedings Of The John L. Junkins Astrodynamics Symposium, George Bush Conference Center, College Station, Texas, May -,. 6. J. D. Turner, "Generalized Gradient Search And Newton's Methods For Multilinear Algebra Root-Solving And Optimization Applications," Paper No. AAS -6, To 6

17 Appear In A Special Issue Of The Journal Of The Astronautical Sciences Commenorating The John L. Junkins Astrodynamics Symposium, Held At The George Bush Conference Center, College Station, Texas, May -,. 7. Crassidis, J.L., and Junkins, J.L., An Introduction to Optimal Estimation of Dynamical Systems, Text book in Press, CRC Press, March. 7

18 APPENDIX A: Fortran 9 Nonlinear Least Squares Measurement Model SUBROUTINE NONLINEAR_FX( T, EB_VAR, EB_FCTN )! THIS PROGRAM EVALUATES A VECTOR FUNCTION USING EMBEDDED! PROCESSING.! THE USER INPUTS A VECTOR OF OCEA-INITIALIZED INDEPENDENT VARIABLES! AND EVALUATES A VECTOR FUNCTION.!! INPUT:! EB_VAR: NVx VECTOR OF OCEA-INITIALIZED INDEPENDENT! VARIABLES! OUTPUT:! EB_FCTN: NFx VECTOR OF OCEA-EVALUATED NONLINEAR FUNCTIONS! = [ F, DEL(F), DEL^(F) ] = [function, gradient, hessian]!=====================================! COPYRIGHT (C) JAMES D. TURNER!===================================== USE EB_HANDLING IMPLICIT NONE! ARGUMENT LIST VARIABLES REAL(DP)::T TYPE(EB), DIMENSION(NV), INTENT(IN ):: EB_VAR TYPE(EB), DIMENSION(NF), INTENT(INOUT):: EB_FCTN! DEFINE LOCAL + EMBEDDED VARIABLES REAL(DP), DIMENSION(NF):: FX, DELX REAL(DP), DIMENSION(NF,NV):: JAC REAL(DP), DIMENSION(NF,NV,NV):: HES REAL(DP), DIMENSION(NV,NV):: A TYPE(EB):: K, K, K, K, K5, LAM, LAM, LAM TYPE(EB):: OMEG, OMEG, OMEG, DEL, DEL, DEL! ASSIGN LOCAL VARIABLES K=EB_VAR();K=EB_VAR();K=EB_VAR();K=EB_VAR();K5=EB_VAR(5) LAM=EB_VAR(6);LAM=EB_VAR(7);LAM=EB_VAR(8) OMEG=EB_VAR(9);OMEG=EB_VAR();OMEG=EB_VAR() DEL=EB_VAR();DEL=EB_VAR();DEL=EB_VAR()! COMPUTE NONLINEAR FUNCTION USING EMBEDDED ALGEBRA EB_FCTN() = K*EXP(LAM*T)*COS(OMEG*T+DEL) + K*EXP(LAM*T)*& COS(OMEG*T+DEL) + K*EXP(LAM*T)*COS(OMEG*T+DEL) + K EB_FCTN() = K*EXP(LAM*T)*SIN(OMEG*T+DEL) + K*EXP(LAM*T)*& SIN(OMEG*T+DEL) + K*EXP(LAM*T)*SIN(OMEG*T+DEL) + K5 END SUBROUTINE NONLINEAR_FX 8

19 APPENDIX B: Sensitivities First order: h = h Φ (B.) i, j i, s sj h i, j hi ( t) = x ( t ) j (B.) Second order: Third order: h = h Φ + h Φ Φ (B.) i, jk i, s sjk i, st tk sj h i, jk = hi ( t) x ( t ) x ( t ) j k (B.) h = h Φ + h Φ Φ + h Φ Φ i, jkl i, s sjkl i, su ul sjk i, st tk sjl + h Φ Φ + h Φ Φ Φ h i, jkl i, st tkl sj i, stu ul tk sj hi ( t) = x ( t ) x ( t ) x ( t ) j k l (B.5) (B.6) Fourth order: h = h Φ + h Φ Φ i, jklm i, s sjklm i, sv vm sjkl + h Φ Φ + h Φ Φ + h Φ Φ Φ i, su ul sjkm i, su ulm sjk i, suv vm ul sjk + h Φ Φ + h Φ Φ + h Φ Φ Φ i, st tk sjlm i, st tkm sjl i, stv vm tk sjl + h Φ Φ + h Φ Φ + h Φ Φ Φ i, st tkl sjm i, st tklm sj i, stv vm tkl sj + h Φ Φ Φ + h Φ Φ Φ + h, Φ Φ Φ i, stu ul tk sjm i, stu ul tkm sj i stu ulm tk sj + h Φ Φ Φ Φ i, stuv vm ul tk sj (B.7) h i, jklm hi ( t) = x ( t ) x ( t ) x ( t ) x ( t ) j k l m (B.8) i =,,..., n m j, k, l, m, s, t, u, v =,,..., n n m s = number of measurements n = number of states s 9

20 APPENDIX C: State Transition Matrix Differential Equations The state transition differential equations presented on pages X and Y are here written in indicial notation. Note: All indices run from to n s where n s is the number of states. Initial conditions are the identity matrix for the first order state transition matrix differential equations, and zeros for second and higher order state transition matrix differential equations. First order: Φ = f Φ (C.) ij i, s sj xi ( t) Φ ij = Φ ij ( t, to) = x ( t ) j o (C.) Second order: Third order: Φ = f Φ + f Φ Φ (C.) ijk i, s sjk i, st tk sj Φ = Φ i ( t) ijk ijk ( t, to, to ) = x x ( t ) x ( t ) j o k o (C.) Φ = f Φ + f Φ Φ + f Φ Φ ijkl i, s sjkl i, su ul sjk i, st tk sjl + f Φ Φ + f Φ Φ Φ i, st tkl sj i, stu ul tk sj xi ( t) Φ ijkl = Φ ijkl ( t, to, to, to ) = x ( t ) x ( t ) x ( t ) j o k o l o (C.5) (C.6) Fourth order: Φ = f Φ + f Φ Φ ijklm i, s sjklm i, sv vm sjkl + f Φ Φ + f Φ Φ + f Φ Φ Φ i, su ul sjkm i, su ulm sjk i, suv vm ul sjk + f Φ Φ + f Φ Φ + f Φ Φ Φ i, st tk sjlm i, st tkm sjl i, stv vm tk sjl + f Φ Φ + f Φ Φ + f Φ Φ Φ i, st tkl sjm i, st tklm sj i, stv vm tkl sj + f Φ Φ Φ + f Φ Φ Φ i, stu ul tk sjm i, stu ul tkm sj i, stu ulm tk sj + f Φ Φ Φ Φ i, stuv vm ul tk sj + f Φ Φ Φ (C.7) xi ( t) Φ ijklm = Φ ijklm ( t, to, to, to, to ) = x ( t ) x ( t ) x ( t ) x ( t ) j o k o l o m o (C.8)