3.2 the CORDIC Algorithm

Home , CORDIC, Instruction cycle

UC San Diego UC San Diego Electronic Theses and Dissertations

Title Improved VLSI architecture for attitude determination computations

Permalink https://escholarship.org/uc/item/5jf926fv

Author Arrigo, Jeanette Fay Freauf

Publication Date 2006

Peer reviewed|Thesis/dissertation

eScholarship.org Powered by the California Digital Library University of California 1

UNIVERSITY OF CALIFORNIA, SAN DIEGO

Improved VLSI Architecture for Attitude Determination Computations

A dissertation submitted in partial satisfaction of the requirements for the degree Doctor of Philosophy

Electrical and Computer Engineering (Electronic Circuits and Systems)

Jeanette Fay Freauf Arrigo

Committee in charge:

Professor Paul M. Chau, Chair Professor C.K. Cheng Professor Sujit Dey Professor Lawrence Larson Professor Alan Schneider 2006

Jeanette Fay Freauf Arrigo, 2006

DEDICATION

This thesis is dedicated to my husband

Dale Arrigo

for his encouragement, support and model of perseverance,

and to my father Eugene Freauf for his patience during my pursuit.

In memory of my mother Fay Freauf and grandmother Fay Linton Thoreson,

incredible mentors and great advocates of the quest for knowledge.

TABLE OF CONTENTS

Signature Page...... iii Dedication … ...... iv Table of Contents ...... v List of Figures...... x List of Tables...... xiv Acknowledgements ...... xv Vita and Publications...... xvii Abstract ……...... xix 1 INTRODUCTION...... 1 1.1 Motivation ...... 1 1.2 Overview of Chapter 2 ...... 2 1.3 Overview of Chapter 3 ...... 3 1.4 Overview of Chapter 4 ...... 4 1.5 Overview of Chapter 5 ...... 5 1.6 Overview of Chapter 6 ...... 6 1.7 Overview of Chapter 7 ...... 7 2 BACKGROUND...... 8 2.1 Introduction ...... 8 2.2 Attitude Parameterization methods ...... 9 2.2.1 Direction Cosine Representation...... 10 2.2.2 Euler Angle Representation...... 122 2.2.3 Euler Axis/Angle Notation...... 13 2.2.4 Quaternion Mathematics ...... 14 2.3 The Kinematic Equation for Rotation ...... 18 2.3.1 Methods of Integration ...... 19 2.3.1.1 Direct Integration...... 19 2.3.1.2 Local Linearization Method ...... 20

2.3.1.3 Exponential Matrix Function...... 21 2.3.1.4 Closed Form Approximation (Constant Angular Rate)...... 22 2.3.2. Closed Form Approximation Errors...... 25 2.3.2.1 Coning Error...... 25 2.3.2.2 Orthogonality Error ...... 27 2.4 Conclusion...... 28 References ...... 29 3 IMPLEMENTATION ...... 30 3.1 Introduction ...... 30 3.2 The CORDIC Algorithm ...... 32 3.3 Parallel Design of the Attitude Update System...... 37 3.3.1 Limitations on the Attitude Update Computation ...... 44 3.3.2 ASIC Synthesis...... 44 3.3.3 Accuracy Analysis...... 45 3.4 Serial Design of the Attitude Update System...... 47 3.4.1 CORDIC Operations...... 47 3.4.2 Computation of the Magnitude of the Angular Displacement ...... 48 3.4.3 Calculation of the Cross Product...... 49 3.4.3.1 Scaling ...... 50 3.4.4 System Operation and Scheduling...... 51 3.4.5 ASIC Synthesis...... 54 3.4.6 Attitude Update Computation Performance Analysis ...... 55 3.4.6.1 Boundary Analysis ...... 55 3.4.6.2 Accuracy Analysis...... 56 3.5 Serial System Design with Parallel Scale Factor Compensation ...... 57 3.5.1 Scale Factor Compensation ...... 58 3.5.2 Vectoring Mode Normalization...... 59 3.5.3 System Architecture ...... 60 3.5.4 System Operation and Scheduling...... 64 3.6 Conclusion...... 67

vi vii

References ...... 68 4 PARAMETRIC ANALYSES FOR CORDIC BASED ALGORITHM AND COMPARISON TO OTHER APPROXIMATION METHODS ...... 70 4.1 Introduction ...... 70 4.2 Parametric Analysis of the CORDIC Based Algorithm...... 72 4.2.1 Accuracy as a Function of CORDIC Processor Bit Width and Iterations...... 73 4.2.2 Orthogonality as a Function of Bit Width and the Number of Iterations...... 78 4.2.3 Area, Update Rate and Power Consumption as a Function of CORDIC Processor Bit Width and Iterations...... 81 4.2.4 Sensor Errors and Output Accuracy...... 85 4.3 Taylor Series Approximation ...... 87 4.3.1 Comparisons of Accuracy ...... 89 4.3.2 Comparisons of Orthogonality ...... 92 4.4 Wie-Barba Method ...... 93 4.4.1 Comparisons of Accuracy ...... 94 4.4.2 Comparisons of Orthogonality ...... 94 4.5 Other Integration Methods ...... 95 4.5.1 Obrechkoff Integration...... 95 4.5.2 Pade Approximation...... 97 4.6 Conclusion...... 98 References ...... 99 5 APPLICATIONS IN COMPUTER ANIMATION...... 101 5.1 Introduction ...... 101 5.2 Quaternion Representation in Computer Animation...... 103 5.3 Physically Based Modeling ...... 106 5.3.1 Explicit Euler’s Method ...... 106 5.3.2 Implicit Euler’s Approximation (Modified Euler’s Predictor-Corrector Method) ...... 106

vii

viii

5.3.3 Runge-Kutta Approximation...... 107 5.4 Motion Capture...... 108 5.4.1 Approximations of the Kinematic Equation for Motion Capture...... 109 5.4.1.1 Explicit Euler’s Method ...... 110 5.4.1.2 Implicit Euler’s Approximation ...... 110 5.4.1.3 Runge-Kutta Approximation...... 111 5.5 Prior Work Using the CORDIC Algorithm in Computer Graphics ...... 112 5.6 Experimental Results...... 113 5.6.1 Motion Capture Sampling Rate...... 113 5.6.2 Initial Test Configuration ...... 113 5.6.3 Attitude Update Comparison...... 116 5.6.4 Comparison at Different Sample Rates ...... 120 5.6.5 Experimental Results with Improved Sensor Block...... 124 5.7 Graphical Simulation...... 130 5.8 Power Consumption Estimates...... 133 5.9 Conclusion...... 135 References ...... 136 6 APPLICATIONS IN VEHICULAR ATTITUDE CONTROL...... 138 6.1 Introduction ...... 138 6.2 Spacecraft Example ...... 140 6.2.1. Single Axis Closed Loop Control...... 141 6.2.2 Three Axis Attitude Control...... 142 6.2.2.1 Error Quaternion Development ...... 143 6.2.2.2 Control Equation ...... 146 6.2.2.3 Vehicle Equation ...... 146 6.2.2.4 Rate Sensing ...... 149 6.2.2.5 Integrated Rate...... 149 6.2.2.6 Kinematic Solution (Attitude Update)...... 149 6.2.2.6.1 Approximation...... 150

viii

6.2.2.7 Orthogonality and Normalization...... 151 6.2.3 Simplified Example...... 151 6.2.3.1 Visual Representation...... 154 6.2.4 Attitude Control System Simulation Comparison...... 155 6.2.5 Other Numerical Examples ...... 159 6.2.6 Coning Error...... 163 6.3 Conclusion...... 165 References ...... 166 7 CONCLUSIONS AND FUTURE WORK ...... 168 7.1 Summary of Contributions ...... 168 7.2 Conclusions ...... 168 7.3 Future Directions...... 174 7.3.1 System Design...... 174 7.3.2 Motion Detection Applications ...... 174 7.3.3 Navigational Applications...... 175

ix x

LIST OF FIGURES

Figure 2.1 Local and Body Coordinate Representation ...... 11

Figure 2.2 Third-First-Third Euler Angle Rotation a) initial orientation, b) rotating about axis 3 by φ , c) rotating about axis 1 by θ, d) rotating about axis 3 by ψ....………………………………………………………………….....13

Figure 2.3 Euler Axis/Angle Representation...... 14

Figure 2.4 Types of Rotational Motion a) Pure Rotation b) Coning...... 27

Figure 3.1 CORDIC Processing Element...... 35

Figure 3.2 Functions for CORDIC Modes a) Circular Rotation, b) Circular Vectoring, c) Linear Rotation, d) Linear Vectoring...... 36

Figure 3.3 Attitude Update Algorithm Data Dependency Graph...... 40

Figure 3.4 Attitude Update Implementation (Block Diagram) ...... 43

Figure 3.5 Accuracy of the Update Computation for Random Quaternion Inputs ...... 46

Figure 3.6 Δθ q Vector Multiplication...... 50

Figure 3.7 System Block Diagram ...... 51

Figure 3.8 Scheduling of CORDIC Operations...... 53

Figure 3.9 Random Quaternion Accuracy...... 57

Figure 3.10 Architecture of an Enhanced CORDIC Processing Element ...... 61

Figure 3.11 Enhanced CORDIC PE, System Architecture ...... 63

Figure 3.12 Scheduling of CORDIC Operations...... 64

Figure 3.13 Computed Accuracy using Random Quaternions...... 66

Figure 4.1 Output Accuracy as a Function of Displacement Angle for Various CORDIC Input Bit Widths ...... 75

Figure 4.2 Output Accuracy as a Function of Displacement Angle for a 40 Bit Wide CORDIC Processor. The number of iterations is shown on the right...... 76

Figure 4.3 Output Accuracy as a Function of Displacement Angle for a 32 Bit Wide CORDIC Processor. The number of iterations is shown on the right...... 77

Figure 4.4 Output Accuracy as a Function of Displacement Angle for a 24 Bit Wide CORDIC Processor. The number of iterations is shown on the right...... 77

Figure 4.5 Orthogonality as a Function of Displacement Angle for Various CORDIC Processor Input Bit Widths. The number of input bits is shown on the right...... 79

Figure 4.6 Orthogonality as a Function of Displacement Angle for a 40 bit wide CORDIC Processor. The number of iterations is shown on the right...... 80

Figure 4.7 Orthogonality as a Function of Displacement Angle for a 32 bit wide CORDIC Processor. The number of iterations is shown on the right...... 80

Figure 4.8 Orthogonality as a Function of Displacement Angle for a 24 bit wide CORDIC Processor. The number of iterations is shown on the right...... 81

Figure 4.9 Taylor Series Errors ...... 90

Figure 4.10 Accuracy of 16 Bit Input, Fixed Point, Taylor Series Approximation a) Fourth Order, b) Third Order Implementations...... 91

Figure 4.11 Orthogonality as a Function of Input Displacement Angle for Various Orders of the Taylor Series Approximation ...... 92

Figure 4.12 Accuracy as a Function of Input Displacement Angle for Various Orders of the Taylor Series Approximation and Wie-Barba’s Method ...... 94

Figure 4.13 Orthogonality as a Function of Input Displacement Angle for Various Orders of the Taylor Series Approximation and Wie-Barba’s Method .....95

Figure 5.1 Object Subjected to Shear a) Original solid body, b) Shear effects on solid body ...... 102

Figure 5.2 Euler Angle Representation ...... 104

Figure 5.3 Great Circle Trajectory ...... 105

Figure 5.4 Sensor Block ...... 114

Figure 5.5 Measured Angular Rate Data Integrated at 20 Sample per second Sample Rate...... 117

Figure 5.6 Closed Form Updated Quaternion Values ...... 118

xii

Figure 5.7 Euler Angle Comparison at 20 Samples per second ...... 119

Figure 5.8 Taylor Series Euler Angle Errors at 20 Samples per second ...... 120

Figure 5.9 Measured Angular Rate as a Function of Time ...... 121

Figure 5.10 Closed Form Quaternion Results at 108 Samples per second ...... 122

Figure 5.11 Euler Angle Comparison at 108 Samples per second...... 122

Figure 5.12 First Order Taylor Series Euler Angle Errors ...... 123

Figure 5.13 Fourth Order Taylor Series Euler Angle Errors...... 123

Figure 5.14 Three-Axis Gyroscope Sensor Block...... 124

Figure 5.15 Measured Angular Rate as a Function of Time ...... 127

Figure 5.16 Closed Form Quaternion Results at 122 Samples per second ...... 128

Figure 5.18 Euler Angle Error Comparison at 61 Samples per second...... 129

Figure 5.19 Euler Angle Error Comparison at 30 Samples per second...... 129

Figure 5.20 Angular Rate as a Function of Time ...... 132

Figure 5.21 Actual Orientation...... 132

Figure 5.22 Animation Results a) CORDIC Implementation, b) First Order Taylor Series, c) Second Order Taylor Series, d) Fourth Order Taylor Series Orientations …………………………………………………….……… 133

Figure 6.1 Second Order Linear System (Single Axis of Rotation and Control)...... 142

Figure 6.2 Vehicular Attitude Control Simulation Block Diagram ...... 145

Figure 6.3 Modified System Diagram...... 148

Figure 6.4 Angular Displacement, Acceleration and Jerk as a Function of Time.....153

Figure 6.5 Animation Results a) Wie-Barba, b) CORDIC, c) Closed Form...... 155

Figure 6.6 Response of Quaternion Elements q1, q2, q3 for Wie-Barba, CORDIC based and Closed Form Implementations...... 157

Figure 6.7 Response of Quaternion Element q4...... 158

xii

xiii

Figure 6.8 Response of Control Signals T1, T2, T3 for Wie-Barba, CORDIC based and Closed Form Implementations...... 158

Figure 6.9 Closed Form Solution for Data Set 1...... 160

Figure 6.10 Wie-Barba versus CORDIC Based Method Data Set 1...... 160

Figure 6.11 CORDIC Based Method Errors for Data Set 1...... 161

Figure 6.12 Closed Form Solution for Data Set 2...... 161

Figure 6.13 Wie-Barba versus CORDIC Based Method Data Set 2...... 162

Figure 6.14 CORDIC Based Method Errors for Data Set 2...... 162

Figure 6.15 Coning results for q4 for complete, CORDIC and Wie-Barba...... 164

Figure 6.16 CORDIC and Wie-Barba Coning Differences...... 164

xiii

xiv

LIST OF TABLES

Table 2.1 Alternate Representations of Three-Axis Attitude...... 17

Table 3.1 CORDIC Functions Implemented by the System ...... 41

Table 3.2 Comparison of Several CORDIC Based Implementations ...... 67

Table 4.1 Output Accuracy as a Function of Displacement Angle for Various CORDIC Input Bit Widths ...... 74

Table 4.2 Orthogonality as a Function of Displacement Angle for Various CORDIC Processor Input Bit Widths...... 79

Table 4.3 Standard Cell Area for Various CORDIC Processor Input Bit Widths ...... 82

Table 4.4 Standard Cell Area for the Controller and CORDIC Processor for Various CORDIC Processor Input Bit Widths...... 82

Table 4.5 Standard Cell Area versus Iteration Time Duration for Various CORDIC Processor Input Bit Widths...... 82

Table 4.6 Maximum Update Rate Various CORDIC Processor Input Bit Widths, CORDIC Iterations...... 84

Table 4.7 Power Consumption at 240 Hz Update Rate...... 84

Table 4.8 Output bit accuracy as a function of input displacement angle and sensor angular bias error using a 33 bit input quaternion...... 86

Table 4.9 Output bit accuracy as a function of input displacement angle and sensor angular bias error using a 16 bit input quaternion...... 86

Table 4.10 Displacement angles at which the accuracy of the CORDIC based method of Chapter 3 first exceeds the accuracy of Taylor Series (TS) based approximation for TS of various orders ...... 90

Table 5.1 Attitude Update Rate versus Power Dissipation ...... 134

xiv xv

ACKNOWLEDGEMENTS

I would like to thank Professor Chau for his guidance during my doctoral pursuit. I was sponsored by Professor Chau for the San Diego Fellowship, and without it I could have never begun this endeavor. In addition I would like to thank

Professor Schneider who shared an interest in quaternion algebra and its relevance to kinematics. The project that I embarked upon in his course provided the spark for this work. His insight and wisdom are invaluable. I would also like to thank Professor

Larson for his encouragement and Professors Dey and Cheng for their helpful comments during my qualification examination.

Professor Jay Harris at San Diego State University has been a continual source of inspiration for higher learning from the moment that we discussed my desire to pursue a doctorate. The research that I assisted him on the enhancements to the Z-J stack algorithm is some of most rewarding time of my life and provided excellent training in the rigor and accuracy of academic research that I have tried to maintain while at UCSD.

My husband Dale Arrigo has continually provided emotional support for the entire duration. He is a tribute to perseverance since he has overcome many health obstacles in his life and still achieved his goals. He has also been my sounding board for many concepts and approaches that I have entertained as part of the academic process and was instrumental in the final assembly of my sensor blocks used in this research.

xvi

I spent several years at BAE Systems as a means of income for myself and my family while pursuing my degree. I would like to thank Nancy Jamison for convincing me to come to work at BAE Systems and Thena Fantasia for hiring me and being so flexible since I was working on my degree. I couldn't have survived without

Martin Gerdom and Elaine McMurray's technical and emotional support. I would also like to thank Jim Reynolds for believing in me and transferring his title of Space

Systems Chief Engineer to me. Carol Anderson provided great guidance and advice that helped me to be more productive in completing my degree.

As my completion of this thesis came to an end, Frank Macklin at SpaceDev offered me a great opportunity to continue chasing the stars. I will always be grateful for the quaternion references that he lent me in the beginning of my research that helped to guide my path.

The text of Chapters 3, 4, and 5 in this dissertation, in part or in full, is a reprint of the material as it appears in our published papers or as it has been submitted for publication in IEEE Transactions on Instrumentation and Measurement, and IEEE

Transactions on Circuits and Systems I. The dissertation author was the primary author listed in these publications and supervised the research which forms the basis for these chapters.

xvi

xvii

VITA AND PUBLICATIONS

Vita

1982 B.S.E.E., University of Nebraska, Lincoln

1982 - 1994 Engineering Specialist, General Dynamics, San Diego

1990 M.S.E.E., San Diego State University

1995-1996, Technical Specialist II, San Diego State University

1997-2004, Senior Engineering Specialist, BAE Systems, San Diego

2004 Chief Engineer, SpaceDev, Poway, California

2006 Ph.D., University of California, San Diego

Publications

J. F. Arrigo, P. M. Chau, “Power Aware Attitude Computation during Rapid Rotational Motion”, IEEE Transactions on Instrumentation and Measurement (Accepted)

J. F. Arrigo, P. M. Chau, “Sensor Based Attitude Determination Using the CORDIC Algorithm”, IEEE Transactions on Circuits and Systems I (Submitted)

J. F. Arrigo, P. M. Chau, “Power Aware Attitude Computation during Rapid Rotational Motion”, IEEE Instrumentation and Measurement Technology Conference 2004, 18-20 May 2004, Lake Como, Italy

J. F. Arrigo, P. M. Chau, “Accurate Motion Capture at High Rotational Rates Using the CORDIC Algorithm”, Conference Record of the Thirty-Seventh Asilomar Conference on Signals, Systems, and Computers, 9-12 Nov 2003, Pacific Grove, CA

J. F. Arrigo, P. M. Chau, “Quaternion-Based Attitude Update Calculations Using The CORDIC Algorithm”, Institute of Navigation National Technical Meeting, January 2001, Long Beach, CA

xvii

xviii

J. F. Arrigo, K. J. Page, Yuhe Wang, P. M Chau, “Adaptive FEC on a Reconfigurable Processor for Wireless Multimedia Communications”. ISCAS '98, Monterey, CA

J. F. Arrigo, K. J. Page, P. M. Chau, N. C. Tien, “Reconfigurable Sensor Signal Processing for Accelerometer-Based Navigation”, GOMAC '98, Crystal City, VA

J. F. Arrigo, P. M. Chau, N. C. Tien, “Reconfigurable Sensor Signal Processing”, FPGA '98, Monterey, CA

J.F. Arrigo, P. M. Anderson, P. M. Chau, Y. H. Chen, N. C. Tien, “ Reconfigurable Hardware Mapping for Accelerometer-based Navigation Applications”, Aerosense ‘98 (SPIE), Orlando, FL

K. J. Page, J. F. Arrigo, P. M. Chau, “ Reconfigurable Hardware Based Digital Signal Processing for Wireless Communications”, 1997 SPIE National Meeting, San Diego, CA

J. F. Arrigo, K. J. Page, P. M. Chau, N. C. Tien,, “The Application of Reconfigurable Processors to MEMS Calibration and Signal Processing in Inertial Navigation Systems”. 1997 IEEE International Conference on Innovative Systems in Silicon, Austin, TX

J. F. Arrigo, J. H. Harris and T. Nguyen, “A Decoding Behavior of a Modified Z-J Decoder”, Soc. For Computer Simulation Western Multiconference, San Diego CA., January 14-17, 1996

xviii

xix

ABSTRACT OF THE DISSERTATION

Improved VLSI Architecture for Attitude Determination Computations

Jeanette Fay Freauf Arrigo

Doctor of Philosophy in Engineering and Computer Engineering

(Electronic Circuits and Systems)

University of California, San Diego, 2006

Professor Paul M. Chau, Chair

Microelectromechanical sensor (MEMS) technology has produced angular rate sensors that are dramatically smaller in size and lower in cost than in the past.

Although the sensor size has decreased significantly, the algorithms that sensor based systems use to compute the attitude of an object have not sustained similar improvements.

The primary goal of this dissertation is to develop a new method for computing the attitude of an object that can be incorporated into an application specific integrated circuit (ASIC) or field programmable gate array (FPGA). Determination of the attitude of an object under conditions of varying angular body rate is one of the most

xix

xx complex computations in motion detection. The kinematic equation for rotation, which is used for this computation, is a differential equation that is solved using approximation methods. Since the closed form approximation for the solution of the kinematic equation involves trigonometric functions, it seems natural to consider use of the CORDIC algorithm to perform these calculations. The CORDIC algorithm can also be used to perform arithmetic operations such as multiplication and division. A

CORDIC processor can therefore represent a general processing element that is useful throughout the kinematic computation.

This dissertation describes the development of a system that uses CORDIC processing, in either a single or parallel processing element configuration, to perform computations needed to solve the kinematic equation for rotation of a solid body.

Results of the CORDIC based computation are compared to other more popular techniques such as the Taylor Series approximation method in the thesis.

One of the major advantages to using the CORDIC method is the accuracy that is achieved in the attitude computation, particularly at high angular rate inputs. Other methods that approximate the closed form solution either incur truncation errors significant enough to call their use into question, or require more system complexity to overcome difficulties.

Extensive simulations and comparisons of the results to other methods have been performed, and they demonstrate the computational accuracy and effectiveness of the new method. Results of this research can be useful in aerospace applications as well as in motion capture aspects of computer graphics. Examples in these areas are presented.

CHAPTER 1

INTRODUCTION

1.1 Motivation

Accurate attitude information for an object can be necessary in applications such as spacecraft control and computer animation. There is a need for physical size reduction and local data processing in the motion capture aspect of computer animation because the sensor used to obtain motion data is mounted on a moving solid body. Microelectromechanical sensor (MEMS) technology has produced angular rate sensors that are continuously getting smaller in size and lower in cost. The physical size of angular rate sensors has decreased significantly, but improvements in the algorithms that use sensor data to compute the attitude of an object have not had a comparable development.

The most complicated computation when solving the kinematic equation for the rotation of an object is the integration of attitude with respect to angular rate, and this integration must be approximated. Since the closed form approximation for the solution of the kinematic equation involves trigonometric functions, the research focused on the use of the CORDIC algorithm, which was developed originally for trigonometric computations, applied to perform these calculations. The CORDIC algorithm can also be used to perform arithmetic operations such as multiplication and

2 division and can therefore provide a general processing element that is useful throughout the kinematic computation.

A system has been developed that uses CORDIC processing to compute the closed form solution of the kinematic equation. One of the major advantages to using this method is the accuracy that is achieved in the computation of attitude at high angular rate inputs. Other methods that approximate the closed form solution either incur truncation errors significant enough to materially affect accuracy, or require more complexity to overcome this difficulty.

1.2 Overview of Chapter 2

The various types of attitude coordinate systems are discussed and compared in this chapter. Both quaternion and non-quaternion based methods are discussed.

Some of the basic quaternion arithmetic operations are also presented since quaternion arithmetic uses hypercomplex principles for computation.

The angular rate of movement for an object can either be simulated or measured. The rotational dynamics of a rigid body in space can be simulated by

Euler’s equations of motion if the moments of inertia are known. The simulated motion is described by a first order derivative of the angular rate for the object. The attitude determination method can also be used in a system in which the angular motion is measured with body mounted sensors in real time and is a function of the angular information and the previous attitude of the object.

These results can be used as inputs to the kinematic equation for rotation to compute the new orientation of the object. The complete solution to the kinematic differential equation cannot be solved in a closed form and must be approximated. If the rotational rate is considered to be constant over the sampling period the resulting solution includes trigonometric functions.

1.3 Overview of Chapter 3

The closed form approximation for attitude determination presented in Chapter

2 uses trigonometric functions and requires the calculation of the Euclidean norm for the integrated angular rate values. There are several techniques available to provide further approximation, resulting in other forms such as a set of recursive equations or a geometric series. These are reviewed in Chapter 3.

The CORDIC algorithm originally proposed by Volder can perform the necessary trigonometric calculations and can provide the square root computation required when computing a Euclidean norm. In addition the multiplication and division properties that Walther discovered can be implemented with the same

CORDIC architecture.

The properties of the CORDIC algorithm are presented in this chapter for the modes that are used in performing the calculations for the closed form approximation.

Three implementations are described using the CORDIC algorithm as the primary processing element (PE). The first implementation uses four CORDIC PE’s in parallel, and leads to a high speed calculation. The second implementation uses only one CORDIC PE and is a low physical area implementation. A third implementation is described in which the scale factor correction is incorporated in the CORDIC processor. ASIC synthesis, accuracy analysis and boundary conditions are also defined for these implementations using a signed 32-bit CORDIC PE.

1.4 Overview of Chapter 4

Results of extensive simulations performed for 16, 24, 32 and 40-bit fixed point CORDIC processors and several different CORDIC iterations are described with a view toward assessing the accuracy and orthogonality of the solution for the kinematic equation. The simulations are replicated using the Taylor Series approximation methods and a large angle approximation method. The large angle approximation method linearizes the computations for the Taylor Series approximation and therefore simplifies their implementation. However, as shown in this chapter, the accuracy is affected by using the large angle method.

1.5 Overview of Chapter 5

In Chapter 5 the computational accuracy of the various approximation techniques is described, and the results obtained are outlined when the computational method is applied to the computer animation problem. In computer animation, objects that move require coordinate translation and rotation. Keyframes are produced to define the desired position and orientation of objects in computer animation. To create smoother movement from one keyframe to another, additional frames are created to interpolate the movement as time passes and to provide for persistence of vision. This process is analogous to manual animation and is commonly refered to as

“in-betweening”.

Translation of a moving solid body can be described in using three dimensional vectors whose components are x, y and z coordinates. Documenting rotational movement is more complicated since the motion of the surface on the solid body is spherical or circular in nature. Rotation has been described in the field of computer animation using many different coordinate systems, e.g., fixed-axis, Euler angle, Euler axis/angle and quaternion coordinates.

Common error sources in computer animation include roundoff errors and the lack of orthonormalization. Roundoff errors result from repetitive calculation of the translation of an object, and can cause the orientation of the rotating object to drift from the expected position. The coordinate axes for an object should always be

6 perpendicular to one another and of unit value, i.e. orthonormal. If orthonormality is not preserved, the object can appear to have a component of shear. When quaternions are used to describe the orientation of an object, orthonormality is automatically preserved as a consequence of the unit magnitude of a quaternion. Another feature of quaternion arithmetic is that fewer computations are needed to calculate a new orientation than other methods.

Computer animation can also be implemented using physical parameters measured during the movement of an object. The orientation of an object can be measured using several different sensor types, including optical and inertial components. When inertial components are used, the object can be tracked in virtually any environment as compared with the confined area that is required to monitor an object that is tracked optically. This chapter introduces use of the CORDIC based quaternion attitude determination method, presented in Chapter 3, to translate motion that has been captured using MEMS gyroscopes. Estimates for the power consumption for such a system are provided.

1.6 Overview of Chapter 6

The description of an attitude control system that includes quaternion feedback for spacecraft large angle maneuvers is reviewed in this chapter and has been the basis for recent attitude control methods in the aerospace field.

The results are applied to a couple of spacecraft examples where the quaternion integration is performed using the CORDIC based solution for the kinematic equation of rotation and is compared to another approximation method.

The first example determines the accuracy of calculating attitude for a quickly rotating spacecraft. The second example determines the accuracy of the attitude for a spacecraft that is performing re-orientation maneuvers after being launched from the shuttle payload bay.

Simulations are also performed using transcendental input data sets to assess the stability and accuracy of these two methods. A brief discussion regarding coning errors concludes this chapter.

1.7 Overview of Chapter 7

Chapter 7 provides a summary of the contributions offered in this thesis and suggests directions in future research.

CHAPTER 2

BACKGROUND

2.1 Introduction

Several methods to describe the attitude of an object are expressed in this chapter.

Due to its computational efficiency, quaternion notation is selected for this research.

A compact form to describe the determination of attitude for an object is also chosen.

Determination of the attitude of an object is a central problem in computational kinematics. The attitude of an object is its orientation in space. The applications for attitude determination and associated control are quite diverse. They range from small personal navigation systems to unmanned vehicle navigation and control [1][2][3].

The principal types of coordinate systems used for analyzing rotational motion are discussed in this chapter. The principal focus is on the use of quaternions. Since quaternion arithmetic uses hypercomplex principles for computation, some of the basic arithmetic operations are also presented.

The rotational dynamics of a rigid body in space can be described by Euler’s equations of motion if the moments of inertia are known [5]. The result is a first order derivative of the angular rates for the object. These results can be used as inputs to the

8 9 kinematic equation for rotation to provide an updated computation of the orientation of the object. The complete solution to the kinematic differential equation cannot be obtained in a closed form and must be approximated. If the rotational rate is considered constant during a computational update, an approximation involving trigonometric functions can be used. The trigonometric function method for determining attitude can also be used in a computational system that combines angular motion measurements with previously determined attitude data. The moments of inertia for the rigid body do not need to be known. An example is given in Chapter 5.

2.2 Attitude Parameterization methods

There are several methods used to represent and compute the orientation of a solid object in space. The most popular representations are direction cosine, Euler angle,

Euler axis/angle and quaternion. The direction cosine method employs the angular difference between a reference frame and its previous orientation to update the attitude and is a popular method when using orthogonal coordinate frames. Another method involves successive rotations about each of the orthogonal axes of an object. The angle of rotation about an axis is called an Euler angle. In general, the Euler angle method has an advantage over the direction cosine representation because only three angles are required to define a unique orientation [5].

A different approach is based upon an eigenaxis rotation theorem by Euler [5] that states that any change in orientation of a solid object can be described by one rotation about a single axis.

A fourth method used to describe the orientation of an object uses quaternion notation. A quaternion consists of four elements that are hypercomplex and can be defined using the eigenaxis notation. The number of calculations performed to update the present orientation of an object can be reduced by using quaternion representation.

2.2.1 Direction Cosine Representation

Given two reference frames A and B in which frame A can be described by the

orthogonal unit vectors [aaa123] and frame B can be described by the orthogonal

unit vectors [bbb123] , unit vectors in frame B can be represented using the unit vectors in frame A by the relationship

⎡ba11⎤⎡⎤ ⎢bCa⎥⎢⎥= B ⎢ 22⎥⎢⎥A (2.1) ⎣⎢ba33⎦⎣⎦⎥⎢⎥ where

⎡CCC11 12 13 ⎤ CCCCB = ⎢ ⎥ A ⎢ 21 22 23 ⎥ (2.2) ⎣⎢CCC31 32 33 ⎦⎥

and Cij is the cosine of the angle between bi and a j . Cij is called the direction cosine

B and CA is called the direction cosine matrix.

Since sensors are mounted on the body of an object, the relationship between sensed body rotation and the fixed, or local, coordinate frame, shown in Figure 2.1, can be written as

L LBn() CCCBBnB= () (2.3)

L where n is the interval in time and CB is the direction cosine matrix that translates

L Bn() body coordinates into the local fixed frame coordinates. Since CB()n and CB are

3x3 matrices, computation of (2.3) requires 27 multiplications to compute a matrix.

3 2

3 B 1

L 1

Figure 2.1 Local and Body Coordinate Representation

2.2.2 Euler Angle Representation

A series of angular rotations about the body coordinates of a solid object can also reposition the object to a new orientation. A 3-1-3 (third-first-third) Euler angle rotation sequence is shown in Figure 2.2 where the object is first rotated about its third body axis by an angle φ, the object is then rotated about its first body axis by an angle

θ, and the object is then rotated about its third body axis by an angle ψ. These rotations can be described using the rotation matrix A313 defined in (2.4), viz.,

AAAA313(φ,,θψ) = 3( ψ) 1( θ) 3 ( φ) , (2.4) where,

⎡⎤cosφφ sin 0 ⎡10 0⎤ ⎡ cosψψ sin 0⎤ A φφφ=−⎢⎥sin cos 0 A θ = ⎢0cossinθθ⎥ A ψψψ=−⎢ sin cos 0⎥ 3 () ⎢⎥, 1 () ⎢ ⎥ , and 3 () ⎢ ⎥ . (2.5) ⎣⎦⎢⎥001 ⎣⎢0sincos− θ θ ⎦⎥ ⎣⎢ 001⎦⎥

2 2 3 φ 3 a) b) 1 θ 1

3 ψ

2 2

c) d)

Figure 2.2 Third-First-Third Euler Angle Rotation a) initial orientation b) rotating about axis 3 by φ , c) rotating about axis 1 by θ, d) rotating about axis 3 by ψ

2.2.3 Euler Axis/Angle Notation

Euler's rotational theorem states that the rigid-body attitude can be changed from one orientation to any other by rotating the body about a single axis that is fixed to the rigid body as depicted in Figure 2.3. The angle of rotation φ in the Figure is commonly referred to as the Euler angle, and the axis of rotation is known as the Euler

axis. The orientation of the Euler axis can be represented by direction cosines e1 , e2 ,

and e3 that define the axis of rotation in a fixed frame. The values of e1 , e2 , and e3 are constrained by the relation

222 eee123+ +=1 . (2.6)

y φ

θ2 e2

θ1 θ3

e1 x

e3 e1=cos(θ1) e2=cos(θ2) z e3=cos(θ3)

Figure 2.3 Euler Axis/Angle Representation

2.2.4 Quaternion Mathematics

As originally described by Hamilton [4], quaternions are hypercomplex numbers that follow the rules of complex number theory. A quaternion is a four-

element vector q =iq+jq1234 +kq +q in which q1 , q2 , q3 , q4 are real numbers and i, j, k are imaginary numbers that follow a right-hand rule for cross multiplication, e.g., ii = -1,ij = k and ji = -k . A normalized quaternion satisfies the relation

222 2 q+q+q+q=1123 4 .

The four elements of a quaternion vector can be defined in terms of the Euler components

⎛⎞φ ⎛⎞φ ⎛⎞φ ⎛⎞φ q=esin11⎜⎟ q=esin22⎜⎟ q=esin33⎜⎟ q=cos4 ⎜⎟ (2.7) ⎝⎠2 ⎝⎠2 ⎝⎠2 ⎝⎠2

where φ is the magnitude of the Euler-axis rotation and ( e 1 , e 2 , e 3 ) are the direction cosines of the Euler axis relative to the inertial reference frame [2]. The absolute

quaternion vector qact defines the spacecraft attitude and describes its orientation as it relates to an inertial reference frame. If the object has rotated, the difference between its previous position and present location in absolute terms can be described as

qqqact(t +T)= r (t +T) act (t) (2.8)

where T is the time increment and qr is the quaternion that characterizes the change in rotation since the last update.

T Note that if qr is [0001] , the object has remained immobile since

T qqact(t +T)= act (t). If qact (t +T)is equal to [0001] , the object has rotated back to its original orientation. It is necessary to use normalized quaternion vectors in order to maintain orthogonality [4].

To rotate from orientation A by an amount determined by B, quaternion multiplication is used

qqbaD =() iqa1 + jq a2 +kq a3 +q a4( iq b1 + jq b2 +kq b3 +q b4 ) (2.9)

which also can be written as

⎡ qq-qqqa4 a3 a2 a1⎤⎡ b1 ⎤ ⎢-q q q q⎥⎢ q ⎥ ⎢ a3 a4 a1 a2⎥⎢ b2 ⎥ qq=baD . (2.10) ⎢ q-qqqqa2 a1 a4 a3⎥⎢ b3 ⎥ ⎢ ⎥⎢ ⎥ ⎣-qa1 -q a2 -q a3 q a4⎦⎣ q b4 ⎦

The following notation allows comparison to (2.3)

L LBn() qqBBnB= ()D q . (2.11)

This method requires 16 multiplication operations. There are 27 minus 16, or 11, fewer operations than required when using direction cosine notation.

Table 2.1 presents a comparison of the advantages and disadvantages of using either direction cosine, quaternion, Euler angle or Euler eigenaxis representation [5].

Wilcox proposed that quaternion notation be used to significantly reduce the number of calculations [7] and is the method used in the thesis.

Table 2.1 Alternate Representations of Three-Axis Attitude Parameter- Notation Advantages Disadvantages Common Applications ization Direction C=[Cij] No Singularities Six Redundant Parameters In Analysis, to Cosine No Trigonometric Transform Vectors Matrix Functions from One Reference Convenient Product Rule Frame to Another. for Successive Rotations φ, θ ,ψ Trigonometric Functions Analytic Studies Euler Angles No Redundant Parameters Singularity at Some θ Input/Output Physical Interpretation is No Convenient Product Rule for Onboard Attitude Clear in Some Cases Successive Rotations Control of 3 Axis Stabilized Spacecraft

e , φ One Redundant Parameter Commanding Slew Euler e Clear Physical Manuever Axis Undefined when sin φ =0 Axis/Angle Interpretation Trigonometric Functions

q , q , Euler 1 2 No Singularities One Redundant Parameter Onboard Inertial q , q Symmetric 3 4 No Trigonometric No Obvious Physical Navigation

Parameters Functions Interpretation (Quaternion) Convenient Product Rule for Successive Rotations

2.3 The Kinematic Equation for Rotation

The kinematic equation of rotation can be written in terms of the cross product

of the orientation quaternion q and the angular velocity components ω1 ,ω 2 ,ω 3 as [1]

d1q =×ω q (2.12). dt 2

⎡⎤q1 ⎡ω1 ⎤ ⎢⎥q ⎢ω ⎥ In (2.12), q = ⎢⎥2 is a normalized quaternion vector and ω = ⎢ 2 ⎥ an unnormalized ⎢⎥q3 ⎢ω3 ⎥ ⎢⎥ ⎢ ⎥ ⎣⎦⎢⎥q4 ⎣ 0 ⎦ quaternion vector. If the cross product in (2.12) is expanded, the matrix form for the kinematic equation results, viz.,

d1q = ψ q (2.13) dt 2

where

⎡ 0 ω321-ωω⎤ ⎢-ω 0 ωω⎥ ψ = ⎢ 312⎥ (2.14). ⎢ ω21-ω 0 ω 3⎥ ⎢ ⎥ ⎣⎢-ω123-ω -ω 0 ⎦⎥

19 is the skew symmetric matrix describing the angular velocity (and replaces ω× ) .

2.3.1 Methods of Integration

Since both the angular rate and the attitude can vary with time, an exact closed- form solution of the equation to be integrated does not exist. Several methods using standard numerical analysis are discussed below. A closed form solution when a constant rate is assumed is also discussed.

2.3.1.1 Direct Integration

One-step methods of integration such as Runge-Kutta are popular since they are easy to apply and their step sizes can be changed as necessary without any additional computations. A popular version of a fourth-order Runge-Kutta solution is

[5]

kfty1 = ( nn, ) (2.15)

h y =+ykkkk() +22 + + (2.16) nn+112346

⎛⎞11 kfthyhk21=+⎜⎟nn, + (2.17) ⎝⎠22

⎛⎞11 kfthyhk32=+⎜⎟nn, + (2.18) ⎝⎠22

kfthyhk43=++( nn, ) (2.19)

It should be noted that many function evaluations are required per integration step using this method.

Only a single function is necessary if a multi-step integration method is used instead. The cost of this approach is therefore limited to a need for additional storage space. The Adams-Bashforth method [8], which provides an explicit integration method, has been used for this purpose. The Adams-Bashforth method has the form

k −1 yyhfnn+11=+∑ β jnjk++ − . (2.20) j=0

An alternative, the Adams-Moulton method, is implicit and takes the form

k yyhfnn+11=+∑ β jnjk++ − (2.21) j=0

where f is a differential function of y, h is the time step duration, and k is the number of steps [5].

2.3.1.2 Local Linearization Method

Another method that has been used to improve on the second-order direct integration methods employs local linearization of the kinematic equation [9], which takes the form

• ψψnnhh−12 qeq(ttn+1 )=+ () n()(ψψψnnn eIhq −− ) () t n . (2.22)

Since

-1 ψ nh ⎛⎞11 ⎛⎞ e =cosI ⎜⎟ωψωωnnnn h+2 sin ⎜⎟ h , (2.23) ⎝⎠22 ⎝⎠

equation 2.23 becomes

ωωhh ω h••⎛⎞ ω h nn24⎛⎞ n 42 n qq()ttn+1=+cos() n sinψψnn q() t n +−+−22⎜⎟ 1 cos qh() t n⎜⎟ sin ψψ nn q() t n 22ωωnn⎝⎠ 2 ωω nn⎝⎠ 2

(2.24)

This technique provides a more accurate solution for large angle displacements than the direct integration methods. The derivative of angular rate is needed to solve the kinematic equation using this method, and this requirement adds additional complexity to the computation.

2.3.1.3 Exponential Matrix Function

When the angular velocity is constant, (2.13) has a closed form solution

1 ψ 2 qqe= 0 . (2.25)

Values of ψ are derived from sampled gyro data. If it is assumed that data sampled at a fixed rate represents a constant angular velocity vector over the interval between samples, then (2.25) may be expressed

1 ψ T 2 n qeq()ttn+1= () n (2.26)

where T is the sampling interval T = t n+1 − t n , ψ n is (2.25) evaluated at time t n , q(tn )

is the attitude quaternion at time t n and q(t n+1 ) is the updated attitude quaternion at

time t n+1 .

2.3.1.4 Closed Form Approximation (Constant Angular Rate)

The exponential matrix function can be rewritten in a more convenient form for numerical computation. Expressing the exponential matrix function in a series expansion provides the closed-form equivalent expression

n2k2k+1 ⎛⎞111⎡ ⎛⎞⎛⎞⎤ 1ψ t ψψψttt⎢ ⎥ ∞∞⎜⎟222 ⎜⎟⎜⎟ e 2 = ∑∑⎝⎠=+⎢ ⎝⎠⎝⎠⎥ . (2.27) n0= n! k=0⎢ () 2k !() 2k +1 ! ⎥ ⎢ ⎥ ⎣⎢ ⎦⎥

2 222 2 Since ψω=≡−-(ω123+ω +ω )I I , (2.27) becomes

2k 2k+1 kk⎛⎞11 ⎛⎞ 1ψ t ∞∞()−−11⎜⎟ω T () ⎜⎟ω T 22−1 e 2 =+I∑∑⎝⎠ψω ⎝⎠ (2.28) k=0()2k ! k=0 () 2k +1 !

where I is the identity matrix. The series in (2.28) can be identified as trigonometric functions, and (2.28) can also be expressed

1ψ t 2 ⎛⎞11-1 ⎛⎞ e =cosI ⎜⎟ωψω t+ sin ⎜⎟ ω t. (2.29) ⎝⎠22 ⎝⎠

The propagated attitude equation (2.26) can now be described as an orthogonal rotation that retains quaternion normalization [1]

⎡⎤⎛⎞11-1 ⎛⎞ q()t=cosT+n+1⎢⎥⎜⎟ω I ωωψ sinT ⎜⎟q() t n . (2.30) ⎣⎦⎝⎠22 ⎝⎠

Since ω is a scalar, (2.30) can be rewritten as

⎛⎞11ψ ⎛⎞ qq()t=tcosT+tsinTn+1 () n⎜⎟ω q() n ⎜⎟ω . (2.31) ⎝⎠22ω ⎝⎠

Assuming that the rate integrating gyroscopes are sampled at a constant rate,

222 Δ=θ ω T , where Δθ = Δθ123+ Δθ + Δθ and Δθ1,Δθ 2 ,Δθ3 are the displacement angles most recently sampled from the rate integrating gyroscopes. In addition, since

ψ consists exclusively of components of angular velocity and is divided by ω , (2.31) becomes

⎛⎞11ψq(tn ) ⎛⎞ qq()t=tcosn+1 () n ⎜⎟ΔΔθ + sin ⎜⎟θ (2.32) ⎝⎠22Δθ ⎝⎠ where

⎡ 0 Δθ321-Δθ Δθ ⎤ ⎢-Δθ 0 Δθ Δθ ⎥ ψ = ⎢ 312⎥ . (2.33) ⎢ Δθ21-Δθ 0 Δθ 3⎥ ⎢ ⎥ ⎣⎢-Δθ123-Δθ -Δθ 0 ⎦⎥

Therefore the attitude update can be calculated from the most recent gyroscopic data and the value of the attitude quaternion vector from the previous update.

This method of attitude propagation summarized in (2.32) is more commonly used than the local linearization method of (2.24) because only the most recent angular sensor data is needed to update attitude values. General-purpose onboard processors have traditionally used approximation techniques to calculate (2.32). A CORDIC algorithm based method for approximating (2.32) has been developed that provides for more accurate results than previous techniques. The method is discussed in detail in

Chapter 3. The CORDIC algorithm uses simple shifts and additions to perform the

25 trigonometric calculations in (2.32). A CORDIC processor can also perform the other required multiplication, division, and square-root operations.

2.3.2 Closed Form Approximation Errors

There are several sources of potential error when performing an attitude update calculation. The errors arise from both the approximation method used and the computational technique applied to the method. When the motion of an object is too rapid for the update rate, the errors in the computation are referred to as coning motion errors. Coning motion errors represent the difference between computations based on the closed form approximation method presented in the previous section and the general solution to the kinematic equation, which more closely represents the actual motion of an object. In addition to issues related to update rate, errors in the computation of (2.32) are generated primarily through inaccuracies in computing trigonometric and other algebraic functions in the closed form solution. Sources of these errors include the use of truncation and finite word sizes. Computational errors are manifested as what are referred to as orthogonality and accuracy errors [6]. computational

2.3.2.1 Coning Error

The most commonly addressed form of coning error occurs when the

frequency of the coning motion is within the bandwidth of the rate sensors, but is

higher than the update frequency of the attitude update algorithm. As a consequence,

the sensors can respond to the movement, but the system does not detect it [10]. The

error that arises from using the closed form solution when the motion is not constant

may be estimated as the difference between the value obtained using the local

linearization method (2.24) and the closed form solution (2.31). The result can be

computed as the quaternion

442⎛⎞ωωhh••⎛⎞ nn. (2.34) qqhqcn()ttn+1=−22⎜⎟1cosψψψ() n +−⎜⎟ sin nn() t n ωωωnnn⎝⎠22⎝⎠

A simple coning motion can be described by an angular rate vector [5]

⎡ ⎤ ⎢ωβcos( ωt )⎥ ⎢ ⎥ . (2.35) ωωβωc = ⎢ sin(t )⎥ ⎢ 1 ⎥ ⎢ ωβ 2 ⎥ ⎣ 2 ⎦

Equation (2.35) holds when the half cone angle β is small. Figure 2.4 shows an example of coning motion and, for comparison purposes, a pure rotation for which L is the angular momentum vector, ω is the instantaneous rotation axis, and P is the principal axis.

The coning motion will be used later to compare errors generated by the different methods used to approximate the closed form solution.

L, ω, P z, L, ω, P

z β

y x y x

a) b)

Figure 2.4 Types of Rotational Motion a) Pure Rotation b) Coning

2.3.2.2 Orthogonality Error

Axes orthogonality is another important performance parameter of the attitude update equation. If the magnitude of the quaternion vector does not equal unity, the rotation can appear distorted, and within some additional update periods, may become unusable. The orthogonality error ε can be described as

22 22 ε =−1 (eeee0123 + + + ) (2.36)

where e0 , e1 , e2 , e3 represent the elements of the output quaternion [6].

2.4 Conclusion

Several different methods computing the attitude of an object have been described. The quaternion representation provides a method for updating the orientation of a rotating object that requires fewer operations than other methods, most particularly, the direction cosine matrix based method.

The kinematic equation for rotation has also been reviewed, along with several methods for integrating results obtained from the equation to provide an attitude reading. The method selected for this work is based on the assumption that the rate is constant over the sampling period. Approximations inherent in the method can generate coning and non-orthogonality errors.

References

[1] A. Kol, B. Laird, B. Leimkuhler, “A Symplectic Method for Rigid-Body Molecular Simulation”, Journal of Chemical Physics, Vol. 7, No. 7, August 1997, pp. 2580-2580.

[2] M. Ribo, P. Lang, H. Ganster, M. Brandner, C. Stock, A. Pinz, “Hybrid Tracking for Outdoor Augmented Reality Applications”, IEEE Computer Graphics and Applications, November-December 2002, pp. 54-63.

[3] S.I. Roumeliotis and G.A. Bekey, "3D Localization for a Mars Rover Prototype", 5th International Symposium on Artificial Intelligence, Robotics and Automation in Space, ESTEC, Noordwijk, The Netherlands, June 1-3, 1999, pp.441-448

[4] W. R. Hamilton, Elements of Quaternions, C. J. Joly, Ed.; Chelsea Publishing Company, 1899.

[5] F. L. Markley, Spacecraft Attitude Determination and Control, J.R. Wertz, Ed.; D. Reidel Publishing Company, 1978.

[6] W. F. Phillips, C. E. Hailey, G. A. Gebert, “Review of Attitude Representations Used for Aircraft Kinematics”, AIAA Journal of Aircraft, Vol. 38, No. 4, July- August 2001, pp. 718-737.

[7] J. C. Wilcox, “A New Algorithm for Strapped-Down Inertial Navigation”, IEEE Transactions on Aerospace and Electronic Systems, Vol. 3 No. 5, Sept. 1967, pp. 796-802.

[8] J. W. Wilson and G. G. Steinmetz, “Analysis of Numerical Integration Techniques for Real-Time Digital Flight Simulation”, NASA Technical Note D- 4900, November 1968.

[9] L. E. Barker, R. L. Bowles, L. H. Williams, “Development and Application of a Local Linearization Algorithm for the Integration of Quaternion Rate Equations in Real-Time Flight Simulation Problems”, NASA Technical Note D-7347, December 1973.

[10] J. R. Coffee and F. Saggio, “Strapdown Gyro Contribution to Coning Motion Errors”, in IEEE Conference on Systems Engineering, Aug. 24-26 1989, pp. 55 – 58.

CHAPTER 3

IMPLEMENTATION

3.1 Introduction

The CORDIC algorithm presents a new and efficient form to perform the attitude determination computations described in Chapter 2. Three systems are developed in this chapter that utilize this algorithm and demonstrate consistently accurate calculations within a very large range of angular displacement.

The closed form approximation for attitude determination presented in Chapter

2 uses trigonometric functions and requires calculation of the Euclidean norm to obtain integrated angular rate values. There are several existing techniques available to provide alternative approximations, which result in other forms that will be described in Chapter 4 such as a set of recursive equations or a geometric series progression. The CORDIC algorithm was used in this research instead to compute attitude determination. A review of the CORDIC algorithm is presented in this chapter and followed by three implementations of the CORDIC algorithm for a closed form computation of attitude determination.

The CORDIC algorithm originally proposed by Volder [1] can perform the necessary trigonometric calculations and can provide the square root computation

31 required when computing a Euclidean norm. In addition, the multiplication and division properties that Walther [2] discovered, that can be implemented with the same

CORDIC architecture, can also be utilized.

Considerable effort was initially placed on remapping the algorithm to better fit the CORDIC structure. During this exploration, several methods in the literature were reviewed to provide additional insight. Previously Hsiao and Delosme [3]-[6] explored and documented methods for mapping quaternion calculations in CORDIC- like structures with a focus on vectoring operations. Mencer, Semeria, Morf and

Delosme [14] proposed a method to remap an adaptive filter application that involved spherical geometry. The recently published work of Lang and Antelo [13] presented other techniques for computing 3D computer graphics operations using CORDIC-type primitives. These operations compute the orientation of an object when it is rotated by a predetermined amount. The structure of the solution to the kinematic equation for rotation presented in this chapter incorporates sensor-based angular inputs that cannot be derived from another source therefore no insight to their behavior is achieved. In addition the equivalent Eigen axis and angle values for the input quaternion would have to be available prior to beginning the calculation to provide any visibility into its geometrical mapping. The decomposition of the quaternion into its Eigen axis and angle components would require additional calculations to be performed. Therefore with these constraints the mapping of the operations necessary to perform the calculations using the CORDIC algorithm proceeded in the manner described in this chapter.

The properties of the CORDIC algorithm presented in this chapter concern modes that are necessary for performing calculations used in the closed form approximation. Three different ways of implementing the calculations are presented, each of which uses the CORDIC algorithm as the primary processing element (PE).

The first implementation employs four CORDIC PE’s in parallel, which improves the speed of the calculation. The second implementation uses only one CORDIC PE, which reduces physical area requirements. The third implementation incorporates scale factor correction circuitry within the CORDIC element structure. ASIC synthesis, accuracy analysis, and boundary conditions for the implementations are described for a 33-bit CORDIC PE. The extra bit above 32 is a sign bit.

3.2 The CORDIC Algorithm

The CORDIC algorithm, originally developed by Volder [1] and extended by

Walther [2], provides a method for calculating elementary functions using shift and addition operations that are relatively straightforward to implement in hardware. The algorithm is based on Givens' method [8], which can be represented by the equations

⎡x1⎤ ⎡ cos β sin β ⎤⎡x0 ⎤ ⎢ ⎥ = ⎢ ⎥⎢ ⎥ (3.1) ⎣y1⎦ ⎣− sin β cos β ⎦⎣y0 ⎦

33 that express the rotation of a 2-dimensional vector at the origin with initial endpoint at coordinates x0 , y0 through an angle β to achieve a new orientation with endpoint at x1 , y1 . Factoring cos β and converting it to the inverse square root form, (3.1) can be expressed in terms of the tangent of β as

−1 ⎡⎤x1 2 ⎡⎤1tanβ ⎡⎤x0 ⎢⎥=+( 1tanβ ) ⎢⎥⎢⎥. (3.2) ⎣⎦y1 ⎣⎦− tanβ 1 ⎣⎦y0

The CORDIC algorithm uses difference equations based on (3.2). A general iterative form for (3.2) that incorporates the possibility of rotation through imaginary angles and also facilitates rotation through prescribed values of tan β can be expressed

⎡ x1mi+1⎤⎡δα i i ⎤⎡⎤x i ⎢ ⎥⎢=km ⎥⎢⎥ (3.3) ⎣ y-i+1⎦⎣δα i1y ⎦⎣⎦ i

and z=z+i+1 iδθ i i (3.4)

⎧tanh−1(α ) if m = −1 ⎪ i −1 where θ = α if m = 0 and k =+1 α 2 . i ⎨ i mi( ) ⎪ tan−1(α ) if m = +1 ⎩ i

In (3.3), i is the index of the iteration that rotates the vector with endpoint coordinates initially at xi , yi to those at xi+1 , yi+1 . Typically αi is set to equal to

2−i so that the computations in (3.3) can be performed as simple shifts and additions of binary data [7]. Values forθi are generally obtained from a look-up table. The value of δ i is either +1 or -1 and is chosen so that either y or z is driven toward zero to obtain the desired type of functions. m specifies whether the class of function being evaluated is linear ( m =0), circular ( m =+1) or hyperbolic ( m =-1).

The CORDIC algorithm is classically implemented using fractional arithmetic arguments. Since the normalized quaternion algebra requires that the magnitude for all elements remain less than or equal to one, the use of CORDIC architecture can be efficiently applied to the description of quaternion operations. In addition, the architecture is reusable for the calculation of all functions required to produce the solution to the kinematic equation of rotation. The error generated when the CORDIC algorithm performs an operation with a quaternion element requiring an input or generating an output of 1 or -1 will be within the resolution of the CORDIC architecture. A bit parallel structure representing a CORDIC processing element is shown in Figure 3.1.

xi yi zi

Registers

Barrel Shifters

α i θ i

δ δ -/+ i +/- i -/+

xi +1 yi +1 zi +1

⎧ sign of zi rotation δ i = ⎨ sign of xy vectoring ⎩ ii

Figure 3.1 CORDIC Processing Element

Several functions that are available from the circular and linear modes of the CORDIC

processor are shown in Figure 3.2 [1]. Note that x0 , y0 and z0 are the initial input

N 2 values. A distortion factor K 0 = ∏ 1+α i ≈1.65 emerges from the evaluation of i=0 circular functions, where N is the number of iterations performed to complete a

CORDIC calculation.

X K0(x cos z - y sin z) 22 x x X Kx0 + y K (y cos z + x sin y Y 0 y Y 0 -1 z Z 0 z Z z + tan ()y/x Circular (m=+1), z→ 0 Circular (m=+1), y → 0 a) b)

x X x x X x y Y y + xz y Y 0 z Z 0 z Z z + (y/x) Linear (m=0), z→ 0 Linear (m=0), y → 0 c) d)

Figure 3.2 Functions for CORDIC Modes a) Circular Rotation, b) Circular Vectoring, c) Linear Rotation, d) Linear Vectoring

3.3 Parallel Design of the Attitude Update System

The CORDIC algorithm can be implemented as a processing element to calculate the attitude update equation expressed in Chapter 2, which is repeated here for convenience

⎛⎞11ψq(tn ) ⎛⎞ qq()t=tcosn+1 () n ⎜⎟ΔΔθ + sin ⎜⎟θ (3.5) ⎝⎠22Δθ ⎝⎠

where

⎡ 0 Δθ321-Δθ Δθ ⎤ ⎢-Δθ 0 Δθ Δθ ⎥ ψ = ⎢ 312⎥ . (3.6) ⎢ Δθ21-Δθ 0 Δθ 3⎥ ⎢ ⎥ ⎣⎢-Δθ123-Δθ -Δθ 0 ⎦⎥

Using CORDIC functions, (3.5) can be partitioned into equations that express the magnitude of rotation, and the in-phase and quadrature components

⎡ ⎡ 2 ⎤⎤ ⎢ −1 ⎢ 2 ⎡ −1 ⎡ 2 2 ⎤⎤ ⎥⎥ Δθ = K • K 0 Δθ 3 + ⎢K • K 0 Δθ1 + Δθ 2 ⎥ (3.7) ⎢ ⎢ ⎣ ⎣⎢ ⎦⎥⎦ ⎥⎥ ⎣⎢ ⎣ ⎦⎦⎥

⎡⎡⎤KK−1 •Δq cos θ ⎤ , (3.8) ⎣⎣⎦0 ⎦

⎡ ⎡⎡⎤ψ ••⎡⎤KK−1 q sin Δθ ⎤ ⎤ and ⎢ ⎣⎣⎦⎣⎦0 ⎦ ⎥ . (3.9) ⎢ Δθ ⎥ ⎣⎢ ⎦⎥

Each set of square brackets above indicates a single CORDIC operation. Pre-scaling is performed to suppress the distortion factor by multiplying by a constant

1 K −1 = ≈ 0.606 . K0

It is possible to combine the sine and cosine operators in a single CORDIC operation. However, division by Δθ in (3.9) could cause output overflow.

Performing the sine calculation prior to dividing by Δθ provides a fractional result that avoids this situation. Functions used in this paper are from either the linear or circular class and are listed in Table 3.1.

A sequential case of a hardware system can be described using a relevant data dependency graph, as shown in Figure 3.3. Processing Element types M, D, V, C, and

S seen in the Figure are defined in Table 3.1. The table also shows abbreviations used and values of input control bits. Each PE in the data dependency graph uses two of the three recursively generated values x, y, z as inputs. Results appear on one output register of each PE. To decrease the processing time it was decided to use four

CORDIC processors in parallel for the entire system. The functions were allocated so that all processors would be in use during every operation.

The algorithm requires the following steps:

1) The input quaternion is scaled by the distortion factor 1/ K 0 to increase the

dynamic range of the algorithm.

2-5) Each row of the ψ matrix is multiplied by the scaled quaternion computed in

the first iteration to produce a four element vector representing an

unnormalized cross product between the rate sensor data and the scaled input

quaternion. Since the ψ matrix only requires three multiplication operations

per row, the calculations for determining Δθ can be performed concurrently in

the unused CORDIC processor.

The following operations are performed to calculate Δθ :

a) Compute the magnitude of the vector created from components Δθ1

and Δθ2 .

b) Scale the result from a) by the distortion factor 1/ K 0 .

c) Compute the magnitude of the three-dimensional vector that

incorporates Δθ3 and the results from iteration b) as its components.

d) Scale the result from c) by the distortion factor 1/ K 0 .

6) If Δθ =0then update q(tn+1 ) as q(tn ) and skip steps 7) and 8). Otherwise

take the results obtained in the second through fifth iterations and compute

⎛⎞1 . ψq()tsinn ⎜⎟Δθ ⎝⎠2

7) Compute ⎛⎞1 . q()tcosn ⎜⎟Δθ ⎝⎠2

8) Divide 6) by Δθ and add to the results from 7).

1 2 3 4 5 6 7 8

Δθ2 1/K0 Δθ3 1/K0

Δθ 1 V M V M 1/K0 Δθ3 Δθ2 q1(tn) M q1(tn+1) C +

M M q2(tn) M q2(tn+1) M C +

q3(tn) M M M q3(tn+1) M C +

M q4(tn) M M q4(tn+1) M C +

M M M S D

S D Δθ1 Δθ2 Δθ3

S D

Figure 3.3 Attitude Update Algorithm Data Dependency Graph

The algorithm described above was implemented in a SIMD (Single Instruction

Multiple Data) structure and simulated using VHDL. A hierarchical block diagram is shown in Figure 3.4. The present sensor data values and the previous quaternion elements are received and stored. For each iteration, the attitude update controller generates control bits representing the convergence mode and class type, as specified in Table 3.1, that configure the CORDIC processing elements so that the desired functions are computed. In addition, the controller provides two n-bit fixed point inputs, in two's complement representation, to each processing element. The processing elements complete their assigned calculations within n clock cycles, the results are then stored in the controller, and the controller is incremented to provide the control bits and inputs to the processing elements for the subsequent iteration. The process is repeated for all eight iterations, then results are latched and become the output for the attitude update system.

Table 3.1 CORDIC Functions Implemented by the System

PE Control Function Type Convergence Class Inputs Output Implementation Cosine C z -> 0 Circular x,z x K0xcos(z) Sine S z -> 0 Circular x,z y K0xsin(z) Square Root V y -> 0 Circular x,y x 22 Kx0 + y Multiplication M z -> 0 Linear x,z y xy Division D y -> 0 Linear x,y z y/’x

Although there have been a number of suggestions for increasing the speed and/or reducing the complexity of CORDIC algorithm processors for special applications [15], the original architectural designs presented by Volder [1] and

Walther [2] were used. The reason for this choice was to simplify the attitude update calculation, which involves an array of different CORDIC based computations, all of which are undertaken in essentially the same time. Although bit-serial input instantiations have been performed that can lead to reduced area requirements [8], it was decided to synthesize a parallel input design in order to focus on high computational speed. It requires on the order of (n+2) clock cycles to complete a

CORDIC computation, where n is the number of bits for each input. Two additional clock cycles are needed to capture the input information and send the final result to the output. Therefore, the computation time to perform the attitude update calculations, using only the four CORDIC processors of this example, is approximately 8(n+2) clock cycles.

Q P Q A u O I r u n a u n e a g t t p v t l e p u i e e r u t o r s n t u n i s i o o n n

Attitude Update Controller

C C C C N N N N T I O T I O T I O T I O R N U R N U R N U R N U L 0 T L 1 T L 2 T L 3 T 0 0 1 1 2 2 3 3

CORDIC CORDIC CORDIC CORDIC Processing Processing Processing Processing Element Element Element Element PE0 PE1 PE2 PE3

Figure 3.4 Attitude Update Implementation (Block Diagram)

3.3.1 Limitations on the Attitude Update Computation

When a minimum value is imposed on the range of the input angle, the input quaternion values can be allowed to be unbounded. Computed results for |Δθ| must have magnitude less than 1 when the CORDIC algorithm is used in the vectoring mode. This imposes a limitation on the incremental angles to which the CORDIC approximation can be applied. When the scale factor is included, imposition of the magnitude limit results in a functional relationship among allowable rate sensor angles in the form

222 1K≥ 01Δθ +Δθ 2+Δθ 3 . (3.10)

A hard limit more convenient to apply can be obtained by assuming all rate sensors are at maximum range. Then Δθmax , which can be calculated from

2 1 ≥ K0 3Δθmax , (3.11)

becomes 62.98 degrees. Therefore, if the range of detection for each rate sensor can be limited to 62.98 degrees in each direction, unconstrained combinations of input angles can be allowed.

3.3.2 ASIC Synthesis

The design for the controller and CORDIC core processors has been synthesized with an Artisan Component standard cell library that utilizes TSMC

0.13um CMOS technology using Synopsys Design Compiler for the bit-parallel

CORDIC processing element and controller. Based on synthesis results, it is estimated that the area of a fully routed core with 4 CORDIC processors and controller will be 0.29 mm2.

The CORDIC implementation requires a minimum operational delay of 0.52 microseconds. This value is obtained by summing 280 clock cycles, which are required to complete the attitude update calculation, and a 1.85 ns critical path delay derived from the results of VHDL synthesis using the Artisan TSMC 0.13um standard cell library.

3.3.3 Accuracy Analysis

The attitude update system was tested using simulations that employed VHDL code developed for the research. The quaternion input vectors used in the simulations were obtained by generating a set of random quaternion elements that were originally uniformly distributed, but values that did not satisfy the relation

222 qq123+ +≤ q1 (3.12)

were eliminated. The fourth quaternion element q4 can be calculated from the

222 2 relationship q+q+q+q=1123 4 . The sign of q4 was selected randomly. One thousand quaternion input vectors were generated in this manner. Matlab was used to provide a variety of input conditions for calculation of the algorithm.

The input angle was varied for each test of the 1000 random input quaternions from 0.01 to 100 degrees for each input axis. The results, which are presented in

Figure 3.5, indicate the minimum accuracy is at least 25 bits. The accuracy in bits was considered as a negative quantity since the bits represent a fractional number, therefore the value 0.5 would be represented by -1 bits.

-27

-26

-25 Accuracy (bits) (bits) Accuracy -24 0.01 0.1 1 10 100 Angle (degrees)

Figure 3.5 Accuracy of the Update Computation for Random Quaternion Inputs

3.4 Serial Design of the Attitude Update System

3.4.1 CORDIC Operations

Computations that the CORDIC algorithm can perform in either the circular or linear mode are enumerated in Figure 3.2. Several of the computations can be performed during one complete CORDIC computational cycle, which results in an efficient method of computation. For example, if a CORDIC processing unit is configured to perform the calculations for a forward rotation, the output obtained in a computation cycle is

yx(t=ytcoszt+tsinztn+1) ( n) ( ( n)) ( n) ( ( n )) . (3.13)

Therefore, a single CORDIC computational cycle accomplishes two multiplications in addition to two computations of trigonometric functions. To implement the rotation operations in (3.5), four CORDIC computational cycles are required, since there are four elements in q. The components in (3.5) appear in (3.13) as the previous

quaternion value y ()t n , the normalized cross product of the previous quaternion value,

the measured angular displacements x (t n ) , and half the magnitude of the angular

displacement in three dimensions, z (t n ) . The arguments x (t n ) , y ()t n and z (tn ) for each quaternion element can be expressed as

ψ q x ()t = i , (3.14) i n Δθ

where ψ i is the ith row of the matrix ψ presented in (3.6), namely,

ytin( ) = qi , (3.15)

i denotes one of the four quaternion elements, and

11 zt()= Δθ = Δθθθ222 +Δ +Δ . (3.16) n 22123

Based on the rules for unit quaternion arithmetic, the magnitude of each quaternion scalar component will not be greater than one. The component angles of

Δθ , in (3.14) and (3.16), viz., Δθ1, Δθ2 , and Δθ3 , are ratios of values in radians relative to π and are each less than one. Since both the displacement angles and the quaternion scalar components have values less than or equal to one, the multiplication can be performed using the CORDIC architecture as originally proposed by Walther

[2] without concern for overflow.

3.4.2 Computation of the Magnitude of the Angular Displacement

With proper scaling, the magnitude of Δθ can be computed using the square

root operation obtainable from the reverse circular CORDIC mode. If Δθ1 , Δθ2 ,and

Δθ3 are appropriately prescaled, the magnitude can be found using two cascaded

CORDIC calculations performed in the following order

22 Δ=Δ+Δθ12θθ 1 2 (3.17)

2 2 Δ=Δθ θθ12 +Δ 3 (3.18)

3.4.3 Calculation of the Cross Product

The vector elements resulting from the cross product operation ψ qt() presented in (3.5) can be computed using the properties of vector multiplication, viz.,

ψ12qqt= ()Δ+θθθ 34132 qt () Δ− qt () Δ, (3.19)

ψ 23qqt= ()Δ+θθθ 14213 qt () Δ− qt () Δ, (3.20)

ψ 31qqt= ()Δ+θθθ 24321 qt () Δ− qt () Δ, (3.21) and

ψ 4112233qqt= −Δ−Δ−Δ()θθθ qt () qt () . (3.22)

The forward linear mode of the CORDIC algorithm can perform a multiply

accumulate (MAC) operation, so that each ψ iq in (3.19-3.22) can be computed using three MAC cycles. In addition to the MAC cycles, two’s complement inversions are required for the subtractions in (3.19-3.22). An example demonstrating the scheduling of the MAC cycles that are necessary to compute (3.19) is shown in Figure 3.6.

q q3 4 q2

X X X 2’s 0 Y Y Y ψ q Cm 1 Z Z Z

m=0 m=0 m=0 Δθ Δθ1 Δθ 2 z0→ z0→ 3 z0→

Figure 3.6 Δθ q Vector Multiplication

Division by Δθ can be performed using the backward linear CORDIC mode as long as the magnitude of the solution remains less than one. This condition is met if division is scheduled to be performed after the MAC computations.

3.4.3.1 Scaling

N −i 2 K0 in Figure 3.2, the distortion factor ∏ 12+ ()that is involved in the i=0 calculation of 3.17 and 3.18, is well known from the CORDIC evaluation of circular

functions. N in K0 is the number of iterations performed to complete a CORDIC

calculation. Since K0 remains constant when the number of iterations is fixed, the terms in (3.5) can be prescaled by multiplying each quaternion component by the

inverse of K0 . The displacement angles Δθ1 and Δθ2 also require prescaling in order

−1 2 to calculate Δθ . They must be multiplied by (K0 ) since they are subjected to two

51 square root operations (3.17) and (3.18). To support hardware reuse, all prescaling tasks involving quaternion components and displacement angles in this study were performed using a single CORDIC processor.

3.4.4 System Operation and Scheduling

A hardware system that performs attitude update calculations and combines a

CORDIC processor and a sequential controller can be described using the diagram presented in Figure 3.7.

qin Controller qout

Δθ clk

CORDIC Registers

Figure 3.7 System Block Diagram

The system of Fig. 3.7 is partitioned so that prescaling operations can be removed as

CORDIC processes without affecting other operations. The algorithm and the number of CORDIC operations involved in each step can be described as follows:

−1 1-4) The input quaternion is multiplied by the distortion factor K0 to prescale the

transcendental CORDIC operations.

5-7) The input displacement angles Δθ1 , Δθ2 and Δθ3 are multiplied by the

−1 2 −1 2 −1 distortion factors (K0 ) , (K0 ) , and K0 , respectively, to prescale their

values prior to performing the square root computations.

8-10) The first row of the ψ matrix is multiplied by the quaternion vector whose

prescaled elements were computed in the first four steps to produce a scalar

element that is the unnormalized cross product of the rate sensor data and the

scaled input quaternion.

11-19) Steps 11 through 13, 14 through 16, and 17 through 19 respectively represent

the second, third, and fourth rows of the product of theψ matrix and the

quaternion vector computed in a manner similar to iterations 8 through 10.

20) The magnitude of the vector created from components Δθ1 and Δθ2 that were

prescaled in the fourth and fifth steps is computed.

21) The magnitude of the three-dimensional vector Δθ that incorporates Δθ , as 3

prescaled in step 7, and the results from step 20 as its components, is

computed.

22-25) If Δθ =0 then update q(tn+1 ) as q(tn ) and skip steps 22 through 29.

Otherwise take the results obtained in steps 10, 13, 16 and 19 and divide them each by

Δθ .

1 ψq t 26-29) Compute ⎛⎞( n ) ⎛⎞1 . q()tcosn ⎜⎟Δθ + sin⎜⎟Δθ ⎝⎠2 Δθ ⎝⎠2

The algorithm has been implemented in a structure that was described and simulated using VHDL and incorporates the scheduling of the operations described in

Figure 3.8.

−1 −−21 Kq()tn KKΔθ1,2, Δθ 3 m=0, z->0 (4) m=0, z->0 (3)

ψq(tn ) m=0, z->0 (12)

ψq (tn ) Δθ ⎛⎞1 ψq()tn ⎛⎞1 q ()tcos Δθ + sin Δθ Δθ n ⎜⎟2 ⎜⎟ m=1, y->0 ⎝⎠Δθ ⎝⎠2 (2) m=0, z->0 (4) m=1, z->0 (4)

Figure 3.8 Scheduling of CORDIC Operations

In the simulations, present sensor data values and previous quaternion elements are received and stored. In each step of the algorithm, the attitude update controller sends two control bits to the CORDIC processing element. The control bits specify the circular or linear mode and either y or z convergence needed by the CORDIC processing element to compute the desired functions. The processing element completes the assigned calculation within n clock cycles, the results are stored in the

54 controller, and the controller is incremented to provide the control bits and inputs for the subsequent CORDIC processing steps. The process is repeated for all 29 steps.

Then results are latched and become the output for the attitude update system. Each

CORDIC iteration step requires on the order of (n+2) clock cycles to complete a computation, where n is the number of bits for each input. Therefore, the computation time to perform the attitude update calculations, in this example, is approximately

29(n+2) clock cycles.

3.4.5 ASIC Synthesis

The design for the controller and CORDIC core processors has been synthesized in a bit-parallel configuration using Synopsys Design Compiler with an

Artisan Component standard cell library that utilizes TSMC 0.13um CMOS technology, as with the parallel system. The cell area in this case is found to be 0.084 mm2. The Artisan TSMC 0.13um standard cell library CORDIC implementation requires a minimum operational delay of 4.95 microseconds, which is computed using

1015 clock cycles required to complete the attitude update calculation and a 4.88 ns critical path delay derived from the results of VHDL synthesis.

3.4.6 Attitude Update Computation Performance Analysis

3.4.6.1 Boundary Analysis

In practice, rate sensor data is often provided by 16-bit analog to digital converters (ADCs). The input bits of all angular measurements lower than the sixteenth most significant bit were forced to zero in simulations in order to support this assumption. Since the maximum angular displacement is 180 degrees, the minimum angle that is measurable with a 16-bit ADC is 0.0027 degrees.

When the CORDIC algorithm is used in the vectoring mode, computed results must have magnitude less than 1. This imposes a limitation on the incremental angles to which the CORDIC approximation can be applied. Imposition of the magnitude limit results in a functional relationship among allowable rate sensor angles in the form

222 Δθ123+Δθ +Δθ ≤180 . (3.23)

A hard limit more convenient to apply can be obtained by assuming all rate sensors are at maximum range, as discussed in Section 3.3.1. Then Δθmax can be calculated from

2 3180Δ≤θmax , (3.24)

which computes to a maximum value of 103.94 degrees. Therefore, the range of detection for each rate sensor can be limited to 103.94 degrees in each direction to allow for an otherwise unconstrained combination of allowable input angles.

3.4.6.2 Accuracy Analysis

An estimate can be performed to determine the number of bits required by the

CORDIC implementation to meet a defined output accuracy. Errors arise in the system since the CORDIC algorithm has both approximation errors due to angle quantization and numerical rounding errors due to the finite word length of the operands x, y and z. Hu [9] has shown that when performing a rotation calculation,

the effective number of significant binary digits d eff is determined by either

⎡ n-1 ⎛⎞n ⎤ ⎢1+∑ ⎜⎟∏ k0 (i) ⎥ j=1 i= j 2=2+2-deff -1 -n -b-0.5 ⎢ ⎝⎠ +1⎥ , (3.25) ⎢ n ⎥ ⎢ ∏ k(i)0 ⎥ ⎣⎢ i=0 ⎦⎥

or, when performing multiplication or division calculations, by

2=2+(n+1)2-deff -1 -n -b-0.5 (3.26)

where b is the fractional number of binary bits per operand and n is the number of iterations. For example, the effective number of significant binary digits for a 32-bit

CORDIC processor with 32 iterations is 28 bits for multiplication and 29 bits for rotation or square root calculations. Comparison of Hu's results with heuristically determined data showed the former to be conservative. The heuristically determined accuracy for this implementation was obtained by testing the proposed algorithm with

1000 quaternion values derived from uniformly random data also included the multiplicative identity quaternion. The input angle was varied for each test of the

1000 random input quaternions from 0.01 to 100 degrees for each input axis. The results are presented in Figure 3.9 and show the minimum accuracy to be at least 24 bits.

-27

-26

-25 Accuracy (bits) Accuracy

-24 0.01 0.1 1 10 100 Angle (degrees)

Figure 3.9 Random Quaternion Accuracy

3.5 Serial System Design with Parallel Scale Factor Compensation

To reduce the number of CORDIC cycles, the CORDIC processor can be modified so the scale factor compensation is performed as an integral part of the

CORDIC computation. The intent of the thesis is to demonstrate the applicability of the original CORDIC algorithm to attitude determination. In this section it is

58 demonstrated how the CORDIC based architecture can be modified to include enhancements such as parallel scale factor compensation. Vectoring mode normalization is also included in the modified system design.

3.5.1 Scale Factor Compensation

A micro-rotation operation in the circular mode can be described as

−i x(1)()2ixiyi+= −σ i (), (3.27)

−i y(1)()2(),iyixi+= +σ i (3.28)

−−1 i zi(1)()+= zi −σ i tan(2) (3.29)

where coefficient σ , which determines the direction of the rotation, takes either +1 or

-1. The circular mode computation of (3.27-3.29) yields a value K = − i 2 12+ ()σ i times larger than expressed in (3.27-3.29). The total scale factor growth during a

N 2 complete CORDIC computational cycle is the product Ko =∏ 12+ ()− i . Ko is the i = 0 scale factor depicted in Figure 3.2. N is the number of iterations performed to complete a CORDIC calculation.

Parallel compensation is a method that can be used to integrate scale factor distortion correction into the CORDIC computation. The carry-analysis method of compensation [10], for example, performs distributed multiplication during each micro-rotation by multiplying each micro-rotation x(i+1) and y(i+1) by K −1 . For

59 illustration, the compensated value of x in (3.27) can be found from the (i+1)th micro- rotation as

n −11−−j x(1)nK+⋅ =∑ 2 ⋅δ xj K (3.30) j=0

The value of δ xj is computed from the previous micro-rotation using the relationship

δ xj=−+2(cyxi−− j2 ) j ( ) (3.31)

th -j where ji=−2 , x− j is the value of the j bit of x(i), i. e., the term of weight 2 , c− j is

-j −−(1)i the carry-out bit generated at position 2 , and y2 is the sign bit of σ i−1 2(1)yi− .

δ xj is an integer in the range -3 to +3. A CORDIC word representing (3.27) or (3.28) must include two integer bits to accommodate the values of the operands that can be as large as the value of 2 K, or approximately 2.32, to avoid overflows [10].

3.5.2 Vectoring Mode Normalization

More accurate results from division and square root operations can be achieved when the inputs are normalized to the range 0.5 to 1 [11]. This is accomplished by shifting both operands to the left by the number of leading zeroes in the largest operand. A leading zero detector circuit [12] counts the number of leading zeros in the largest operand.

3.5.3 System Architecture

The enhanced CORDIC processing element is shown in Figure 3.10. When m is set to 1, the CORDIC operation is circular, and scale factor compensated values for x and y can be computed. When m is 0, the processing element is the standard linear

CORDIC architecture mode as defined by Volder [1]. The calculations for the z component are not affected by the scale factor compensation method.

-1 -1 -1 Seven values for δiK that range from -3K to +3K can be stored in a table and selected depending upon the value of δi for either the x or y path. As shown in

-1 the figure, when m=1, the appropriate value of δiK is shifted to the right by index value i and added to the previous result.

xi yi zi

Scale Factor Compensated CORDIC -1 -1 δxiK δyiK

Standard i shift shift CORDIC Architecture + +

reg reg

m=1

xo yo zo

Figure 3.10 Architecture of an Enhanced CORDIC Processing Element

The system architecture shown in Figure 3.11 includes an enhanced CORDIC processing element, state controller, shift registers, a leading zero detector circuit LZ, and memory. The state controller controls the CORDIC operations and scheduling.

After reading the input data from memory, the state controller passes the mode and direction of rotation to the enhanced CORDIC processing element, the shift registers, and the leading zero detector. After a full CORDIC computational cycle is completed, the state controller stores the results in memory. When control signal r is 0, the leading zero detector determines d, the smaller number of either leading zeros or, if x or y is a negative number, leading ones. The shift registers located ahead of the data inputs to the CORDIC processing element shift input data for circular or vectoring operations in the manner described in Table 3.1. After a complete CORDIC cycle has concluded, the shift registers that receive the output data from the CORDIC processing element perform the same operation but in the opposite direction to reconstruct the results into fractional two’s complement representations. The state controller then stores the results in memory and accesses the set of input data needed for the next

CORDIC computational cycle.

read Memory

xin or yin xin yin zin r d LZ

s s Shift Shift State Controller xi yi zi Enhanced m, r CORDIC Processing Element

xo yo zo s s Shift Shift

xout yout zout write Memory

Figure 3.11 Enhanced CORDIC PE, System Architecture

3.5.4 System Operation and Scheduling

The scheduling of a hardware system that performs attitude update calculations and combines a CORDIC processor and a sequential controller can be described using the diagram presented in Figure 3.12.

ψq (t ) ⎛⎞1 ψq()tn ⎛⎞1 Δθ ψq()tn n q()tcos Δθ + sin Δθ n ⎜⎟2 ⎜⎟ m=1, m=0, Δθ ⎝⎠Δθ ⎝⎠2 y->0 z->0 m=0, (2) (12) z->0 (4) m=1, z->0 (4)

Figure 3.12 Scheduling of CORDIC Operations

The enhanced algorithm and the number of CORDIC operations involved in each step is

1) The magnitude of the vector created from components Δθ1 and Δθ2 is

computed.

2) The magnitude of the three-dimensional vector Δθ that incorporates Δθ3 and

the results from step 1 as its components is computed. If Δθ =0 then update

q ()tn+1 as q(tn ) and skip steps 3 through 22.

3-5) The first row of the ψ matrix is multiplied by the quaternion vector whose

prescaled elements were computed in the first four steps to produce a scalar

element that is the unnormalized cross product of the rate sensor data and the

scaled input quaternion.

6-14) Steps 6 through 8, 9 through 11, and 12 through 14 respectively represent the second, third, and fourth rows of the product of theψ matrix and the quaternion vector computed in a manner similar to steps 3 through 5.

15-18) Take the results obtained in steps 5, 8, 11 and 14 and divide them each by

Δθ .

1 ψq t 19-22) Compute ⎛⎞( n ) ⎛⎞1 . q()tcosn ⎜⎟Δθ + sin⎜⎟Δθ ⎝⎠2 Δθ ⎝⎠2

The algorithm has been described and simulated using VHDL. In the simulations, present sensor data values and previous quaternion elements are received and stored. In each step of the algorithm, the attitude update controller sends two control bits to the CORDIC processing element. The control bits specify the circular or linear mode and either y or z convergence needed by the CORDIC processing element to compute the desired functions. The processing element completes the assigned calculation within n clock cycles, the results are stored in the controller, and the controller is incremented to provide the control bits and inputs for the subsequent

CORDIC processing steps. The process is repeated for all 22 steps. Then results are latched and become the output for the attitude update system. Each CORDIC iteration step requires on the order of (n+2) clock cycles to complete a computation, where n is the number of bits for each input. Therefore, the computation time to perform the attitude update calculations, in this example, is approximately 22(n+2) clock cycles.

The design for the controller and CORDIC core processors has been synthesized in a bit-parallel configuration using Synopsys Design Compiler with the same Artisan Component TSMC 0.13um CMOS standard cell library used in the other simulations. The synthesized cell area is found to be 0.12 mm2. The CORDIC implementation using the Artisan TSMC 0.13um standard cell library requires a minimum operational delay of 1.77 microseconds, which consists of 770 clock cycles required to complete the attitude update calculation and 2.3 ns critical path delay. The latter is derived from the results of VHDL synthesis.

The results are presented in Figure 3.13 and indicate the minimum accuracy is at least 24 bits, as was found with the series design example.

-27

-26

-25 Accuracy (bits)

-24 0.01 0.1 1 10 100 Angle (degrees)

Figure 3.13 Computed Accuracy using Random Quaternions

3.6 Conclusion

CORDIC algorithm based systems that implement the attitude update method selected in Chapter 2 to solve the kinematic equation have been described in this chapter. Three different implementations were described. Synthesis results for the implementations are summarized in Table 3.2. Although the series CORDIC implementation appears to be the slowest, since most applications needing attitude determination do not need fast updating, it is a viable choice, and may even be the preferred choice because it requires the least area.

Table 3.2 Comparison of Several CORDIC Based Implementations

Parallel Series Series with Scale Factor Correction Number of 8 29 22 CORDIC Cycles Std Cell Area 0.29 mm sq. 0.084 mm sq. 0.12 mm sq.

ComputationTime 0.518 microseconds 4.95 microseconds 1.77 microseconds

Output Accuracy 25 bits 24 bits 24 bits

Max Angular 63 degrees 103 degrees 103 degrees Displacement (in all directions)

References

[1] J. E. Volder, "The CORDIC Trigonometric Computing Technique", IRE Transactions on Electronic Computers, September 1959, pp. 330-334.

[2] J. S. Walther, "A Unified Algorithm for Elementary Functions", in AFIPS Spring Joint Conference Proceedings, 1971, pp. 379-385.

[3] S.-F. Hsiao and J.-M. Delosme, "CORDIC Algorithms in Four Dimensions", in SPIE Vol. 1348, Advanced Signal-Processing Algorithms, Architectures, and Implementations, 1990, pp. 349-360.

[4] S.-F. Hsiao and J.-M. Delosme, "Householder CORDIC Algorithms", IEEE Transactions on Computers, Vol. 4, No. 8, August 1995, pp. 990 - 1001.

[5] S.-F. Hsiao and J.-M. Delosme, "Parallel Singular Value Decomposition of Complex Matrices Using Multidimensional CORDIC Algorithms", IEEE Transactions on Signal Processing, Vol. 44, No. 3, March 1996, pp. 685-697.

[6] S.-F. Hsiao, Multi-Dimensional CORDIC Algorithms, Ph. D. Dissertation, Yale University, November 1993.

[7] X. Hu and R. G. Harber, "Expanding the Range of Convergence of the CORDIC Algorithm", IEEE Transactions on Computers, Vol. 40, No. 1, January 1991, pp. 13-21.

[8] R. Andraka, "A Survey of CORDIC Algorithms for FPGA Based Computers", ACM/SIGDA International Symposium on Field Programmable Gate Arrays, Monterey, CA, February 22-24 1998, pp. 191-200.

[9] Y. H. Hu, “The Quantization Effects of the CORDIC Algorithm”, IEEE Transactions on Signal Processing, Vol. 40, No. 4, April 1992, pp. 834 – 844.

[10] J. Villalba, T. Lang and E. L. Zapata, “Parallel Compensation of Scale Factor for the CORDIC Algorithm”, Journal of VLSI Signal Processing Systems, Vol. 19, No. 3, August 1998, pp. 227-241.

[11] K. Kota and J. R. Cavallaro, “Numerical Accuracy and Hardware Tradeoffs for CORDIC Arithmetic for Special-Purpose Processors”, IEEE Transactions on Computers,Vol. 42, No.7, July 1993, pp. 769-779.

[12] V. G. Oklobdzija, “An Algorithmic and Novel Design of a Leading Zero Detector Circuit: Comparison with Logic Synthesis”, IEEE Transactions on VLSI Systems, Vol. 2, No. 1, Mar. 1996, pp. 124 – 128.

[13] T. Lang, E. Antelo, “High-Throughput CORDIC-Based Geometry Operations for 3D Computer Graphics”, IEEE Transactions on Computers, Vol. 54, No. 3, March 2005, pp. 347-361.

[14] Oskar Mencer, Luc Semeria, Martin Morf, Jean-Marc Delosme, ”Application of Reconfigurable CORDIC Architectures”, Journal of VLSI Signal Processing, Vol. 24, No. 2, March 2000, pp. 211-221.

[15] Y. H. Hu, “CORDIC-based VLSI architectures for digital signal processing”, IEEE Signal Processing Magazine, Vol. 9, No. 3, July 1992, pp. 16 – 35.

The text of Chapter 3, in part, is a reprint of the material as it appears in the IEEE

Transactions on Instrumentation and Measurement and, in part, has also been submitted for publication in the IEEE Transactions on Circuits and Systems I. The dissertation author was the primary researcher and/or author and the co-author listed in this publication directed and supervised the research which forms the basis for this chapter.

CHAPTER 4

PARAMETRIC ANALYSES FOR CORDIC BASED

ALGORITHM AND COMPARISON TO OTHER

APPROXIMATION METHODS

4.1 Introduction

The method implemented in Chapter 3 provides a simpler way to determine the attitude of a moving object when compared to the traditional approximation techniques. In addition the most widely used existing methods have significant reduction in accuracy at large input displacement angles even though floating point arithmetic is utilized. The method developed during this research was implemented in using fixed point notation and does not incur the same loss in accuracy.

The previous chapter described several techniques for solving the kinematic equation for rotation using the CORDIC algorithm. The accuracy of these methods was assessed over a wide range of values for displacement angles, which are inputs to a signed 32 bit CORDIC processor structure. In this chapter the accuracy of the output is explored for various input bit widths of the CORDIC processor.

Orthogonality is also addressed, since it is an important property for quaternion based calculations. Results of a rotation can become distorted in the absence of orthogonality. Rotational systems can normalize the unit quaternion output

71 periodically, but it is possible to perform this process less frequently if the probability of computational error is reduced. In addition to characterizing the performance of the

CORDIC based algorithm, a brief comparison of die area and update rate is presented for several representative bit widths of a CORDIC processor.

For purposes of comparison, performance characteristics for two popular attitude update approximation methods are described in this chapter. The attitude update algorithm, which is described in Chapter 2, includes trigonometric functions for which several approximation methods have been developed. One of the most popular methods uses Taylor Series approximation [1][3]. The Taylor Series based approximation method has been investigated in this chapter for several different series lengths and compared results with the proposed CORDIC based method. An approximation method proposed by Wie [2] is also examined, which is also based on a

Taylor Series expansion [4]. Wie’s method linearizes the computations and therefore simplifies their implementation. However, as shown here, the accuracy is affected.

Two other methods that involve Obrechkoff and Pade approximation methods are also discussed in this chapter. The method using Obrechkoff integration [5] is more stable than a competitive fourth order Runge-Kutta approximation, but leads to a complex calculation [10]. Pade approximation is more accurate than Taylor Series methods, but requires more complex calculations because it involves approximating exponential functions as rational fractions rather than polynomial series [6].

4.2 Parametric Analysis of the CORDIC Based Algorithm

The CORDIC processor in this work is instantiated in fixed point representation. The performance of the system that implements the algorithm is therefore affected by the input bit width. Errors in the updated attitude output of the system are primarily concerned with accuracy and orthogonality. Since the angular data input to the system is typically supplied by an Analog-to-Digital Converter

(ADC), the number of bits in an input data word would usually be no more than 16.

The minimum CORDIC bit width analyzed is taken to be the same size, viz., 16. To account for the possibility of more accurate inputs, and taking note of the fact that the size of much digital hardware is incremented in units of bytes, other CORDIC input data widths that are also addressed here include 24, 32, and 40 bits.

Since the CORDIC algorithm based computations are iterative, with the number of iterations dependent on the rate of convergence, reducing the number of

CORDIC based iterations implies a cost in accuracy. This was described in more detail in Chapter 3 by discussing the results obtained by Hu [11], for example. In our work, the number of iterations taken initially is equal to the number of bits in the input data. Then the results are compared with those obtained when the number of iterations is reduced by 4 and also by 8, all of which is done for the 24-, 32-, and 40-bit wide input implementations of the CORDIC processor Performance characteristics are compared when fewer iterations are used and when the number of input bits is

73 reduced. Physical characteristics are also compared for the various CORDIC input bit widths. Analyses were performed for the CORDIC processor that was originally defined by Volder [12] and Walther [13], i. e., for the second method discussed previously in Chapter 3.

4.2.1 Accuracy as a Function of CORDIC Processor Bit Width and Iterations

The solution to the kinematic equation for rotation was computed one thousand times using randomly generated input quaternions, as described in Chapter 3, while all three displacement angle inputs remained fixed at 0.01 degrees. This process was repeated for 0.05, 0.1, 0.5, 1, 5, 10, 50 and 100 degrees for 16, 24, 32, and 40 bit wide

CORDIC instantiations. All the systems were described in VHDL. The absolute maximum value for any quaternion element was documented. Results, which are shown in Table 4.1 and Figure 4.1, indicate that an upper bound on the number of digits of accuracy in the output can be estimated heuristically. Note that for all of the figures, the accuracy in bits was considered as negative since the bits are a fractional representation of a value, therefore a value of 0.5 would be represented by -1 bits. The expected accuracy of an output quaternion element, deff , as a function of the number of output bits, can be conservatively described by the relations

ddeff= in − 7 for +θ1 <1 degree (4.1)

⎛⎞din ddeff=−− in 7log2 ⎜⎟ for +θ1 >1 degree . (4.2) ⎝⎠16

In (4.1) and (4.2), deff is the effective number of output bits, din is the number of input

bits in each quaternion element, and +θ1 represents one of the three input displacement angles.

Table 4.1 Output Accuracy as a Function of Displacement Angle for Various CORDIC Input Bit Widths Angle Input Bit Widths (Degrees) 16 24 32 40

0.01 -10 -17 -25 -33

0.05 -10 -17 -25 -33

0.1 -10 -17 -25 -33 0.5 -9 -17 -25 -33 1 -9 -17 -25 -33 5 -9 -17 -25 -32 10 -9 -17 -25 -33 50 -9 -17 -24 -32 100 -9 -16 -24 -31

-33

-28 40 32 -23 24 -18 16

-13 Output Accuracy(bits)

0.01 0.1 1 10 100

Displacement Angle (Degrees)

Figure 4.1 Output Accuracy as a Function of Displacement Angle for Various CORDIC Input Bit Widths

The number of iterations was varied for each CORDIC calculation to determine the effect of iterations on output accuracy for the various input bit widths. The VHDL code was modified to compute each CORDIC cycle for 4 fewer iterations and for 8 fewer iterations, both for the 24, 32, and 40 bit width instantiations. Results are shown in Figures 4.2, 4.3, and 4.4. From the figures, a conservative estimate for the effective number of output bits may be established as

ddneffiter= eff − (4.3)

where nn=−i 3 if ni > 3, and n = 0 otherwise.

The number of iterations removed from each CORDIC computation is represented by ni .

The computed results indicate that the output accuracy can be improved by 3 bits by using a CORDIC processor that is 8 bits wider than the conventional CORDIC processor for the same number of iterations. For example, when the number of iterations used to calculate the 40 bit wide CORDIC implementation is reduced from

40 to 32 computational cycles, the output accuracy is found to be at least 28 bits and requires 1015 cycles to compute. For the same number of total clock cycles, the output accuracy using the 32 bit wide CORDIC can only be accurate to up to 25 bits.

-34 -33

-32

-31 40 -30 36 32 -29

Output Accuracy (Bits) -28

-27 0.01 0.1 1 10 100

Displacement Angle (Degrees)

Figure 4.2 Output Accuracy as a Function of Displacement Angle for a 40 Bit Wide CORDIC Processor. The number of iterations is shown on the right.

-25

-24

-23 32 -22 28 24 -21 Output Accuracy (Bits) -20 0.01 0.1 1 10 100

Displacement Angle (Degrees)

Figure 4.3 Output Accuracy as a Function of Displacement Angle for a 32 Bit Wide CORDIC Processor. The number of iterations is shown on the right.

-17 -16

-15 24 -14 20 -13 16 -12 Output Accuracy (Bits) -11 0.01 0.1 1 10 100

Displacement Angle (Degrees)

Figure 4.4 Output Accuracy as a Function of Displacement Angle for a 24 Bit Wide CORDIC Processor. The number of iterations is shown on the right.

4.2.2 Orthogonality as a Function of Bit Width and the Number of Iterations

The orthogonality of the computed axes is another important performance parameter of the attitude update equation. If the magnitude of the quaternion vector is not equal to unity, the rotation can appear distorted, and within some additional update periods may become unusable. The orthogonality error ε can be described as

22 22 ε =−1 ()eeee0123 + + +

where e0 , e1 , e2 , e3 represent the elements of the output quaternion [9].

Computed values of orthogonality error ε as a function of displacement angle are shown in Figure 4.5 and Table 4.2. Although non-orthogonality is generally described as an angular measure, for comparison purposes, the more direct measure of the equivalent number of bits in ε that represents the error has been used in Figure 4.5 and Table 4.2. Note that the orthogonality measured in bits was considered as negative since the bits are a fractional representation of a value, therefore a value of

0.5 would be represented by -1 bits. Functions can be created using a method similar to the earlier section that conservatively describe the orthogonality of an output

quaternion element in effective output bits as ddorth_ eff= in −8 . A heuristic expression for the number of effective bits of orthogonality is derived from Figures 4.6, 4.7, and

4.8 and can be obtained from the computed data as was done previously for the computational accuracy (4.3). The result is

ddnorth__ iter= orth eff − (4.4)

where nn=−i 3 if ni > 3 and n = 0 otherwise.

Table 4.2 Orthogonality as a Function of Displacement Angle for Various CORDIC Processor Input Bit Widths

Angle Input Bit Widths (Degrees) 16 24 32 40 0.01 -10 -17 -25 -32 0.05 -9 -17 -24 -32 0.1 -9 -17 -24 -33 0.5 -9 -16 -24 -32 1 -9 -16 -24 -32 5 -9 -17 -24 -33 10 -9 -17 -25 -33 50 -10 -17 -26 -32 100 -9 -19 -25 -32

-35

-30

-25 40 -20 32 -15 24 16 -10 Orthogonality (bits) Orthogonality -5

0 0.01 0.1 1 10 100

Displacement Angle (Degrees)

Figure 4.5 Orthogonality as a Function of Displacement Angle for Various CORDIC Processor Input Bit Widths. The number of input bits is shown on the right.

-33 -32 -31 40 -30 36 -29 32 -28

Orthogonality (bits) Orthogonality -27 -26 0.01 0.1 1 10 100

Displacement Angle (Degrees)

Figure 4.6 Orthogonality as a Function of Displacement Angle for a 40 bit wide CORDIC Processor. The number of iterations is shown on the right.

-25

-24

-23 32 28 -22 24 -21

Orthogonality (Bits) -20

-19 0.01 0.1 1 10 100

Displacement Angle (Degrees)

Figure 4.7 Orthogonality as a Function of Displacement Angle for a 32 bit wide CORDIC Processor. The number of iterations is shown on the right.

-17

-16

-15 24 -14 20 -13 16

Othogonality (bits) Othogonality -12 -11 0.01 0.1 1 10 100

Displacement Angle (Degrees)

Figure 4.8 Orthogonality as a Function of Displacement Angle for a 24 bit wide CORDIC Processor. The number of iterations is shown on the right.

4.2.3 Area, Update Rate and Power Consumption as a Function of

CORDIC Processor Bit Width and Iterations

In Chapter 3 the area required for the standard cells needed to instantiate the

33-bit CORDIC based algorithm was discussed. The area is obtained from a synthesis process. Computation of the area was repeated for the CORDIC processors that are

16, 24, 32 and 40 input bits wide. The computation included the area needed for the sequence controller. The results are shown in Tables 4.3 and 4.4. As expected, the results indicate that the total area of a processor increases approximately linearly with the number of input bits.

Table 4.3 Standard Cell Area for Various CORDIC Processor Input Bit Widths

Area (mm squared) CORDIC Bit Non- Width Combinational Combinational Total 40 0.037 0.052 0.089 32 0.028 0.042 0.070 24 0.022 0.032 0.053 16 0.013 0.021 0.035

Table 4.4 Standard Cell Area for the Controller and CORDIC Processor for Various CORDIC Processor Input Bit Widths

Area (mm squared) CORDIC Bit c0 pe0 Width (Controller) (CORDIC) Total 40 0.063 0.026 0.089 32 0.049 0.021 0.070 24 0.037 0.016 0.053 16 0.025 0.010 0.035

The time needed to perform one CORDIC iteration was also determined as part of the synthesis process. Results for a minimum time constraint are shown in Table

4.5. The initial synthesis, result of which are shown in Table 4.4, did not constrain the area or provide a minimum allowable delay. The synthesis was performed iteratively to minimize the time delay shown in Table 4.5. The table shows the area of the standard cells when the processing time is minimized. The areas shown in Table 4.5 are not significantly greater than those obtained for the unconstrained synthesis, which are shown in Table 4.4.

Table 4.5 Standard Cell Area versus Iteration Time Duration for Various CORDIC Processor Input Bit Widths

Area (mm squared) CORDIC Bit delay c0 pe0 Width (ns) (Controller) (CORDIC) total 40 2.1 0.066 0.036 0.103 32 2 0.054 0.029 0.083 24 1.8 0.040 0.023 0.063 16 1.6 0.028 0.014 0.042

The calculation of the maximum possible attitude update rate is a function of the number of clock cycles, or iterations, per CORDIC cycle, viz.,

ttntot_max___ delay=⋅ delay iterations per CORDIC ⋅ n CORDIC _ comps (4.5)

where tmax_ delay is the minimum delay time for one clock cycle, niterations__ per CORDIC is the

number of iterations for one complete CORDIC cycle, and nCORDIC_ comps is the number of CORDIC cycles to complete the attitude update calculation. The maximum update rate given in Table 4.6 is the inverse of the total delay. This table demonstrates that although the number of clock cycles may be made the same for a CORDIC processor of larger input bit width if the number of iterations is reduced, the maximum update rate will still be lower due to the increased delay per iteration. For example, although the 40 bit wide CORDIC processor has the same number of computational cycles as the 32 bit wide CORDIC processor when the iterations per CORDIC cycle is reduced by 8 clock cycles, the maximum update rate is 5 percent less for the 40 bit wide input

CORDIC than the 32 bit wide input CORDIC due to the presence of additional

CORDIC circuitry.

Table 4.6 Maximum Update Rate Various CORDIC Processor Input Bit Widths, CORDIC Iterations

CORDIC Max Total Update Bit Clock Delay delay Rate Accuracy Orthogonality Width Iterations cycles (ns) (us) (kHz) (bits) (bits) 40 40 1218 2.1 2.56 391 -32 -31 40 36 1102 2.1 2.31 432 -31 -30 40 32 986 2.1 2.07 483 -28 -27 32 32 986 2 1.97 507 -24 -23 32 28 870 2 1.74 575 -23 -22 32 24 754 2 1.51 663 -20 -19 24 24 754 1.8 1.36 737 -16 -16 24 20 638 1.8 1.15 871 -15 -15 24 16 522 1.8 0.94 1,064 -11 -11 16 16 522 1.6 0.84 1,197 -8 -8

The minimum power consumption for various input bit widths is shown in

Table 4.7. Results were obtained from simulations following system synthesis. The update rate was selected as 240 Hz for this analysis since this is near the limit that would be addressed for sensor-based inputs. Power consumption in all cases was less than 0.1 milliwatts.

Table 4.7 Power Consumption at 240 Hz Update Rate

CORDIC Power Bit (micro- Width Watts) 40 60.8 32 52.2 24 40 16 26

4.2.4 Sensor Errors and Output Accuracy

The effect of sensor errors can also affect the accuracy of the calculated results. The computation of (2.32) that exhibits the largest reduction in accuracy due to sensor error occurs when either the last value of element q1, q2, or q3 is equal to 1 and the other elements of the input quaternion, including q4, are zero. For instance, if q1 from the last update is equal to 1, then

−Δθ Δθ −Δθ q =Δcos θ , q =Δ3 sin θ , q = 2 sin Δθ , and q = 1 sin Δθ (4.6) 1 2 Δθ 3 Δθ 4 Δθ

Replacing Δθ1 with Δ+θ11ε , Δθ2 with Δθ22− ε , and Δθ3 with Δ−θ33ε , setting the

value of Δθ1 , Δθ2 , and Δθ3 equal to each other, and applying the same approach to

ε1 ,ε 2 and ε 3 , the effect of the input errors is accentuated. For instance, term q3 in

(4.6) becomes

Δ+θε q =Δ++Δ−11 sin (θε )222 ( θε ) (4.7) 3111122( ) ()()Δ+θε11 +Δ−2 θε 11

and is more sensitive to errors than q1.

Table 4.8 describes the output error of q3 based upon the displacement angle delta theta and sensor angle error epsilon. Table 4.9 is the output error using the same angular inputs except the input quaternion is only accurate to 16 bits.

Table 4.8 Output bit accuracy as a function of input displacement angle and sensor angular bias error using a 33 bit input quaternion. ε (degrees) Δθ (degrees) 0.0001 0.001 0.01 0.1 1 0.01 -19 -15 -12 -9 -5 0.1 -19 -15 -12 -9 -5 1 -19 -15 -12 -9 -5 10 -19 -15 -12 -9 -5

Table 4.9 Output bit accuracy as a function of input displacement angle and sensor angular bias error using a 16 bit input quaternion. ε (degrees) Δθ (degrees) 0.0001 0.001 0.01 0.1 1 0.01 -19 -15 -12 -9 -5 0.1 -19 -15 -12 -9 -5 1 -18 -15 -12 -9 -5 10 -17 -15 -12 -9 -5

There are several sources of errors in rate sensors, but the more predominant one is the bias stability error regardless of the technology of the device. Gyro bias is typically described in degrees per hour. Some typical values are 0.01 degrees per hour for inertial grade and 1 to 10 degrees per hour for tactical grade gyroscopes. The error ε becomes 0.0001 degrees for a gyro bias of 10 degrees per hour when the sample rate is 30 samples per second.

From the results of Table 4.9 it is evident that the calculation is affected by quaternion word length when the gyro is tactical grade or better. For example, if the quaternion word length is 16, the output begins to degrade at displacement angles of 1 degree. When the word length is increased to 20, the degradation is eliminated.

Therefore the CORDIC based solution would need to utilize a 28 bit wide PE for computation using a tactical grade gyro. A similar computation can be performed for an inertial grade gyro and results in requiring a CORDIC processor with word length of 38 bits for a 30 bit accurate calculation.

4.3 Taylor Series Approximation

Taylor Series approximations are popular methods for computing the closed form solution presented in equation (2.32) [1][3]. Since the matrix ψ represents constant angular rate, the explicit Euler approximation, the modified Euler predictor- corrector method, and the fourth order Runge-Kutta method are equivalent to the

Taylor Series first, second, and fourth order approximations, respectively.

The fourth order approximation of (2.32) is

111111123 4 q(t+T ) =[I +ψψ + ( ) + ( ψ ) + ( ψ ) ]q( t ) . (4.8) 22262242

Due to the skew-symmetric nature of ψ in (4.8), the square of ψ in (4.8) is

2 equal to − Δθ I . Wherever ψ is involved, using this identity reduces the number of calculations in four-vector multiplications to four scalar multiplications that require

2 only one coefficient − Δθ . This feature greatly simplifies the calculations. Making this substitution, (4.8) reduces to

⎛⎞⎛⎞⎛⎞1124 11 2 q(t+T)=⎜⎟⎜⎟⎜⎟ 1- |ΔθΔθ | + | |I + 1- |Δθ | ψ q (t). (4.9) ⎝⎠⎝⎠⎝⎠8 384 2 24

The explicit Euler’s method can be directly computed from the first order Taylor

Series approximation

⎛⎞1 . (4.10) q(t+T ) =⎜⎟I +ψ q( t ) ⎝⎠2

Implicit Euler’s approximation methods are used for physically based modeling in computer graphics due to the improvement in numerical stability [16]. A predictor- corrector procedure uses an explicit method to predict the solution and an implicit method to correct the solution [15]. Since the matrix ψ represents an incrementally constant angular rate, using the second order Taylor Series approximation leads to the following approximation for the modified Euler’s predictor-corrector method

⎛⎞⎛⎞112 q(t+T ) =⎜⎟⎜⎟ 1 - Δθ I +ψ q( t ) . (4.11) ⎝⎠⎝⎠82

A third order Taylor Series approximation can be expressed as

⎛⎞⎛⎞⎛⎞11122 q(t +T)=⎜⎟⎜⎟⎜⎟ 1- |Δθ |I + 1- |Δθ | ψ q (t). (4.12) ⎝⎠⎝⎠⎝⎠8224

The most popular Runge-Kutta approximation is the fourth-order method

[7][15], which can be expressed as

1 Δ=qtqtψ Δ() (4.13) 1 2

11⎛⎞ (4.14) Δ=qtqtq21ψ Δ⎜⎟() +Δ 22⎝⎠

11⎛⎞ (4.15) Δ=qtqtq32ψ Δ⎜⎟() +Δ 22⎝⎠

1 Δ=qtqtq43ψ Δ()() +Δ (4.16) 2

1 and q(t + T)= q() t+() Δ q +Δ 2 q +Δ 2 q +Δ q (4.17) 6 1234

and is equivalent to the fourth order Taylor Series approximation method when Δt is

1. The accuracy of the Runge-Kutta solution increases substantially with decreasing

Δt but requires a much higher computational rate.

4.3.1 Comparisons of Accuracy

The first, second, third, and fourth order Taylor Series approximations were computed using floating point representation in Matlab for the same quaternion inputs and angular ranges discussed in Chapter 3. The results shown in Figure 4.9 indicate the truncation error due to increasing angular inputs. The angles at which the method described in Chapter 3 exceeds the accuracy of the other approximation methods is presented in Table 4.8.

Angle (Degrees) -60.0 -50.0 ts1 -40.0 ts2 -30.0 ts3 -20.0 ts4 -10.0 Accuracy (Bits) 0.0 0.01 0.1 1 10 100

Figure 4.9 Taylor Series Errors

Table 4.10 Displacement angles at which the accuracy of the CORDIC based method of Chapter 3 first exceeds the accuracy of Taylor Series (TS) based approximation for TS of various orders. TS1 TS2 TS3 TS4 (degrees) (degrees) (degrees) (degrees) cord 0.02 0.2 1 5

For a direct comparison to fixed point representation using a DSP processor, the third and fourth order Taylor Series approximations were computed using a c++ fixed-point simulation library that allows user defined word lengths [17]. The input quaternions and angles that were created to test the CORDIC implementation were reapplied to test the Taylor series based approximations. A 16 bit fixed point implementation yields an output accuracy of 11 bits at input angles up to 10 degrees or

25 degrees, depending on the order of the Taylor series, as shown in Figure 4.10. The maximum output accuracy decreases with increasing angle, eventually becoming 7

91 bits at 60 degrees. This degradation in accuracy can be attributed to the truncation errors caused by using an approximation method.

a) Fourth Order Taylor Series Implementation (16 bit Fixed Point Input)

-15 -10

(bits) -5 0 Output Accuracy 0 102030405060

Input Angle (degrees)

b) Third Order Taylor Series Implementation (16 bit Fixed Point Input)

-15 -10

(bits) -5 0 Output Accuracy 0 102030405060

Input Angle (degrees)

Figure 4.10 Accuracy of 16 Bit Input, Fixed Point, Taylor Series Approximation a) Fourth Order, b) Third Order Implementations

The number of instruction cycles required to implement the Taylor Series approximations using a Texas Instruments TMS3C6416 fixed point processor was calculated using TMS320C64x DSP Library functions [8]. The instruction cycle time is 1.7 ns and the computation of the fourth order Taylor Series approximation can be calculated to take 0.64 microseconds. A description of a 16-bit CORDIC processor ported to a similar DSP chip has been previously programmed [14], therefore the attitude update could also be computed using this implementation. Since the focus of

92 the research was to assess performance with varying CORDIC parameters like input bit width, the DSP processor implementation was not pursued further since it constrains the possible variations on the algorithm.

4.3.2 Comparisons of Orthogonality

The orthogonality error was computed and results are shown in Figure 4.11.

At 0.1 degrees angular displacement, the first order Taylor Series approximation may be seen to begin to degrade. The higher orders follow suit at angular displacements that exceed 5 degrees.

-60

-50

) -40 ts1 (bits -30 ts2 ts3 -20 ts4

Orthogonality -10

0 0.01 0.1 1 10 100

Angle (degrees)

Figure 4.11 Orthogonality as a Function of Input Displacement Angle for Various Orders of the Taylor Series Approximation

4.4 Wie-Barba Method

Another attitude update method that is popular in aerospace applications was described by Wie and Barba [2]. The work was originally presented by Cunningham et. al. in a description of the Annular Suspension and Pointing System (ASPS) used in the Space Shuttle [14]. The updated attitude is calculated as

DR24( t) DQ() t Qt(+= T ) Qt () + Rt () − DQt2 () −ii + (4.18) iiii36 where

2 222 D =Δ()()()θ123 +Δθθ +Δ (4.19)

1 R =Δ[]θθθQQQ −Δ+Δ (4.20) 11423322

1 R =Δ[]θθθQQQ +Δ−Δ (4.21) 21324312

1 R =−Δ+Δ+Δ[]θθθQQQ (4.22) 31221342

1 R =−Δ−Δ+Δ[]θθQQQ θ (4.23) 11122332

and ΔΔΔθ123,,θθ are the incremental angle outputs from the gyros over the quaternion integration step T.

4.4.1 Comparisons of Accuracy

Updated attitude found using (4.18) was computed using the same randomized quaternion and displacement angle inputs as in the Taylor Series analyses. The resulting accuracy is shown in Figure 4.12. The results indicate that any order of the

Taylor Series performs better than this method.

-60.0 -50.0 ts1 -40.0 ts2 -30.0 ts3 -20.0 ts4 Wie

Accuracy (Bits) -10.0 0.0 0.01 0.1 1 10 100 Angle (Degrees)

Figure 4.12 Accuracy as a Function of Input Displacement Angle for Various Orders of the Taylor Series Approximation and Wie-Barba’s Method

4.4.2 Comparisons of Orthogonality

The orthogonality resulting from use of (4.18) (and using the same randomized quaternion and displacement angle inputs as in the Taylor Series analyses) is shown in

Figure 4.13. The results indicate that any order of the Taylor Series performs better than this method.

-60

-50

-40 ts1 (bits) -30 ts2 ts3 -20 ts4

Orthogonality Orthogonality qwie -10

0 0.01 0.1 1 10 100

Angle (degrees)

Figure 4.13 Orthogonality as a Function of Input Displacement Angle for Various Orders of the Taylor Series Approximation and Wie-Barba’s Method

4.4 Other Integration Methods

4.5.1 Obrechkoff Integration

A method for numerical integration that can result in a multistep form is the

Obrechkoff method. The general k-step Obrechkoff method containing up to second order derivatives is given by [10]

kk k 2 ()1 ∑∑αβj yfhfn+ j=h j n+ j+ ∑ γ j n+ j (4.24) jj==00 j = 0

' " ⎛⎞∂f ()1 where yfy= ( ) , yffy==⎜⎟ (). (4.25) ⎝⎠∂y

An implicit 1-step Obrechkoff method for dy/, dt= f( t y) and

∂ff∂ f ' =+()ty,,,()() ty fty (4.26) nnnnndn∂∂ty can be given by

2 '' yyn+1=+h n(αβ f n++ f n+1) hff( γδ n + n+1 ) (4.27) and can provide a solution to the kinematic equation of motion [5]

1 ⎡ 11• ⎤ qhIhhq= 1 ++Ω+22δω 2 (4.28) n+12 ⎢() n++11 nΩ n+1 ⎥ n μ n+1 ⎣ 22⎦

where h is the time step and

222 222 ⎛⎞⎛⎞⎛⎞••• 2 ⎛⎞ ⎛⎞ ⎛⎞ 2 ⎜⎟⎜⎟⎜⎟111 111 111 μ22= 1 +Δ+Δ+Δ+Δ+Δ+Δ+Δhhhhhhh δθ θ 2 δθ⎜⎟ θ 2 δθ ⎜⎟ θ 2 δθ ⎜⎟ ()⎜⎟⎜⎟⎜⎟112233 ⎜⎟⎜⎟⎜⎟422⎜⎟ 422 ⎜⎟ 422 ⎜⎟ ⎝⎠⎝⎠⎝⎠⎝⎠ ⎝⎠ ⎝⎠ (4.29)

and 1 δ =- + kh() . (4.30) 12

Since the time step h is variable, the actual solution may incur many iterations prior to estimating a final solution.

4.5.2 Pade Approximation

Using this approach, the approximation for the attitude update algorithm takes the form of a rational fraction instead of a polynomial like the Taylor Series method.

The Pade approximation of a polynomial can be expressed as [6]

Nq( ) eat = (4.31) D ()q

where N(q) and D(q) can be expressed in terms up to the kth order

k j Nq= α qt , (4.32) () ∑ j=0 j ( )

k j D qq=−α t , and (4.33) () ∑ j=0 j ( )

(21!!kj− ) α = . (4.34) j ()2!!kjk (− 1! )

Therefore the third order Pade approximation can be expressed as

at a22 t a 33 t 1 ++ + eat ≈ 210120. (4.35) at a22 t a 33 t 1 −+ − 210120

The fifth order Pade approximation can be similarly expressed

at a22 t a 33 t a 44 t a 55 t 1 +++++ eat ≈ 2 9 72 1008 30240 . (4.36) at at22 at 33 at 44 at 55 1 −+ − + − 2 9 72 1008 30240

Although the Pade approximation can provide a more accurate computational method than Taylor Series, the computational cost of the algorithm is quite large.

4.6 Conclusion

The results of the analysis indicate that the accuracy and orthogonality of the attitude update calculation increases almost linearly with the number of input bits used per CORDIC processing element and is virtually independent of the input displacement angle. The accuracy and orthogonality was also investigated when the number of iterations per CORDIC cycle is reduced and was found to degrade, as expected. In designing a system, the physical size and total number of iterations can be selected if a minimum output bit accuracy or orthogonality has been predetermined.

Accuracy and orthogonality for the Taylor Series and Wie-Barba methods were also determined using simulation based methods. These simulations were computed using floating point operations, yet both suffered severe degradation in their accuracy at larger input angular displacements. The CORDIC-based implementation has superior performance for this situation. Two other methods were briefly described but, since they require significantly more extensive computation, simulations were not performed.

References

[1] F. L. Markley, Spacecraft Attitude Determination and Control, J.R. Wertz, Ed.; D. Reidel Publishing Company, 1978.

[2] B. Wie and P. M. Barba, "Quaternion Feedback for Spacecraft Large Angle Maneuvers", Journal of Guidance, Vol. 8, No. 3, pp. 360 - 365.

[3] V.N. Branets and I.P. Shmyglevskiy, "Application of Quaternions to Rigid Body Rotation Problems", NASA Technical Translation, NASA-TT-F-15414, 352 pages, July 1974.

[4] D. C. Cunningham, T. P. Gismondi and G. W. Wilson, " System Design of the Annular Suspension and Pointing System (ASPS)", in Proceedings of the AIAA Guidance and Control Conference, Palo Alto, CA 1978, pp. 471-489.

[5] D. P. Stapleton, “Numerical Simulation of a Rigid Rotating Body by Obrechkoff Integration”, Computer Physics Communications, Vol. 98, pp. 153-186.

[6] S. Gupta, "Linear Quaternion Equations with Application to Spacecraft Attitude Propagation", in 1998 IEEE Aerospace Conference Proceedings, Aspen, CO, pp. 69-76.

[7] M. J. Romanelli, "Runge-Kutta methods for the solution of ordinary differential equations", Mathematical Methods for Digital Computers, A. Ralston, H. S. Wilf, Editors, John Wiley and Sons, 1961.

[8] “TMS320C64x DSP Library Programmer’s Reference”, Texas Instruments Application Report SPRU-565A, April 2002, 146 pages.

[9] W. F. Phillips, C. E. Hailey, G. A. Gebert, “Review of Attitude Representations Used for Aircraft Kinematics”, AIAA Journal of Aircraft, Vol. 38, No. 4, July- August 2001, pp. 718-737.

[10] J. D. Lambert, Computational Methods in Ordinary Differential Equations, Wiley, 1979, p. 251.

[11] Y. H. Hu, “The Quantization Effects of the CORDIC Algorithm”, IEEE Transactions on Signal Processing, Vol. 40, No. 4, April 1992, pp. 834 – 844.

[12] J. E. Volder, "The CORDIC Trigonometric Computing Technique", IRE Transactions on Electronic Computers, September 1959, pp. 330-334.

100

[13] J. S. Walther, "A Unified Algorithm for Elementary Functions", in AFIPS Spring Joint Conference Proceedings, 1971, pp. 379-385.

[14] M. B. Yeary, R. J. Fink, H. Sundaresan, and D. W. Guidry. “Design of a CORDIC Processor for Mixed-Signal A/D Conversion”, IEEE Transactions on Instrumentation and Measurement, Vol. 51, No. 4, August 2002, pp. 804-809.

[15] J. Hoffman, Numerical Methods for Engineers and Scientists, B. J. Clark and J. M. Morriss, Ed.; McGraw-Hill, Inc, 1992.

[16] D. A. Baraff, “Implicit Methods for Differential Equations”, Physically Based Modelling Course Notes, SIGGRAPH 2003, San Diego, California 2003.

[17] S. Kim, K.I. Kum, and W. Sung, "Fixed-Point Optimization Utility for C and C++ Based Digital Signal Processing Programs", IEEE Transactions on Circuits and Systems--II. Analog and Digital Signal Processing, Vol. 45, No. 22, November 1998, pp. 1455-1464.

The text of Chapter 4, in part, is a reprint of the material as it appears in the IEEE

101

CHAPTER 5

APPLICATIONS IN COMPUTER ANIMATION

5.1 Introduction

The technique developed in Chapter 3 is introduced as a new method to compute attitude from motion capture data. This CORDIC based method was consistently within 0.00052 degrees (2 arcseconds) of the results from the closed form solution whereas the other popular techniques had varying levels of degradation. In addition it was confirmed that accuracy is not dependent upon the sensor update rate using the CORDIC based method, unlike the consequences incurred using existing methods.

Objects that move in computer animation require coordinate translation and rotation. Translation of the axes for a moving solid body can be described in three dimensional vectors that represent x, y and z coordinates. Documenting rotational movement is more complicated, since the motion of the surface on a solid body is spherical or circular in nature. Rotational motion has been described in the field of computer animation using many different coordinate systems, e.g., fixed-axis, Euler angle, Euler axis/angle and quaternion coordinates [7].

101

102

Shoemake [8] introduced a method for interpolating movement between keyframes and championed the use of quaternion arithmetic for computer graphics.

Keyframes are generated to define a desired position and orientation for objects in computer animation. To create smooth movement from one keyframe to another, other frames are created that interpolate the movement as time passes and provide a persistence of vision. This process is analogous to manual animation and is typically called in-betweening.

Common error sources in computer animation include roundoff errors and a failure of orthonormalization [7]. Roundoff errors are generated during calculation of the translation of an object. When the translation is performed repetitively they can cause the resulting orientation of the rotating object to drift from the expected position. The coordinate axes for an object should always be perpendicular to one another and of unit value, i.e. orthonormal. If orthonormality is not preserved, the object can appear to have a component of shear. An example of an object subjected to shear is illustrated in Figure 5.1.

a) b)

Figure 5.1 Object Subjected to Shear a) Original solid body b) Shear effects on solid body

103

If quaternions are used to describe the coordinates of an object’s orientation, then orthonormality is preserved since the magnitude of a quaternion is unity. In addition quaternion arithmetic requires less computations to calculate a new orientation than the other methods.

Computer animation can also be produced that is based upon physical parameters measured during the movement of an object. The orientation of an object can be measured using several different sensor types, including optical and inertial components. When inertial components are used, the object can be tracked in virtually any environment versus the confined area that is required to monitor an object that is tracked optically. This chapter uses the CORDIC based quaternion attitude determination method presented in Chapter 3 to translate motion that has been captured from two different sensor blocks using MEMS gyroscopes into a quaternion value and compares the accuracy of this computation to other approximation methods.

In addition this chapter estimates the power consumption for such a system [3].

5.2 Quaternion Representation in Computer

Animation

Prior to Shoemake’s paper regarding the advantages of using quaternions in computer graphics, rotational motion in computer animation was typically represented

104

in terms of Euler Angles [9]. This Euler transform consists of angles known as roll,

pitch and heading as shown in Figure 5.2, and in order to properly compute the present

orientation, the matrices representing the displacement of the three rotational axis

must be multiplied in a specific order. A common order [9] used to determine the new

orientation is

Ehpr(, ,)= Rzx () rR ( pR ) y () h (5.1) where

⎡⎤cos(rr) − sin( ) 0 ⎡10 0⎤ ⎡ cos() h 0 sin () h ⎤ ⎢⎥⎢ ⎥ Rr()= sin r cos r 0 , Rp()=− 0 cos p sin p , ⎢ ⎥ .(5.2) z ⎢⎥() () x ⎢ () ()⎥ Rhy ()= ⎢ 0 1 0 ⎥ ⎢⎥⎢ ⎥ ⎣⎦001 ⎣0sin()p cos ()p ⎦ ⎣⎢−sin()h 0 cos () h ⎦⎥

y heading

-z

pitch

x roll

Figure 5.2 Euler Angle Representation

The rotational matrix becomes

⎡cos(rh) cos( ) −− sin( rph) sin( ) sin( ) sin( rp) cos( ) cos( rh) sin( ) + sin( rph) sin( ) cos( )⎤ ⎢ ⎥ E =+⎢sin()rh cos () cos () rph sin ( ) sin () cos () rp cos ( ) sin () rh sin () − cos () rph sin ( ) cos ()⎥ .(5.3) ⎢⎣ −cos()ph sin () sin () p cos () p cos () h ⎥⎦

105

The interpolated frames from which the objects were generated from the original frames is much more efficient and the trajectories are smoother when using quaternion arithmetic. Shoemake [8] presented a method to compute the Spherical

Linear Interpolation (Slerp) between two orientations using quaternions. Since a great circle trajectory as shown in Figure 5.3 provides the shortest distance between two points on a sphere, he considered

sin(φφ (1− tt )) sin( ) slerp(,,) q r t=+ q r (5.4) sin(φ ) sin(φ )

where q and r are the initial and final quaternion orientations, t is the increment size of the interpolation and φ can be found from

cosφ = qr11+++ qr 22 qr 33 qr 44. (5.5)

q φ

Figure 5.3 Great Circle Trajectory

106

5.3 Physically Based Modeling

Physically-based modeling has commonly used quaternion arithmetic for rotation since Shoemake’s seminal paper on the subject. Time must be included as a parameter to appropriately model the change in orientation of a moving object.

Several methods for integration with respect to time [10] have been utilized depending on their computational simplicity, accuracy, and stability. There are several types of finite difference equations (DFE’s) that are popular in this field.

5.3.1 Explicit Euler’s Method

Explicit methods have been used in physical modeling due to their simplicity.

The method generates the updated value based upon the previous value and a weighted function [5]

yyn+1= n+ Δtf n . (5.6)

However these explicit methods can be unstable for stiff systems [10]. An example of a stiff system is a spring based structure where the goal is to keep the object of interest close to y=0. As Δt becomes large, y may not converge to a solution. This leads to implicit approaches.

5.3.2 Implicit Euler’s Approximation (Modified Euler’s Predictor-Corrector

Method)

Implicit methods are preferable for large Δt in physical modeling since the explicit methods can be unstable for this timestep [10]. An implicit method attempts

107 to include a prediction for the next value as part of its calculation and provides an inherently stable calculation. An example is the two step Euler predictor-corrector method [5]

1 yyCP= +Δ tff + (5.7) nn+1 2 () nn+1 and

p yytfnnn+1 = + Δ (5.8)

where yp is the prediction of the next incremental update, yc is the corrected output, and

PP ftynnn+++111=f( , ) . (5.9)

5.3.3 Runge-Kutta Approximation

When more accurate approximations are necessary, multistep methods that calculate intermediate values are used [5]. The fourth-order Runge-Kutta approximation is a popular method for physical based modeling [10] including molecular simulation [16] where accuracy is required at the expense of computational power and time steps are sufficiently small to ensure convergence. The most popular

Runge-Kutta method is [5]

1 yy= +() Δ y +Δ22 y +Δ yy +Δ (5.10) nn+112346

108 where

ytfty1 =Δ ( nn, ) , (5.11)

y ⎛⎞Δt 1 , (5.12) ytfty2 =Δ⎜⎟nn +, + ⎝⎠22

Δt y ⎛⎞2 , (5.13) ytfty3 =Δ⎜⎟nn +, + ⎝⎠22 and

ytfttyy43=Δ( nn +Δ, + ) . (5.14)

5.4 Motion Capture

For computer animation the movements are predictable, i.e. the beginning and either the displacement or final attitude values are known and the final or interpolated values can be computed. However, for computer animation utilizing motion capture data, the final attitude value is not known. There are several methods that can be used to capture the data. A popular method is to employ a system where the object must have reflective markers attached so that a camera can receive the reflected light. The light sources are mounted to a fixed structure and the object is constrained within the boundaries of this structure. Bachmann [14] utilized inertial angular rate sensors and quaternion attitude coordinates for a system that defined basic human joint movement.

This system allows multiple objects over an area that is only limited by the range of a wireless LAN [14].

Head mounted displays (HMDs) are used for virtual reality and augmented reality applications. The location and orientation of the head is tracked and the scene on the user’s lenses is updated accordingly. Inertial angular rate sensors have been

109 used to track the head orientation in HMDs [13]. Some tracking systems that utilize inertial angular rate sensors to assess orientation use quaternion arithmetic for their attitude calculations [4][12]. The quaternion data that has been captured during a virtual reality event can be filtered to remove the effects of noise[11].

Rotational kinematic algorithms using quaternions that were proposed by [1] only concentrated on the first and second order approximations since it was felt by the author that any higher order would require processing time difficult to achieve for a real-time motion capture system. Obtaining an independent hardware solution was not suggested. The CORDIC based method presented in Chapter 3 can be implemented in an ASIC or FPGA which reduces the computational impact in a real- time motion capture system.

5.4.1 Approximations of the Kinematic Equation for Motion Capture

Revisiting the kinematic differential equation presented in Chapter 2

. 1 q = ψq , (5.15) 2 and specifying the variable yn function fn and Δt as

1 yq(t)n = , f = ψq , and Δt = 1 , (5.16) n 2 the approximations for the differential equation solutions presented in section 5.3 can be expressed for the kinematic equation for rotation and represented by quaternion coordinates.

110

5.4.1.1 Explicit Euler’s Method

The solution to the kinematic equation of rotation can be expressed using the explicit Euler’s method as

⎛⎞1 . (5.17) q(t+T ) =⎜⎟I +ψ q( t ) ⎝⎠2

5.4.1.2 Implicit Euler’s Approximation

The prediction part of this method can be described as

p ⎛⎞1 q ()t+T =⎜⎟I + ψ qt() (5.18) ⎝⎠2 and the corrector function becomes

c 11⎛⎞⎛⎞⎛⎞1 qq()()t+T = t++⎜⎟ψqf() t⎜⎟⎜⎟I +ψ q () t (5.19) 22⎝⎠⎝⎠⎝⎠2 which can be also be described as

11⎛⎞11 c ⎛⎞ (5.20) qq()t+T =n ++⎜⎟ψq () tψψ⎜⎟I +q () t 22⎝⎠22⎝⎠

2 and since ψ 2 =-Δθ I,

⎛⎞⎛⎞112 q(t+T ) =⎜⎟⎜⎟ 1 - Δθ I +ψ q( t ). (5.21) ⎝⎠⎝⎠82

This expression is equivalent to the second order Taylor Series approximation method presented in Chapter 4.

111

5.4.1.3 Runge-Kutta Approximation

Applying the Runge-Kutta approximation method for solving differential equations to the kinematic equation for rotation offers

1 Δ=qtqtψ Δ(), (5.22) 1 2

11⎛⎞, (5.23) Δ=qtqtq21ψ Δ⎜⎟() +Δ 22⎝⎠

11⎛⎞, (5.24) Δ=qtqtq32ψ Δ⎜⎟() +Δ 22⎝⎠

1 Δ=qtqtq43ψ Δ()() +Δ , (5.25) 2

1 and q(t +T)= qt()+() Δ q +Δ 2 q +Δ 2 q +Δ q . (5.26) 6 1234

Equations (5.23) through (5.26) can be expressed in terms of q(t)

11 Δ=qqtqtψψ() + 2 () (5.27) 2 28

11 1 Δ=qqtqtqtψψ() +23() + ψ() (5.28) 3 28 32

⎛⎞11234 1 1 (5.29) Δ=qqt4 ⎜⎟ψψ + + ψ + ψ() ⎝⎠2 4 16 64

⎛⎞1123 1 1 4 therefore q(t +T)= qt()+++⎜⎟ψψ ψ + ψ qt (), (5.30) ⎝⎠28 48384

112 and provides the same result as presented in Chapter 4 for the fourth order Taylor

Series approximation.

5.5 Prior Work Using the CORDIC Algorithm in

Computer Graphics

The CORDIC algorithm has been used for computer graphics applications to provide the antialiasing of lines and polygons [6]. Efforts in the past three years include using the CORDIC algorithm to reduce the power consumption of 3D applications [2], and to increase the throughput of vector rotations [15].

The aliasing of lines can be a problem when projecting them on a computer raster display. An early application of the CORDIC algorithm in computer graphics was to provide a method to compute the point-line distance that was necessary in the elimination of line and polygon aliasing [17].

Three dimensional computer graphics outranks other computer applications in terms of power consumption due to bandwidth and intensive computation requirements. In [2] vector interpolation which is necessary for Phong shading is implemented using the CORDIC algorithm with an adjustable bit width that aids in the reduction of power consumption at the expense of the resolution of the bit width.

113

The CORDIC algorithm has also been proposed as a method to provide a high throughput system for three dimensional rotations and normalizations [15]. A three dimensional CORDIC architecture is presented as an extension in the Euler axis/angle system of the two dimensional implementation. The work in [15] presented other techniques for computing 3D computer graphics operations using CORDIC-type primitives. In this effort the orientation of an object is computed when it is rotated by a predetermined amount.

5.6 Experimental Results

5.6.1 Motion Capture Sampling Rate

Real-time tracking of all objects in the scene requires input data tracking rates of at least 30 Hertz [18]. To overcome the possibility of dropping frames during rendering, the sensor sampling rate should be at least 60 Hertz and to minimize the effects that latency imposes the sensor sampling rate should be a minimum of 120

Hertz [19].

5.6.2 Initial Test Configuration

The sensor block shown in Figure 5.4 was constructed using three

ADXRS150EB MEMS rate gyroscopes mounted orthogonally to provide input data representative of solid body rotation. The sensors selected, which were manufactured

114 by Analog Devices, are capable of measuring over 600 degrees per second of angular rate.

Figure 5.4 Sensor Block

The sensor block was calibrated using a one-axis rate table. Bias, scale factor, and misalignment data were recorded. The sensor block was then connected to a

Dataq DI-700 16 bit data acquisition system. This system was configured to record at its maximum sampling rate, which equated to 240 samples/second/channel for three channels of rate input data and one channel of input voltage. The recorded information was converted to ASCII format for post-processing purposes. The incremental angular displacement was increased to observe the effects on the

115 approximation methods since there are MEMS gyroscopes that have larger angular rate ranges. Data from the original 240 samples/second/channel file was decimated to provide files containing 20 samples/second/channel data to distinguish the differences between the results of the various approximation methods more clearly.

The 16-bit data acquisition system collects and records three channels of the analog output voltages. The captured sensor data is post-processed to correct for relevant gyroscope bias and is converted to equivalent angular rate using the c++ software language. After the conversions are complete, the angular rate is integrated by multiplying ω by the sample period and the applicable decimation rate. A file is generated of the quantized 16 bit values.

For the CORDIC implementation, a VHDL testbench was constructed in which the integrated angular rate data file is imported into the parallel CORDIC based subsystem presented in Chapter 3. Since the input data is 16 bits, an additional 17 bits, valued as all ones, were appended to each angular input to minimize the possible errors incurred when all 16 bits of the input data are zeroes. The integrated angular rate data is concurrent with the previously computed attitude quaternion. When the attitude computation has completed its 1015 clock cycles, the output quaternion is updated and the next set of integrated rate data is entered. For comparison purposes, the same 33-bit integrated rate and quaternion data generated to test the system was used as input to compute the closed form and alternate approximation methods. The latter computations were done in Matlab.

116

5.6.3 Attitude Update Comparison

The sensor block was held in a skew orientation and rapidly rotated in a clockwise and counterclockwise fashion with wrist and arm motion. This trajectory provides a large variation for each of the quaternion elements since it changes the equivalent Euler axis orientation in addition to the Euler angle during rotation. The measured angular rate data integrated at 20 samples per second is presented in Figure

5.5. The quaternion elements representing the closed form solution presented in

Chapter 2 are shown in Figure 5.6 and demonstrate that the Euler axis and angle both incur large dispersions during this experiment. For clearer comparison between the

method of this paper and the Taylor Series approximation methods, in Figure 5.6, q4 was converted to its Euler angle equivalent φ . The results presented in Figure 5.7 indicate that the first order Taylor Series approximation method is not able to reliably represent the orientation after the first twisting motion. The difference between the

Euler angle of the other approximation methods and the closed form solution is presented in Figure 5.8. The implementation using the CORDIC as a processing element does not diverge from the closed form solution by more than 0.0004 degrees or 1.44 arcseconds at a 20 sample/second input data rate.

117

40 30 20 10 0 -10 dT0 (degrees) dT0 -20 -30 -40 40 30 20

10 0 -10 dT1 (degrees) dT1 -20 -30 -40 40 30 20 10 0 -10 dT2 (degrees) -20 -30 -40 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 time (seconds)

Figure 5.5 Measured Angular Rate Data Integrated at 20 Samples per second Sample Rate

118

1 0.8 0.6 0.4 0.2

q1 0 -0.2 -0.4 -0.6 -0.8 -1 1 0.8 0.6 0.4 0.2

q2 0 -0.2 -0.4 -0.6 -0.8 -1 1 0.8 0.6 0.4 0.2

q3 0 -0.2 -0.4 -0.6 -0.8 -1 1 0.8

0.6 0.4 0.2

q4 0 -0.2

-0.4

-0.6

-0.8 -1 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 Figure 5.6 Closed Form Updated Quaternion Values

119

350 qact qcord 300

250

200

150 Euler Angle (degrees) Angle Euler

100

0 350 qts1 qts2 300

250

200

150 (degrees) Angle Euler 100

0 350 qt s3 qt s4 300

250

200

150 Euler Angle (degrees) (degrees) Angle Euler 100

0 01234567891011121314151617181920 time (seconds) Figure 5.7 Euler Angle Comparison at 20 samples per second

120

0.2 45

0.1 35 qts4 Euler Angle Errors (degrees) Errors Angle Euler qts4

25 0

15 -0.1

5 -0.2 -5 qts2, qts3 Euler Angle Errors (degrees) qts2 -0.3 -15 qts3 qts4 -25 -0.4 0 2 5 7 10 12 15 17 20 time (seconds)

Figure 5.8 Taylor Series Euler Angle Errors at 20 Samples per second

5.6.4 Comparison at Different Sample Rates The experiment was implemented by holding the assembly and performing various movements that generate high angular rates. A rotation of the wrist is an example. Figures 5.9 depicts a portion of the movement recorded when the sensor block was held in a skew position and the hand was rotated 180 degrees in both directions repeatedly. Figures 5.10 and 5.11 illustrate the attitude determination for the movement using closed form, CORDIC based and Taylor Series approximation methods and the similarity between the two former techniques and the diversion of the latter method.

121

Differences between the Taylor Series approximation and the closed form solution were calculated and compared for each sample rate. The results shown in

Figures 5.12 and 5.13 demonstrate that Taylor Series approximation errors increase for decreasing sample rate. Even at the maximum sample rate a 1 degree maximum error could be observed with the fourth order Taylor Series approximation method. The maximum observable difference between the fixed point CORDIC and the floating point closed form calculation was 0.00145 degrees or 5.2 arcseconds at any data sample rate and was independent of the input sample rate.

1000 800 600 400

200 x 0 y -200 z -400 Rate (degrees/second)Rate -600 -800 -1000 0.00 0.15 0.31 time (seconds)

Figure 5.9 Measured Angular Rate as a Function of Time

122

0.25 1

0.2 0.8 0.15

0.1 0.6 0.05 q1 q2 0 0.4 q3 -0.05 q4 0.2 -0.1

-0.15 0 -0.2 0.00 0.15 0.30 -0.25 -0.2 time (seconds)

Figure 5.10 Closed Form Quaternion Results at 108 Samples per second

20 cmplt ts4 15 cord ts1 angle (degrees) angle 10

0 0.00 0.15 0.30 time (seconds)

Figure 5.11 Euler Angle Comparison at 108 Samples per second

123

10 325 S/s 8 108 S/s 54 S/s 6 angle (degrees) 4

0 0.00 0.15 0.31 time (seconds)

Figure 5.12 First Order Taylor Series Euler Angle Errors

-16

-14

-12

-10 325 S/s -8 108 S/s 54 S/s -6 angle (degrees)

-4

-2

0 0.00 0.15 0.31 time (seconds)

Figure 5.13 Fourth Order Taylor Series Euler Angle Errors

124

5.6.5 Experimental Results with Improved Sensor Block

Another three axis gyro sensor block was constructed to obtain angular rate data to validate the accuracy of the approach described above and is shown in Figure

5.14. Since the purpose of the testing was to validate the computational approach with large angular dispersions, it was decided that minimization of the biases of the devices was not necessary. The gyroscopes were upgraded to increase the range of sensed angular movement.

Figure 5.14 Three-Axis Gyroscope Sensor Block

125

The ADXRS300EB Analog Devices MEMS single axis gyroscope demonstration boards were selected since the requisite compensation circuitry is resident on the assembly. The nominal maximum rate output of this configuration is

+/- 300 degrees per second. External circuitry was added so that the minimum achievable rate per gyro was over 1200 degrees per second. The MEMS single axis gyroscopes were powered by a 9V battery through a 5 VDC regulator. The sensor block was calibrated using a one-axis rate table. Bias, scale factor, and misalignment data were recorded. The sensor block was then connected to a Dataq DI-700 16 bit data acquisition system.

This system was configured to record at 244.1406 samples/second/channel for three channels of input data. The recorded information was converted to ASCII format for post-processing purposes. To compare the results between several different update rates, data from the original 244.1406 samples/second/channel file was decimated to provide files containing 122.0703 samples/second/channel, 61.0352 samples/second/channel and 30.5176 samples/second/channel data.

The experiment was implemented by holding the assembly and performing various movements that generate high angular rates. A rotation of the wrist is an example. Figure 5.15 depicts a portion of the movement recorded when the sensor block was held in a skew position and the hand was rotated in both directions repeatedly. The recorded sensor measurements were converted from voltages to equivalent angular rates.

126

The recorded data was processed using a program written in the c++ language.

First the angular rate measurements were translated from voltage to degrees per second based upon the gyro sensitivity and bias information. Next the instantaneous angular displacement for each gyro was computed from the angular rate output value and the decimated data acquisition rate. The attitude update information for the closed form, for the first, second and fourth order Taylor Series approximation, and for the fixed point single CORDIC method presented in Chapter 3 were subsequently calculated.

Since the closed form solution to the kinematic equation that was presented in

(2.32) is considered the most accurate calculation for the attitude of the sensor block, the quaternion components shown in Figure 5.16 were calculated using this method.

127

2000

1500 d 1000

500 x y 0 z

-500 Angular Rate (degrees/secon -1000

-1500 012345678 Time (seconds)

Figure 5.15 Measured Angular Rate as a Function of Time

The Euler axis representation presented in Chapter 2 provides better way to visualize the comparison between the various methods of attitude. The equivalent

Euler angle φ can be calculated directly from the fourth quaternion element as shown in (2.7). Figures 5.17, 5.18 and 5.19 demonstrate that the CORDIC calculation follows the complete closed form solution very closely while the first, second and fourth ordered Taylor Series approximations have errors that increase in magnitude with decreasing sample rate. The q4 term computed using the first order Taylor Series was frequently observed to be greater than 1 at data sample rates less than 120 samples per second and significantly impacts the Euler angle error. The maximum observable difference between the fixed point CORDIC and the floating point closed form calculation was 0.00052 degrees or 1.9 arcseconds at any data sample rate.

128

0.75

0.5 q1 q2 0.25 q3 q4 0

-0.25

-0.5 012345678 Time (seconds)

Figure 5.16 Closed Form Quaternion Results at 122 Samples per second

5 0.025

4 0.02 ts4, CORDIC Euler Angle Error (degrees) (degrees) Error Angle Euler CORDIC ts4,

3 0.015

2 0.01 ts1 ts2 ts4 1 0.005 CORDIC

0 0 ts1,ts2 Euler Angle Error(degrees)

-1 -0.005

-2 -0.01 012345678 Time (seconds)

Figure 5.17 Euler Angle Error Comparison at 122 Samples per second

129

30 0.03

25 0.025 ts4, CORDIC Euler Angle Error (degrees) (degrees) Error Angle Euler CORDIC ts4,

20 0.02

15 0.015 ts1 ts2 ts4 10 0.01 CORDIC

5 0.005 ts1, ts2 Euler Angle Error (degrees) (degrees) Error Angle Euler ts2 ts1,

0 0

-5 -0.005 012345678 Time (seconds)

Figure 5.18 Euler Angle Error Comparison at 61 Samples per second

60 0.4

0.3 Euler Error Angle (degrees) ts4,CORDIC 40

30 0.2

20 ts1 ts2 0.1 ts4 10 CORDIC

0 0

ts1, ts2 Euler Angle Error (degrees) (degrees) Angle Error Euler ts2 ts1, -10 -0.1 -20

-30 -0.2 012345678 Time (seconds)

Figure 5.19 Euler Angle Error Comparison at 30 Samples per second

130

5.7 Graphical Simulation

A graphical simulation using Macromedia Flash was constructed of a block rotating in space to visually observe the differences between the Taylor Series and

CORDIC methods.

The rectangular block is expressed in Cartesian coordinates to place the eight nodes and six faces. Each face of the block is removed when it should no longer be visible to the observer. A portion of the quaternion attitude update results from the motion capture data presented in Figure 5.18 is saved as an array of data in the Action

Script file and the coordinates for the nodes and faces of the block can be updated as follows [7]

rAr' = (5.31)

⎡ x '⎤ ⎡ x⎤ where ⎢ ⎥ , ⎢ ⎥ (5.32) ry''= ⎢ ⎥ ry= ⎢ ⎥ ⎣⎢ z '⎦⎥ ⎣⎢ z ⎦⎥

2222 ⎡⎤q1−−+ q 2 q 3 q 42( qq 12 + qq 34 ) 2( qq 13 − qq 24 ) and ⎢⎥2222 . (5.33) Aqqqqqqqqqqqq=−−+−++⎢⎥2(12 34 ) 1 2 3 4 2( 23 14 ) ⎢⎥2(qq+−−−++ qq ) 2( qq qq ) q2222 q q q ⎣⎦13 24 23 14 1 2 3 4

2222 Since qqqq1234+++=1, A can be simplified [7] to

131

⎡⎤22 12−+(q2 q 3) 2( qq 12 + qq 34 ) 2( qq 13 − qq 24 ) ⎢⎥ 22 . (5.34) Aqqqqqqqqqq=−⎢⎥2( ) 1 −+ 2 2( + ) ⎢⎥12 34() 1 3 23 14 ⎢⎥2(qq+−−+ qq ) 2( qq qq ) 1 2 q22 q ⎣⎦13 24 23 14() 1 2

The initial orientation for each node is represented by r. The terms in A are created from the components in Q, the updated quaternion attitude value and r’ is translated to this new orientation.

Figure 5.20 depicts the portion of the data sampled at approximately 60 Hz from the quaternion attitude calculations illustrated in Figure 5.18 that was used as inputs to the computer animation program. The final orientation of the block is shown in Figure 5.21 for an attitude updated using closed form and Figure 5.22 represents the proposed CORDIC method, and the first, second and fourth Taylor Series methods.

Figure 5.22 demonstrates that the orientation of the block that was transformed with the quaternion generated using the CORDIC method appears to be identical to the closed form quaternion solution, whereas the first and second order Taylor Series appears skew from these positions. It also appears that the orientation determined by the fourth order Taylor Series approximation varies only slightly from the closed form solution.

132

1000 800 600

400

200 x 0 y -200 z -400 Rate (degrees/second)Rate -600 -800 -1000 0.00 0.15 0.31 time (seconds)

Figure 5.20 Angular Rate as a Function of Time

Figure 5.21 Actual Orientation

133

a) b)

c) d)

Figure 5.22 Animation Results a) CORDIC Implementation, b) First Order Taylor Series, c) Second Order Taylor Series, d) Fourth Order Taylor Series Orientations

5.8 Power Consumption Estimates

The power consumption of the gyroscopes has been considerably improved by using MEMS technniques. These sensors do not require a large power source. For example, the nominal power dissipation of the Analog Devices MEMS single axis gyroscope is 25 milliWatts. Using the proposed computational method conserves energy since it can be implemented in dedicated hardware, is formatted in fixed point notation and computationally efficient.

The power consumption was calculated for both FPGA and ASIC implementations. The Xilinx Virtex series XCV300 was selected for the FPGA

134 implementation. The synthesized design required 61% logic slice resource utilization.

The maximum achievable attitude update rate using this implementation is 28.6 kHz.

The quiescent power rating for this device is nominally 29.84 mW. Xpower software from Xilinx was used to assess the power consumption at different input clock frequencies. In addition the controller and CORDIC processing element have been synthesized as a dedicated ASIC using a 0.13 um CMOS TSMC standard cell library.

The ASIC implementation requires 0.084 mm2 in die area and has a 202 kHz maximum attitude update rate. Power Compiler from Synopsys was used to estimate the power dissipated by the circuitry for various attitude update rates. Table 5.1 compares the power dissipation for several popular attitude update data rates and also provides the equivalent clock frequency since it requires 1015 clock cycles to perform an attitude update computation.

Table 5.1 Attitude Update Rate versus Power Dissipation

Attitude Clock Power Dissipation

Update Rate Frequency FPGA ASIC

5000 Hz 5.075 MHz 35.95 mW 7.955 mW

1000 Hz 1.015 MHz 7.89 mW 1.582 mW

500 Hz 507.5 kHz 4.28 mW 0.806 mW

300 Hz 304.5 kHz 2.88 mW 0.497 mW

100 Hz 101.5 kHz 1.47 mW 0.185 mW 50 Hz 50.75 kHz 1.12 mW 0.109 mW

135

The table suggests that the power dissipation increases proportionately with increasing attitude update rates.

5.9 Conclusion

Attitude determination using the CORDIC algorithm has been shown to provide a more accurate measurement than other approximation methods at lower sample rates and higher rotational rates. Reduced power consumption in performing the calculation is an added benefit for portable systems at these lower angular input data rates.

136

References

[1] S. Y. Belyaev, B. S. Grigoriev, D. B. Smirnov, “Numerical Solution of Kinematic Equations in Problems of Rigid Body Dynamics for Applications in Computer Graphics”, Fifth International Workshop on Nondestructive Testing and Computer Simulations in Science and Engineering, February 2002, Proc. SPIE Vol. 4627, pp. 327-332.

[2] J. Euh, J. Chittamuru, and W. Burleson, "CORDIC Vector Interpolator for Power- Aware 3D Computer Graphics", IEEE Workshop on Signal Processing Systems, Oct. 2002, pp.240-245.

[3] J. F. Arrigo, P. M. Chau, “Power Aware Attitude Computation during Rapid Rotational Motion”, IEEE Instrumentation and Measurement Technology Conference 2004, 18-20 May 2004, Lake Como, Italy

[4] P. Lang, A. Kusej, A. Pinz, G. Brasseur, “Inertial Tracking for Mobile Augmented Reality”, IEEE Instrumentation and Measurement Technology Conference, May 2002, Anchorage, AK, pp. 1583-1587.

[5] J. Hoffman, Numerical Methods for Engineers and Scientists, B. J. Clark and J. M. Morriss, Ed.; McGraw-Hill, Inc, 1992.

[6] D. Pletinckx, “Quaternion calculus as a basic tool in Computer Graphics”, The Visual Computer, October 1989, pp. 2-13.

[7] R. Parent, Computer Animation, Algorithms and Techninques, B. Barsky, Ed., Elsevier Science, 2002.

[8] K. Shoemake, "Animating Rotation with Quaternion Curves." Computer Graphics Vol. 19, pp. 245-254, July 1985.

[9] T. Akenine-Moller, E. Haines, Real-Time Rendering, A. K. Peters, Ltd, 2003.

[10] D. A. Baraff, “Implicit Methods for Differential Equations”, Physically Based Modeling Course Notes, SIGGRAPH 2003, San Diego, California 2003.

[11] J. Lee and S. Y. Shin, “General Construction of Time-Domain Filters for Orientation Data”, IEEE Transactions on Visualization and Computer Graphics, Vol. 8, No. 2 , April-June 2002, pp. 119 – 128.

[12] M. Ribo, P. Lang, H. Ganster, M. Brandner, C. Stock, A. Pinz, “Hybrid Tracking for Outdoor Augmented Reality Applications”, IEEE Computer Graphics and Applications, Vol. 22, No. 6, Nov.-Dec. 2002, pp. 54 - 63.

137

[13] E. Foxlin, L. Naimark, “VIS-Tracker: A Wearable Vision-Inertial Self-Tracker”, IEEE Virtual Reality Conference Proceedings, pp. 199-206, Los Angeles, California (USA) March 2003.

[14] E.R. Bachmann, R.B. McGhee, X. Yun, M. J. Zyda, “Tracking: Inertial and magnetic posture tracking for inserting humans into networked virtual environments”, Proceedings of the ACM Symposium on Virtual Reality Software and Technology, pp. 9-16, Banff, Canada November 15-17 2001.

[15] T. Lang, E. Antelo, “High-throughput 3D Rotations and Normalizations”, Proc. of 35th Asilomar Conference on Signals, Systems, and Computers, pp. 846-851, Asilomar Hotel and Conference Grounds, Pacific Grove, California (USA), Nov. 2001.

[16] A. Koi, B. B. Laird, B. J. Leimkuler, “Symplectic Method for Rigid-Body Molecular Simulation”, AIP Journal of Chemical Physics, Vol. 7, pp. 2580- 2588, August 1997.

[17] K. E. Turkowski, “Anti-aliasing through the use of coordinate transformations”, ACM Transactions on Graphics, vol. 1, no. 3, July 1982, pp. 215-234.

[18] M. Ribo, A. Pinz, A. L. Fuhrmann, “A New Optical Tracking System for Virtual and Augmented Reality Applications”, IEEE Instrumentation and Measurement Technology Conference, pp. 1932-1936, Budapest, Hungary, May 21-23, 2001

[19] E. Foxlin, “Motion Tracking Requirements and Technologies”, Handbook of Virtual Environment Technology, K. Stanney, Ed., Lawrence Erlbaum Associates, 2002.

The text of Chapter 5, in part, is a reprint of the material as it appears in the IEEE

138

CHAPTER 6

APPLICATIONS IN VEHICULAR ATTITUDE

CONTROL

6.1 Introduction

An application of attitude determination as part of a closed loop attitude control system using the new computational method is simulated. The elements of the output quaternion are accurate to within 0.00001 of the existing approximation technique and provided superior performance for other proposed dynamic examples and in the presence of coning motion.

Motion of a solid body can be described in terms of its translation and rotation, viz., by the translational position, velocity, and acceleration of the center of mass of the body and by the angular displacement, velocity, and acceleration about the center of mass. Attitude is the angular displacement of the object about its center of mass referenced to an inertial coordinate frame. Especially in the case of self-propulsion, the orientation of an object must be known with some accuracy prior to the application of thrust if unstable or otherwise deleterious dynamical properties are to be avoided.

In practice, to achieve a desired trajectory, the attitude of the vehicle relative to its initial orientation must be periodically updated. Alignment of spacecraft telescopes,

138

139 antennas and solar panels, and avoidance of solar or atmospheric damage to components [1] provide additional impetus for having accurate attitude data available.

Among the several different parameterization techniques for describing attitude [1], the most popular involve the use of either direction cosines or quaternion transformations. The quaternion representation requires less computation as presented in Chapter 2 and lends itself well to an efficient implementation using the CORDIC algorithm. The CORDIC based approximation method and a computational technique first introduced by Cunningham [13] and popularized by Wie and Barba in [2] are simulated in this chapter for several aerospace examples.

Wie and Barba [2] developed an attitude control system that included quaternion feedback for spacecraft large angle maneuvers and has been the basis of more recent attitude control methods [6][12]. Wie and Barba used Lyaponov methods to analyze the stability of their control system [2].

In addition to describing spacecraft orientation, quaternions have been used to describe the rotation of aircraft. In 1963, Pope proposed an algorithm called the exponential method to reduce computation time without sacrificing accuracy [11].

This algorithm was first used at NASA Langley research Center in 1973 to integrate the quaternion formulation for aircraft flight simulation and is commonly known as the local linearization method [7]. Yen and Cook [10] attempted to improve the local linearization process and developed a method to assess accuracy based upon two input

140 functions. Chiou and Jan [9] extended this work to include the assessment of the algorithm by using the input functions presented in [10].

Scale factor, drift, orthogonality and coning errors are the main causes of inaccuracy. Phillips and Hailey documented the effects of orthogonality error for an aircraft environment [7] and compared it to the impact of a drift error. The magnitude of coning errors for a third-order approximation method of attitude determination has also been studied [3].

An example of a simplified spacecraft attitude control system will be presented in this chapter. The accuracy for a fast rotation will be assessed when using the

CORDIC method for the calculation of the closed form solution to the kinematic equation in addition to more traditional approximations. A brief discussion regarding the orthogonality errors that is presented in [7] will be illustrated using the results of a computer animation. Another simulation will also be performed using the more model elaborate provided in [2] since many attitude control systems are developed from this reference [6]. Two additional angular rate profiles and one profile with coning errors are assessed using the methods presented in [9], [10] and [5] concludes this chapter.

6.2 Spacecraft Example

A closed-loop system that has been used as a basis for spacecraft attitude control systems that might need to support large angle perturbations for spacecraft

141 such as Clementine [6]. Quaternion kinematic control was used for the Apollo mission and has been subsequently used for other manned space missions e.g. Space

Shuttle, Space Station, and for unmanned planetary missions such as Clementine and

Cassini [6].

6.2.1. Single Axis Closed Loop Control

The closed loop equation to describe the control about a single axis of rotation without external forces present is a second order differential equation

DDTT D + KKcc+=0 (6.1) θ 1 JJθθ

where Tc is the vehicular torque, K and Ki are the angular error and rate gains, J is the moment of inertia and θ is the angular displacement. Since the angular velocity and acceleration can be respectively represented by α and ω, (1) can be rewritten as

TTcc αωθ+ KK1 +=0 . (6.2) JJ

The block diagram in Figure 6.1 depicts how the actual angular displacement can be described as a function of a desired angular input value.

142

Vehicle Response

ω actual θe α θ θdesired K TC 1/J ∫ ∫ - -

Figure 6.1 Second Order Linear System (Single Axis of Rotation and Control)

The equation that describes Figure 6.1 can be written as

TTTccc αωθθ++=KKK1 desired . (6.3) JJJ

6.2.2 Three Axis Attitude Control

Wie and Barba [2] described a three-axis spacecraft attitude control system for controlling large angle maneuvers that is based on the second order control equation presented in (3). This method will be used as the closed loop attitude control application for this example since it has demonstrated its stability [2] and is the basis for several more recent interplanetary missions [6]. A simplified block diagram for this quaternion based attitude control system is shown in Figure 6.2.

143

To reorient the vehicle a thrust Tc is applied that rotates the body. Gyroscopic sensors detect the incremental change in the rotational position Δθ of the vehicle. The data from the gyroscopes and the previous attitude information are used to compute the new vehicular attitude Q by solving the relevant kinematic equation. Less frequently the updated attitude is slightly modified to renormalize its magnitude. An error signal qerror completes the control loop by providing the new thrust requirements and is derived from the desired orientation and the updated attitude.

The attitude update solution is the most computationally intensive block and is also the portion of this system that is most prone to errors in accuracy. It is proposed to use the CORDIC based quaternion solution for the kinematic equation in this chapter.

The development of simplified equations describing the error quaternion are presented herein since the components illustrated in Figure 6.2 include a conjugated error term qerror* and a transposed Qdesired.

6.2.2.1 Error Quaternion Development

The conjugated error term qerror* can also be described as

⎡⎤⎡⎤QQ43dd−− QQQ 21 dd1 ⎢⎥−−QQ Q Q⎢⎥Q * T ⎢⎥34dd 1 d 2 d⎢⎥2 qQQerror=°=() desired actual ⎢⎥⎢⎥QQQQ21433dddd−−Q (6.4) ⎢⎥⎢⎥ ⎣⎦QQQQ1234dddd⎣⎦Q4

144 where the operator ° describes a quaternion multiplication operation and is analogous to the classical error equation

−θerror=−θθ desired + actual (6.5)

therefore since the classical error equation can be written as

θerror=θθ desired− actual . (6.6)

145

Vehicle Response dQ/ Q Normal- Q * α ω Kinematic dt actual T qerror Δ θ ∫ -T 1/J ∫ τ Equn ization Qdesired ° K C + Attitude Update

⎡K1⎤ ⎢ ⎥ K2 ⎢ ⎥ ⎣⎢K3⎦⎥

Figure 6.2 Vehicular Attitude Control Simulation Block Diagram

146

6.2.2.2 Control Equation

The selected control method is the first type described by Wie [2] that is a linear combination of rate feedback ω and the quaternion position error q

⎡T1 ⎤ TT= ⎢ ⎥ (6.7) ⎢ 2 ⎥ ⎣⎢T3 ⎦⎥

where

TKqK1111= e − ω (6.8)

TKqK2222= e − ω (6.9)

TKqK3333= e − ω (6.10)

and K, K1, , K2, K3 are positive control gains.

6.2.2.3 Vehicle Equation

The equations of motion for a solid body can be described in terms of the torque T generated by the spacecraft and its gyroscopic term

• JJTωω+ ×= ω (6.11)

where the moment of inertia, angular rate and external torque can be described as

147

⎡J1 00⎤ ⎡ω1 ⎤ ⎡T1 ⎤ JJ= ⎢ 00⎥ , ω = ⎢ω ⎥ , TT= ⎢ ⎥ . (6.12) ⎢ 2 ⎥ ⎢ 2 ⎥ ⎢ 2 ⎥ ⎣⎢ 00J3 ⎦⎥ ⎣⎢ω3 ⎦⎥ ⎣⎢T3 ⎦⎥

The angular acceleration can be solved by reorganizing (6.12) as

• ω =−×JT−−11 Jωω J (6.13)

which can be written in terms of the respective elements

⎡⎤• ω ⎢⎥1 −−11 ⎡⎤⎡⎤JJ1100 00⎛−⎡ 0ωω 3211⎤⎡ J 00 ⎤⎡ ω ⎤ ⎞ ⎢⎥• ⎢⎥⎢⎥⎜ ⎟ ωωωω=−00JTJ−−11 00⎢ 0 −⎥⎢ 00 J ⎥⎢ ⎥ (6.14) ⎢⎥22⎢⎥⎢⎥⎜ 2⎢ 3122⎥⎢ ⎥⎢ ⎥ ⎟ ⎢⎥• ⎢⎥⎢⎥00JJ−−11 00⎜⎟⎢−ωω 0⎥⎢ 00 J ⎥⎢ ω ⎥ ⎢⎥ω ⎣⎦⎣⎦332133⎝⎠⎣ ⎦⎣ ⎦⎣ ⎦ ⎣⎦3

and can also be expressed as

• −−11 ω11=−JT J 1() J 3223 − Jωω (6.15)

• −−11 ω22=−JTJ 21313() J − Jωω (6.16) and

• −−11 ω33=−JT J 3() J 2112 − Jωω (6.17)

and (6.15)-(6.17) would be the calculations that are performed in block 1/J in Figures

6.2 and 6.3.

148

Vehicle Response dQ/ Q Normal- Kinematic dt Qactual qerror α ω Δθ TC 1/J ∫ τ Equn ∫ ization °Qdesired K - (Taylor T Series Q actual Approx)

Transpose ⎡K1⎤ ⎢K2⎥ ⎢ ⎥ ⎣⎢K3⎦⎥

Figure 6.3 Modified System Diagram

149

6.2.2.4. Rate Sensing

The updated angular rate can be described as

• ω(1)ttt+=ωω + () (6.18)

• Where ω()t is the previous angular rate value and ω t describes the latest updated rate difference.

6.2.2.5. Integrated Rate

Since the kinematic solution is dependent upon the angular displacement instead of theangular rate, the integrated rate is integrated can be calculated from

Δθ =Δω1 t . (6.19)

6.2.2.6. Kinematic Solution (Attitude Update)

The particular kinematic equation that expresses the rotational motion of an object is a first order differential equation that characterizes the rate of attitude change with respect to a given reference. This equation incorporates the latest information gathered from incremental angle data gathered from rate integrating gyroscopic sensors with respect to its previous orientation. The solution to the differential equation is the previous attitude quaternion multiplied by an exponential matrix function with elements constructed from rate integrating gyroscope data.

150

The resulting exponential matrix function involves the use of trigonometric functions that are not easily computed with traditional on-board computers. Instead a

Taylor Series approximation to the closed form solution is often used [3][4]. For large angle maneuvers the resulting attitude errors due to truncation by function approximation could require additional updates to meet the requirements of the desired orientation.

6.2.2.6.1 Approximation

For large angle maneuvers, the approximation method used by Wie and Barba

[2] was originally presented by Cunningham [13] as an approximation technique based upon the Taylor Series method and can be described by

DR24( t) DQ() t Qt(+= T ) Qt () + Rt () − DQt2 () −ii + (6.20) iiii36

where

2 222 D =Δ()()()θ123 +Δθθ +Δ (6.21)

1 R =Δ[]θθθQQQ −Δ+Δ (6.22) 11423322

1 R =Δ[]θθθQQQ +Δ−Δ (6.23) 21324312

1 R =−Δ+Δ+Δ[]θθθQQQ (6.24) 31221342 and

151

1 R =−Δ−Δ−Δ[]θθQQQ θ. (6.25) 41122332

and ΔΔΔθ123,,θθ are the incremental angle outputs from the gyros over the quaternion integration step, T.

6.2.2.7 Orthogonality and Normalization

The unit quaternion must be maintained at a magnitude of one otherwise the coordinates become distorted and nonorthogonal. One method that can be applied periodically is

qQQnorm =−(1.5 0.5 ) (6.26)

Where |Q| represents the absolute value of the unnormalized quaternion [8].

6.2.3 Simplified Example

In the simplified example, the spacecraft shape is considered spherical, therefore the gyroscopic term ω × Jω is eliminated from the vehicle equations. The

2 values forTc , J, and K become 1 N-m, 1 kg-m and 3600 respectively. The values for

coefficients K1 , K2 , and K3 are chosen to be identical and equal to 60. The original orientation was selected as Q= [0.0050383, 0.0050383, 0.0050383, -

0.999962]T which is equivalent an Euler axis oriented 60 degrees from each major cardinal axis in the third quadrant. The desired orientation was selected as Q=

[0.0050383, 0.0050383, 0.0050383, 0.999962]T since this represents the equivalent Euler axis oriented 60 degrees from each axis in the first quadrant

152 requiring the object to rotate 180 degrees from its initial position. The incremental solid body rate information is sampled at a constant rate of 20 milliseconds. The equivalent displacement angle information that can be interpreted by integrating the sampled rate is shown in Figure 6.4 and provides a clearer interpretation of the performance characteristics. The three relationships depicted in each graph shown in

Figure 6.4 include the results when the closed form equation is used as the update method in addition to repeating the same simulation for both approximation method used by Wie and Barba, annotated as wb in the figure, and the CORDIC based algorithm. The derived acceleration and jerk graphs also depicted in Figure 6.4 verify that the CORDIC based algorithm closely follows the results obtained using the closed form equation during most of the rotation. The Wie Barba method has more jerk and therefore does not provide as smooth a transition from one orientation to another.

153

ang_wb -40 ang_cmplt ang_cord -30

-20 (degrees) -10 angular displacement 0 0 0.08 0.18 0.28 0.38 time (seconds)

alpha_wb 30000 alpha_cmplt alpha_cord 10000

-10000

-30000 ang acceleration (deg/sec^2) 0 0.08 0.18 0.28 0.38 time (seconds)

2000000 jerk_wb 1500000 ) 1000000 jerk_cmplt 500000 jerk_cord 0 -500000 -1000000

jerk (deg/sec^3 jerk -1500000 -2000000 -2500000 0 0.08 0.18 0.28 0.38 time (seconds)

Figure 6.4 Angular Displacement, Acceleration and Jerk as a Function of Time

154

6.2.3.1 Visual Representation

A graphical simulation using Macromedia Flash using the same process as described in Chapter 5, but this time the command, control, and equations of motion were imported from this chapter to visually assess the effect on the solid body. The process was repeated for the update calculation complete trigonometric solution, the

CORDIC implementation and the method suggested by Wie and Barba. Figure 6.5 illustrates the beginning, middle and end of the simulation that has the same parameters that generated the results in Figure 6.4. Note that cube that used the integration method suggested by Wie and Barba incurs severe distortion that is predominantly caused by the orthogonality error while the CORDIC implementation performs like the closed form solution.

155

Figure 6.5 Animation Results a) Wie-Barba, b) CORDIC, c) Closed Form

6.2.4 Attitude Control System Simulation Comparison

The CORDIC based attitude determination method was directly compared to the integration method presented in [2] by comparing their performance to the example given in [2] for re-orientation maneuvers after a spacecraft is released from the Space Shuttle payload bay.

The mission operations begin with the release of the spacecraft from the Space

Shuttle. All actuators (i.e. reaction jets or reaction wheels) shall be disabled and the spacecraft shall coast for 200 seconds due to Space Shuttle safety requirements. The

156 drift of the spacecraft during this time is expected to be 0.5 degrees per second in its transverse axes. After being separated for this period of time, the actuators will be enabled to orient the spacecraft in the desired attitude. This reorientation maneuver may be as large as 180 degrees. Note that the moments of inertia are

2 2 2 Jkgm1 =⋅10000 , Jkgm2 =⋅9000 , and Jkgm3 =12000 ⋅ . Each reaction jet is

capable of delivering thrust Tc up to 20 Newton-Meter. The control laws given in

(6.6)-(6.8) are used with K assigned the value 5, and K1, K2, K3 each assigned the 200 for their value. The solution was updated every 64 milliseconds and the simulation was performed over 600 seconds. Note that simulation was performed for the complete trigonometric closed form integration solution, the integration method described in (6.18)-(6.23) and using the CORDIC method previously described in

Chapter 3. The simulations were performed using c++ and the latter method was instantiated in 32 bit fixed point representation.

Figures 6.6 through 6.8 present the responses of the angular velocity, the first three quaternion elements and the fourth quaternion element and the control signals in the same format as Lim[12]. Note that since the angular displacement does not exceed

0.06 degrees during this maneuver the results from the approximation method presented in (6.18)-(6.23) are similar to the closed form trigonometric solution.

157

Wie- Barba

CORDIC

Closed Form

Figure 6.6 Response of Quaternion Elements q1, q2, q3 for Wie-Barba, CORDIC based and Closed Form Implementations

158

1.2

0.8 CORDIC 0.6 complt q4 wb 0.4

0.2

0 0 128 256 384 512 time (seconds)

Figure 6.7 Response of Quaternion Element q4

Wie- Barba

CORDIC

Closed Form

Figure 6.8 Response of Control Signals T1, T2, T3 for Wie-Barba, CORDIC based and Closed Form Implementations

159

6.2.5 Other Numerical Examples

To demonstrate the performance of the local linearization method presented in

[10] and an enhanced version [9], results from the integration methods were documented for the following two input data sets

ω1 = 2sin(t) , ω2 = 2sin(t) , ω3 =10sin( 0.5t) (6.27)

ω1 = 0.25sin( 12t) , ω2 = 0.25cos( 12t) , ω3 = 5sin() 0.25t . (6.28)

The performance was characterized for the CORDIC based implementation, the Wie-Barba integration method presented in (6.20)-(6.25) [2][13] and the closed form trigonometric solution. The update rate was selected as 10 milliseconds since this was the highest update rate used for the numerical examples given in [9][10]. The results are presented in Figures 6.9 through 6.14. The integration method in [13] that was also used by Wie[2] is not able to update properly since the maximum angular displacement is 5.8 degrees and 2.9 degrees from the first data and second sets respectively. The accuracy for this method is significantly degraded for this range of angular inputs. The function of output accuracy versus input angular displacement is explored further in Chapter 4.

160

1 0.8 0.6 0.4 0.2 0 q4 -0.2 -0.4 -0.6 -0.8 -1 020406080100 time (seconds)

Figure 6.9 Closed Form Solution for Data Set 1

0.5

CORDIC 0 q4 wb

-0.5

-1 0 20406080100 time (seconds)

Figure 6.10 Wie-Barba versus CORDIC Based Method Data Set 1

161

1.00E-04

5.00E-05

0.00E+00 q4 error -5.00E-05

-1.00E-04 020406080100 time (seconds)

Figure 6.11 CORDIC Based Method Errors for Data Set 1

1 0.8 0.6 0.4 0.2 0 q4 -0.2 -0.4 -0.6 -0.8 -1 020406080100 time (seconds)

Figure 6.12 Closed Form Solution for Data Set 2

162

0.5

CORDIC 0 q4 wb

-0.5

-1 0 20406080100 time (seconds)

Figure 6.13 Wie-Barba versus CORDIC Based Method Data Set 2

1.00E-04

5.00E-05

0.00E+00 q4 error -5.00E-05

-1.00E-04 020406080100 time (seconds)

Figure 6.14 CORDIC Based Method Errors for Data Set 2

163

6.2.6 Coning Error

A simple coning motion can be described as an angular rate vector [1]

⎡ ⎤ ⎢ωβcos( ωt )⎥ ⎢ ⎥ (6.29) ωωβωc = ⎢ sin(t )⎥ ⎢ 1 ⎥ ⎢ ωβ 2 ⎥ ⎣ 2 ⎦

where the half cone angle β is assumed to be small. The data set in (6.28) was modified to model a system with coning error

⎛⎞1 2 ω1 = ()(2 0.0174533 ) cos ( 2t ), ω2 =(2)( 0.0174533) sin( 2t) , ω3 =⎜⎟()(2 0.0174533 ) . ⎝⎠2

(6.30)

This data set was selected since it provides a more dynamic input to the system. The half cone angle was selected as 1 degree. The results shown in Figure 6.15 demonstrate the results from the CORDIC method are similar to the complete closed form trigonometric method whereas the method from [2] quickly degrades. The difference between the complete closed form method and the other two methods shown in Figure 6.16 also depicts this degradation.

164

1.0005

0.9995 cmplt 0.999 wb q4 CORDIC 0.9985

0.998

0.9975 0 102030405060708090 time (seconds)

Figure 6.15 Coning results for q4 for complete, CORDIC and Wie-Barba.

1.20E-03

1.00E-03

8.00E-04 wb 6.00E-04 CORDIC q4 error q4 4.00E-04

2.00E-04

0.00E+00 0 102030405060708090 time (seconds)

Figure 6.16 CORDIC and Wie-Barba Coning Differences

165

6.3 Conclusion

This chapter dealt with applying the CORDIC based algorithm to several aerospace examples, including a satellite deployment scenario. Spacecraft closed loop control methods were reviewed. In addition coning errors were computed and the response to sinusoidal inputs previously used to assess the accuracy of more complex methods was addressed. These analyses were repeated using the approximation from method from Wie and Barba [2]. In the case of the shuttle deployment, the two methods performed similarly, but for the rest of the examples the results from the method using the CORDIC based algorithm was better.

166

References

[1] F. L. Markley, Spacecraft Attitude Determination and Control, J.R. Wertz, Ed.; D. Reidel Publishing Company, 1978.

[2] B. Wie and P. M. Barba, "Quaternion Feedback for Spacecraft Large Angle Maneuvers", Journal of Guidance, Vol. 8, No. 3, pp. 360 - 365.

[3] M. Ronen and Y. Oshman, "Third-Order, Minimal-Parameter Solution of the Orthogonal Matrix Differential Equation", Journal of Guidance, Control, and Dynamics, Vol. 20, No. 3, May-June 1997, pp. 516-521

[4] V.N. Branets and I.P. Shmyglevskiy, "Application of Quaternions to Rigid Body Rotation Problems", NASA Technical Translation, NASA-TT-F-15414, 352 pages, July 1974.

[5] S. Gupta, "Linear Quaternion Equations with Application to Spacecraft Attitude Propagation", in 1998 IEEE Aerospace Conference Proceedings, Aspen, CO, pp. 69-76.

[6] J. L. Junkins, “Adventures on the Interface of Dynamics and Control”, AIAA Journal of Guidance, Control, and Dynamics, Vol. 20, No. 6, pp. 1058-1071.

[7] W. F. Phillips, C. E. Hailey, G. A. Gebert, “Review of Attitude Representations Used for Aircraft Kinematics”, AIAA Journal of Aircraft, Vol. 38, No. 4, July- August 2001, pp. 718-737.

[8] A.C. Fang and B. G. Zimmerman, “Digital Simulation of Rotational Kinematics”, NASA Technical Note D-5302, October 1969.

[9] J. C. Chiou, Y. W. Jan, S. D. Wu, “Family of Constraint-Preserving Integrators for Solving Quaternion Equations”, AIAA Journal of Guidance, Control, and Dynamics, Vol. 24, No. 1, January-February 2001, pp. 72-78.

[10] K. Yen, G. Cook, “Improved Local Linearization Algorithm for Solving the Quaternion Equations”, AIAA Journal Guidance and Control, Vol. 3, No. 3, September-October 1980, pp. 468-471.

[11] D. A. Pope, “An Exponential Method of Numerical Integration of Ordinary Differential Equations”, Communications of the ACM, Vol. 6, No. 8, 1963, pp. 491-493.

167

[12] S. Lim., “New Quaternion Feedback Control for Efficient Large Angle Maneuvers”, AIAA Guidance, Navigation and Control Conference, August 6-9, 2001, Montreal Canada

[13] D. C. Cunningham, T. P. Gismondi and G. W. Wilson, " System Design of the Annular Suspension and Pointing System (ASPS)", in Proceedings of the AIAA Guidance and Control Conference, Palo Alto, CA 1978, pp. 471-489.

168

CHAPTER 7

CONCLUSIONS AND FUTURE WORK

7.1 Summary of Contributions

A new method for computing attitude that uses angular sensor data obtained from rotating objects has been introduced. The method uses the CORDIC algorithm to compute the trignometric, arithmetic, and Euclidean norm functions involved in the attitude calculation. The CORDIC based computations can be implemented as an

FPGA or VLSI component.

Several variations of computational systems for the new attitude determination method have been developed, all of which can be implemented as FPGA and ASIC components. Three systems have been fully described in a hardware descriptive language (VHDL). All of the systems use the CORDIC algorithm as the processing element and compute the attitude of rotating objects from input sensor data and previous orientations. The systems demonstrate methods for decreasing computational time and physical size without greatly affecting computational accuracy.

A procedure for comparing errors in accuracy and orthogonality of the new computational method, which features fixed point arithmetic, with those of existing approximation methods that employ floating point arithmetic has been developed.

The procedure uses simulated random quaternion inputs and varying input

168

169 displacement angles. Results show that computed values obtained using the new method are more accurate at displacement angles greater than 5 degrees.

Motion capture applications of attitude determination to computer animation have been addressed for the new computational method. The errors in accuracy of the new method have been compared to those of other existing methods when input data is obtained from a MEMS rate gyro sensor block. The accuracy of the existing methods can vary widely, depending on the sensor sample rate or on how large the displacement angles are. The new method provides consistent results for these same conditions and was able to accurately track the closed form attitude calculation to within 0.002 degrees.

Applications of attitude determination to closed loop attitude control have been addressed. Closed loop attitude control simulations indicate that the fixed point notation based method of the dissertation yields results similar to those of existing floating point notation based approximation methods for small angular disturbances, but is superior for larger disturbances.

7.2 Conclusions

A method for computing the attitude of a moving object that has improved accuracy over previous techniques, particularly for large displacement angles, and a digital system that implements the method have been described. The system employs

170 a fixed point implementation, but yields an accuracy greater than that of customary approximation techniques that involve floating point operations. The method is applicable to real time computation of the orientation of moving objects when angular input data to the algorithm comes from sensors mounted on the bodies of the objects.

Various methods for representing the attitude of an object have been investigated and it has been concluded that the quaternion representation leads to fewer computations than other methods. This investigation is described in Chapter 2.

A brief review of quaternion arithmetic is also provided in the chapter. The quaternions are used in obtaining solutions to the kinematic equation for rotation.

Several methods for integrating the kinematic equation, which is a differential equation, have been reviewed. A method was used in which the current attitude is represented by a reference quaternion and the attitude is updated using sample values of angular rate inputs. This leads to a closed form approximation method for obtaining solutions to the kinematic equation for rotation that was selected because only present angular rate inputs are required in the computation. Coning and orthogonality errors are known consequences of use of the approximation method.

The method requires computation of trigonometric functions.

The CORDIC algorithm was investigated as a possible method to calculate the closed form solution to the kinematic equation since it can compute trigonometric functions and also perform square root, multiplication and division operations. A brief review of the algorithm is presented in Chapter 3. Three digital systems have been

171 designed to implement the CORDIC algorithm used in the attitude update computation. One system features a SIMD controller and four CORDIC processing elements. The second system features only one CORDIC processing element. This is a more power efficient system and power consumption is a more critical constraint than speed in most attitude determination applications. The third system uses a

CORDIC with a parallel scale factor correction and provides an increase in computational speed over the second system. VHDL code was developed to describe all three designs. The systems differ in size and speed, but all lead to approximately the same computational accuracy.

The accuracy, orthogonality, size, and speed of the attitude determination system that uses a single CORDIC processing element has been explored as a function of the width of the input data to the CORDIC processor. The exploration was based on simulation of the VHDL code description of the system. The results lead to a specification of the input bit width, which is to say the size of the CORDIC processor, and the number of CORDIC iterations required to achieve a prescribed accuracy. The results, which are tabulated in Chapter 4, can be used to specify a system when the output accuracy and orthogonality are known. They show, for example, that an accuracy of 24 bits is achieved with a system that has a bit width of 32 bits and completes a computation in 986 clock cycles. Taylor Series approximation methods of the closed form solution to the kinematic equation were also simulated, using both floating point and fixed point representations, and the accuracy and orthogonality errors were assessed. The 32-bit CORDIC based computational method is more

172 accurate than the commonly used fourth order floating point Taylor Series approximation method when the angular displacements exceed 5 degrees.

A significant application of the CORDIC based computational method is the capture of natural motion for use in computer graphics. Quaternion representation for the attitude of objects in computer animation has been widely accepted since development of the Spherical Linear Interpolation method for rotational updates between keyframes. To explore the accuracy that would be achieved in such an application, experiments were performed using angular rate data captured from human body motion consisting of arbitrary wrist motions. Results of the experiments are described in Chapter 5. Two sensor blocks were designed and constructed to determine the angular orientation of the hand. The blocks used MEMS gyroscopes of differing sensitivity, as expressed by the dynamic range of the angular rate measurement. CORDIC and Taylor series based methods, as well as the closed form solution, were used to post-process the attitude update calculation. Results verified that the CORDIC based method was more accurate at lower data sampling rates and higher angular body rates. The CORDIC based computation was always shown to lie within 0.0005 degrees of the closed form solution, while the Taylor series based computation exhibited errors as high as 0.3 degrees for the fourth order approximation and up to 50 degrees for the first order Taylor series approximation.

To observe application of the computational methods to computer animation, the wrist motion was converted to visual animated form using a graphical block that

173 rotated with the same motion as the wrist, as determined by the quaternion computations. Both Taylor series and CORDIC based computations were used. The latter replicated normal motion while the former exhibited clearly inaccurate effects.

At a 50 Hz. update rate, the system that implements the CORDIC based computation, when implemented as an FPGA, dissipates 31 milliwatts, and, when implemented as an ASIC, dissipates 100 microwatts. These values, which are obtained from simulation results using Synopsys Power Compiler and Xilinx Xpower, appear modest and support the usefulness of the computational method to portable systems .

Potential aerospace applications of attitude determination using the CORDIC based method have also been explored. Simulations were performed using typical shuttle data compiled by Wie and Barba and available in the literature. The shuttle data does not exhibit rapidly changing rotations. Quaternion trajectories were computed using a model for a satellite attitude control system that employs the

CORDIC based fixed point method. The model is described in C++. Results of the simulations, which are presented in Chapter 6, were compared with those of Wie and

Barba for the same input satellite data. A C++ model for the Wie and Barba model was written in order to make the comparison. The results of the simulations show that the two methods exhibit similar accuracy. A similar comparison made for a rapidly moving object, in which rotations as large as 40 degrees between samples occur, show the Wie and Barba model to incur substantially greater jerk error than the CORDIC based model. When applied to an animation model, the CORDIC based method shows a close resemblance to values obtained from a closed form solution. The Wie

174 and Barba model fails under these conditions. Simulations using sinusoidal angular inputs and also using coning motion inputs were also performed on both the CORDIC based and Wie and Barba models and showed similar results. The CORDIC based method was able to update the attitude effectively, whereas the other method degraded significantly.

7.3 Future Directions

7.3.1 System Design

The characteristics of three different system level architectures for implementing the CORDIC processor were explored. They included both single and multiple processor versions and an enhanced version that incorporated parallel scale factor compensation in the design. The latter implementation used two’s complement arithmetic. This was done to maintain consistency in comparisons among the methods in the thesis. On further reflection, and based on the results obtained by Villalba, a

CORDIC with a sign magnitude representation might simplify the scale factor compensation circuitry. Also, other CORDIC structures could be explored. For instance a CORDIC that has a serial bit input could reduce the physical area requirements for the die.

7.3.2 Motion Detection Applications

Potential application of the CORDIC based attitude determination method to problems in the field of natural motion detection and computer animation seems very

175 broad. The CORDIC based device can help facilitate the design of portable motion monitors that can be very small in size and can function using limited power. Small devices based on the concept can be developed that are self contained and can represent a distributed processing system that provides reduced harnessing and/or wireless transmission problems and alters the relationship between local processors and a central processing station.

In addition, the motion capture feature could enhance personal navigation systems that use GPS, since the orientation of the user would be known and would enable fusion of the information between orientation and location. For example, the head-mounted display of a user could move virtual scenery as the person rotates his head and provide updated locations based on combined head mounted processor data and GPS information.

7.3.3 Navigational Applications

In recent years there has been a significant effort to develop MEMS-based

IMU’s (Inertial Measurement Units). Work on accurate and efficient systems for attitude update computations, as described in this thesis, fits into this activity.

Integrated IMU’s and distributed computational systems seem a natural merger in the effort to design strap-down navigation components of all kinds for determining local, as well as, large scale motions.